dm_robotics
dm_robotics copied to clipboard
How to set joint position for position actuators
Hi, I'm trying to adapt my robot that utilizes position actuators to the RGB_stacking environment. I implemented the python class for the robot based on the sawyer robot implementation, but it keeps failing with the following error:
WARNING:absl:Unable to solve the inverse kinematics for ref_pose: Pose(position=[0.54648661 0.12579726 0.4226723 ], quaternion=[ 6.03993497e-17 9.86396237e-01 -1.64385105e-01 1.00656846e-17])
The following is how I set the joint angles:
def set_joint_angles(self, physics: mjcf.Physics, joint_angles: np.ndarray) -> None:
physics_joints = models_utils.binding(physics, self._joints)
physics_actuators = models_utils.binding(physics, self._actuators)
physics_joints.qpos[:] = joint_angles
physics_joints.qvel[:] = np.zeros_like(joint_angles)
physics.forward()
Can anyone help me locate the problem? Thank you very much!