From 8a2fce5a45b6933086257c9e5a6b9283eb901be1 Mon Sep 17 00:00:00 2001 From: yjinzero Date: Sat, 15 Feb 2025 11:16:42 +0900 Subject: [PATCH] add qmot --- deploy/deploy_real/act_to_dof.py | 16 +++++++++++----- 1 file changed, 11 insertions(+), 5 deletions(-) diff --git a/deploy/deploy_real/act_to_dof.py b/deploy/deploy_real/act_to_dof.py index b0f2a6e..36c9424 100644 --- a/deploy/deploy_real/act_to_dof.py +++ b/deploy/deploy_real/act_to_dof.py @@ -24,6 +24,10 @@ class ActToDof: self.config.motor_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.config.motor_joint, self.config.non_arm_joint @@ -32,11 +36,13 @@ class ActToDof: hands_command = obs[..., 119:225] non_arm_joint_pos = action[..., :22] left_arm_residual = action[..., 22:29] - + + q_lab = obs[..., 32:61] q_pin = np.zeros_like(self.ikctrl.cfg.q) - for i_mot in range(len(self.config.motor_joint)): - i_pin = self.pin_from_mot[i_mot] - q_pin[i_pin] = self.low_state.motor_state[i_mot].q + q_pin[self.pin_from_lab] = q_lab + + q_mot = np.zeros(29) + q_mot[self.mot_from_lab] = q_lab d_quat = quat_from_angle_axis( torch.from_numpy(hands_command[..., 3:]) @@ -63,7 +69,7 @@ class ActToDof: i_mot = self.mot_from_arm[i_arm] i_pin = self.pin_from_mot[i_mot] target_q = ( - self.low_state.motor_state[i_mot].q + q_mot[i_mot] + res_q_ik[i_arm] + np.clip(0.3 * left_arm_residual[i_arm], -0.2, 0.2)