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)