working...integration...??

deploy/deploy_real/act_to_dof.py

@@ -7,12 +7,12 @@ from config import Config
 from ikctrl import IKCtrl
 
 class ActToDof:
-    def __init__(self, config):
+    def __init__(self, config, ikctrl):
         self.config=config
-        self.ikctrl = IKCtrl(
+        self.ikctrl = ikctrl
-            '../../resources/robots/g1_description/g1_29dof_with_hand_rev_1_0.urdf',
+        self.lim_lo_pin = self.ikctrl.robot.model.lowerPositionLimit
-            config.arm_joint
+        self.lim_hi_pin = self.ikctrl.robot.model.upperPositionLimit
-        )
+
         self.mot_from_pin = index_map(
             self.config.motor_joint,
             self.ikctrl.joint_names)
@@ -24,6 +24,10 @@ class ActToDof:
             self.ikctrl.joint_names,
             self.config.lab_joint
         )
+        self.pin_from_arm = index_map(
+            self.ikctrl.joint_names,
+            self.config.arm_joint
+        )
         self.mot_from_arm = index_map(
             self.config.motor_joint,
             self.config.arm_joint
@@ -32,12 +36,25 @@ class ActToDof:
             self.config.motor_joint,
             self.config.lab_joint
         )
+        self.lab_from_mot = index_map(
+            self.config.lab_joint,
+            self.config.motor_joint
+        )
         self.mot_from_nonarm = index_map(
             self.config.motor_joint,
             self.config.non_arm_joint
         )
+
+        self.lab_from_nonarm = index_map(
+            self.config.lab_joint,
+            self.config.non_arm_joint
+        )
+        self.default_nonarm = (
+            np.asarray(self.config.lab_joint_offsets)[self.lab_from_nonarm]
+        )
+
     def __call__(self, obs, action):
-        hands_command = obs[..., 119:225]
+        hands_command = obs[..., 119:125]
         non_arm_joint_pos = action[..., :22]
         left_arm_residual = action[..., 22:29]
         
@@ -48,6 +65,9 @@ class ActToDof:
         q_mot = np.zeros(29)
         q_mot[self.mot_from_lab] = q_lab
 
+        # print('q0', q_mot[self.mot_from_arm])
+        # q_mot[i_mot] = q_lab[ lab_from_mot[i_mot] ]
+
         d_quat = quat_from_angle_axis(
             torch.from_numpy(hands_command[..., 3:])
         ).detach().cpu().numpy()
@@ -66,40 +86,112 @@ class ActToDof:
             q_pin,
             target
         )
-        print('res_q_ik', res_q_ik)
+
+        # q_pin2 = np.copy(q_pin)
+        # q_pin2[self.pin_from_arm] += res_q_ik
+        # print('target', target)
+        # se3=self.ikctrl.fk(q_pin2)
+        # print('fk(IK(target))', se3.translation,
+        #         xyzw2wxyz(pin.Quaternion(se3.rotation).coeffs()))
+
+        # print('res_q_ik', res_q_ik)
+        # print('left_arm_residual',
+        #         0.3 * left_arm_residual,
+        #         np.clip(0.3 * left_arm_residual, -0.2, 0.2))
 
         target_dof_pos = np.zeros(29)
-        for i_arm in range(len(res_q_ik)):
+        target_dof_pos += q_mot
-            i_mot = self.mot_from_arm[i_arm]
+        target_dof_pos[self.mot_from_arm] += res_q_ik
-            i_pin = self.pin_from_mot[i_mot]
+        target_dof_pos[self.mot_from_arm] += np.clip(
-            target_q = (
+                0.3 * left_arm_residual,
-                q_mot[i_mot]
+                -0.2, 0.2)
-                + res_q_ik[i_arm]
+
-                + np.clip(0.3 * left_arm_residual[i_arm],
+        # print('default joint pos', self.default_nonarm)
-                          -0.2, 0.2)
+        # print('joint order', self.config.non_arm_joint)
-            )
+        # print('mot_from_nonarm', self.mot_from_nonarm)
-            target_q = np.clip(target_q,
+        target_dof_pos[self.mot_from_nonarm] = (
-                               self.lim_lo_pin[i_pin],
+            self.default_nonarm + 0.5 * non_arm_joint_pos
-                               self.lim_hi_pin[i_pin])
+        )
-            target_dof_pos[i_mot] = target_q
+
+        target_dof_pos[self.mot_from_arm] = np.clip(
+                target_dof_pos[self.mot_from_arm],
+                self.lim_lo_pin[self.pin_from_arm],
+                self.lim_hi_pin[self.pin_from_arm]
+        )
+
         return target_dof_pos
 
 def main():
+    from matplotlib import pyplot as plt
     import yaml
 
     with open('configs/g1_eetrack.yaml', 'r') as fp:
         d = yaml.safe_load(fp)
