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