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