+    config = Config('configs/g1_eetrack.yaml')
+    ikctrl = IKCtrl(
+        '../../resources/robots/g1_description/g1_29dof_with_hand_rev_1_0.urdf',
+        config.arm_joint
+    )
+    act_to_dof = ActToDof(config, ikctrl)
 
-    act_to_dof = ActToDof(Config('configs/g1_eetrack.yaml'))
+    exps = []
-    obs = np.load('/tmp/eet4/obs001.npy')[0]
+    cals = []
-    act = np.load('/tmp/eet4/act001.npy')[0]
+    poss = []
-    dof_lab = np.load('/tmp/eet4/dof001.npy')[0]
+
-    mot_from_lab = index_map(d['motor_joint'], d['lab_joint'])
+    for i in range(70):
-    target_dof_pos = np.zeros_like(dof_lab)
+        obs = np.load(F'/tmp/eet5/obs{i:03d}.npy')[0]
-    target_dof_pos[mot_from_lab] = dof_lab
+        # print(obs.shape)
+        act = np.load(F'/tmp/eet5/act{i:03d}.npy')[0]
+        # print('act', act.shape)
+        dof_lab = np.load(F'/tmp/eet5/dof{i:03d}.npy')[0]
+        mot_from_lab = index_map(d['motor_joint'], d['lab_joint'])
+        target_dof_pos = np.zeros_like(dof_lab)
+        target_dof_pos[mot_from_lab] = dof_lab
 
 
-    dof = act_to_dof(obs, act)
+        dof = act_to_dof(obs, act)
+
+        export = target_dof_pos
+        calc   = dof
+
+        mot_from_arm = index_map(d['motor_joint'], d['arm_joint'])
+        # print('exported', target_dof_pos[mot_from_arm],
+        #       'calculated', dof[mot_from_arm])
+        # print( (export - calc)[mot_from_arm] )
+        exps.append( target_dof_pos[mot_from_arm] )
+        cals.append( dof[mot_from_arm] )
+
+        q_lab = obs[..., 32:61]
+        q_mot = q_lab[act_to_dof.lab_from_mot]
+        q_arm = q_mot[mot_from_arm]
+        poss.append(q_arm)
+
+    exps=np.asarray(exps, dtype=np.float32)
+    cals=np.asarray(cals, dtype=np.float32)
+    poss=np.asarray(poss, dtype=np.float32)
+    print(exps.shape)
+    print(cals.shape)
+
+    fig, ax=plt.subplots(7,1)
+    
+    q_lo =act_to_dof.lim_lo_pin[act_to_dof.pin_from_arm]
+    q_hi =act_to_dof.lim_hi_pin[act_to_dof.pin_from_arm]
+    RES = True
+    for i in range(7):
+        if RES:
+            ax[i].axhline(0)
+        else:
+            ax[i].axhline(q_lo[i], color='k', linestyle='--')
+            ax[i].axhline(q_hi[i], color='k', linestyle='--')
+        if RES:
+            ax[i].plot(exps[:, i]-poss[:,i], label='sim')
+            ax[i].plot(cals[:, i]-poss[:,i], label='real')
+        else:
+            ax[i].plot(poss[:, i], label='pos')
+            ax[i].plot(exps[:, i], label='sim')
+            ax[i].plot(cals[:, i], label='real')
+        ax[i].legend()
+    plt.show()
 
 if __name__ == '__main__':
     main()

deploy/deploy_real/deploy_real_ros_eetrack.py

@@ -24,6 +24,7 @@ from yourdfpy import URDF
 
 from math_utils import *
 import random as rd
+from act_to_dof import ActToDof
 
 
 class Mode(Enum):
@@ -499,6 +500,7 @@ class Controller:
         self.ikctrl = IKCtrl(
             '../../resources/robots/g1_description/g1_29dof_with_hand_rev_1_0.urdf',
             config.arm_joint)
+        self.actmap = ActToDof(config, self.ikctrl)
         self.lim_lo_pin = self.ikctrl.robot.model.lowerPositionLimit
         self.lim_hi_pin = self.ikctrl.robot.model.upperPositionLimit
 
@@ -519,6 +521,11 @@ class Controller:
             self.config.motor_joint,
             self.config.non_arm_joint
         )
+        self.lab_from_mot = index_map(self.config.lab_joint,
+                                      self.config.motor_joint)
+        self.config.default_angles = np.asarray(self.config.lab_joint_offsets)[
+                self.lab_from_mot
+        ]
 
         # Data buffers
         self.obs = np.zeros(config.num_obs, dtype=np.float32)
@@ -627,18 +634,25 @@ class Controller:
         self._dof_idx = dof_idx
 
         # record the current pos
-        self._init_dof_pos = np.zeros(self._dof_size,
+        # self._init_dof_pos = np.zeros(self._dof_size,
-                                      dtype=np.float32)
+        #                               dtype=np.float32)
-        for i in range(self._dof_size):
+        # for i in range(self._dof_size):
-            self._init_dof_pos[i] = self.low_state.motor_state[dof_idx[i]].q
+        #     self._init_dof_pos[i] = self.low_state.motor_state[dof_idx[i]].q
+        self._init_dof_pos = np.zeros(29)
+        for i in range(29):
+            self._init_dof_pos[i] = self.low_state.motor_state[i].q
 
     def move_to_default_pos(self):
         # move to default pos
         if self.counter < self._num_step:
             alpha = self.counter / self._num_step
