From e2f21cefd042b3c565b0f0530f8b23edce5a3bed Mon Sep 17 00:00:00 2001 From: Michael Zechmair Date: Mon, 16 May 2022 13:48:01 +0200 Subject: [PATCH] Explicitly compute link velocity when retrieving joint states --- arm_1dof/robot_arm_1dof.py | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/arm_1dof/robot_arm_1dof.py b/arm_1dof/robot_arm_1dof.py index 9081cf9..3b40c3f 100644 --- a/arm_1dof/robot_arm_1dof.py +++ b/arm_1dof/robot_arm_1dof.py @@ -45,6 +45,7 @@ class RobotArm1Dof: physicsClientId=self._bullet_ctrl.server_id) self._ee_link_state = pybullet.getLinkState(self._body_id, RobotArm1Dof.HAND_LINK_ID, \ + computeLinkVelocity=True, physicsClientId=self._bullet_ctrl.server_id) def JointPos(self, joint_id) -> float: @@ -63,8 +64,8 @@ class RobotArm1Dof: """Get end effector vel, linear and angular""" return (self._ee_link_state[6], self._ee_link_state[7]) - def SetJointTorque(self, joint_ids, torques) -> None: - """Sets torque applied to specified joint""" - pybullet.setJointMotorControl2(self._body_id, joint_ids, \ + def SetJointTorques(self, joint_ids, torques) -> None: + """Sets torques applied to specified joints.""" + pybullet.setJointMotorControlArray(self._body_id, jointIndices=joint_ids, \ controlMode=pybullet.TORQUE_CONTROL, forces=torques, \ physicsClientId=self._bullet_ctrl.server_id) \ No newline at end of file