From e3ca28fb99646c4677429b31b76a07f63d68f362 Mon Sep 17 00:00:00 2001 From: LiuzhengSJ Date: Mon, 6 Jul 2026 14:18:34 +0100 Subject: [PATCH] update arm ctrl rm py --- harvest_arm_ctrl/config/config.yaml | 37 +++++++++++++++++++ harvest_arm_ctrl/harvest_arm_ctrl/arm_ctrl.py | 6 --- .../harvest_arm_ctrl/rm75_kine_qp.py | 4 +- .../harvest_arm_ctrl/rm75_kine_rm.py | 29 +++++++++++++-- .../harvest_arm_ctrl/run_kinematics.py | 6 --- .../harvest_arm_ctrl/run_kinematics_arm.py | 11 ++++++ harvest_arm_ctrl/setup.py | 2 +- harvest_arm_rm/launch/rm_arm_rviz.launch.py | 20 ++++++++-- 8 files changed, 94 insertions(+), 21 deletions(-) create mode 100644 harvest_arm_ctrl/config/config.yaml delete mode 100644 harvest_arm_ctrl/harvest_arm_ctrl/arm_ctrl.py delete mode 100644 harvest_arm_ctrl/harvest_arm_ctrl/run_kinematics.py create mode 100644 harvest_arm_ctrl/harvest_arm_ctrl/run_kinematics_arm.py diff --git a/harvest_arm_ctrl/config/config.yaml b/harvest_arm_ctrl/config/config.yaml new file mode 100644 index 0000000..9789875 --- /dev/null +++ b/harvest_arm_ctrl/config/config.yaml @@ -0,0 +1,37 @@ + +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 + ] + diff --git a/harvest_arm_ctrl/harvest_arm_ctrl/arm_ctrl.py b/harvest_arm_ctrl/harvest_arm_ctrl/arm_ctrl.py deleted file mode 100644 index 931284f..0000000 --- a/harvest_arm_ctrl/harvest_arm_ctrl/arm_ctrl.py +++ /dev/null @@ -1,6 +0,0 @@ -def main(): - print('Hi from harvest_arm_ctrl.') - - -if __name__ == '__main__': - main() diff --git a/harvest_arm_ctrl/harvest_arm_ctrl/rm75_kine_qp.py b/harvest_arm_ctrl/harvest_arm_ctrl/rm75_kine_qp.py index 6bbf00d..c05b166 100644 --- a/harvest_arm_ctrl/harvest_arm_ctrl/rm75_kine_qp.py +++ b/harvest_arm_ctrl/harvest_arm_ctrl/rm75_kine_qp.py @@ -12,7 +12,7 @@ import threading -class KinematicsSolver(): +class KinematicsSolver_QP(): def __init__(self,urdf_path="urdf_rm75/RM75-B.urdf", mesh_dir="urdf_rm75"): """ for realman 75b @@ -712,7 +712,7 @@ def main(): """Main test function""" - rm75 = KinematicsSolver() + rm75 = KinematicsSolver_QP() # Test 1: Forward Kinematics print("\n1. Forward Kinematics Test") diff --git a/harvest_arm_ctrl/harvest_arm_ctrl/rm75_kine_rm.py b/harvest_arm_ctrl/harvest_arm_ctrl/rm75_kine_rm.py index 34fa4cb..9b34ddb 100644 --- a/harvest_arm_ctrl/harvest_arm_ctrl/rm75_kine_rm.py +++ b/harvest_arm_ctrl/harvest_arm_ctrl/rm75_kine_rm.py @@ -7,7 +7,7 @@ class rm75_kine_api(): def __init__(self): # ---------- rm75 official algorithm ----------- print(f'------- the realman official kinematic initialising -------') - arm_model = rm_robot_arm_model_e.RM_MODEL_RM_75_E # RM_65 Robotic arm + arm_model = rm_robot_arm_model_e.RM_MODEL_RM_75_E # RM_75 Robotic arm force_type = rm_force_type_e.RM_MODEL_RM_B_E # Standard version # Initialize the robotic arm model and sensor type in the algorithm self.robot_kine_rm = Algo(arm_model, force_type) @@ -120,14 +120,37 @@ class rm75_kine_api(): self.work_name = work self.cfg_work_frame(work) - target = target_position + target_rpy + target = list(target_position) + list(target_rpy) if initial_guess is not None: q_ref = [ 180/math.pi * ig for ig in initial_guess ] else: q_ref = [0.0, 110.0, 20.0, 40.0, 30.0, 180.0, 20.0] ret, phi = self.robot_kine_rm.rm_algo_calculate_arm_angle_from_config_rm75(q_ref) + # print(f'the arm angle is ret = {ret}, and phi = {phi}') params = rm_inverse_kinematics_params_t(q_ref, target, 1) ret, q_out = self.robot_kine_rm.rm_algo_inverse_kinematics_rm75_for_arm_angle(params, phi) - return ret, [ q/180*math.pi for q in q_out] \ No newline at end of file + p_fk = self.robot_kine_rm.rm_algo_forward_kinematics(joint=q_out, flag=1) + pose_dis = cal_pose_deviation(p_fk, target) + + # print(f'target pose is {target}, fk pose is {p_fk}, dis of poses is {pose_dis}') + # + # 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: + 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: + return -10, [ q/180*math.pi for q in q_out] + +def cal_pose_deviation(pose1, pose2): + d_fk_p1 = np.array(pose1) - np.array(pose2) + for j in [3, 4, 5]: + while d_fk_p1[j] > math.pi: + d_fk_p1[j] -= 2 * math.pi + while d_fk_p1[j] < -math.pi: + d_fk_p1[j] += 2 * math.pi + d_fk = np.linalg.norm(d_fk_p1) + return d_fk diff --git a/harvest_arm_ctrl/harvest_arm_ctrl/run_kinematics.py b/harvest_arm_ctrl/harvest_arm_ctrl/run_kinematics.py deleted file mode 100644 index 931284f..0000000 --- a/harvest_arm_ctrl/harvest_arm_ctrl/run_kinematics.py +++ /dev/null @@ -1,6 +0,0 @@ -def main(): - print('Hi from harvest_arm_ctrl.') - - -if __name__ == '__main__': - main() diff --git a/harvest_arm_ctrl/harvest_arm_ctrl/run_kinematics_arm.py b/harvest_arm_ctrl/harvest_arm_ctrl/run_kinematics_arm.py new file mode 100644 index 0000000..5415de9 --- /dev/null +++ b/harvest_arm_ctrl/harvest_arm_ctrl/run_kinematics_arm.py @@ -0,0 +1,11 @@ +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.') + + +if __name__ == '__main__': + main() diff --git a/harvest_arm_ctrl/setup.py b/harvest_arm_ctrl/setup.py index 7a2d33e..93ba02c 100644 --- a/harvest_arm_ctrl/setup.py +++ b/harvest_arm_ctrl/setup.py @@ -24,7 +24,7 @@ setup( }, entry_points={ 'console_scripts': [ - 'arm_ctrl = harvest_arm_ctrl.arm_ctrl:main' + 'kinematics_arm = harvest_arm_ctrl.run_kinematics_arm:main', ], }, ) diff --git a/harvest_arm_rm/launch/rm_arm_rviz.launch.py b/harvest_arm_rm/launch/rm_arm_rviz.launch.py index 6d13a51..243740d 100644 --- a/harvest_arm_rm/launch/rm_arm_rviz.launch.py +++ b/harvest_arm_rm/launch/rm_arm_rviz.launch.py @@ -73,13 +73,27 @@ def generate_launch_description(): ''' + + ros2 launch harvest_arm_rm rm_arm_rviz.launch.py -ros2 topic pub /action/arm sensor_msgs/msg/JointState " + + +while true; do + ros2 topic pub /action/arm sensor_msgs/msg/JointState " header: stamp: - sec: 2 - nanosec: 0 + sec: $(date +%s) + nanosec: $(date +%N) + frame_id: '' +name: [] position: [1.0, 82.6, -20.28, 32.15, -50.28, -17.28, -64.055, 20.62, 1.0] +velocity: [] +effort: [] " --once + sleep 1 +done + + + ''' \ No newline at end of file