add tool cfg
This commit is contained in:
LiuzhengSJ
@ -7,6 +7,8 @@ harvest_arm_rm:
|
||||
|
||||
tool_name: "scissor"
|
||||
|
||||
tool_names: ["scissor", "omnipic", "minisci", "no_tool"]
|
||||
|
||||
initial_joint: [-82.60, 20.28, -32.15, 50.28, 17.28, 64.055, -20.62]
|
||||
|
||||
max_joint: [155.0, 115.0, 172.0, 130.0, 175.0, 125.0, 179.0]
|
||||
@ -20,13 +22,17 @@ harvest_arm_rm:
|
||||
max_cartesian_acc: 1.0
|
||||
max_cartesian_angular_acc: 2.0
|
||||
|
||||
bound_cartesian: [
|
||||
-0.3, -0.5, -0.1, 0.3, 0.5, 0.6
|
||||
]
|
||||
|
||||
|
||||
arm_installation: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
|
||||
|
||||
# 0~2 position, x, y, z, m
|
||||
# 3~6 orientation, quaternion, x, y, z, w
|
||||
# 7 mass, kg
|
||||
# 8~11 center of mass, x, y, z, m
|
||||
# 8~10 center of mass, x, y, z, m
|
||||
tools_in_ee.scissor: [
|
||||
0.0, 0.0, 0.19, 0.0, 0.0, 0.0, 1.0,
|
||||
0.66, 0.0, 0.0, 0.06
|
||||
@ -47,14 +53,19 @@ harvest_arm_rm:
|
||||
0.0, 0.0, 0.0, 0.0
|
||||
]
|
||||
|
||||
joint_cmd_topic: "/joint_cmd"
|
||||
joint_state_topic: "/joint_sts"
|
||||
arm_enable_topic: "/arm_enable"
|
||||
tool_cmd_topic: "/tool_cmd"
|
||||
|
||||
|
||||
|
||||
|
||||
action_topic: "/action/arm"
|
||||
observation_topic: "/observation/arm"
|
||||
|
||||
interval: 0.02
|
||||
kp_j : 1.0
|
||||
|
||||
arm_ctrl_voltage: 2 # 0: 0V, 2: 12V, 3: 24V
|
||||
|
||||
# https://develop.realman-robotics.com/en/robot/apipython/classes/controllerIOConfig/#set-the-io-mode-rm-set-io-mode
|
||||
arm_ctrl_io_mode: [0, 0, 1, 1] # 0: input, 1: output
|
||||
|
||||
arm_tool_io_voltage: 2
|
||||
arm_tool_io_mode: [1, 1] # 0: input, 1: output
|
||||
|
||||
|
||||
|
||||
|
||||
@ -4,9 +4,6 @@ from rclpy.node import Node
|
||||
from rclpy.executors import MultiThreadedExecutor
|
||||
import time
|
||||
import numpy as np
|
||||
from arm_pkg.arm_realman.arg_cfg import *
|
||||
from arm_pkg.arm_realman.fun_basic import *
|
||||
from arm_pkg.arm_realman.fun_savefile import *
|
||||
from tf2_msgs.msg import TFMessage
|
||||
from sensor_msgs.msg import JointState
|
||||
from geometry_msgs.msg import PoseStamped, TransformStamped
|
||||
@ -15,9 +12,6 @@ from std_msgs.msg import Float32MultiArray
|
||||
import math
|
||||
|
||||
from Robotic_Arm.rm_robot_interface import *
|
||||
from arm_pkg.arm_realman.arg_cfg import *
|
||||
from arm_pkg.arm_realman.fun_rviz import Rviz_single_arm as Rviz_arm
|
||||
from arm_pkg.arm_realman.fun_peripheral import *
|
||||
|
||||
|
||||
class RMArmNode(Node):
|
||||
@ -33,25 +27,34 @@ class RMArmNode(Node):
|
||||
super().__init__('harvest_arm_rm')
|
||||
self.config, self.sts, self.arm_arg = self.get_arg()
|
||||
|
||||
self.arm_topic()
|
||||
|
||||
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)
|
||||
if self.arm_arg['arm_handle'].id == -1:
|
||||
print("Failed to connect to the real robot arm (socket error)!")
|
||||
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.armcfg()
|
||||
self.arm_motion_lmt()
|
||||
self.udp_config()
|
||||
self.arm_arg['arm'] = peripheral_cfg(self.arm_arg['arm'], self.config)
|
||||
self.init_done = True
|
||||
self.arm_arg['arm'] = self.peripheral_cfg(self.arm_arg['arm'], self.config)
|
||||
# self.move_to_init()
|
||||
except Exception as e:
|
||||
self.get_logger().error(f"Robot arm hardware init error: {e}, enter RViz simulation mode!")
|
||||
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.timer = self.create_timer(
|
||||
self.config["interval"],
|
||||
self.rm_movej_timer
|
||||
)
|
||||
|
||||
|
||||
def get_arg(self):
|
||||
"""
|
||||
@ -59,46 +62,61 @@ class RMArmNode(Node):
|
||||
Returns:
|
||||
dict: Configuration dictionary containing robot IP, host IP, prefix, left/right status, port, savefile flag, and scissor gripper status.
|
||||
"""
|
||||
tool_offsets = {}
|
||||
tool_offsets['scissor'] = self.declare_parameter('tools_in_ee.scissor', 0.0).value
|
||||
tool_offsets['omnipick'] = self.declare_parameter('tools_in_ee.omnipick', 0.0).value
|
||||
tool_offsets['minisci'] = self.declare_parameter('tools_in_ee.minisci', 0.0).value
|
||||
tool_offsets['no_tool'] = self.declare_parameter('tools_in_ee.no_tool', 0.0).value
|
||||
tool_name = self.declare_parameter('tool_name', "scissor").value
|
||||
tool_offset_list = tool_offsets[tool_name]
|
||||
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', joint_max).value,
|
||||
"min_joint": self.declare_parameter('min_joint', joint_min).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,
|
||||
"max_joint_acc": self.declare_parameter('max_joint_acc', 600.0).value,
|
||||
"att_2": 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,
|
||||
|
||||
"joint_cmd_topic": self.declare_parameter('joint_cmd_topic', "/joint_cmd").value,
|
||||
"joint_state_topic": self.declare_parameter('joint_state_topic', "/joint_sts").value,
|
||||
"arm_enable_topic": self.declare_parameter('arm_enable_topic', "/arm_enable").value,
|
||||
"tool_cmd_topic": self.declare_parameter('tool_cmd_topic', "/tool_cmd").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,
|
||||
|
||||
"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
|
||||
}
|
||||
|
||||
sts = {
|
||||
"joint_cmd": [0.0] * 7,
|
||||
"joint_sts": [0.0] * 7,
|
||||
"joint_speed": [0.0] * 7,
|
||||
"arm_enable": False,
|
||||
"tool_cmd": False,
|
||||
"joint_target": [0.0] * 7,
|
||||
"joint_pos_sts": [0.0] * 7,
|
||||
"joint_speed_sts": [0.0] * 7,
|
||||
"arm_enable": int(0),
|
||||
"tool_cmd": int(0),
|
||||
"pose_sts": [0.0] * 6,
|
||||
"timer_cnt": 0,
|
||||
}
|
||||
|
||||
arm_arg = {
|
||||
@ -108,52 +126,29 @@ class RMArmNode(Node):
|
||||
|
||||
return config, sts, arm_arg
|
||||
|
||||
def arm_topic(self):
|
||||
"""
|
||||
Initialize ROS2 topics for the robotic arm.
|
||||
"""
|
||||
self.subname = self.config["joint_cmd_topic"]
|
||||
|
||||
# Create publisher for joint states
|
||||
self.joint_sts_pub = self.create_publisher(JointState, self.config["joint_state_topic"], 10)
|
||||
# Create subscriber for arm enable state
|
||||
self.arm_en_sub = self.create_subscription(Bool, self.config["arm_enable_topic"], self.arm_enable_callback, 10)
|
||||
# Create subscriber for tool command
|
||||
self.tool_sub = self.create_subscription(Bool, self.config["tool_cmd_topic"], self.tool_callback, 10)
|
||||
# Create subcriber for target joint
|
||||
self.joint_cmd_sub = self.create_subscription(JointState, self.config["joint_cmd_topic"], self.joint_cmd_callback, 10)
|
||||
|
||||
def arm_enable_callback(self, msg):
|
||||
"""
|
||||
Callback function for arm enable state.
|
||||
Args:
|
||||
msg (Bool): Message containing the arm enable state.
|
||||
"""
|
||||
self.sts["arm_enable"] = msg.data
|
||||
if self.sts["arm_enable"]:
|
||||
self.get_logger().info(f'\n------------------Arm Enabled ----------------------')
|
||||
else:
|
||||
self.get_logger().info(f'\n------------------Arm Disabled ----------------------')
|
||||
|
||||
def tool_callback(self, msg):
|
||||
"""
|
||||
Callback function for tool command.
|
||||
Args:
|
||||
msg (Bool): Message containing the tool command state.
|
||||
"""
|
||||
self.sts["tool_cmd"] = msg.data
|
||||
if self.sts["tool_cmd"]:
|
||||
self.get_logger().info(f'\n------------------Tool Command Enabled ----------------------')
|
||||
else:
|
||||
self.get_logger().info(f'\n------------------Tool Command Disabled ----------------------')
|
||||
|
||||
def joint_cmd_callback(self, msg):
|
||||
def arm_action_callback(self, msg):
|
||||
"""
|
||||
Callback function for joint command.
|
||||
Args:
|
||||
msg (JointState): Message containing the joint command.
|
||||
"""
|
||||
self.sts["joint_cmd"] = list(msg.position)
|
||||
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])
|
||||
|
||||
def arm_observation_pub(self):
|
||||
"""
|
||||
Publish the current state of the robotic arm.
|
||||
"""
|
||||
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.velocity = self.sts["joint_speed_sts"]
|
||||
msg.effort = []
|
||||
self.observation_pub.publish(msg)
|
||||
|
||||
|
||||
def udp_config(self):
|
||||
@ -168,9 +163,9 @@ class RMArmNode(Node):
|
||||
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_sts"]))) < 0.0001:
|
||||
while sum(abs(self.sts["pose_sts"])) < 0.001 or sum(abs(np.array(self.sts["joint_pos_sts"]))) < 0.0001:
|
||||
time.sleep(1)
|
||||
print(f'p_measured = {self.sts["pose_sts"]}, j_measured = {self.sts["joint_sts"]}')
|
||||
self.get_logger().info("Waiting for initial robot state...")
|
||||
|
||||
def udp_callback(self, data):
|
||||
# extract the pose data from the UDP data
|
||||
@ -206,10 +201,7 @@ class RMArmNode(Node):
|
||||
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_sts"] = joint_position
|
||||
self.sts["joint_speed"] = joint_speed
|
||||
|
||||
# ------------- send cmd to the Rviz ----------------------------------
|
||||
self.rviz.pub_target_j(self.sts["joint_sts"])
|
||||
self.sts["joint_speed_sts"] = joint_speed
|
||||
|
||||
except Exception as e:
|
||||
self.get_logger().error(f"Error in UDP callback: {e}")
|
||||
@ -220,10 +212,9 @@ class RMArmNode(Node):
|
||||
if not self.initialized:
|
||||
# Perform movej motion for robot arm
|
||||
self.movej(self.config["initial_joint"])
|
||||
|
||||
self.get_logger().info('\n------------------- ready ------------------')
|
||||
|
||||
def armcfg(self ):
|
||||
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"])
|
||||
@ -231,266 +222,152 @@ class RMArmNode(Node):
|
||||
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.robot.rm_set_joint_max_speed(i, self.config["max_joint_speed"])
|
||||
self.robot.rm_set_joint_max_acc(i, self.config["max_joint_acc"])
|
||||
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.robot.rm_delete_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(self, joint, v=20, r=0, connect=0, block=1):
|
||||
"""
|
||||
Perform movej motion.
|
||||
Args:
|
||||
joint (list of float): Joint positions.
|
||||
v (float, optional): Speed of the motion. Defaults to 20.
|
||||
connect (int, optional): Trajectory connection flag. Defaults to 0.
|
||||
block (int, optional): Whether the function is blocking (1 for blocking, 0 for non-blocking). Defaults to 1.
|
||||
r (float, optional): Blending radius. Defaults to 0.
|
||||
"""
|
||||
if not self.robot_handle.id == -1 :
|
||||
movej_result = self.robot.rm_movej(joint, v, r, connect, block)
|
||||
if movej_result == 0:
|
||||
self.get_logger().info("\n real robot movej motion succeeded\n")
|
||||
else:
|
||||
self.get_logger().error(f"\n real robot movej motion failed, Error code: {movej_result}\n")
|
||||
else:
|
||||
# --------------- movej in RViz start ---------------------------
|
||||
self.j_t, self.j_measured = self.rviz.movej(joint , self.j_measured)
|
||||
self.p_measured = np.array( self.algo_handle.rm_algo_forward_kinematics(self.j_measured, flag=1) )
|
||||
|
||||
# --------------- movej in RViz end ---------------------------
|
||||
|
||||
|
||||
def movel(self, pose, v=20, r=0, connect=0, block=1):
|
||||
"""
|
||||
Perform movel motion.
|
||||
Args:
|
||||
pose (list of float): End position [x, y, z, rx, ry, rz].
|
||||
v (float, optional): Speed of the motion. Defaults to 20.
|
||||
connect (int, optional): Trajectory connection flag. Defaults to 0.
|
||||
block (int, optional): Whether the function is blocking (1 for blocking, 0 for non-blocking). Defaults to 1.
|
||||
r (float, optional): Blending radius. Defaults to 0.
|
||||
"""
|
||||
if not self.robot_handle.id == -1 :
|
||||
self.get_logger().info(f"\n Entering the movel() with pose = {pose} \n")
|
||||
movel_result = self.robot.rm_movel(pose, v, r, connect, block)
|
||||
if movel_result == 0:
|
||||
self.get_logger().info("\n movel motion succeeded\n")
|
||||
else:
|
||||
self.get_logger().error(f"\n real robot movel motion failed, Error code: {movel_result} \n")
|
||||
|
||||
self.get_logger().info("\n Starting the Rviz movel() \n")
|
||||
else:
|
||||
# --------------- movel in RViz start ---------------------------
|
||||
self.j_measured = self.rviz.movel(p_t=pose,j_m=self.j_measured,p_m=self.p_measured)
|
||||
|
||||
self.get_logger().info("Rviz movel() done\n")
|
||||
# --------------- movel in RViz end ---------------------------
|
||||
|
||||
|
||||
def movej_canfd(self, j):
|
||||
if self.robot_handle.id == -1:
|
||||
self.rviz.pub_target_j(j)
|
||||
self.j_measured = j
|
||||
else:
|
||||
movej_canfd_result = self.robot.rm_movej_canfd(j, False)
|
||||
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")
|
||||
|
||||
|
||||
|
||||
|
||||
class ArmCtlNode(RMArmNode):
|
||||
def __init__(self, ik = True):
|
||||
super().__init__()
|
||||
|
||||
self.dof = 7
|
||||
self.a_max = acc_max_j
|
||||
self.v_max = vel_max_j
|
||||
# initialize the state of the inverse kinematics
|
||||
self.k = k_j
|
||||
|
||||
self.interval = 0.02
|
||||
|
||||
self.lmt_05att = np.array([[-1]*self.dof , [1]*self.dof ]) * self.interval ** 2 * 0.5 * self.a_max * lmt_scale
|
||||
|
||||
# initialize the target joint positions and former target joint positions
|
||||
self.j_t_f = self.j_measured
|
||||
self.p_measured_f = self.p_measured
|
||||
|
||||
# state of whether the target pose has already been received
|
||||
self.p_t_rcvd = False
|
||||
# define the state of whether the teleoperation is enabled
|
||||
self.tele_en = False
|
||||
# define the former state of whether the teleoperation is enabled
|
||||
self.tele_en_f = False
|
||||
# define the state of the scissor. True: open; False: close
|
||||
self.scissor_en = False
|
||||
|
||||
self.scale = np.ones(self.dof)
|
||||
|
||||
self.frame_id = 0
|
||||
|
||||
# -------------------------- create the timer -----------------------------
|
||||
# create timer for main controller
|
||||
self.timer = self.create_timer(
|
||||
self.interval,
|
||||
self.rm_move_arm
|
||||
)
|
||||
self.timer_count = 0
|
||||
# create the subscriber to receive target joints
|
||||
self.pose_sub = self.create_subscription(
|
||||
JointState,
|
||||
self.subname,
|
||||
self.target_update_callback,
|
||||
2)
|
||||
# Create subscription for Bool topic to receive teleoperation enable state
|
||||
self.en_sub = self.create_subscription(
|
||||
Bool,
|
||||
en_topic,
|
||||
self.tele_en_callback,
|
||||
2)
|
||||
|
||||
# Create subscription for Bool topic to receive the scissor state
|
||||
self.sci_sub = self.create_subscription(
|
||||
Bool,
|
||||
self.scissor_topic,
|
||||
self.scissor_callback,
|
||||
2)
|
||||
|
||||
self.tool_flg = 0
|
||||
self.tool_timer = self.create_timer(
|
||||
0.2,
|
||||
self.rm_tool_timer
|
||||
)
|
||||
|
||||
# handle for the data saving
|
||||
self.file_hd = SaveFile( self.savefile_flg , self.left)
|
||||
|
||||
print('---------------- End of ArmCtlNode initial -----------------------')
|
||||
|
||||
|
||||
# -------------------------- timer call fun --------------------------
|
||||
def rm_move_arm(self):
|
||||
def rm_movej_timer(self):
|
||||
"""
|
||||
50Hz timer callback function, independent thread, not in main loop
|
||||
send pose/joint command to the robot
|
||||
send joint command to the arm
|
||||
"""
|
||||
if self.tele_en:
|
||||
if not self.tele_en_f:
|
||||
self.p_t_f = self.p_measured
|
||||
self.j_t_f = self.j_measured
|
||||
if self.p_t_rcvd:
|
||||
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"]
|
||||
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"])
|
||||
|
||||
j_cmd = self.cal_j_t(self.j_t)
|
||||
self.movej_canfd(j_cmd)
|
||||
t = self.j_t
|
||||
|
||||
self.file_hd.write(t, self.j_measured, self.p_measured)
|
||||
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')
|
||||
|
||||
|
||||
def peripheral_cfg(self, arm, config):
|
||||
"""
|
||||
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)
|
||||
|
||||
return arm
|
||||
|
||||
def tool_cfg(self, arm, config):
|
||||
"""
|
||||
Configure the tool 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_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()
|
||||
if tool_name in info["tool_names"]:
|
||||
ret = arm.rm_update_tool_frame(tool_frame)
|
||||
else:
|
||||
self.p_t_f = self.p_measured
|
||||
self.j_t_f = self.j_measured
|
||||
ret = arm.rm_set_manual_tool_frame(tool_frame)
|
||||
|
||||
if self.robot_handle.id == -1:
|
||||
self.rviz.pub_target_j(self.j_measured)
|
||||
ret = arm.rm_change_tool_frame(tool_name)
|
||||
if ret != 0:
|
||||
raise RuntimeError(f"Failed to change tool frame to {tool_name}, ret={ret}")
|
||||
|
||||
self.timer_count += 1
|
||||
self.tele_en_f = self.tele_en
|
||||
def connector_cfg(self, arm, config):
|
||||
"""
|
||||
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
|
||||
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.p_measured_f = self.p_measured
|
||||
self.j_measured_f = self.j_measured
|
||||
if config["tool_name"] == config["tool_names"][0]: # scissor
|
||||
arm.rm_set_tool_voltage(config["arm_tool_io_voltage"])
|
||||
|
||||
if self.timer_count % (50 * 2 ) == 0:
|
||||
print(f'at {self.timer_count * self.interval * 2} s, the en_state is {self.tele_en}, p_measure = {self.p_measured}, j cmd = {self.j_t_f}\n\n')
|
||||
|
||||
|
||||
|
||||
def cal_j_t(self, j_t):
|
||||
'''
|
||||
adjust the target joint positions to comply with limitations
|
||||
'''
|
||||
d_cmd = motion_lmt(target=j_t, measure=self.j_measured, k=self.scale * self.k, lmt=np.array(self.v_measured)*self.interval + self.lmt_05att, v_max=self.v_max, interval=self.interval)
|
||||
self.j_t_f = (d_cmd + np.array(self.j_measured)).tolist()
|
||||
return self.j_t_f
|
||||
|
||||
def target_update_callback(self, msg):
|
||||
"""Callback for target joint update"""
|
||||
if not self.tele_en:
|
||||
return
|
||||
'''
|
||||
@cnt: count, len = 1
|
||||
@sts: ik calculation return value, 0: success, <0: fail, len = 1
|
||||
@suc: to distinguish which solver finish the ik, len = 1
|
||||
@init_guess:current joint angles, degreee, len = 7
|
||||
@pose: forward kinematics calculation result based on current joint angles, len = 6, unit: m, rad in euler
|
||||
@ik_j: ik calculation results, joint angles, rad, len = 7
|
||||
@p_t: target pose, x y z qw qx qy qz (quaternion), len = 7, quaternion
|
||||
@p_t_euler: the target pose in euler
|
||||
'''
|
||||
data = msg.position
|
||||
|
||||
cnt = int(data[0])
|
||||
ik_state = int(data[1])
|
||||
suc = int(data[2])
|
||||
j = data[3:10]
|
||||
pose = data[10:16]
|
||||
ik_j = data[16:23]
|
||||
p_t = data[23:30]
|
||||
p_t_euler = data[30:36]
|
||||
|
||||
frame_id = cnt
|
||||
# print(f'in left = {self.left} target_update_callback function, frame_id = {frame_id}')
|
||||
if frame_id > self.frame_id:
|
||||
self.frame_id = frame_id
|
||||
|
||||
if ik_state < -0.5:
|
||||
print('error in ik cal, unreachable, ret = ', ik_state)
|
||||
if not hasattr(self, 'p_t_rcvd') or not self.p_t_rcvd:
|
||||
# j_t has never been set by a successful IK, hold measured joints to avoid commanding zero position
|
||||
self.j_t = list(self.j_measured)
|
||||
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 modbus_sts != 0:
|
||||
self.get_logger().warn(f"Warning: Failed to set Modbus mode: {modbus_sts}")
|
||||
else:
|
||||
self.j_t = [j* 180 / math.pi for j in ik_j]
|
||||
self.p_t_rcvd = True
|
||||
# print(f'j_t: {self.j_t} in target_update_callback')
|
||||
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)
|
||||
arm.rm_write_single_register(write_params, reg_value[i])
|
||||
time.sleep(0.5)
|
||||
elif config["tool_name"] == config["tool_names"][2]: # minisci
|
||||
pass
|
||||
|
||||
def set_tool_position(arm, config, percent, device = 1, tool_name = 0):
|
||||
"""
|
||||
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)
|
||||
|
||||
def tele_en_callback(self, msg):
|
||||
"""Callback for Bool topic"""
|
||||
self.tele_en = msg.data
|
||||
if not self.tele_en:
|
||||
self.p_t_rcvd = False
|
||||
else:
|
||||
self.p_t_rcvd = False
|
||||
self.j_t = list(self.j_measured)
|
||||
self.j_t_f = list(self.j_measured)
|
||||
self.p_t_f = np.array(self.p_measured)
|
||||
self.frame_id = -1
|
||||
if self.tele_en:
|
||||
self.get_logger().info(f'\n------------------Teleoperation Enabled ----------------------')
|
||||
else:
|
||||
self.get_logger().info(f'\n------------------Teleoperation Disabled ----------------------')
|
||||
|
||||
def scissor_callback(self, msg):
|
||||
''' for scissor state '''
|
||||
self.scissor_en = msg.data
|
||||
tool_exe(self.robot, self.scissorgripper, self.scissor_en)
|
||||
|
||||
def rm_tool_timer(self):
|
||||
if self.init_done and ( not self.scissorgripper == -1):
|
||||
tool_button = self.robot.rm_get_io_input()[1][0]
|
||||
if tool_button == 1 and self.tool_flg == 0:
|
||||
tool_exe(self.robot, self.scissorgripper, True)
|
||||
elif tool_button == 0 and self.tool_flg == 1:
|
||||
tool_exe(self.robot, self.scissorgripper, False)
|
||||
self.tool_flg = tool_button
|
||||
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)
|
||||
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user