From f4fec8f51ccd7b9cef8d7df42b833ba7f2050ead Mon Sep 17 00:00:00 2001 From: yutang Date: Wed, 11 Jun 2025 16:17:39 +0800 Subject: [PATCH] change pose control api --- .../common/robot_devices/motors/realman.py | 6 +- .../common/robot_devices/robots/configs.py | 2 +- .../common/robot_devices/robots/realman.py | 62 ++++++++----------- .../common/robot_devices/teleop/gamepad.py | 5 -- realman_src/movep_canfd.py | 15 +++++ 5 files changed, 48 insertions(+), 42 deletions(-) create mode 100644 realman_src/movep_canfd.py diff --git a/lerobot/common/robot_devices/motors/realman.py b/lerobot/common/robot_devices/motors/realman.py index d30b401d..d45c4084 100644 --- a/lerobot/common/robot_devices/motors/realman.py +++ b/lerobot/common/robot_devices/motors/realman.py @@ -90,9 +90,13 @@ class RealmanMotorsBus: self.write(target_joint=self.init_joint_position) def write(self, target_joint:list): - self.rmarm.rm_movej(target_joint[:-1], 50, 0, 0, 1) + # self.rmarm.rm_movej(target_joint[:-1], 50, 0, 0, 1) + self.rmarm.rm_movej_follow(target_joint[:-1]) self.rmarm.rm_set_gripper_position(target_joint[-1], block=False, timeout=2) + def write_endpose(self, target_endpose: list, gripper: int): + self.rmarm.rm_movej_p(target_endpose, 50, 0, 0, 1) + self.rmarm.rm_set_gripper_position(gripper, block=False, timeout=2) def read(self) -> Dict: """ diff --git a/lerobot/common/robot_devices/robots/configs.py b/lerobot/common/robot_devices/robots/configs.py index 52f3e8fd..778cbe62 100644 --- a/lerobot/common/robot_devices/robots/configs.py +++ b/lerobot/common/robot_devices/robots/configs.py @@ -701,7 +701,7 @@ class RealmanRobotConfig(RobotConfig): "joint_6": [6, "realman"], "gripper": [7, "realman"], }, - init_joint = {'joint': [-90, 90, 90, 170, 90, -90, 1000]} + init_joint = {'joint': [-90, 90, 90, -90, -90, 90, 1000]} ) } ) diff --git a/lerobot/common/robot_devices/robots/realman.py b/lerobot/common/robot_devices/robots/realman.py index 9d070cc3..2ace37b2 100644 --- a/lerobot/common/robot_devices/robots/realman.py +++ b/lerobot/common/robot_devices/robots/realman.py @@ -28,24 +28,24 @@ class RealmanRobot: # build realman motors self.piper_motors = make_motors_buses_from_configs(self.config.left_follower_arm) self.arm = self.piper_motors['main'] - self.arm.rmarm.rm_movej(self.arm.init_joint_position[:-1], 50, 0, 0, 0) + self.arm.rmarm.rm_movej(self.arm.init_joint_position[:-1], 50, 0, 0, 1) + time.sleep(2) ret = self.arm.rmarm.rm_get_current_arm_state() init_pose = ret[1]['pose'] - + # build init teleop info + self.init_info = { + 'init_joint': self.arm.init_joint_position, + 'init_pose': init_pose, + 'max_gripper': config.max_gripper, + 'min_gripper': config.min_gripper, + 'servo_config_file': config.servo_config_file + } # build state-action cache self.joint_queue = deque(maxlen=2) # build gamepad teleop if not self.inference_time: - # build init teleop info - self.init_info = { - 'init_joint': self.arm.init_joint_position, - 'init_pose': init_pose, - 'max_gripper': config.max_gripper, - 'min_gripper': config.min_gripper, - 'servo_config_file': config.servo_config_file - } self.teleop = HybridController(self.init_info) else: self.teleop = None @@ -159,7 +159,6 @@ class RealmanRobot: state = self.arm.read() # read current joint position from robot action = self.teleop.get_action() # target joint position and pose end pos from gamepad self.logs["read_pos_dt_s"] = time.perf_counter() - before_read_t - # print('gripper', action['gripper']) if action['control_mode'] == 'joint': # 关节控制模式（主模式） @@ -186,32 +185,25 @@ class RealmanRobot: # do action before_write_t = time.perf_counter() - # 使用笛卡尔空间运动控制末端位姿 - if action['tozero']: - self.arm.rmarm.rm_movej(self.arm.init_joint_position[:-1], 50, 0, 0, 0) - ret = self.arm.rmarm.rm_get_current_arm_state() - target_joints = ret[1].get('joint', self.arm.init_joint_position) - current_pose = ret[1]['pose'] - self.teleop.update_endpose_state(current_pose) - self.teleop.update_joint_state(target_joints) - self.teleop.update_tozero_state(False) - else: - result = self.arm.rmarm.rm_movej_p(target_pose, 50, 0, 0, 0) - # 夹爪控制 - self.arm.rmarm.rm_set_gripper_position(action['gripper'], False, 2) - ret = self.arm.rmarm.rm_get_current_arm_state() - target_joints = ret[1].get('joint', self.arm.init_joint_position) - self.joint_queue.append(list(self.arm.read().values())) - self.teleop.update_joint_state(target_joints) + + # result = self.arm.rmarm.rm_movej_p(target_pose, 100, 0, 0, 0) + self.arm.rmarm.rm_movep_follow(target_pose) + self.arm.rmarm.rm_set_gripper_position(action['gripper'], False, 2) + + ret = self.arm.rmarm.rm_get_current_arm_state() + target_joints = ret[1].get('joint', self.arm.init_joint_position) + self.joint_queue.append(list(self.arm.read().values())) + self.teleop.update_joint_state(target_joints) self.logs["write_pos_dt_s"] = time.perf_counter() - before_write_t - print('-'*80) - print('mode: ', action['control_mode']) - print('state: ', list(state.values())) - print('action: ', target_joints) - print('cache[0]: ', self.joint_queue[0]) - print('cache[-1]: ', self.joint_queue[-1]) - print('-'*80) + # print('-'*80) + # print('mode: ', action['control_mode']) + # print('state: ', list(state.values())) + # print('action: ', target_joints) + # print('cache[0]: ', self.joint_queue[0]) + # print('cache[-1]: ', self.joint_queue[-1]) + # print('time: ', time.perf_counter() - before_write_t) + # print('-'*80) # time.sleep(1) if not record_data: return diff --git a/lerobot/common/robot_devices/teleop/gamepad.py b/lerobot/common/robot_devices/teleop/gamepad.py index f52c4226..ecae100c 100644 --- a/lerobot/common/robot_devices/teleop/gamepad.py +++ b/lerobot/common/robot_devices/teleop/gamepad.py @@ -399,11 +399,6 @@ class HybridController: def update_tozero_state(self, tozero): self.tozero = tozero - # def update_state(self, state): - # """更新状态信息（从机械臂获取当前状态）""" - # self.pose = state['end_pose'] - # self.joint = state['joint'] - # self.gripper = state['gripper'] def get_action(self) -> Dict: """获取当前控制命令""" diff --git a/realman_src/movep_canfd.py b/realman_src/movep_canfd.py new file mode 100644 index 00000000..c8520f29 --- /dev/null +++ b/realman_src/movep_canfd.py @@ -0,0 +1,15 @@ +from Robotic_Arm.rm_robot_interface import * +import time + +# 实例化RoboticArm类 +arm = RoboticArm(rm_thread_mode_e.RM_TRIPLE_MODE_E) +# 创建机械臂连接，打印连接id +handle = arm.rm_create_robot_arm("192.168.3.18", 8080) +print(handle.id) + +print(arm.rm_movep_follow([-0.330512, 0.255993, -0.161205, 3.141, 0.0, -1.57])) +time.sleep(2) +# print(arm.rm_movep_follow([0.3, 0, 0.3, 3.14, 0, 0])) +# time.sleep(2) + +arm.rm_delete_robot_arm() \ No newline at end of file