diff --git a/harvest_arm_rm/config/config.yaml b/harvest_arm_rm/config/config.yaml index 5f35fa4..4aa21fc 100644 --- a/harvest_arm_rm/config/config.yaml +++ b/harvest_arm_rm/config/config.yaml @@ -7,7 +7,7 @@ harvest_arm_rm: tool_name: "scissor" - tool_names: ["scissor", "omnipic", "minisci", "no_tool"] + tool_names: ["scissor", "omnipic", "minisci", "vacuum", "no_tool"] initial_joint: [-82.60, 20.28, -32.15, 50.28, 17.28, 64.055, -20.62] diff --git a/harvest_arm_rm/harvest_arm_rm/run_realman_arm.py b/harvest_arm_rm/harvest_arm_rm/run_realman_arm.py index 8093385..c8f4387 100644 --- a/harvest_arm_rm/harvest_arm_rm/run_realman_arm.py +++ b/harvest_arm_rm/harvest_arm_rm/run_realman_arm.py @@ -14,7 +14,7 @@ import math from Robotic_Arm.rm_robot_interface import * -class RMArmNode(Node): +class RM_Arm(Node): def __init__(self, port=8080, level=3, mode=2, node_name="harvest_arm_rm"): """ Initialize and connect to the robotic arm. @@ -30,7 +30,7 @@ class RMArmNode(Node): try: self.arm_arg['arm'] = RoboticArm(rm_thread_mode_e(mode)) - self.arm_arg['arm_handle'] = self.arm_arg['arm'].rm_create_robot_arm(self.arm_arg['robot_ip'], self.arm_arg['port'], level) + self.arm_arg['arm_handle'] = self.arm_arg['arm'].rm_create_robot_arm(self.config['robot_ip'], self.config['port'], level) if self.arm_arg['arm_handle'].id == -1: self.get_logger().error("Failed to connect to the real robot arm (socket error)!") else: @@ -38,7 +38,7 @@ class RMArmNode(Node): # if real robot connected, initialize following cfg self.arm_motion_lmt() self.udp_config() - self.arm_arg['arm'] = self.peripheral_cfg(self.arm_arg['arm'], self.config) + self.arm_arg['arm'] = self.peripheral_cfg() # self.move_to_init() except Exception as e: self.get_logger().error(f"Robot arm hardware init error: {e} !") @@ -52,7 +52,7 @@ class RMArmNode(Node): self.timer = self.create_timer( self.config["interval"], - self.rm_movej_timer + self.rm_timer ) @@ -62,7 +62,7 @@ class RMArmNode(Node): Returns: dict: Configuration dictionary containing robot IP, host IP, prefix, left/right status, port, savefile flag, and scissor gripper status. """ - tool_name = self.declare_parameter('tool_name', "scissor").value + tool_name = self.declare_parameter('tool_name', "no_tool").value tool_name_param = 'tools_in_ee.' + tool_name try: tool_offset_list = self.declare_parameter(tool_name_param, [0.0] * 11).value @@ -82,7 +82,7 @@ class RMArmNode(Node): "max_joint": self.declare_parameter('max_joint', [0.0] * 7).value, "min_joint": self.declare_parameter('min_joint', [0.0] * 7).value, "max_joint_speed": self.declare_parameter('max_joint_speed', 180.0).value, - "att_2": acc_lmt * interval * interval * 0.5, + "lmt_05att": acc_lmt * interval * interval * 0.5, "max_cartesian_speed": self.declare_parameter('max_cartesian_speed', 1.0).value, "max_cartesian_angular_speed": self.declare_parameter('max_cartesian_angular_speed', 1.5).value, @@ -114,9 +114,10 @@ class RMArmNode(Node): "joint_pos_sts": [0.0] * 7, "joint_speed_sts": [0.0] * 7, "arm_enable": int(0), - "tool_cmd": int(0), + "tool_cmd": [int(0), int(0)], "pose_sts": [0.0] * 6, "timer_cnt": 0, + "action_timestamp": 0.0, } arm_arg = { @@ -134,10 +135,17 @@ class RMArmNode(Node): Args: msg (JointState): Message containing the joint command. """ - action_list = list(msg.position) - self.sts["arm_enable"] = int(action_list[0]) - self.sts["joint_target"] = action_list[1:8] - self.sts["tool_cmd"] = int(action_list[8]) + self.sts["tool_cmd"][1] = self.sts["tool_cmd"][0] + + total_seconds = msg.header.stamp.sec + (msg.header.stamp.nanosec * 1e-9) + if total_seconds > self.sts["action_timestamp"]: + self.sts["action_timestamp"] = total_seconds + + action_list = list(msg.position) + self.sts["arm_enable"] = int(action_list[0]) + self.sts["joint_target"] = action_list[1:8] + self.sts["tool_cmd"][0] = int(action_list[8]) + def arm_observation_pub(self): """ @@ -145,7 +153,7 @@ class RMArmNode(Node): """ msg = JointState() msg.header.stamp = self.get_clock().now().to_msg() - msg.position = [float(self.sts["arm_enable"])] + list(self.sts["joint_pos_sts"]) + [float(self.sts["tool_cmd"])] + [float(self.sts["timer_cnt"])] + msg.position = [float(self.sts["arm_enable"])] + list(self.sts["joint_pos_sts"]) + [float(self.sts["tool_cmd"][0])] + [float(self.sts["timer_cnt"])] msg.velocity = self.sts["joint_speed_sts"] msg.effort = [] self.observation_pub.publish(msg) @@ -238,16 +246,18 @@ class RMArmNode(Node): def movej_canfd(self, j): - movej_canfd_result = self.sts["arm"].rm_movej_canfd(j, False) - if movej_canfd_result != 0: - self.get_logger().error("\n movej_canfd motion failed, Error code: ", movej_canfd_result, "\n") + if self.arm_arg['arm_handle'].id != -1: + movej_canfd_result = self.arm_arg["arm"].rm_movej_canfd(j, False) + if movej_canfd_result != 0: + self.get_logger().error("\n movej_canfd motion failed, Error code: ", movej_canfd_result, "\n") - def rm_movej_timer(self): + def rm_timer(self): """ 50Hz timer callback function, independent thread, not in main loop send joint command to the arm """ + ############# movej --> arm if self.sts["arm_enable"] == 1: dis_max_vt = self.config["max_joint_speed"] * self.config["interval"] dis_vt = np.array(self.sts["joint_speed_sts"]) * self.config["interval"] @@ -258,50 +268,68 @@ class RMArmNode(Node): self.sts["joint_cmd"] = (d_j + np.array(self.sts["joint_pos_sts"])).tolist() self.movej_canfd(self.sts["joint_cmd"]) + ############# tool + if self.sts["tool_cmd"][0] == 1 and self.sts["tool_cmd"][1] == 0: + self.set_tool_position(self.arm_arg['arm'], self.config, percent=1, device=1, tool_name=self.config["tool_name"]) + elif self.sts["tool_cmd"][0] == 0 and self.sts["tool_cmd"][1] == 1: + self.set_tool_position(self.arm_arg['arm'], self.config, percent=0, device=1, tool_name=self.config["tool_name"]) self.arm_observation_pub() self.sts["timer_cnt"] += 1 if self.sts["timer_cnt"] % (50 * 2 ) == 0: - self.get_logger().info(f'at {self.sts["timer_cnt"] * self.config["interval"] * 2} s, the en_state is {self.sts["arm_enable"]}, pose = {self.sts["pose_sts"]}, j cmd = {self.sts["joint_cmd"]}\n\n') + self.get_logger().info(f'at {self.sts["timer_cnt"] * self.config["interval"] * 1} s, the en_state is {self.sts["arm_enable"]}, pose = {self.sts["pose_sts"]}, j cmd = {self.sts["joint_cmd"]}\n\n') - def peripheral_cfg(self, arm, config): + def peripheral_cfg(self): """ Configure the peripheral settings of the robotic arm. Args: arm (RoboticArm): Instance of the RoboticArm class. config (dict): Configuration dictionary containing tool offsets. """ - self.tool_cfg(arm, config) - self.connector_cfg(arm, config) + self.frame_cfg() + self.connector_cfg() - return arm - def tool_cfg(self, arm, config): + def frame_cfg(self): """ - Configure the tool settings of the robotic arm. + Configure the tool/work frame settings of the robotic arm. Args: arm (RoboticArm): Instance of the RoboticArm class. config (dict): Configuration dictionary containing tool offsets. """ - tool_offset = config["tool_offset"] - tool_name = config["tool_name"] - eu = arm.rm_algo_quanternion2euler([tool_offset[6], tool_offset[3], tool_offset[4], tool_offset[5]]) + tool_offset = self.config["tool_offset"] + tool_name = self.config["tool_name"] + eu = self.arm_arg['arm'].rm_algo_quanternion2euler([tool_offset[6], tool_offset[3], tool_offset[4], tool_offset[5]]) tool_frame = rm_frame_t(tool_name,[tool_offset[0], tool_offset[1], tool_offset[2], eu[0], eu[1], eu[2]], tool_offset[7], [tool_offset[8], tool_offset[9], tool_offset[10]]) - info = arm.rm_get_total_tool_frame() + info = self.arm_arg['arm'].rm_get_total_tool_frame() if tool_name in info["tool_names"]: - ret = arm.rm_update_tool_frame(tool_frame) + ret = self.arm_arg['arm'].rm_update_tool_frame(tool_frame) else: - ret = arm.rm_set_manual_tool_frame(tool_frame) + ret = self.arm_arg['arm'].rm_set_manual_tool_frame(tool_frame) - ret = arm.rm_change_tool_frame(tool_name) + ret = self.arm_arg['arm'].rm_change_tool_frame(tool_name) if ret != 0: raise RuntimeError(f"Failed to change tool frame to {tool_name}, ret={ret}") - def connector_cfg(self, arm, config): + + work_offset = self.config["arm_installation"] + ret, info, n = self.arm_arg['arm'].rm_get_total_work_frame() + if ret != 0: + raise RuntimeError(f"Failed to get total work frame, ret={ret}") + if n > 0: + if 'b' in info["tool_names"]: + self.arm_arg['arm'].rm_update_work_frame('b', self.config["arm_installation"]) + else: + self.arm_arg['arm'].rm_set_manual_work_frame('b', self.config["arm_installation"]) + + self.arm_arg['arm'].rm_change_work_frame('b') + + + def connector_cfg(self): """ Configure the connector settings of the robotic arm, tool connector and controller board connector. Args: @@ -309,19 +337,19 @@ class RMArmNode(Node): config (dict): Configuration dictionary containing connector settings. """ # Implement connector configuration logic here - arm.rm_set_voltage(config["arm_ctrl_voltage"]) - arm.rm_set_io_mode(1, config["arm_ctrl_io_mode"][0], 50, 2) - arm.rm_set_io_mode(2, config["arm_ctrl_io_mode"][1], 50, 2) - arm.rm_set_io_mode(3, config["arm_ctrl_io_mode"][2], 50, 2) - arm.rm_set_io_mode(4, config["arm_ctrl_io_mode"][3], 50, 2) + self.arm_arg['arm'].rm_set_voltage(self.config["arm_ctrl_voltage"]) - if config["tool_name"] == config["tool_names"][0]: # scissor - arm.rm_set_tool_voltage(config["arm_tool_io_voltage"]) + self.arm_arg['arm'].rm_set_io_mode(1, self.config["arm_ctrl_io_mode"][0], 50, 2) + self.arm_arg['arm'].rm_set_io_mode(2, self.config["arm_ctrl_io_mode"][1], 50, 2) + - arm.rm_set_tool_IO_mode(1, config["arm_tool_io_mode"][0]) - arm.rm_set_tool_IO_mode(2, config["arm_tool_io_mode"][1]) - elif config["tool_name"] == config["tool_names"][1]: # omnipic - modbus_sts = arm.rm_set_modbus_mode(port=1, baudrate=115200, timeout=2) + if self.config["tool_name"] == 'scissor': # scissor + self.arm_arg['arm'].rm_set_tool_voltage(self.config["arm_tool_io_voltage"]) + + self.arm_arg['arm'].rm_set_tool_IO_mode(1, self.config["arm_tool_io_mode"][0]) + self.arm_arg['arm'].rm_set_tool_IO_mode(2, self.config["arm_tool_io_mode"][1]) + elif self.config["tool_name"] == 'omnipic': # omnipic + modbus_sts = self.arm_arg['arm'].rm_set_modbus_mode(port=1, baudrate=115200, timeout=2) if modbus_sts != 0: self.get_logger().warn(f"Warning: Failed to set Modbus mode: {modbus_sts}") else: @@ -333,48 +361,50 @@ class RMArmNode(Node): reg_value = [255, 60, 255, 255] for i, reg_addr in enumerate([11, 12, 13, 14]) : write_params = rm_peripheral_read_write_params_t(1, reg_addr, addr, 1) - arm.rm_write_single_register(write_params, reg_value[i]) + self.arm_arg['arm'].rm_write_single_register(write_params, reg_value[i]) time.sleep(0.5) - elif config["tool_name"] == config["tool_names"][2]: # minisci - pass + elif self.config["tool_name"] == 'minisci': # minisci + self.arm_arg['arm'].rm_set_io_mode(3, self.config["arm_ctrl_io_mode"][2], 50, 2) + self.arm_arg['arm'].rm_set_io_mode(4, self.config["arm_ctrl_io_mode"][3], 50, 2) - def set_tool_position(arm, config, percent, device = 1, tool_name = 0): + def set_tool_position(self, percent, device = 1, tool_name = 'scissor'): """ Set gripper position (0.0 = closed, 1.0 = fully open) """ - if tool_name == config["tool_names"][1]: # scissor - pos_value = int(percent * 255) - write_params = rm_peripheral_read_write_params_t(1, 10, device, 1) - arm.rm_write_single_register(write_params, pos_value) - # trigger - write_params = rm_peripheral_read_write_params_t(1, 15, device, 1) - time.sleep(0.5) - arm.rm_write_single_register(write_params, 0x01) - time.sleep(0.5) + if self.arm_arg['arm_handle'].id != -1: + if tool_name == 'scissor': + #scissor gripper only has two states, fully open and fully closed, so we can set a threshold to determine the state + if percent == 0: + self.arm_arg['arm'].rm_set_tool_do_state(1, 1) + self.arm_arg['arm'].rm_set_tool_do_state(2, 0) + elif percent == 1: + self.arm_arg['arm'].rm_set_tool_do_state(1, 0) + self.arm_arg['arm'].rm_set_tool_do_state(2, 1) + elif tool_name == 'omnipic': + pos_value = int(percent * 255) + write_params = rm_peripheral_read_write_params_t(1, 10, device, 1) + self.arm_arg['arm'].rm_write_single_register(write_params, pos_value) + # trigger + write_params = rm_peripheral_read_write_params_t(1, 15, device, 1) + time.sleep(0.5) + self.arm_arg['arm'].rm_write_single_register(write_params, 0x01) + time.sleep(0.5) - elif tool_name == config["tool_names"][0]: - #scissor gripper only has two states, fully open and fully closed, so we can set a threshold to determine the state - if percent == 0: - arm.rm_set_tool_do_state(1, 1) - arm.rm_set_tool_do_state(2, 0) - elif percent == 1: - arm.rm_set_tool_do_state(1, 0) - arm.rm_set_tool_do_state(2, 1) - elif tool_name == config["tool_names"][2]: # minisci - # mini scissor - if percent == 0: - arm.rm_set_do_state(3, 1) - arm.rm_set_do_state(4, 0) - elif percent == 1: - arm.rm_set_do_state(3, 0) - arm.rm_set_do_state(4, 1) + elif tool_name == 'minisci': + # mini scissor + if percent == 0: + self.arm_arg['arm'].rm_set_do_state(3, 1) + self.arm_arg['arm'].rm_set_do_state(4, 0) + elif percent == 1: + self.arm_arg['arm'].rm_set_do_state(3, 0) + self.arm_arg['arm'].rm_set_do_state(4, 1) def main(args=None): rclpy.init(args=args) - arm_node = RMArmNode() + arm_node = RM_Arm() executor = MultiThreadedExecutor() executor.add_node(arm_node)