def __callback_workspace_poses(self, req):
try:
ws = self.ws_manager.read(req.workspace)
return (200, "SUCCESS", ws.robot_poses[0], ws.robot_poses[1],
ws.robot_poses[2], ws.robot_poses[3])
except Exception as e:
return 400, str(
e), RobotState(), RobotState(), RobotState(), RobotState()
def publish_state(self, event):
msg = RobotState()
msg.position.x = self.position[0]
msg.position.y = self.position[1]
msg.position.z = self.position[2]
msg.rpy.roll = self.rpy[0]
msg.rpy.pitch = self.rpy[1]
msg.rpy.yaw = self.rpy[2]
self.niryo_one_robot_state_publisher.publish(msg)
def from_dict(cls, dict_):
ws = cls(dict_["name"])
ws.robot_poses = [RobotState(position=Point(*pose_list[:3]), rpy=RPY(*pose_list[3:]))
for pose_list in dict_["robot_poses"]]
ws.points = dict_["points"]
ws.x_width = dict_["x_width"]
ws.y_width = dict_["y_width"]
ws.yaw_center = dict_["yaw_center"]
ws.position_matrix = np.array(dict_["position_matrix"])
ws.rotation_matrix = np.array(dict_["rotation_matrix"])
return ws
def list_to_robot_state_msg(list_pos):
"""
Translate (x, y, z, roll, pitch, yaw) to a RobotState Object
"""
r = RobotState()
r.position.x = list_pos[0]
r.position.y = list_pos[1]
r.position.z = list_pos[2]
r.rpy.roll = list_pos[3]
r.rpy.pitch = list_pos[4]
r.rpy.yaw = list_pos[5]
return r
def __callback_target_pose(self, req):
try:
if req.grip == req.GRIP_AUTO:
pose = self.get_target_pose_auto_grip(req.workspace,
req.tool_id,
req.height_offset,
req.x_rel, req.y_rel,
req.yaw_rel)
else:
pose = self.get_target_pose(req.workspace, req.grip,
req.height_offset, req.x_rel,
req.y_rel, req.yaw_rel)
return 200, "SUCCESS", pose
except Exception as e:
return 400, str(e), RobotState()
def remove_workspace(self, name):
"""
Call worksspace manager to remove a certain workspace
:param name: workspace name
:type name: str
:return: status, message
:rtype: (GoalStatus, str)
"""
r = RobotState() # dummy state needed to call the service
result = self.call_service(
'niryo_one/pose_converter/edit_workspace', EditWorkspace,
[-1, name, r, r, r, r])
if result.status != OK:
raise NiryoOneException(result.message)
def get_target_pose(self, workspace, grip, height_offset, x_rel, y_rel,
yaw_rel):
"""
Computes the robot pose that can be used to grab an object which is
positioned relative to the given workspace using the specified grip.
:param workspace: name of the workspace the object is in
:param grip: name of the grip to use to pick the object
:param height_offset: z-offset that is added to the grip
:param x_rel: x relative position of the object inside working zone
:param y_rel: y relative position of the object inside working zone
:param yaw_rel: angle of the object inside working zone
"""
current_ws = self.ws_manager.read(workspace)
current_grip = self.grip_manager.read(grip)
current_grip.transform.transform.translation.z += height_offset
self.transform_handler.set_grip(current_grip)
self.transform_handler.set_relative_pose_object(
current_ws,
x_rel,
y_rel,
yaw_rel,
yaw_center=current_ws.yaw_center)
base_link_to_tool_target = self.transform_handler.get_gripping_transform(
)
q = base_link_to_tool_target.transform.rotation
roll, pitch, yaw = tfc.transformations.euler_from_quaternion(
[q.x, q.y, q.z, q.w])
pose = RobotState()
pose.position.x = base_link_to_tool_target.transform.translation.x
pose.position.y = base_link_to_tool_target.transform.translation.y
pose.position.z = base_link_to_tool_target.transform.translation.z
pose.rpy.roll = roll
pose.rpy.pitch = pitch
pose.rpy.yaw = yaw
return pose