454 lines
21 KiB
Python
454 lines
21 KiB
Python
#!/usr/bin/env python3
|
|
import rclpy
|
|
from rclpy.node import Node
|
|
from rclpy.executors import MultiThreadedExecutor
|
|
import time
|
|
import numpy as np
|
|
from tf2_msgs.msg import TFMessage
|
|
from sensor_msgs.msg import JointState
|
|
from geometry_msgs.msg import PoseStamped, TransformStamped
|
|
from std_msgs.msg import Bool
|
|
from std_msgs.msg import Float32MultiArray
|
|
import math
|
|
|
|
from Robotic_Arm.rm_robot_interface import *
|
|
|
|
|
|
class RM_Arm(Node):
|
|
def __init__(self, port=8080, level=3, mode=2, node_name="realman_run_node"):
|
|
"""
|
|
Initialize and connect to the robotic arm.
|
|
Args:
|
|
ip (str): IP address of the robot arm.
|
|
port (int): Port number.
|
|
level (int, optional): Connection level. Defaults to 3.
|
|
mode (int, optional): Thread mode (0: single, 1: dual, 2: triple). Defaults to 2.
|
|
"""
|
|
super().__init__('realman_run_node')
|
|
self.config, self.sts, self.arm_arg = self.get_arg()
|
|
|
|
|
|
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.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:
|
|
self.get_logger().info(f"Successfully connected to the real robot arm: {self.arm_arg['arm_handle'].id}")
|
|
# if real robot connected, initialize following cfg
|
|
self.arm_motion_lmt()
|
|
self.udp_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} !")
|
|
self.arm_arg['arm'] = None
|
|
|
|
# Create publisher for joint states
|
|
self.observation_pub = self.create_publisher(JointState, self.config["observation_topic"], 10)
|
|
|
|
# Create subcriber for target joint
|
|
self.action_sub = self.create_subscription(JointState, self.config["action_topic"], self.arm_action_callback, 10)
|
|
|
|
self.observation_rviz_sub = self.create_subscription(JointState, self.config["observation_rviz_topic"], self.arm_observation_rviz_callback, 10)
|
|
self.action_rviz_pub = self.create_publisher(JointState, self.config["action_rviz_topic"], 10)
|
|
|
|
self.timer = self.create_timer(
|
|
self.config["interval"],
|
|
self.rm_timer
|
|
)
|
|
|
|
|
|
def get_arg(self):
|
|
"""
|
|
Get the configuration of the robotic arm.
|
|
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', "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
|
|
# except:
|
|
# self.get_logger().error(f"Tool name '{tool_name}' not found in parameters. Please check the configuration.")
|
|
|
|
|
|
acc_lmt = self.declare_parameter('max_joint_acc', 600.0).value
|
|
interval = self.declare_parameter('interval', 0.02).value
|
|
|
|
|
|
config = {
|
|
"robot_ip": self.declare_parameter('robot_ip', "192.168.1.18").value,
|
|
"host_ip": self.declare_parameter('host_ip', "192.168.1.101").value,
|
|
"port": self.declare_parameter('port', 8089).value,
|
|
|
|
"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,
|
|
"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,
|
|
"max_cartesian_acc": self.declare_parameter('max_cartesian_acc', 1.5).value,
|
|
"max_cartesian_angular_acc": self.declare_parameter('max_cartesian_angular_acc', 2.0).value,
|
|
# "bound_cartesian": self.declare_parameter('bound_cartesian', [-0.3, -0.5, -0.1, 0.3, 0.5, 0.6]).value,
|
|
|
|
# "arm_installation": self.declare_parameter('arm_installation', [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]).value,
|
|
"tool_name": tool_name,
|
|
# "tool_offset": tool_offset_list,
|
|
# "tool_names": self.declare_parameter('tool_names', ["scissor", "omnipic", "minisci", "no_tool"]).value,
|
|
|
|
"initial_joint": self.declare_parameter('initial_joint', [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]).value,
|
|
"kp_j": self.declare_parameter('kp_j', 1.0).value,
|
|
|
|
"interval": interval,
|
|
"action_topic": self.declare_parameter('action_topic', "/action/arm").value,
|
|
"observation_topic": self.declare_parameter('observation_topic', "/observation/arm").value,
|
|
"action_rviz_topic": self.declare_parameter('action_rviz_topic', "/action/arm_rviz").value,
|
|
"observation_rviz_topic": self.declare_parameter('observation_rviz_topic', "/observation/arm_rviz").value,
|
|
|
|
"arm_ctrl_voltage": self.declare_parameter('arm_ctrl_voltage', 2).value,
|
|
"arm_ctrl_io_mode": self.declare_parameter('arm_ctrl_io_mode', [0, 0, 1, 1]).value,
|
|
"arm_tool_io_voltage": self.declare_parameter('arm_tool_io_voltage', 2).value,
|
|
"arm_tool_io_mode": self.declare_parameter('arm_tool_io_mode', [1, 1]).value
|
|
}
|
|
|
|
# joint_cmd: the joint command to the arm, unit: degree;
|
|
# joint_target: the joint target from the /action/arm, unit: degree;
|
|
# joint_pos_sts: the joint position from the arm udp (real robot) or from the /observation/arm_rviz (if no real robot), unit: degree;
|
|
# joint_speed_sts: the joint speed from the arm udp (real robot) or from the /observation/arm_rviz (if no real robot), unit: degree/s;
|
|
# arm_enable: 1 or 0, the arm enable status from the /action/arm;
|
|
# tool_cmd: the tool command from the /action/arm, 1 or 0;
|
|
# pose_sts: the pose from the arm udp (real robot), unit: m and rad;
|
|
sts = {
|
|
"joint_cmd": [0.0] * 7,
|
|
"joint_target": [0.0] * 7,
|
|
"joint_pos_sts": [0.0] * 7,
|
|
"joint_speed_sts": [0.0] * 7,
|
|
"arm_enable": int(0),
|
|
"ik_ret": int(0),
|
|
"tool_cmd": [int(0), int(0)],
|
|
"pose_sts": [0.0] * 6,
|
|
"timer_cnt": 0,
|
|
"action_timestamp": 0.0,
|
|
}
|
|
|
|
arm_arg = {
|
|
"arm" : None,
|
|
"arm_handle" : None,
|
|
}
|
|
|
|
return config, sts, arm_arg
|
|
|
|
def arm_observation_rviz_callback(self, msg):
|
|
"""
|
|
Callback function for joint observation from RViz.
|
|
Args:
|
|
msg (JointState): Message containing the joint observation.
|
|
"""
|
|
if self.arm_arg['arm_handle'].id == -1:
|
|
self.sts["joint_pos_sts"] = list(msg.position)
|
|
self.sts["joint_speed_sts"] = list(msg.velocity)
|
|
|
|
def arm_action_callback(self, msg):
|
|
"""
|
|
Callback function for joint command.
|
|
Args:
|
|
msg (JointState): Message containing the joint command.
|
|
"""
|
|
|
|
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["ik_ret"] = int(action_list[1])
|
|
self.sts["joint_target"] = action_list[2:9]
|
|
self.sts["tool_cmd"][0] = int(action_list[9])
|
|
|
|
|
|
def arm_observation_relay_pub(self):
|
|
"""
|
|
Publish the current state of the robotic arm to rviz.
|
|
"""
|
|
msg0= JointState()
|
|
msg0.header.stamp = self.get_clock().now().to_msg()
|
|
msg0.position = [float(self.sts["arm_enable"])] + list(self.sts["joint_cmd"]) + [float(self.sts["tool_cmd"][0])]
|
|
msg0.velocity = []
|
|
msg0.effort = []
|
|
self.action_rviz_pub.publish(msg0)
|
|
|
|
|
|
'''
|
|
publish the joint state to the harvest_arm_ctrl pkg.
|
|
'''
|
|
msg = JointState()
|
|
msg.header.stamp = self.get_clock().now().to_msg()
|
|
arm_real = self.arm_arg['arm_handle'].id
|
|
msg.position = [float(arm_real)] + [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)
|
|
|
|
|
|
def udp_config(self):
|
|
# configure the periodical UDP data transmission from the robot to the computer
|
|
custom = rm_udp_custom_config_t()
|
|
custom.joint_speed = 1
|
|
custom.lift_state = 1
|
|
custom.expand_state = 1
|
|
config = rm_realtime_push_config_t(3, True, self.config["port"], 0, self.config["host_ip"], custom)
|
|
self.config["arm"].rm_set_realtime_push(config)
|
|
# Store the callback as an instance variable to prevent garbage collection
|
|
self.arm_state_callback = rm_realtime_arm_state_callback_ptr(self.udp_callback)
|
|
# Use the stored callback
|
|
self.config["arm"].rm_realtime_arm_state_call_back(self.arm_state_callback)
|
|
while sum(abs(self.sts["pose_sts"])) < 0.001 or sum(abs(np.array(self.sts["joint_pos_sts"]))) < 0.0001:
|
|
time.sleep(1)
|
|
self.get_logger().info("Waiting for initial robot state...")
|
|
|
|
def udp_callback(self, data):
|
|
# extract the pose data from the UDP data
|
|
try:
|
|
# Check if data is valid
|
|
if data is None or data.joint_status is None:
|
|
self.get_logger().warn("Warning: Received None data in UDP callback")
|
|
return
|
|
|
|
# Convert to dict safely
|
|
#-------------------------------- pose 1*6 ------------------------------------
|
|
pose_dict = data.waypoint.to_dict()
|
|
if (pose_dict is None) or ('position' not in pose_dict) or ('quaternion' not in pose_dict) :
|
|
self.get_logger().warn(f"Warning: no 'position' or 'quaternion' key. Available keys: {list(pose_dict.keys())}")
|
|
return
|
|
|
|
pose = [ pose_dict['position']['x'], pose_dict['position']['y'], pose_dict['position']['z'], pose_dict['euler']['rx'], pose_dict['euler']['ry'], pose_dict['euler']['rz'] ]
|
|
self.sts["pose_sts"] = pose
|
|
|
|
#--------------------------------------- joint 1*7 ----------------------------------
|
|
# Convert to dict safely
|
|
joint_dict = data.joint_status.to_dict()
|
|
if (joint_dict is None) or ('joint_position' not in joint_dict):
|
|
self.get_logger().warn(f"Warning: 'joint_position' not in joint_dict. Available keys: {list(joint_dict.keys())}")
|
|
return
|
|
|
|
# Now safe to access
|
|
joint_position = joint_dict['joint_position']
|
|
joint_speed = joint_dict.get('joint_speed', [0.0] * 7)
|
|
|
|
# Ensure we have exactly 7 joints
|
|
if len(joint_position) != 7:
|
|
self.get_logger().warn(f"Warning: Expected 7 joint positions, got {len(joint_position)}: {joint_position}")
|
|
# Pad with zeros or truncate to 7
|
|
self.sts["joint_pos_sts"] = joint_position
|
|
self.sts["joint_speed_sts"] = joint_speed
|
|
|
|
except Exception as e:
|
|
self.get_logger().error(f"Error in UDP callback: {e}")
|
|
import traceback
|
|
self.get_logger().error( traceback.print_exc() )
|
|
|
|
def move_to_init(self):
|
|
if not self.initialized:
|
|
# Perform movej motion for robot arm
|
|
self.movej(self.config["initial_joint"])
|
|
self.get_logger().info('\n------------------- ready ------------------')
|
|
|
|
def arm_motion_lmt(self ):
|
|
# configure the motion limitations for the robot
|
|
self.arm_arg['arm'].rm_set_avoid_singularity_mode(True)
|
|
self.arm_arg['arm'].rm_set_arm_max_line_speed(self.config["max_cartesian_speed"])
|
|
self.arm_arg['arm'].rm_set_arm_max_angular_speed(self.config["max_cartesian_angular_speed"])
|
|
self.arm_arg['arm'].rm_set_arm_max_line_acc(self.config["max_cartesian_acc"])
|
|
self.arm_arg['arm'].rm_set_arm_max_angular_acc(self.config["max_cartesian_angular_acc"])
|
|
for i in range(1,8):
|
|
self.arm_arg['arm'].rm_set_joint_max_speed(i, self.config["max_joint_speed"])
|
|
self.arm_arg['arm'].rm_set_joint_max_acc(i, self.config["max_joint_acc"])
|
|
|
|
def disconnect(self):
|
|
"""
|
|
Disconnect from the robot arm.
|
|
"""
|
|
handle = self.arm_arg['arm'].rm_delete_robot_arm()
|
|
if handle == 0:
|
|
self.get_logger().info("\nSuccessfully disconnected from the robot arm\n")
|
|
else:
|
|
self.get_logger().info("\nFailed to disconnect from the robot arm\n")
|
|
|
|
|
|
|
|
def movej_canfd(self, j):
|
|
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_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 and self.sts["ik_ret"] == 1:
|
|
dis_max_vt = self.config["max_joint_speed"] * self.config["interval"]
|
|
dis_vt = np.array(self.sts["joint_speed_sts"]) * self.config["interval"] * 0.8
|
|
d_j = np.clip(a=(np.array(self.sts["joint_target"]) - np.array(self.sts["joint_pos_sts"])) * self.config["kp_j"],
|
|
a_min= dis_vt - self.config["lmt_05att"] ,
|
|
a_max=dis_vt + self.config["lmt_05att"])
|
|
d_j = np.clip(d_j, -dis_max_vt, dis_max_vt)
|
|
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( 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( percent=0, device=1, tool_name=self.config["tool_name"])
|
|
|
|
self.arm_observation_relay_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"] * 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):
|
|
"""
|
|
Configure the peripheral settings of the robotic arm.
|
|
Args:
|
|
arm (RoboticArm): Instance of the RoboticArm class.
|
|
config (dict): Configuration dictionary containing tool offsets.
|
|
"""
|
|
# self.frame_cfg()
|
|
self.connector_cfg()
|
|
|
|
|
|
def frame_cfg(self):
|
|
"""
|
|
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 = 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 = self.arm_arg['arm'].rm_get_total_tool_frame()
|
|
if tool_name in info["tool_names"]:
|
|
ret = self.arm_arg['arm'].rm_update_tool_frame(tool_frame)
|
|
else:
|
|
ret = self.arm_arg['arm'].rm_set_manual_tool_frame(tool_frame)
|
|
|
|
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}")
|
|
|
|
|
|
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:
|
|
arm (RoboticArm): Instance of the RoboticArm class.
|
|
config (dict): Configuration dictionary containing connector settings.
|
|
"""
|
|
# Implement connector configuration logic here
|
|
self.arm_arg['arm'].rm_set_voltage(self.config["arm_ctrl_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)
|
|
|
|
|
|
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:
|
|
self.get_logger().info("Modbus mode configured successfully")
|
|
|
|
addr = 1
|
|
# reg-11: target vel, reg-12: target torque, reg-13: target acc, reg14: target dea
|
|
# value: 255, 70, 255, 255
|
|
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)
|
|
self.arm_arg['arm'].rm_write_single_register(write_params, reg_value[i])
|
|
time.sleep(0.5)
|
|
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(self, percent, device = 1, tool_name = 'scissor'):
|
|
"""
|
|
Set gripper position (0.0 = closed, 1.0 = fully open)
|
|
"""
|
|
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 == '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 = RM_Arm()
|
|
executor = MultiThreadedExecutor()
|
|
executor.add_node(arm_node)
|
|
|
|
try:
|
|
executor.spin()
|
|
except KeyboardInterrupt:
|
|
pass
|
|
finally:
|
|
arm_node.disconnect()
|
|
arm_node.destroy_node()
|
|
rclpy.shutdown() |