def get_jacobian(self):
"""Calculates the Jacobian.
:return: the row-major Jacobian matix.
"""
self._ik_target.set_matrix(self._ik_tip.get_matrix())
sim.simCheckIkGroup(self._ik_group,
[j.get_handle() for j in self.joints])
jacobian, (rows, cols) = sim.simGetIkGroupMatrix(self._ik_group, 0)
jacobian = np.array(jacobian).reshape((rows, cols), order='F')
return jacobian
def solve_ik(
self,
position: Union[List[float], np.ndarray],
euler: Union[List[float], np.ndarray] = None,
quaternion: Union[List[float], np.ndarray] = None) -> List[float]:
"""Solves an IK group and returns the calculated joint values.
Must specify either rotation in euler or quaternions, but not both!
:param position: The x, y, z position of the target.
:param euler: The x, y, z orientation of the target (in radians).
:param quaternion: A list containing the quaternion (x,y,z,w).
:return: A list containing the calculated joint values.
"""
self._ik_target.set_position(position)
if euler is not None:
self._ik_target.set_orientation(euler)
elif quaternion is not None:
self._ik_target.set_quaternion(quaternion)
ik_result, joint_values = sim.simCheckIkGroup(
self._ik_group, [j.get_handle() for j in self.joints])
if ik_result == sim.sim_ikresult_fail:
raise IKError(
'IK failed. Perhaps the distance was between the tip '
' and target was too large.')
elif ik_result == sim.sim_ikresult_not_performed:
raise IKError('IK not performed.')
return joint_values
def solve_ik_via_jacobian(self,
position: Union[List[float], np.ndarray],
euler: Union[List[float], np.ndarray] = None,
quaternion: Union[List[float],
np.ndarray] = None,
relative_to: Object = None) -> List[float]:
"""Solves an IK group and returns the calculated joint values.
This IK method performs a linearisation around the current robot
configuration via the Jacobian. The linearisation is valid when the
start and goal pose are not too far away, but after a certain point,
linearisation will no longer be valid. In that case, the user is better
off using 'solve_ik_via_sampling'.
Must specify either rotation in euler or quaternions, but not both!
:param position: The x, y, z position of the target.
:param euler: The x, y, z orientation of the target (in radians).
:param quaternion: A list containing the quaternion (x,y,z,w).
:param relative_to: Indicates relative to which reference frame we want
the target pose. Specify None to retrieve the absolute pose,
or an Object relative to whose reference frame we want the pose.
:return: A list containing the calculated joint values.
"""
self._ik_target.set_position(position, relative_to)
if euler is not None:
self._ik_target.set_orientation(euler, relative_to)
elif quaternion is not None:
self._ik_target.set_quaternion(quaternion, relative_to)
ik_result, joint_values = sim.simCheckIkGroup(
self._ik_group, [j.get_handle() for j in self.joints])
if ik_result == sim.sim_ikresult_fail:
raise IKError(
'IK failed. Perhaps the distance was between the tip '
' and target was too large.')
elif ik_result == sim.sim_ikresult_not_performed:
raise IKError('IK not performed.')
return joint_values
