[add] add set qpos functionality

dex_retargeting/__init__.py

@@ -1 +1 @@
-__version__ = "0.4.1"
+__version__ = "0.4.2"

dex_retargeting/seq_retarget.py

@@ -124,6 +124,10 @@ def retarget(self, ref_value, fixed_qpos=np.array([])):
             robot_qpos = self.filter.next(robot_qpos)
         return robot_qpos
 
+    def set_qpos(self, robot_qpos: np.ndarray):
+        target_qpos = robot_qpos[self.optimizer.idx_pin2target]
+        self.last_qpos = target_qpos
+
     def verbose(self):
         min_value = self.optimizer.opt.last_optimum_value()
         print(f"Retargeting {self.num_retargeting} times takes: {self.accumulated_time}s")

tests/test_optimizer.py

@@ -30,7 +30,7 @@ def sample_qpos(optimizer: Optimizer):
 
         init_qpos = np.clip(
             random_qpos + np.random.randn(robot.dof) * 0.5, joint_limit[:, 0] + joint_eps, joint_limit[:, 1] - joint_eps
-        )[optimizer.idx_pin2target]
+        )
         return random_qpos, init_qpos
 
     @staticmethod
@@ -85,12 +85,16 @@ def test_position_optimizer(self, robot_name, hand_type):
         errors = dict(pos=[], joint=[])
         np.random.seed(1)
         for i in range(num_optimization):
+
             # Sampled random position
             random_qpos, init_qpos, random_target_pos = self.generate_position_retargeting_data_gt(robot, optimizer)
             fixed_qpos = random_qpos[optimizer.idx_pin2fixed]
 
+            # Set the initial qpos for retargeting
+            retargeting.set_qpos(init_qpos)
+
             # Optimized position
-            computed_qpos = optimizer.retarget(random_target_pos, fixed_qpos=fixed_qpos, last_qpos=init_qpos[:])
+            computed_qpos = retargeting.retarget(random_target_pos, fixed_qpos=fixed_qpos)[optimizer.idx_pin2target]
 
             # Check results
             robot.compute_forward_kinematics(self.compute_pin_qpos(optimizer, computed_qpos, fixed_qpos))
@@ -137,6 +141,9 @@ def test_vector_optimizer(self, robot_name, hand_type):
             random_qpos, init_qpos, random_target_vector = self.generate_vector_retargeting_data_gt(robot, optimizer)
             fixed_qpos = random_qpos[optimizer.idx_pin2fixed]
 
+            # Using a different method compared to position retargeting to set initial qpos and perform optimization
+            init_qpos = init_qpos[optimizer.idx_pin2target]
+
             # Optimized vector
             computed_qpos = optimizer.retarget(random_target_vector, fixed_qpos=fixed_qpos, last_qpos=init_qpos[:])
 
@@ -188,6 +195,9 @@ def test_dexpilot_optimizer(self, robot_name, hand_type):
             random_qpos, init_qpos, random_target_vector = self.generate_vector_retargeting_data_gt(robot, optimizer)
             fixed_qpos = random_qpos[optimizer.idx_pin2fixed]
 
+            # Using a different method compared to position retargeting to set initial qpos and perform optimization
+            init_qpos = init_qpos[optimizer.idx_pin2target]
+
             # Optimized vector
             computed_qpos = optimizer.retarget(random_target_vector, fixed_qpos=fixed_qpos, last_qpos=init_qpos[:])