def shift_pose(self, axis, value):
goal = RobotMoveGoal()
goal.cmd.cmd_type = MoveCommandType.SHIFT_POSE
goal.cmd.shift.axis_number = axis
goal.cmd.shift.value = value
return self.execute_action('niryo_one/commander/robot_action',
RobotMoveAction, goal)
def deactivate_electromagnet(self, electromagnet_id, pin):
goal = RobotMoveGoal()
goal.cmd.cmd_type = MoveCommandType.TOOL
goal.cmd.tool_cmd.tool_id = int(electromagnet_id)
goal.cmd.tool_cmd.cmd_type = self.tool_command_list['deactivate_digital_io']
goal.cmd.tool_cmd.gpio = pin
return self.execute_action('niryo_one/commander/robot_action', RobotMoveAction, goal)
def close_gripper(self, gripper_id, speed):
goal = RobotMoveGoal()
goal.cmd.cmd_type = MoveCommandType.TOOL
goal.cmd.tool_cmd.tool_id = int(gripper_id)
goal.cmd.tool_cmd.cmd_type = self.tool_command_list['close_gripper']
goal.cmd.tool_cmd.gripper_close_speed = speed
self.start_execution_thread(goal)
def close_gripper(self, gripper_id, speed):
goal = RobotMoveGoal()
goal.cmd.cmd_type = MoveCommandType.TOOL
goal.cmd.tool_cmd.tool_id = int(gripper_id)
goal.cmd.tool_cmd.cmd_type = self.tool_command_list['close_gripper']
goal.cmd.tool_cmd.gripper_close_speed = speed
return self.execute_action('niryo_one/commander/robot_action', RobotMoveAction, goal)
def push_air_vacuum_pump(self, vacuum_pump_id):
goal = RobotMoveGoal()
goal.cmd.cmd_type = MoveCommandType.TOOL
goal.cmd.tool_cmd.tool_id = int(vacuum_pump_id)
goal.cmd.tool_cmd.cmd_type = self.tool_command_list[
'push_air_vacuum_pump']
self.start_execution_thread(goal)
def __deal_with_vacuum_pump(self, vacuum_pump_id, command_str):
goal = RobotMoveGoal()
goal.cmd.cmd_type = MoveCommandType.TOOL
goal.cmd.tool_cmd.tool_id = int(vacuum_pump_id)
goal.cmd.tool_cmd.cmd_type = self.tool_command_list[command_str]
return self.execute_action('niryo_one/commander/robot_action',
RobotMoveAction, goal)
def push_air_vacuum_pump(self, vacuum_pump_id):
goal = RobotMoveGoal()
goal.cmd.cmd_type = MoveCommandType.TOOL
goal.cmd.tool_cmd.tool_id = int(vacuum_pump_id)
goal.cmd.tool_cmd.cmd_type = self.tool_command_list[
'push_air_vacuum_pump']
return self.execute_action('niryo_one/commander/robot_action',
RobotMoveAction, goal)
def __deal_with_electromagnet(self, electromagnet_id, pin,
command_str):
goal = RobotMoveGoal()
goal.cmd.cmd_type = MoveCommandType.TOOL
goal.cmd.tool_cmd.tool_id = int(electromagnet_id)
goal.cmd.tool_cmd.cmd_type = self.tool_command_list[command_str]
goal.cmd.tool_cmd.gpio = pin
return self.execute_action('niryo_one/commander/robot_action',
RobotMoveAction, goal)
def move_pose(self, pose):
goal = RobotMoveGoal()
goal.cmd.cmd_type = MoveCommandType.POSE
goal.cmd.position.x = pose[0]
goal.cmd.position.y = pose[1]
goal.cmd.position.z = pose[2]
goal.cmd.rpy.roll = pose[3]
goal.cmd.rpy.pitch = pose[4]
goal.cmd.rpy.yaw = pose[5]
self.start_execution_thread(goal)
def move_pose(self, x, y, z, roll, pitch, yaw):
goal = RobotMoveGoal()
goal.cmd.cmd_type = MoveCommandType.POSE
goal.cmd.position.x = x
goal.cmd.position.y = y
goal.cmd.position.z = z
goal.cmd.rpy.roll = roll
goal.cmd.rpy.pitch = pitch
goal.cmd.rpy.yaw = yaw
return self.execute_action('niryo_one/commander/robot_action', RobotMoveAction, goal)
def __deal_with_gripper(self, gripper_id, speed, command_str):
goal = RobotMoveGoal()
goal.cmd.cmd_type = MoveCommandType.TOOL
goal.cmd.tool_cmd.tool_id = int(gripper_id)
goal.cmd.tool_cmd.cmd_type = self.tool_command_list[command_str]
if "open" in command_str:
goal.cmd.tool_cmd.gripper_open_speed = speed
else:
goal.cmd.tool_cmd.gripper_close_speed = speed
return self.execute_action('niryo_one/commander/robot_action',
RobotMoveAction, goal)
def send_trajectory_goal(client, trajectory):
# Build the goal
goal = RobotMoveGoal()
goal.cmd.Trajectory = trajectory
goal.cmd.cmd_type = TRAJECTORY_COMMAND_ID
client.send_goal(goal)
client.wait_for_result()
return
def send_robot_to_sleep_position():
client = actionlib.SimpleActionClient(
'/niryo_one/commander/robot_action', RobotMoveAction)
if not client.wait_for_server(rospy.Duration(1.0)):
return
goal = RobotMoveGoal()
goal.cmd.cmd_type = MoveCommandType.JOINTS
goal.cmd.joints = [0, 0, -1.34, 0, 0, 0]
client.send_goal(goal)
client.wait_for_result()
result = client.get_result()
return result.status
def move_joints(self, joints):
"""
Execute Move joints action
:param joints: list of joint
:return: status, message
:rtype: (GoalStatus, str)
"""
goal = RobotMoveGoal()
goal.cmd.cmd_type = MoveCommandType.JOINTS
goal.cmd.joints = joints
return self.execute_action('niryo_one/commander/robot_action',
RobotMoveAction, goal)
def send_tool_goal(client, gripper_command):
tool_command = ToolCommand()
tool_command.tool_id = TOOL_ID
tool_command.cmd_type = gripper_command
tool_command.gripper_open_speed = GRIPPER_SPEED
tool_command.gripper_close_speed = GRIPPER_SPEED
goal = RobotMoveGoal()
goal.cmd.tool_cmd = tool_command
goal.cmd.cmd_type = TOOL_COMMAND_ID
client.send_goal(goal)
client.wait_for_result()
return
def send_matlab_goal(self,cmd,cmd_type):
self.action_client_matlab= actionlib.SimpleActionClient('/niryo_one/commander/robot_action',niryo_one_msgs.msg.RobotMoveAction)
rospy.loginfo("waiting for action server: niryo_one/robot_action....")
self.action_client_matlab.wait_for_server()
rospy.loginfo("Found action server : niryo_one/robot_action")
goal=RobotMoveGoal()
goal.cmd.joints=cmd
goal.cmd.cmd_type=cmd_type
self.action_client_matlab.send_goal(goal)
rospy.loginfo("waiting for result")
self.action_client_matlab.wait_for_result()
response= self.action_client_matlab.get_result()
rospy.loginfo("..........result.................")
rospy.loginfo(response)
result = self.create_response(response.status,response.message)
return response
def shift_pose(self, axis, value):
"""
Execute Shift pose action
:param axis: Value of RobotAxis enum corresponding to where the shift happens
:type axis: ShiftPose
:param value: shift value
:type value: float
:return: status, message
:rtype: (GoalStatus, str)
"""
goal = RobotMoveGoal()
goal.cmd.cmd_type = MoveCommandType.SHIFT_POSE
goal.cmd.shift.axis_number = axis
goal.cmd.shift.value = value
return self.execute_action('niryo_one/commander/robot_action',
RobotMoveAction, goal)
def move_joints(self, joints):
goal = RobotMoveGoal()
goal.cmd.cmd_type = MoveCommandType.JOINTS
goal.cmd.joints = joints
self.start_execution_thread(goal)
def move_joints_non_blocking(self, joints):
goal = RobotMoveGoal()
goal.cmd.cmd_type = MoveCommandType.JOINTS
goal.cmd.joints = joints
return self.execute_action_non_blocking('niryo_one/commander/robot_action', RobotMoveAction, goal)