def solve_ik(self, position: List[float], euler: List[float] = None, quaternion: List[float] = 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 = vrep.simCheckIkGroup( self._ik_group, [j.get_handle() for j in self.joints]) if ik_result == vrep.sim_ikresult_fail: raise IKError( 'IK failed. Perhaps the distance was between the tip ' ' and target was too large.') elif ik_result == vrep.sim_ikresult_not_performed: raise IKError('IK not performed.') return joint_values
def solve_ik(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. 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
def ik(self, H_target, q_guess): ''' Ik slover for panda para --- H_target: target Homegenous Matrix q_guess: init joint return --- list[7] speed --- ~= 80ms ''' def opt_fun(q): return np.linalg.norm(self.fk(q) - H_target) res = scipy.optimize.minimize(opt_fun,q_guess, method='L-BFGS-B', bounds=self.joint_bonds, tol=1e-8, options={'maxiter':1000}) if res.success: return res.x else: raise IKError('d')
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