Here are the functions I use to set joint positions and speeds. self.joint_map is used to get the joint index. I'm using the default max_force in my test.
Code: Select all
def set_joint_positions(self, joint_positions: dict, speed: float):
for joint, position in joint_positions.items():
p.setJointMotorControl2(self.device, self.joint_map[joint], p.POSITION_CONTROL,
targetPosition=position,
maxVelocity=speed)
def set_joint_speeds(self, joint_speeds: dict, max_force: float = 500.0):
"""Set the speed of each joint in rads/s"""
for joint, velocity in joint_speeds.items():
p.setJointMotorControl2(self.device, self.joint_map[joint], p.VELOCITY_CONTROL,
targetVelocity=velocity,
force=max_force)
Code: Select all
u = [0.5, 0.0, 0.0, 0.0, 0.0, 0.0] #np.dot(J.T, v)
signals = {name: vel for name, vel in zip(robot.JOINT_NAMES, u)}
robot.set_joint_speeds(signals)