ZhengLiu-cart/HarvestingRealman
1
1
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

remove kine parameters, like tool/work frames, from rm pkg to ctrl pkg

Browse Source
This commit is contained in:
LiuzhengSJ
2026-07-06 15:38:14 +01:00
parent e3ca28fb99
commit 97e3cc00d3
5 changed files with 157 additions and 65 deletions
Download Patch File Download Diff File Expand all files Collapse all files

37
harvest_arm_ctrl/config/config.yaml
View File

@ -1,37 +0,0 @@
harvest_arm_ctrl:
ros__parameters:
urdf_path: '/home/zl/ws_Harvest/src/harvest_arm_rm/urdf/urdf_rm75/RM75.urdf'
meshes_path: '/home/zl/ws_Harvest/src/harvest_arm_rm/urdf/urdf_rm75'
qp_iteration: 200
joint_upper : [155.0, 115.0, 172.0, 130, 175.0, 125.0, 350.0 ]
joint_lower : [-155.0, -30.0, -172.0, -130, -175.0, -125.0, -350.0 ]
tool_name: "scissor"
# 0~2 position, x, y, z, m
# 3~6 orientation, quaternion, x, y, z, w
# 7 mass, kg
# 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
]
tools_in_ee.omnipic: [
0.0, 0.0, 0.16, 0.0, 0.0, 0.0, 1.0,
0.43, 0.0, 0.0, 0.06
]
tools_in_ee.minisci: [
0.0, 0.0, 0.19, 0.0, 0.0, 0.0, 1.0,
0.46, 0.0, 0.0, 0.06
]
tools_in_ee.no_tool: [
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0,
0.0, 0.0, 0.0, 0.0
]

9
harvest_arm_ctrl/harvest_arm_ctrl/rm75_kine_rm.py
View File

@ -139,11 +139,14 @@ class rm75_kine_api():
# print(f'\nin the rm75_kine_rm, l133, inverse_kinematics, q_ref = {q_ref}, target = {target} phi = {phi}, q_out = {q_out}, ret = {ret}\n\n')
# print(f'the tool frame is {self.robot_kine_rm.rm_algo_get_curr_toolframe()}')
if int(ret) < 0:
# fail in ik calculation
return ret, [ q/180*math.pi for q in q_out]
elif pose_dis < 0.01:
return ret, [ q/180*math.pi for q in q_out]
else:
elif pose_dis >= 0.01:
# success in ik calculation, but the pose deviation is too large
return -10, [ q/180*math.pi for q in q_out]
else:
# success in ik calculation
return ret, [ q/180*math.pi for q in q_out]
def cal_pose_deviation(pose1, pose2):
d_fk_p1 = np.array(pose1) - np.array(pose2)

119
harvest_arm_ctrl/harvest_arm_ctrl/run_kinematics_arm.py
View File

@ -1,11 +1,122 @@
#!/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 harvest_arm_ctrl.rm75_kine_qp import KinematicsSolver_QP as kine_qp
from harvest_arm_ctrl.rm75_kine_rm import KinematicsSolver_RM as kine_rm
def main():
print('Hi from harvest_arm_ctrl.')
class KinematicsSolver():
def __init__(self, node_name="kine_run_node"):
super().__init__('realman_run_node')
self.config, self.sts, self.arm_arg = self.get_arg()
# ----------- rm75 qp based kine ------------
self.robot_kine_qp = kine_qp(urdf_path='/home/zl/Downloads/urdf_rm75/RM75-B.urdf', mesh_dir='/home/zl/Downloads/urdf_rm75')
self.robot_kine_qp.add_tool_frames(tools_in_ee)
self.robot_kine_qp.cfg_j_limit(min_j=lb, max_j=ub, rad_flag=True)
# ---------- rm75 official algorithm -----------
self.robot_kine_rm = kine_rm()
self.robot_kine_rm.add_tool_frames(tools_in_ee)
self.robot_kine_rm.cfg_j_limit(min_j=lb, max_j=ub, rad_flag=True)
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.")
if __name__ == '__main__':
main()
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
}
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),
"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 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()