def compute_ik(self, position, orientation, qinit=None, numerical=True): """ Inverse kinematics computes joints for robot arm to desired end-effector pose (w.r.t. 'ARM_BASE_FRAME'). :param position: end effector position (shape: :math:`[3,]`) :param orientation: end effector orientation. It can be quaternion ([x,y,z,w], shape: :math:`[4,]`), euler angles (yaw, pitch, roll angles (shape: :math:`[3,]`)), or rotation matrix (shape: :math:`[3, 3]`) :param qinit: initial joint positions for numerical IK :param numerical: use numerical IK or analytical IK :type position: np.ndarray :type orientation: np.ndarray :type qinit: list :type numerical: bool :return: None or joint positions :rtype: np.ndarray """ position = position.flatten() if orientation.size == 4: quat = orientation.flatten() elif orientation.size == 3: quat = prutil.euler_to_quat(orientation) elif orientation.size == 9: quat = prutil.rot_mat_to_quat(orientation) else: raise TypeError("Orientation must be in one " "of the following forms:" "rotation matrix, euler angles, or quaternion") # This is needed to conform compute_ik to core.py's compute ik quat_world_to_base_link = np.asarray( self.arm_base_link.get_quaternion()) quat = self._quaternion_multiply(quat_world_to_base_link, quat) rot_world_to_base_link = np.transpose( prutil.quat_to_rot_mat( np.asarray(self.arm_base_link.get_quaternion()))) position = np.matmul(position, rot_world_to_base_link) position = position + np.asarray(self.arm_base_link.get_position()) print(position, prutil.quat_to_rot_mat(quat)) # print(position, quat) try: joint_angles = self.arm.solve_ik(position.tolist(), euler=None, quaternion=quat.tolist()) except IKError as error: print(error) return None joint_angles = np.asarray(joint_angles) return joint_angles
def make_plan_pose( self, position, orientation, wait=True, numerical=True, **kwargs ): if not self.use_moveit: raise ValueError( "Using plan=True when moveit is not" " initialized, did you pass " "in use_moveit=True?" ) pose = Pose() position = position.flatten() if orientation.size == 4: orientation = orientation.flatten() ori_x = orientation[0] ori_y = orientation[1] ori_z = orientation[2] ori_w = orientation[3] elif orientation.size == 3: quat = prutil.euler_to_quat(orientation) ori_x = quat[0] ori_y = quat[1] ori_z = quat[2] ori_w = quat[3] elif orientation.size == 9: quat = prutil.rot_mat_to_quat(orientation) ori_x = quat[0] ori_y = quat[1] ori_z = quat[2] ori_w = quat[3] else: raise TypeError( "Orientation must be in one " "of the following forms:" "rotation matrix, euler angles, or quaternion" ) pose.position.x, pose.position.y, pose.position.z = ( position[0], position[1], position[2], ) ( pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w, ) = (ori_x, ori_y, ori_z, ori_w) result = self.moveit_group.motionPlanToPose(pose, wait=True) return [p.positions for p in result]
def test_ee_pose_control(create_robot, position, orientation, numerical): bot = create_robot bot.arm.go_home() time.sleep(1) bot.arm.set_ee_pose(position=position, orientation=orientation, numerical=numerical) time.sleep(1) trans, rot, quat = bot.arm.pose_ee pos_error = np.linalg.norm(position.flatten() - trans.flatten()) if orientation.size == 4: tgt_quat = orientation.flatten() elif orientation.size == 3: tgt_quat = prutil.euler_to_quat(orientation) elif orientation.size == 9: tgt_quat = prutil.rot_mat_to_quat(orientation) else: raise TypeError("Orientation must be in one of the following forms:" "rotation matrix, euler angles, or quaternion") quat_diff = tft.quaternion_multiply(tft.quaternion_inverse(tgt_quat), quat) rot_similarity = quat_diff[3] assert rot_similarity > 0.98 and pos_error < 0.02
def compute_ik(self, position, orientation, qinit=None, numerical=True): """ Inverse kinematics :param position: end effector position (shape: :math:`[3,]`) :param orientation: end effector orientation. It can be quaternion ([x,y,z,w], shape: :math:`[4,]`), euler angles (yaw, pitch, roll angles (shape: :math:`[3,]`)), or rotation matrix (shape: :math:`[3, 3]`) :param qinit: initial joint positions for numerical IK :param numerical: use numerical IK or analytical IK :type position: np.ndarray :type orientation: np.ndarray :type qinit: list :type numerical: bool :return: None or joint positions :rtype: np.ndarray """ position = position.flatten() if orientation.size == 4: orientation = orientation.flatten() ori_x = orientation[0] ori_y = orientation[1] ori_z = orientation[2] ori_w = orientation[3] elif orientation.size == 3: quat = prutil.euler_to_quat(orientation) ori_x = quat[0] ori_y = quat[1] ori_z = quat[2] ori_w = quat[3] elif orientation.size == 9: quat = prutil.rot_mat_to_quat(orientation) ori_x = quat[0] ori_y = quat[1] ori_z = quat[2] ori_w = quat[3] else: raise TypeError("Orientation must be in one " "of the following forms:" "rotation matrix, euler angles, or quaternion") if qinit is None: qinit = self.get_joint_angles().tolist() elif isinstance(qinit, np.ndarray): qinit = qinit.flatten().tolist() if numerical: pos_tol = self.configs.ARM.IK_POSITION_TOLERANCE ori_tol = self.configs.ARM.IK_ORIENTATION_TOLERANCE req = IkCommandRequest() req.init_joint_positions = qinit req.pose = [ position[0], position[1], position[2], ori_x, ori_y, ori_z, ori_w, ] req.tolerance = 3 * [pos_tol] + 3 * [ori_tol] try: resp = self._ik_service(req) except: rospy.logerr("IK Service call failed") resp = IkCommandResponse() resp.success = False if not resp.success: joint_positions = None else: joint_positions = resp.joint_positions else: if not hasattr(self, "ana_ik_solver"): raise TypeError("Analytical solver not provided, " "please use `numerical=True`" "to use the numerical method " "for inverse kinematics") else: joint_positions = self.ana_ik_solver.get_ik( qinit, position[0], position[1], position[2], ori_x, ori_y, ori_z, ori_w, ) if joint_positions is None: return None print(joint_positions) return np.array(joint_positions)
def set_ee_pose(self, position, orientation, plan=True, wait=True, numerical=True, **kwargs): """ Commands robot arm to desired end-effector pose (w.r.t. 'ARM_BASE_FRAME'). Computes IK solution in joint space and calls set_joint_positions. Will wait for command to complete if wait is set to True. :param position: position of the end effector (shape: :math:`[3,]`) :param orientation: orientation of the end effector (can be rotation matrix, euler angles (yaw, pitch, roll), or quaternion) (shape: :math:`[3, 3]`, :math:`[3,]` or :math:`[4,]`) The convention of the Euler angles here is z-y'-x' (intrinsic rotations), which is one type of Tait-Bryan angles. :param plan: use moveit the plan a path to move to the desired pose :param wait: wait until the desired pose is achieved :param numerical: use numerical inverse kinematics solver or analytical inverse kinematics solver :type position: np.ndarray :type orientation: np.ndarray :type plan: bool :type wait: bool :type numerical: bool :return: Returns True if command succeeded, False otherwise :rtype: bool """ if orientation.size == 4: quat = orientation.flatten() elif orientation.size == 3: quat = prutil.euler_to_quat(orientation) elif orientation.size == 9: quat = prutil.rot_mat_to_quat(orientation) quat_world_to_base_link = np.asarray( self.arm_base_link.get_quaternion()) quat = self._quaternion_multiply(quat_world_to_base_link, quat) rot_world_to_base_link = np.transpose( prutil.quat_to_rot_mat( np.asarray(self.arm_base_link.get_quaternion()))) position = np.matmul(position, rot_world_to_base_link) position = position + np.asarray(self.arm_base_link.get_position()) try: arm_path = self.arm.get_path(position.tolist(), quaternion=quat.tolist(), ignore_collisions=False) except ConfigurationPathError as error: print(error) return False # arm_path.visualize() # done = False # while not done: # done = arm_path.step() # pr.step() # arm_path.clear_visualization() arm_path.set_to_end() return True
def set_ee_pose( self, position, orientation, plan=True, wait=True, numerical=True, **kwargs ): """ Commands robot arm to desired end-effector pose (w.r.t. 'ARM_BASE_FRAME'). Computes IK solution in joint space and calls set_joint_positions. Will wait for command to complete if wait is set to True. :param position: position of the end effector (shape: :math:`[3,]`) :param orientation: orientation of the end effector (can be rotation matrix, euler angles (yaw, pitch, roll), or quaternion) (shape: :math:`[3, 3]`, :math:`[3,]` or :math:`[4,]`) The convention of the Euler angles here is z-y'-x' (intrinsic rotations), which is one type of Tait-Bryan angles. :param plan: use moveit the plan a path to move to the desired pose :param wait: wait until the desired pose is achieved :param numerical: use numerical inverse kinematics solver or analytical inverse kinematics solver :type position: np.ndarray :type orientation: np.ndarray :type plan: bool :type wait: bool :type numerical: bool :return: Returns True if command succeeded, False otherwise :rtype: bool """ if plan: if not self.use_moveit: raise ValueError( "Using plan=True when moveit is not" " initialized, did you pass " "in use_moveit=True?" ) pose = Pose() position = position.flatten() if orientation.size == 4: orientation = orientation.flatten() ori_x = orientation[0] ori_y = orientation[1] ori_z = orientation[2] ori_w = orientation[3] elif orientation.size == 3: quat = prutil.euler_to_quat(orientation) ori_x = quat[0] ori_y = quat[1] ori_z = quat[2] ori_w = quat[3] elif orientation.size == 9: quat = prutil.rot_mat_to_quat(orientation) ori_x = quat[0] ori_y = quat[1] ori_z = quat[2] ori_w = quat[3] else: raise TypeError( "Orientation must be in one " "of the following forms:" "rotation matrix, euler angles, or quaternion" ) pose.position.x, pose.position.y, pose.position.z = ( position[0], position[1], position[2], ) ( pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w, ) = (ori_x, ori_y, ori_z, ori_w) result = self.moveit_group.moveToPose(pose, wait=True) else: joint_positions = self.compute_ik( position, orientation, numerical=numerical ) result = False if joint_positions is None: rospy.logerr("No IK solution found; check if target_pose is valid") else: result = self.set_joint_positions(joint_positions, plan=plan, wait=wait) return result