@@ -24,10 +24,12 @@ def __init__(self):
2424 self .q = None
2525 self .dq = None
2626
27+
2728class G1_29_LowState :
2829 def __init__ (self ):
2930 self .motor_state = [MotorState () for _ in range (G1_29_Num_Motors )]
3031
32+
3133class DataBuffer :
3234 def __init__ (self ):
3335 self .data = None
@@ -41,6 +43,7 @@ def SetData(self, data):
4143 with self .lock :
4244 self .data = data
4345
46+
4447class G1_29_ArmController :
4548 def __init__ (self ):
4649 print ("Initialize G1_29_ArmController..." )
@@ -107,7 +110,7 @@ def __init__(self):
107110 else :
108111 self .msg .motor_cmd [id ].kp = self .kp_high
109112 self .msg .motor_cmd [id ].kd = self .kd_high
110- self .msg .motor_cmd [id ].q = self .all_motor_q [id ]
113+ self .msg .motor_cmd [id ].q = self .all_motor_q [id ]
111114 print ("Lock OK!\n " )
112115
113116 # initialize publish thread
@@ -124,7 +127,7 @@ def _subscribe_motor_state(self):
124127 if msg is not None :
125128 lowstate = G1_29_LowState ()
126129 for id in range (G1_29_Num_Motors ):
127- lowstate .motor_state [id ].q = msg .motor_state [id ].q
130+ lowstate .motor_state [id ].q = msg .motor_state [id ].q
128131 lowstate .motor_state [id ].dq = msg .motor_state [id ].dq
129132 self .lowstate_buffer .SetData (lowstate )
130133 time .sleep (0.002 )
@@ -141,10 +144,10 @@ def _ctrl_motor_state(self):
141144 start_time = time .time ()
142145
143146 with self .ctrl_lock :
144- arm_q_target = self .q_target
147+ arm_q_target = self .q_target
145148 arm_tauff_target = self .tauff_target
146149
147- cliped_arm_q_target = self .clip_arm_q_target (arm_q_target , velocity_limit = self .arm_velocity_limit )
150+ cliped_arm_q_target = self .clip_arm_q_target (arm_q_target , velocity_limit = self .arm_velocity_limit )
148151
149152 for idx , id in enumerate (G1_29_JointArmIndex ):
150153 self .msg .motor_cmd [id ].q = cliped_arm_q_target [idx ]
@@ -201,7 +204,7 @@ def ctrl_dual_arm_go_home(self):
201204 break
202205 time .sleep (0.05 )
203206
204- def speed_gradual_max (self , t = 5.0 ):
207+ def speed_gradual_max (self , t = 5.0 ):
205208 """Parameter t is the total time required for arms velocity to gradually increase to its maximum value, in seconds. The default is 5.0."""
206209 self ._gradual_start_time = time .time ()
207210 self ._gradual_time = t
@@ -239,6 +242,7 @@ def _Is_wrist_motor(self, motor_index):
239242 ]
240243 return motor_index .value in wrist_motors
241244
245+
242246class G1_29_JointArmIndex (IntEnum ):
243247 # Left arm
244248 kLeftShoulderPitch = 15
@@ -258,6 +262,7 @@ class G1_29_JointArmIndex(IntEnum):
258262 kRightWristPitch = 27
259263 kRightWristYaw = 28
260264
265+
261266class G1_29_JointIndex (IntEnum ):
262267 # Left leg
263268 kLeftHipPitch = 0
0 commit comments