def get_workspace_poses(self, name): ws_raw = self.__ws_manager.read(name) poses = [ RobotState(position=Point(*pose[0]), rpy=RPY(*pose[1])) for pose in ws_raw.robot_poses ] return poses
def _get_new_joints_w_ik(self, shift_command): quat_jog = quaternion_from_euler(shift_command[3], shift_command[4], shift_command[5]) quat_target = quaternion_multiply(quat_jog, [ self._last_robot_state_published.orientation.x, self._last_robot_state_published.orientation.y, self._last_robot_state_published.orientation.z, self._last_robot_state_published.orientation.w ]) rpy_target = RPY(*euler_from_quaternion(quat_target)) self._new_robot_state = RobotState() self._new_robot_state.position.x = self._last_robot_state_published.position.x + shift_command[ 0] self._new_robot_state.position.y = self._last_robot_state_published.position.y + shift_command[ 1] self._new_robot_state.position.z = self._last_robot_state_published.position.z + shift_command[ 2] self._new_robot_state.orientation = Quaternion(*quat_target) self._new_robot_state.rpy = rpy_target self.__validate_params_pose(self._new_robot_state) success, joints = self.__kinematics_handler.get_inverse_kinematics( Pose(self._new_robot_state.position, self._new_robot_state.orientation)) return success, joints
def get_forward_kinematics(self, joints): """ Get forward kinematic from joints :param joints: list of joints value :type joints: Pose :return: A RobotState object """ try: rospy.wait_for_service('compute_fk', 2) except (rospy.ServiceException, rospy.ROSException) as e: rospy.logerr("Arm commander - Impossible to connect to FK service : " + str(e)) return RobotState() try: moveit_fk = rospy.ServiceProxy('compute_fk', GetPositionFK) fk_link = ['base_link', 'tool_link'] header = Header(0, rospy.Time.now(), "world") rs = RobotStateMoveIt() rs.joint_state.name = self.__arm_state.joints_name rs.joint_state.position = joints response = moveit_fk(header, fk_link, rs) except rospy.ServiceException as e: rospy.logerr("Arm commander - Failed to get FK : " + str(e)) return RobotState() pose = self.__transform_handler.ee_link_to_tcp_pose_target(response.pose_stamped[1].pose, "tool_link") quaternion = [pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w] rpy = euler_from_quaternion(quaternion) quaternion = (round(quaternion[0], 3), round(quaternion[1], 3), round(quaternion[2], 3), round(quaternion[3], 3)) point = (round(pose.position.x, 3), round(pose.position.y, 3), round(pose.position.z, 3)) return RobotState(position=Point(*point), rpy=RPY(*rpy), orientation=Quaternion(*quaternion))
def save_pose(name, x, y, z, roll, pitch, yaw): req = ManagePoseRequest() req.cmd = ManagePoseRequest.SAVE req.pose.name = name req.pose.position = Point(x, y, z) req.pose.rpy = RPY(roll, pitch, yaw) return call_service('/niryo_robot_poses_handlers/manage_pose', ManagePose, req)
def save_workspace(name, list_poses_raw): list_poses = [] for p in list_poses_raw: point = Point(*p[0]) rpy = RPY(*p[1]) list_poses.append(RobotState(point, rpy, Quaternion())) req = ManageWorkspaceRequest() req.cmd = ManageWorkspaceRequest.SAVE req.workspace.name = name req.workspace.poses = list_poses return call_service('/niryo_robot_poses_handlers/manage_workspace', ManageWorkspace, req)
def set_rpy_target(self, arm_cmd): """ Set MoveIt target to a rpy target Then execute the trajectory to the target :param arm_cmd: ArmMoveCommand message containing target values :type : ArmMoveCommand :return: status, message """ tcp_roll, tcp_pitch, tcp_yaw = arm_cmd.rpy.roll, arm_cmd.rpy.pitch, arm_cmd.rpy.yaw ee_roll, ee_pitch, ee_yaw = self.__transform_handler.tcp_to_ee_link_rpy_target( tcp_roll, tcp_pitch, tcp_yaw, self.__end_effector_link) self.__validate_params_move(ArmMoveCommand.RPY, RPY(ee_roll, ee_pitch, ee_yaw)) self.__arm.set_rpy_target([ee_roll, ee_pitch, ee_yaw], self.__end_effector_link) return self.__traj_executor.compute_and_execute_plan_to_target()
def get_pose(self, name): """ Get pose from pose manager and return NiryoPose :param name: pose name :type name: str :return: The pose object :rtype: NiryoPose """ pose_raw = self.__pos_manager.read(name) pose = NiryoPose() pose.joints = pose_raw.joints pose.position = Point(*pose_raw.position) pose.rpy = RPY(*pose_raw.rpy) pose.orientation = Quaternion(*pose_raw.orientation) return pose