EmbodiedAI
/
unitree_rl_gym
mirror of https://github.com/unitreerobotics/unitree_rl_gym.git
3
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

add qmot

This commit is contained in:
yjinzero 2025-02-15 11:16:42 +09:00
parent 1a92a43e41
commit 8a2fce5a45
1 changed files with 11 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
deploy/deploy_real/act_to_dof.py
View File

@ -24,6 +24,10 @@ class ActToDof:
self.config.motor_joint, self.config.motor_joint,
self.config.arm_joint self.config.arm_joint
) )
self.mot_from_lab = index_map(
self.config.motor_joint,
self.config.lab_joint
)
self.mot_from_nonarm = index_map( self.mot_from_nonarm = index_map(
self.config.motor_joint, self.config.motor_joint,
self.config.non_arm_joint self.config.non_arm_joint
@ -33,10 +37,12 @@ class ActToDof:
non_arm_joint_pos = action[..., :22] non_arm_joint_pos = action[..., :22]
left_arm_residual = action[..., 22:29] left_arm_residual = action[..., 22:29]
q_lab = obs[..., 32:61]
q_pin = np.zeros_like(self.ikctrl.cfg.q) q_pin = np.zeros_like(self.ikctrl.cfg.q)
for i_mot in range(len(self.config.motor_joint)): q_pin[self.pin_from_lab] = q_lab
i_pin = self.pin_from_mot[i_mot]
q_pin[i_pin] = self.low_state.motor_state[i_mot].q q_mot = np.zeros(29)
q_mot[self.mot_from_lab] = q_lab
d_quat = quat_from_angle_axis( d_quat = quat_from_angle_axis(
torch.from_numpy(hands_command[..., 3:]) torch.from_numpy(hands_command[..., 3:])
@ -63,7 +69,7 @@ class ActToDof:
i_mot = self.mot_from_arm[i_arm] i_mot = self.mot_from_arm[i_arm]
i_pin = self.pin_from_mot[i_mot] i_pin = self.pin_from_mot[i_mot]
target_q = ( target_q = (
self.low_state.motor_state[i_mot].q q_mot[i_mot]
+ res_q_ik[i_arm] + res_q_ik[i_arm]
+ np.clip(0.3 * left_arm_residual[i_arm], + np.clip(0.3 * left_arm_residual[i_arm],
-0.2, 0.2) -0.2, 0.2)