-            for j in range(self._dof_size):
+            #for j in range(self._dof_size):
-                motor_idx = self._dof_idx[j]
+            for j in range(29):
-                target_pos = self._default_pos[j]
+                # motor_idx = self._dof_idx[j]
+                # target_pos = self._default_pos[j]
+                motor_idx = j
+                target_pos = self.config.default_angles[j]
+
                 self.low_cmd.motor_cmd[motor_idx].q = (
                     self._init_dof_pos[j] * (1 - alpha) + target_pos * alpha)
                 self.low_cmd.motor_cmd[motor_idx].dq = 0.0
@@ -682,79 +696,41 @@ class Controller:
         self.counter += 1
 
         # TODO(ycho): consider using `cmd` for `hands_command`
-        # self.cmd[0] = self.remote_controller.ly
+        self.cmd[0] = self.remote_controller.ly
-        # self.cmd[1] = self.remote_controller.lx * -1
+        self.cmd[1] = self.remote_controller.lx * -1
-        # self.cmd[2] = self.remote_controller.rx * -1
+        self.cmd[2] = self.remote_controller.rx * -1
 
         # FIXME(ycho): implement `_hands_command_`
         # to use the output of `eetrack`.
         _hands_command_ = np.zeros(6)
+        _hands_command_[0] = self.cmd[0] * 0.03
+        _hands_command_[2] = self.cmd[1] * 0.03
 
         self.obs[:] = self.obsmap(self.low_state,
                                   self.action,
                                   _hands_command_)
-        Path('/tmp/eet/').mkdir(parents=True,
+        # logpath = Path('/tmp/eet3/')
-                                exist_ok=True)
+        # logpath.mkdir(parents=True, exist_ok=True)
-        np.save(F'/tmp/eet/obs{self.counter:03d}.npy',
+        # np.save(F'{logpath}/obs{self.counter:03d}.npy',
-                self.obs)
+        #         self.obs)
 
         # Get the action from the policy network
         obs_tensor = torch.from_numpy(self.obs).unsqueeze(0)
         self.action = self.policy(obs_tensor).detach().numpy().squeeze()
-        np.save(F'/tmp/eet/act{self.counter:03d}.npy',
+        # np.save(F'{logpath}/act{self.counter:03d}.npy',
+        #         self.action)
+
+        target_dof_pos = self.actmap(self.obs,
                 self.action)
-
+        # np.save(F'{logpath}/dof{self.counter:03d}.npy',
-        non_arm_joint_pos = self.action[..., :22]
+        #         target_dof_pos)
-        left_arm_residual = self.action[..., 22:29]
-
-        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
-
-        d_quat = quat_from_angle_axis(
-            torch.from_numpy(_hands_command_[..., 3:])
-        ).detach().cpu().numpy()
-
-        source_pose = self.ikctrl.fk(q_pin)
-        source_xyz = source_pose.translation
-        source_quat = xyzw2wxyz(pin.Quaternion(source_pose.rotation).coeffs())
-
-        target_xyz = source_xyz + _hands_command_[..., :3]
-        target_quat = quat_mul(
-            torch.from_numpy(d_quat),
-            torch.from_numpy(source_quat)).detach().cpu().numpy()
-        target = np.concatenate([target_xyz, target_quat])
-
-        res_q_ik = self.ikctrl(
-            q_pin,
-            target
-        )
-        print('res_q_ik', res_q_ik)
-
-        target_dof_pos = np.zeros(29)
-        for i_arm in range(len(res_q_ik)):
-            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
-                + res_q_ik[i_arm]
-                + np.clip(0.3 * left_arm_residual[i_arm],
-                          -0.2, 0.2)
-            )
-            target_q = np.clip(target_q,
-                               self.lim_lo_pin[i_pin],
-                               self.lim_hi_pin[i_pin])
-            target_dof_pos[i_mot] = target_q
-        np.save(F'/tmp/eet/dof{self.counter:03d}.npy',
-                target_dof_pos)
 
         # Build low cmd
         for i in range(len(self.config.motor_joint)):
             self.low_cmd.motor_cmd[i].q = float(target_dof_pos[i])
             self.low_cmd.motor_cmd[i].dq = 0.0
-            self.low_cmd.motor_cmd[i].kp = 0.0 * float(self.config.kps[i])
+            self.low_cmd.motor_cmd[i].kp = 0.05 * float(self.config.kps[i])
-            self.low_cmd.motor_cmd[i].kd = 0.0 * float(self.config.kds[i])
+            self.low_cmd.motor_cmd[i].kd = 0.05 * float(self.config.kds[i])
             self.low_cmd.motor_cmd[i].tau = 0.0
 
         # send the command