From 1a25ee0d49e0cd604f11c6227d6e35df4cd7dd4a Mon Sep 17 00:00:00 2001 From: silencht Date: Wed, 2 Apr 2025 12:01:39 +0800 Subject: [PATCH] [update] support g1-23 dof --- README.md | 5 + assets/g1/g1_body23.urdf | 903 +++++++++++++++++++++++++++ teleop/robot_control/robot_arm.py | 261 ++++++++ teleop/robot_control/robot_arm_ik.py | 236 ++++++- teleop/teleop_hand_and_arm.py | 17 +- 5 files changed, 1414 insertions(+), 8 deletions(-) create mode 100644 assets/g1/g1_body23.urdf diff --git a/README.md b/README.md index 6ea826d..8165819 100644 --- a/README.md +++ b/README.md @@ -33,6 +33,11 @@ Here are the robots that will be supported, ✅ Completed main branch + + G1 (23DoF) + ✅ Completed + main branch + H1 (Arm 4DoF) ⏱ In Progress diff --git a/assets/g1/g1_body23.urdf b/assets/g1/g1_body23.urdf new file mode 100644 index 0000000..6ce66ec --- /dev/null +++ b/assets/g1/g1_body23.urdf @@ -0,0 +1,903 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/teleop/robot_control/robot_arm.py b/teleop/robot_control/robot_arm.py index da3687e..d940dcf 100644 --- a/teleop/robot_control/robot_arm.py +++ b/teleop/robot_control/robot_arm.py @@ -11,6 +11,7 @@ from unitree_sdk2py.utils.crc import CRC kTopicLowCommand = "rt/lowcmd" kTopicLowState = "rt/lowstate" G1_29_Num_Motors = 35 +G1_23_Num_Motors = 35 H1_2_Num_Motors = 35 @@ -23,6 +24,10 @@ class G1_29_LowState: def __init__(self): self.motor_state = [MotorState() for _ in range(G1_29_Num_Motors)] +class G1_23_LowState: + def __init__(self): + self.motor_state = [MotorState() for _ in range(G1_23_Num_Motors)] + class H1_2_LowState: def __init__(self): self.motor_state = [MotorState() for _ in range(H1_2_Num_Motors)] @@ -303,6 +308,262 @@ class G1_29_JointIndex(IntEnum): kNotUsedJoint4 = 33 kNotUsedJoint5 = 34 +class G1_23_ArmController: + def __init__(self): + print("Initialize G1_23_ArmController...") + self.q_target = np.zeros(10) + self.tauff_target = np.zeros(10) + + self.kp_high = 300.0 + self.kd_high = 3.0 + self.kp_low = 80.0 + self.kd_low = 3.0 + self.kp_wrist = 40.0 + self.kd_wrist = 1.5 + + self.all_motor_q = None + self.arm_velocity_limit = 20.0 + self.control_dt = 1.0 / 250.0 + + self._speed_gradual_max = False + self._gradual_start_time = None + self._gradual_time = None + + # initialize lowcmd publisher and lowstate subscriber + ChannelFactoryInitialize(0) + self.lowcmd_publisher = ChannelPublisher(kTopicLowCommand, LowCmd_) + self.lowcmd_publisher.Init() + self.lowstate_subscriber = ChannelSubscriber(kTopicLowState, LowState_) + self.lowstate_subscriber.Init() + self.lowstate_buffer = DataBuffer() + + # initialize subscribe thread + self.subscribe_thread = threading.Thread(target=self._subscribe_motor_state) + self.subscribe_thread.daemon = True + self.subscribe_thread.start() + + while not self.lowstate_buffer.GetData(): + time.sleep(0.01) + print("[G1_23_ArmController] Waiting to subscribe dds...") + + # initialize hg's lowcmd msg + self.crc = CRC() + self.msg = unitree_hg_msg_dds__LowCmd_() + self.msg.mode_pr = 0 + self.msg.mode_machine = self.get_mode_machine() + + self.all_motor_q = self.get_current_motor_q() + print(f"Current all body motor state q:\n{self.all_motor_q} \n") + print(f"Current two arms motor state q:\n{self.get_current_dual_arm_q()}\n") + print("Lock all joints except two arms...\n") + + arm_indices = set(member.value for member in G1_23_JointArmIndex) + for id in G1_23_JointIndex: + self.msg.motor_cmd[id].mode = 1 + if id.value in arm_indices: + if self._Is_wrist_motor(id): + self.msg.motor_cmd[id].kp = self.kp_wrist + self.msg.motor_cmd[id].kd = self.kd_wrist + else: + self.msg.motor_cmd[id].kp = self.kp_low + self.msg.motor_cmd[id].kd = self.kd_low + else: + if self._Is_weak_motor(id): + self.msg.motor_cmd[id].kp = self.kp_low + self.msg.motor_cmd[id].kd = self.kd_low + else: + self.msg.motor_cmd[id].kp = self.kp_high + self.msg.motor_cmd[id].kd = self.kd_high + self.msg.motor_cmd[id].q = self.all_motor_q[id] + print("Lock OK!\n") + + # initialize publish thread + self.publish_thread = threading.Thread(target=self._ctrl_motor_state) + self.ctrl_lock = threading.Lock() + self.publish_thread.daemon = True + self.publish_thread.start() + + print("Initialize G1_23_ArmController OK!\n") + + def _subscribe_motor_state(self): + while True: + msg = self.lowstate_subscriber.Read() + if msg is not None: + lowstate = G1_23_LowState() + for id in range(G1_23_Num_Motors): + lowstate.motor_state[id].q = msg.motor_state[id].q + lowstate.motor_state[id].dq = msg.motor_state[id].dq + self.lowstate_buffer.SetData(lowstate) + time.sleep(0.002) + + def clip_arm_q_target(self, target_q, velocity_limit): + current_q = self.get_current_dual_arm_q() + delta = target_q - current_q + motion_scale = np.max(np.abs(delta)) / (velocity_limit * self.control_dt) + cliped_arm_q_target = current_q + delta / max(motion_scale, 1.0) + return cliped_arm_q_target + + def _ctrl_motor_state(self): + while True: + start_time = time.time() + + with self.ctrl_lock: + arm_q_target = self.q_target + arm_tauff_target = self.tauff_target + + cliped_arm_q_target = self.clip_arm_q_target(arm_q_target, velocity_limit = self.arm_velocity_limit) + + for idx, id in enumerate(G1_23_JointArmIndex): + self.msg.motor_cmd[id].q = cliped_arm_q_target[idx] + self.msg.motor_cmd[id].dq = 0 + self.msg.motor_cmd[id].tau = arm_tauff_target[idx] + + self.msg.crc = self.crc.Crc(self.msg) + self.lowcmd_publisher.Write(self.msg) + + if self._speed_gradual_max is True: + t_elapsed = start_time - self._gradual_start_time + self.arm_velocity_limit = 20.0 + (10.0 * min(1.0, t_elapsed / 5.0)) + + current_time = time.time() + all_t_elapsed = current_time - start_time + sleep_time = max(0, (self.control_dt - all_t_elapsed)) + time.sleep(sleep_time) + # print(f"arm_velocity_limit:{self.arm_velocity_limit}") + # print(f"sleep_time:{sleep_time}") + + def ctrl_dual_arm(self, q_target, tauff_target): + '''Set control target values q & tau of the left and right arm motors.''' + with self.ctrl_lock: + self.q_target = q_target + self.tauff_target = tauff_target + + def get_mode_machine(self): + '''Return current dds mode machine.''' + return self.lowstate_subscriber.Read().mode_machine + + def get_current_motor_q(self): + '''Return current state q of all body motors.''' + return np.array([self.lowstate_buffer.GetData().motor_state[id].q for id in G1_23_JointIndex]) + + def get_current_dual_arm_q(self): + '''Return current state q of the left and right arm motors.''' + return np.array([self.lowstate_buffer.GetData().motor_state[id].q for id in G1_23_JointArmIndex]) + + def get_current_dual_arm_dq(self): + '''Return current state dq of the left and right arm motors.''' + return np.array([self.lowstate_buffer.GetData().motor_state[id].dq for id in G1_23_JointArmIndex]) + + def ctrl_dual_arm_go_home(self): + '''Move both the left and right arms of the robot to their home position by setting the target joint angles (q) and torques (tau) to zero.''' + print("[G1_23_ArmController] ctrl_dual_arm_go_home start...") + with self.ctrl_lock: + self.q_target = np.zeros(10) + # self.tauff_target = np.zeros(10) + tolerance = 0.05 # Tolerance threshold for joint angles to determine "close to zero", can be adjusted based on your motor's precision requirements + while True: + current_q = self.get_current_dual_arm_q() + if np.all(np.abs(current_q) < tolerance): + print("[G1_23_ArmController] both arms have reached the home position.") + break + time.sleep(0.05) + + def speed_gradual_max(self, t = 5.0): + '''Parameter t is the total time required for arms velocity to gradually increase to its maximum value, in seconds. The default is 5.0.''' + self._gradual_start_time = time.time() + self._gradual_time = t + self._speed_gradual_max = True + + def speed_instant_max(self): + '''set arms velocity to the maximum value immediately, instead of gradually increasing.''' + self.arm_velocity_limit = 30.0 + + def _Is_weak_motor(self, motor_index): + weak_motors = [ + G1_23_JointIndex.kLeftAnklePitch.value, + G1_23_JointIndex.kRightAnklePitch.value, + # Left arm + G1_23_JointIndex.kLeftShoulderPitch.value, + G1_23_JointIndex.kLeftShoulderRoll.value, + G1_23_JointIndex.kLeftShoulderYaw.value, + G1_23_JointIndex.kLeftElbow.value, + # Right arm + G1_23_JointIndex.kRightShoulderPitch.value, + G1_23_JointIndex.kRightShoulderRoll.value, + G1_23_JointIndex.kRightShoulderYaw.value, + G1_23_JointIndex.kRightElbow.value, + ] + return motor_index.value in weak_motors + + def _Is_wrist_motor(self, motor_index): + wrist_motors = [ + G1_23_JointIndex.kLeftWristRoll.value, + G1_23_JointIndex.kRightWristRoll.value, + ] + return motor_index.value in wrist_motors + +class G1_23_JointArmIndex(IntEnum): + # Left arm + kLeftShoulderPitch = 15 + kLeftShoulderRoll = 16 + kLeftShoulderYaw = 17 + kLeftElbow = 18 + kLeftWristRoll = 19 + + # Right arm + kRightShoulderPitch = 22 + kRightShoulderRoll = 23 + kRightShoulderYaw = 24 + kRightElbow = 25 + kRightWristRoll = 26 + +class G1_23_JointIndex(IntEnum): + # Left leg + kLeftHipPitch = 0 + kLeftHipRoll = 1 + kLeftHipYaw = 2 + kLeftKnee = 3 + kLeftAnklePitch = 4 + kLeftAnkleRoll = 5 + + # Right leg + kRightHipPitch = 6 + kRightHipRoll = 7 + kRightHipYaw = 8 + kRightKnee = 9 + kRightAnklePitch = 10 + kRightAnkleRoll = 11 + + kWaistYaw = 12 + kWaistRollNotUsed = 13 + kWaistPitchNotUsed = 14 + + # Left arm + kLeftShoulderPitch = 15 + kLeftShoulderRoll = 16 + kLeftShoulderYaw = 17 + kLeftElbow = 18 + kLeftWristRoll = 19 + kLeftWristPitchNotUsed = 20 + kLeftWristyawNotUsed = 21 + + # Right arm + kRightShoulderPitch = 22 + kRightShoulderRoll = 23 + kRightShoulderYaw = 24 + kRightElbow = 25 + kRightWristRoll = 26 + kRightWristPitchNotUsed = 27 + kRightWristYawNotUsed = 28 + + # not used + kNotUsedJoint0 = 29 + kNotUsedJoint1 = 30 + kNotUsedJoint2 = 31 + kNotUsedJoint3 = 32 + kNotUsedJoint4 = 33 + kNotUsedJoint5 = 34 + class H1_2_ArmController: def __init__(self): print("Initialize H1_2_ArmController...") diff --git a/teleop/robot_control/robot_arm_ik.py b/teleop/robot_control/robot_arm_ik.py index 8d1392f..07f15a4 100644 --- a/teleop/robot_control/robot_arm_ik.py +++ b/teleop/robot_control/robot_arm_ik.py @@ -160,7 +160,7 @@ class G1_29_ArmIK: self.vis = MeshcatVisualizer(self.reduced_robot.model, self.reduced_robot.collision_model, self.reduced_robot.visual_model) self.vis.initViewer(open=True) self.vis.loadViewerModel("pinocchio") - self.vis.displayFrames(True, frame_ids=[101, 102], axis_length = 0.15, axis_width = 5) + self.vis.displayFrames(True, frame_ids=[107, 108], axis_length = 0.15, axis_width = 5) self.vis.display(pin.neutral(self.reduced_robot.model)) # Enable the display of end effector target frames with short axis lengths and greater width. @@ -176,7 +176,233 @@ class G1_29_ArmIK: [0, 0, 1], [0, 0.6, 1]]).astype(np.float32).T ) axis_length = 0.1 - axis_width = 10 + axis_width = 20 + for frame_viz_name in frame_viz_names: + self.vis.viewer[frame_viz_name].set_object( + mg.LineSegments( + mg.PointsGeometry( + position=axis_length * FRAME_AXIS_POSITIONS, + color=FRAME_AXIS_COLORS, + ), + mg.LineBasicMaterial( + linewidth=axis_width, + vertexColors=True, + ), + ) + ) + # If the robot arm is not the same size as your arm :) + def scale_arms(self, human_left_pose, human_right_pose, human_arm_length=0.60, robot_arm_length=0.75): + scale_factor = robot_arm_length / human_arm_length + robot_left_pose = human_left_pose.copy() + robot_right_pose = human_right_pose.copy() + robot_left_pose[:3, 3] *= scale_factor + robot_right_pose[:3, 3] *= scale_factor + return robot_left_pose, robot_right_pose + + def solve_ik(self, left_wrist, right_wrist, current_lr_arm_motor_q = None, current_lr_arm_motor_dq = None): + if current_lr_arm_motor_q is not None: + self.init_data = current_lr_arm_motor_q + self.opti.set_initial(self.var_q, self.init_data) + + # left_wrist, right_wrist = self.scale_arms(left_wrist, right_wrist) + if self.Visualization: + self.vis.viewer['L_ee_target'].set_transform(left_wrist) # for visualization + self.vis.viewer['R_ee_target'].set_transform(right_wrist) # for visualization + + self.opti.set_value(self.param_tf_l, left_wrist) + self.opti.set_value(self.param_tf_r, right_wrist) + self.opti.set_value(self.var_q_last, self.init_data) # for smooth + + try: + sol = self.opti.solve() + # sol = self.opti.solve_limited() + + sol_q = self.opti.value(self.var_q) + self.smooth_filter.add_data(sol_q) + sol_q = self.smooth_filter.filtered_data + + if current_lr_arm_motor_dq is not None: + v = current_lr_arm_motor_dq * 0.0 + else: + v = (sol_q - self.init_data) * 0.0 + + self.init_data = sol_q + + sol_tauff = pin.rnea(self.reduced_robot.model, self.reduced_robot.data, sol_q, v, np.zeros(self.reduced_robot.model.nv)) + + if self.Visualization: + self.vis.display(sol_q) # for visualization + + return sol_q, sol_tauff + + except Exception as e: + print(f"ERROR in convergence, plotting debug info.{e}") + + sol_q = self.opti.debug.value(self.var_q) + self.smooth_filter.add_data(sol_q) + sol_q = self.smooth_filter.filtered_data + + if current_lr_arm_motor_dq is not None: + v = current_lr_arm_motor_dq * 0.0 + else: + v = (sol_q - self.init_data) * 0.0 + + self.init_data = sol_q + + sol_tauff = pin.rnea(self.reduced_robot.model, self.reduced_robot.data, sol_q, v, np.zeros(self.reduced_robot.model.nv)) + + print(f"sol_q:{sol_q} \nmotorstate: \n{current_lr_arm_motor_q} \nleft_pose: \n{left_wrist} \nright_pose: \n{right_wrist}") + if self.Visualization: + self.vis.display(sol_q) # for visualization + + # return sol_q, sol_tauff + return current_lr_arm_motor_q, np.zeros(self.reduced_robot.model.nv) + +class G1_23_ArmIK: + def __init__(self, Unit_Test = False, Visualization = False): + np.set_printoptions(precision=5, suppress=True, linewidth=200) + + self.Unit_Test = Unit_Test + self.Visualization = Visualization + + if not self.Unit_Test: + self.robot = pin.RobotWrapper.BuildFromURDF('../assets/g1/g1_body23.urdf', '../assets/g1/') + else: + self.robot = pin.RobotWrapper.BuildFromURDF('../../assets/g1/g1_body23.urdf', '../../assets/g1/') # for test + + self.mixed_jointsToLockIDs = [ + "left_hip_pitch_joint" , + "left_hip_roll_joint" , + "left_hip_yaw_joint" , + "left_knee_joint" , + "left_ankle_pitch_joint" , + "left_ankle_roll_joint" , + "right_hip_pitch_joint" , + "right_hip_roll_joint" , + "right_hip_yaw_joint" , + "right_knee_joint" , + "right_ankle_pitch_joint" , + "right_ankle_roll_joint" , + "waist_yaw_joint" , + ] + + self.reduced_robot = self.robot.buildReducedRobot( + list_of_joints_to_lock=self.mixed_jointsToLockIDs, + reference_configuration=np.array([0.0] * self.robot.model.nq), + ) + + self.reduced_robot.model.addFrame( + pin.Frame('L_ee', + self.reduced_robot.model.getJointId('left_wrist_roll_joint'), + pin.SE3(np.eye(3), + np.array([0.20,0,0]).T), + pin.FrameType.OP_FRAME) + ) + + self.reduced_robot.model.addFrame( + pin.Frame('R_ee', + self.reduced_robot.model.getJointId('right_wrist_roll_joint'), + pin.SE3(np.eye(3), + np.array([0.20,0,0]).T), + pin.FrameType.OP_FRAME) + ) + + # for i in range(self.reduced_robot.model.nframes): + # frame = self.reduced_robot.model.frames[i] + # frame_id = self.reduced_robot.model.getFrameId(frame.name) + # print(f"Frame ID: {frame_id}, Name: {frame.name}") + + # Creating Casadi models and data for symbolic computing + self.cmodel = cpin.Model(self.reduced_robot.model) + self.cdata = self.cmodel.createData() + + # Creating symbolic variables + self.cq = casadi.SX.sym("q", self.reduced_robot.model.nq, 1) + self.cTf_l = casadi.SX.sym("tf_l", 4, 4) + self.cTf_r = casadi.SX.sym("tf_r", 4, 4) + cpin.framesForwardKinematics(self.cmodel, self.cdata, self.cq) + + # Get the hand joint ID and define the error function + self.L_hand_id = self.reduced_robot.model.getFrameId("L_ee") + self.R_hand_id = self.reduced_robot.model.getFrameId("R_ee") + + self.translational_error = casadi.Function( + "translational_error", + [self.cq, self.cTf_l, self.cTf_r], + [ + casadi.vertcat( + self.cdata.oMf[self.L_hand_id].translation - self.cTf_l[:3,3], + self.cdata.oMf[self.R_hand_id].translation - self.cTf_r[:3,3] + ) + ], + ) + self.rotational_error = casadi.Function( + "rotational_error", + [self.cq, self.cTf_l, self.cTf_r], + [ + casadi.vertcat( + cpin.log3(self.cdata.oMf[self.L_hand_id].rotation @ self.cTf_l[:3,:3].T), + cpin.log3(self.cdata.oMf[self.R_hand_id].rotation @ self.cTf_r[:3,:3].T) + ) + ], + ) + + # Defining the optimization problem + self.opti = casadi.Opti() + self.var_q = self.opti.variable(self.reduced_robot.model.nq) + self.var_q_last = self.opti.parameter(self.reduced_robot.model.nq) # for smooth + self.param_tf_l = self.opti.parameter(4, 4) + self.param_tf_r = self.opti.parameter(4, 4) + self.translational_cost = casadi.sumsqr(self.translational_error(self.var_q, self.param_tf_l, self.param_tf_r)) + self.rotation_cost = casadi.sumsqr(self.rotational_error(self.var_q, self.param_tf_l, self.param_tf_r)) + self.regularization_cost = casadi.sumsqr(self.var_q) + self.smooth_cost = casadi.sumsqr(self.var_q - self.var_q_last) + + # Setting optimization constraints and goals + self.opti.subject_to(self.opti.bounded( + self.reduced_robot.model.lowerPositionLimit, + self.var_q, + self.reduced_robot.model.upperPositionLimit) + ) + self.opti.minimize(50 * self.translational_cost + 0.5 * self.rotation_cost + 0.02 * self.regularization_cost + 0.1 * self.smooth_cost) + + opts = { + 'ipopt':{ + 'print_level':0, + 'max_iter':50, + 'tol':1e-6 + }, + 'print_time':False,# print or not + 'calc_lam_p':False # https://github.com/casadi/casadi/wiki/FAQ:-Why-am-I-getting-%22NaN-detected%22in-my-optimization%3F + } + self.opti.solver("ipopt", opts) + + self.init_data = np.zeros(self.reduced_robot.model.nq) + self.smooth_filter = WeightedMovingFilter(np.array([0.4, 0.3, 0.2, 0.1]), 10) + self.vis = None + + if self.Visualization: + # Initialize the Meshcat visualizer for visualization + self.vis = MeshcatVisualizer(self.reduced_robot.model, self.reduced_robot.collision_model, self.reduced_robot.visual_model) + self.vis.initViewer(open=True) + self.vis.loadViewerModel("pinocchio") + self.vis.displayFrames(True, frame_ids=[67, 68], axis_length = 0.15, axis_width = 5) + self.vis.display(pin.neutral(self.reduced_robot.model)) + + # Enable the display of end effector target frames with short axis lengths and greater width. + frame_viz_names = ['L_ee_target', 'R_ee_target'] + FRAME_AXIS_POSITIONS = ( + np.array([[0, 0, 0], [1, 0, 0], + [0, 0, 0], [0, 1, 0], + [0, 0, 0], [0, 0, 1]]).astype(np.float32).T + ) + FRAME_AXIS_COLORS = ( + np.array([[1, 0, 0], [1, 0.6, 0], + [0, 1, 0], [0.6, 1, 0], + [0, 0, 1], [0, 0.6, 1]]).astype(np.float32).T + ) + axis_length = 0.1 + axis_width = 20 for frame_viz_name in frame_viz_names: self.vis.viewer[frame_viz_name].set_object( mg.LineSegments( @@ -258,6 +484,7 @@ class G1_29_ArmIK: # return sol_q, sol_tauff return current_lr_arm_motor_q, np.zeros(self.reduced_robot.model.nv) + class H1_2_ArmIK: def __init__(self, Unit_Test = False, Visualization = False): np.set_printoptions(precision=5, suppress=True, linewidth=200) @@ -511,7 +738,8 @@ class H1_2_ArmIK: if __name__ == "__main__": # arm_ik = G1_29_ArmIK(Unit_Test = True, Visualization = True) - arm_ik = H1_2_ArmIK(Unit_Test = True, Visualization = True) + # arm_ik = H1_2_ArmIK(Unit_Test = True, Visualization = True) + arm_ik = G1_23_ArmIK(Unit_Test = True, Visualization = True) # initial positon L_tf_target = pin.SE3( @@ -526,7 +754,7 @@ if __name__ == "__main__": rotation_speed = 0.005 noise_amplitude_translation = 0.001 - noise_amplitude_rotation = 0.1 + noise_amplitude_rotation = 0.05 user_input = input("Please enter the start signal (enter 's' to start the subsequent program):\n") if user_input.lower() == 's': diff --git a/teleop/teleop_hand_and_arm.py b/teleop/teleop_hand_and_arm.py index de325c8..de281c3 100644 --- a/teleop/teleop_hand_and_arm.py +++ b/teleop/teleop_hand_and_arm.py @@ -12,8 +12,8 @@ parent_dir = os.path.dirname(current_dir) sys.path.append(parent_dir) from teleop.open_television.tv_wrapper import TeleVisionWrapper -from teleop.robot_control.robot_arm import G1_29_ArmController, H1_2_ArmController -from teleop.robot_control.robot_arm_ik import G1_29_ArmIK, H1_2_ArmIK +from teleop.robot_control.robot_arm import G1_29_ArmController, G1_23_ArmController, H1_2_ArmController +from teleop.robot_control.robot_arm_ik import G1_29_ArmIK, G1_23_ArmIK, H1_2_ArmIK from teleop.robot_control.robot_hand_unitree import Dex3_1_Controller, Gripper_Controller from teleop.image_server.image_client import ImageClient from teleop.utils.episode_writer import EpisodeWriter @@ -28,7 +28,7 @@ if __name__ == '__main__': parser.add_argument('--no-record', dest = 'record', action = 'store_false', help = 'Do not save data') parser.set_defaults(record = False) - parser.add_argument('--arm', type=str, choices=['G1_29', 'H1_2'], default='G1_29', help='Select arm controller') + parser.add_argument('--arm', type=str, choices=['G1_29', 'G1_23', 'H1_2'], default='G1_29', help='Select arm controller') parser.add_argument('--hand', type=str, choices=['dex3', 'gripper', 'inspire1'], help='Select hand controller') @@ -83,6 +83,9 @@ if __name__ == '__main__': if args.arm == 'G1_29': arm_ctrl = G1_29_ArmController() arm_ik = G1_29_ArmIK() + elif args.arm == 'G1_23': + arm_ctrl = G1_23_ArmController() + arm_ik = G1_23_ArmIK() elif args.arm == 'H1_2': arm_ctrl = H1_2_ArmController() arm_ik = H1_2_ArmIK() @@ -160,12 +163,18 @@ if __name__ == '__main__': right_hand_state = dual_hand_state_array[-7:] left_hand_action = dual_hand_action_array[:7] right_hand_action = dual_hand_action_array[-7:] - else: + elif args.hand == "gripper": with dual_gripper_data_lock: left_hand_state = [dual_gripper_state_array[1]] right_hand_state = [dual_gripper_state_array[0]] left_hand_action = [dual_gripper_action_array[1]] right_hand_action = [dual_gripper_action_array[0]] + elif args.hand == "inspire1": + print("Inspire1_Controller comming soon.") + pass + else: + print("No dexterous hand set.") + pass # head image current_tv_image = tv_img_array.copy() # wrist image