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