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

add kine pkg pub/sub

Browse Source
This commit is contained in:
LiuzhengSJ
2026-07-16 17:11:56 +01:00
parent 120a252ccc
commit 9119837294
4 changed files with 86 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files

48
harvest_arm_ctrl/harvest_arm_ctrl/run_kinematics_arm.py
View File

@ -31,12 +31,22 @@ class KinematicsSolver(Node):
self.robot_kine_rm.add_tool_frames(self.config['tools_in_ee'])
self.robot_kine_rm.cfg_j_limit(min_j=self.config['min_joint'], max_j=self.config['max_joint'], rad_flag=False)
self.action_arm_pub = self.create_publisher(JointState, self.config["observation_topic"], 10)
# pub, arm joint command
self.action_arm_pub = self.create_publisher(JointState, self.config["action_arm_topic"], 10)
# Create subcriber for target joint
# sub, get the desired pose command in task frame
self.action_pose_sub = self.create_subscription(PoseStamped, self.config["action_pose_topic"], self.pose_callback, 10)
# sub, get the desired tool command
self.action_tool_sub = self.create_subscription(Bool, self.config["action_tool_topic"], self.tool_callback, 10)
# sub, get the teleoperation enable command
self.action_enable_sub = self.create_subscription(Bool, self.config["action_arm_enable_topic"], self.arm_enable_callback, 10)
# sub, get the joint sts from arm
self.obser_arm_sub = self.create_subscription(JointState, self.config["observation_topic"], self.arm_observation_callback, 10)
self.timer = self.create_timer(
self.config["interval"],
self.kine_timer
)
self.get_logger().info(f'the config is {self.config}')
@ -65,13 +75,15 @@ class KinematicsSolver(Node):
"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,
"arm_installation": self.declare_parameter('arm_installation', [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]).value,
"tool_name": tool_name,
"tools_in_ee": tools_in_ee,
"interval": self.declare_parameter('interval', 0.02).value,
"urdf_path": self.declare_parameter('urdf_path', "/home/zl/Downloads/urdf_rm75/RM75-B.urdf").value,
"mesh_dir": self.declare_parameter('mesh_dir', "/home/zl/Downloads/urdf_rm75").value,
"action_arm_topic": self.declare_parameter('action_topic', "/action/arm").value,
"observation_topic": self.declare_parameter('observation_topic', "/observation/arm").value,
"action_pose_topic": self.declare_parameter('action_pose_topic', "/action/pose").value,
@ -87,7 +99,7 @@ class KinematicsSolver(Node):
"tool_cmd": 0,
"pose_sts": [0.0] * 6,
"pose_target_timestamp": 0.0,
"ik_ret": False,
"ik_ret": 0,
}
return config, sts
@ -95,10 +107,13 @@ class KinematicsSolver(Node):
def update_msg(self):
msg = JointState()
msg.header.stamp = self.get_clock().now().to_msg()
msg.name = ['joint1', 'joint2', 'joint3', 'joint4', 'joint5', 'joint6', 'joint7']
msg.position = [float(self.sts["arm_enable"])] + self.sts["joint_solved"] + [float(self.sts["tool_cmd"])]
msg.position = [float(self.sts["arm_enable"])] + [float(self.sts["ik_ret"])] + self.sts["joint_solved"] + [float(self.sts["tool_cmd"])]
return msg
def kine_timer(self):
msg = self.update_msg()
self.action_arm_pub.publish(msg)
def arm_enable_callback(self, msg):
if msg.data:
self.sts["arm_enable"] = 1
@ -111,6 +126,12 @@ class KinematicsSolver(Node):
else:
self.sts["tool_cmd"] = 0
def arm_observation_callback(self, msg):
if isinstance(msg, JointState):
self.sts["joint_pos_sts"] = list(msg.position[2:9])
else:
self.get_logger().error("Received message of unsupported type.")
def pose_callback(self, msg):
if isinstance(msg, PoseStamped):
timestamp = msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9
@ -136,22 +157,27 @@ class KinematicsSolver(Node):
ret_rm_ik, q_out = self.robot_kine_rm.inverse_kinematics(target_position, target_rpy, initial_guess, tool)
if ret_rm_ik == 0:
self.sts["joint_solved"] = q_out
self.sts["ik_ret"] = True
self.sts["ik_ret"] = 1
else:
ret_qp_ik, q_out = self.robot_kine_qp.inverse_kinematics(target_position= target_position, target_rpy=target_rpy, initial_guess=initial_guess, tool=tool, max_iter=300, work=work)
if ret_qp_ik == 0:
self.sts["joint_solved"] = q_out
self.sts["ik_ret"] = True
self.sts["ik_ret"] = 1
else:
self.sts["ik_ret"] = False
self.sts["ik_ret"] = 0
def pose_transform(self, pos, quat, pos_frame, quat_frame):
'''
pos: position in the task frame
quat: quaternion in the task frame
pos_frame: position in the arm base frame
quat_frame: quaternion in the arm base frame
pos_frame: task frame position in the arm base frame
quat_frame: task framequaternion in the arm base frame
'''
pos_frame = np.asarray(pos_frame, dtype=np.float32)
quat_frame = np.asarray(quat_frame, dtype=np.float32)
pos = np.asarray(pos, dtype=np.float32)
quat = np.asarray(quat, dtype=np.float32)
pos_arm = rotate_vector_by_quaternion( quat=quat_frame, v=pos) + pos_frame
quat_arm = quaternion_multiply(quat_frame, quat)
return pos_arm, quat_arm