add mimic joint constraints & updated jacobian; need to set maxeval t…

…o limit optimization time

dex_retargeting/optimizer.py

@@ -23,7 +23,6 @@ def __init__(
         self.wrist_link_name = wrist_link_name
 
         joint_names = [joint.get_name() for joint in robot.get_active_joints()]
-        # print(joint_names)
         target_joint_index = []
         for target_joint_name in target_joint_names:
             if target_joint_name not in joint_names:
@@ -128,8 +127,7 @@ def __init__(
         super().__init__(robot, wrist_link_name, target_joint_names, target_link_human_indices)
         self.body_names = target_link_names
         self.huber_loss = torch.nn.SmoothL1Loss(beta=huber_delta)
-        self.norm_delta = np.float64(norm_delta)
-        print(self.norm_delta)
+        self.norm_delta = norm_delta
 
         # Sanity check and cache link indices
         self.target_link_indices = self.get_link_indices(target_link_names)
@@ -542,85 +540,3 @@ def retarget(self, ref_value, fixed_qpos, last_qpos=None):
         )
         return self.optimize(objective_fn, last_qpos)
 
-
-# This is a simple IK calculator
-# for the convenience of sequential retargeting pipeline
-class IKOptimizer(Optimizer):
-    retargeting_type = "ik"
-
-    def __init__(
-        self,
-        robot: sapien.Articulation,
-        target_joint_names: List[str],
-        wrist_link_name: str,
-        huber_delta=0.02,
-        norm_delta=4e-3,
-    ):
-        super().__init__(robot, wrist_link_name, target_joint_names, [])
-        self.huber_loss = torch.nn.SmoothL1Loss(beta=huber_delta)
-        self.norm_delta = norm_delta
-        self.target_link_index = self.get_link_indices([wrist_link_name])[0]
-        self.use_sparse_jacobian = True
-        self.opt.set_ftol_abs(1e-5)
-
-    def _get_objective_function(self, target_pos: np.ndarray, fixed_qpos: np.ndarray, last_qpos: np.ndarray):
-        qpos = np.zeros(self.robot_dof)
-        qpos[self.fixed_joint_indices] = fixed_qpos
-        torch_target_pos = torch.as_tensor(target_pos[0:3])
-        torch_target_pos.requires_grad_(False)
-        torch_target_rot = torch.as_tensor(target_pos[3:7])
-        torch_target_rot.requires_grad_(False)
-
-        def objective(x: np.ndarray, grad: np.ndarray) -> float:
-            qpos[self.target_joint_indices] = x
-            self.model.compute_forward_kinematics(qpos)
-            target_link_pose = self.model.get_link_pose(self.target_link_index)
-            body_pos = target_link_pose.p
-            body_rot = target_link_pose.q
-
-            # Torch computation for accurate loss and grad
-            torch_body_pos = torch.as_tensor(body_pos)
-            torch_body_pos.requires_grad_()
-            torch_body_rot = torch.as_tensor(body_rot)
-            torch_body_rot.requires_grad_()
-
-            # Loss term for kinematics retargeting based on 3D position error
-            pos_distance = self.huber_loss(torch_body_pos, torch_target_pos)
-            rot_distance = 1 - torch.sum(torch_body_rot * torch_target_rot) ** 2
-            huber_distance = pos_distance
-            # huber_distance = pos_distance + rot_distance
-            # huber_distance = torch.norm(torch_body_pos - torch_target_pos, dim=1).mean()
-            result = huber_distance.cpu().detach().item()
-
-            if grad.size > 0:
-                link_spatial_jacobian = self.model.compute_single_link_local_jacobian(qpos, self.target_link_index)[
-                    :3, self.target_joint_indices
-                ]
-                link_rot = self.model.get_link_pose(self.target_link_index).to_transformation_matrix()[:3, :3]
-                jacobian = link_rot @ link_spatial_jacobian
-
-                huber_distance.backward()
-                grad_pos = torch_body_pos.grad.cpu().numpy()[None, :]
-                grad_qpos = np.matmul(grad_pos, np.array(jacobian))
-                grad_qpos = grad_qpos.mean(1).sum(0)
-
-                grad_qpos += 2 * self.norm_delta * (x - last_qpos)
-
-                grad[:] = grad_qpos[:]
-
-            return result
-
-        return objective
-
-    def retarget(self, ref_value, fixed_qpos, last_qpos=None):
-        if len(fixed_qpos) != len(self.fixed_joint_indices):
-            raise ValueError(
-                f"Optimizer has {len(self.fixed_joint_indices)} joints but non_target_qpos {fixed_qpos} is given"
-            )
-        if last_qpos is None:
-            last_qpos = np.zeros(self.dof)
-        if isinstance(last_qpos, np.ndarray):
-            last_qpos = last_qpos.astype(np.float32)
-        last_qpos = list(last_qpos)
-        objective_fn = self._get_objective_function(ref_value, fixed_qpos, np.array(last_qpos).astype(np.float32))
-        return self.optimize(objective_fn, last_qpos)