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 compute_fk_position(self, joint_positions, des_frame): """ Given joint angles, compute the pose of desired_frame with respect to the base frame (self.configs.ARM.ARM_BASE_FRAME). The desired frame must be in self.arm_link_names :param joint_positions: joint angles :param des_frame: desired frame :type joint_positions: np.ndarray :type des_frame: string :return: translational vector and rotational matrix :rtype: np.ndarray, np.ndarray """ joint_positions = joint_positions.flatten() req = FkCommandRequest() req.joint_angles = list(joint_positions) req.end_frame = des_frame try: resp = self._fk_service(req) except: rospy.logerr("FK Service call failed") resp = FkCommandResponse() resp.success = False if not resp.success: return None pos = np.asarray(resp.pos).reshape(3, 1) rot = prutil.quat_to_rot_mat(resp.quat) return pos, rot
def get_transform(self, src_frame, dest_frame): """ Return the transform from the src_frame to dest_frame :param src_frame: source frame :param dest_frame: destination frame :type src_frame: string :type dest_frame: basestring :return: tuple (trans, rot_mat, quat ) trans: translational vector (shape: :math:`[3, 1]`) rot_mat: rotational matrix (shape: :math:`[3, 3]`) quat: rotational matrix in the form of quaternion (shape: :math:`[4,]`) :rtype: tuple or ROS PoseStamped """ trans, quat = prutil.get_tf_transform(self.tf_listener, src_frame, dest_frame) rot_mat = prutil.quat_to_rot_mat(quat) trans = np.array(trans).reshape(-1, 1) rot_mat = np.array(rot_mat) quat = np.array(quat) return trans, rot_mat, quat
def get_ee_pose(self, base_frame=None): """ Return the end effector pose with respect to the base_frame :param base_frame: reference frame :type base_frame: string :return: tuple (trans, rot_mat, quat) trans: translational vector (shape: :math:`[3, 1]`) rot_mat: rotational matrix (shape: :math:`[3, 3]`) quat: rotational matrix in the form of quaternion (shape: :math:`[4,]`) :rtype: tuple or ROS PoseStamped """ # raise NotImplementedError pos = (self.ee_link.get_position(), ) quat = self.ee_link.get_quaternion() pos = np.asarray(pos).flatten() quat = np.asarray(quat).flatten() rot = prutil.quat_to_rot_mat(quat) return pos, rot, quat
def pose_ee(self): """ Return the end effector pose w.r.t 'ARM_BASE_FRAME' :return: trans: translational vector (shape: :math:`[3, 1]`) rot_mat: rotational matrix (shape: :math:`[3, 3]`) quat: rotational matrix in the form of quaternion (shape: :math:`[4,]`) :rtype: tuple (trans, rot_mat, quat) """ pos = (self.ee_link.get_position(self.arm_base_link), ) quat = self.ee_link.get_quaternion(self.arm_base_link) # print("#################Debug start########################") # print(quat) # print("#################Debug end########################") pos = np.asarray(pos).flatten() quat = np.asarray(quat).flatten() rot = prutil.quat_to_rot_mat(quat) return pos, rot, quat
def get_link_transform(self, src, tgt): """ Returns the latest transformation from the target_frame to the source frame, i.e., the transform of source frame w.r.t target frame. If the returned transform is applied to data, it will transform data in the source_frame into the target_frame For more information, please refer to http://wiki.ros.org/tf/Overview/Using%20Published%20Transforms :param src: source frame :param tgt: target frame :type src: string :type tgt: string :returns: tuple(trans, rot, T) trans: translational vector (shape: :math:`[3,]`) rot: rotation matrix (shape: :math:`[3, 3]`) T: transofrmation matrix (shape: :math:`[4, 4]`) :rtype: tuple(np.ndarray, np.ndarray, np.ndarray) """ trans, quat = prutil.get_tf_transform(self._tf_listener, tgt, src) rot = prutil.quat_to_rot_mat(quat) T = np.eye(4) T[:3, :3] = rot T[:3, 3] = trans return trans, rot, T
def compute_fk_position(self, joint_positions): """ Given joint angles, compute the pose of desired_frame with respect to the base frame (self.configs.ARM.ARM_BASE_FRAME). The desired frame must be in self.arm_link_names :param joint_positions: joint angles :param des_frame: desired frame :type joint_positions: np.ndarray :type des_frame: string :return: translational vector and rotational matrix :rtype: np.ndarray, np.ndarray """ # raise NotImplementedError pos, quat = self.ee_link.get_position(), self.ee_link.get_quaternion() pos = np.asarray(pos) rot = prutil.quat_to_rot_mat(quat) return pos, rot
def _rot_matrix(self, habitat_quat): quat_list = [habitat_quat.x, habitat_quat.y, habitat_quat.z, habitat_quat.w] return prutil.quat_to_rot_mat(quat_list)
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