abs-ik, test ik

deploy/deploy_real/config.py

@@ -47,6 +47,11 @@ class Config:
             else:
                 self.motor_joint=[]
 
+            if 'act_joint' in config:
+                self.act_joint = config['act_joint']
+            else:
+                self.act_joint=[]
+                
             self.ang_vel_scale = config["ang_vel_scale"]
             self.dof_pos_scale = config["dof_pos_scale"]
             self.dof_vel_scale = config["dof_vel_scale"]

deploy/deploy_real/configs/ik.yaml

@@ -9,6 +9,15 @@ lowstate_topic: "rt/lowstate"
 
 policy_path: "{LEGGED_GYM_ROOT_DIR}/deploy/pre_train/g1/policy.pt"
 
+act_joint: [
+  "left_shoulder_pitch_joint",
+  "left_shoulder_roll_joint",
+  "left_shoulder_yaw_joint",
+  "left_elbow_joint",
+  "left_wrist_roll_joint",
+  "left_wrist_pitch_joint",
+  "left_wrist_yaw_joint"
+]
 motor_joint: [
         'left_hip_pitch_joint',
         'left_hip_roll_joint',
@@ -59,18 +68,24 @@ kps: [
   100, 100, 50, 50, 20, 20, 20
   ]
 kds: [
-  2, 2, 2, 4, 2, 2, 2, 2, 2, 4, 2, 2,
+  # left leg
+  2, 2, 2, 4, 2, 2,
+  # right leg
+  2, 2, 2, 4, 2, 2,
+  # waist
   3, 3, 3, 
+  # left arm
   2, 2, 2, 2, 1, 1, 1,
+  # right arm
   2, 2, 2, 2, 1, 1, 1
   ]
 default_angles: [
   -0.2,  0.0,  0.0,  0.42, -0.23, 0.0, 
   -0.2,  0.0,  0.0,  0.42, -0.23, 0.0, 
   0., 0., 0.,
-  0.35, 0.16, 0., 0.87, 0., 0., 0., 
+  0.0, 0.0, 0., 0.0, 0., 0., 0., 
-  0.35, -0.16, 0., 0.87, 0., 0., 0., 
+  0.0, 0.0, 0., 0.0, 0., 0., 0., 
-  ]
+]
 
 # arm_waist_joint2motor_idx: [12, 13, 14, 
 #                             15, 16, 17, 18, 19, 20, 21, 

deploy/deploy_real/deploy_real_ros_ikctrl.py

@@ -14,7 +14,8 @@ from config import Config
 from common.crc import CRC
 
 from enum import Enum
-from ikctrl import IKCtrl
+import pinocchio as pin
+from ikctrl import IKCtrl, xyzw2wxyz
 
 class Mode(Enum):
     wait = 0
@@ -28,29 +29,35 @@ class Controller:
     def __init__(self, config: Config) -> None:
         self.config = config
         self.remote_controller = RemoteController()
-        act_joint = ["left_shoulder_pitch_joint",
+
-            "left_shoulder_roll_joint",
+        act_joint = config.act_joint
-            "left_shoulder_yaw_joint",
-            "left_elbow_joint",
-            "left_wrist_roll_joint",
-            "left_wrist_pitch_joint",
-            "left_wrist_yaw_joint"]
         self.ikctrl = IKCtrl('../../resources/robots/g1_description/g1_29dof_with_hand_rev_1_0.urdf',
                              act_joint)
         self.lim_lo_pin = self.ikctrl.robot.model.lowerPositionLimit
         self.lim_hi_pin = self.ikctrl.robot.model.upperPositionLimit
 
+        # == build index map ==
         self.pin_from_mot = np.zeros(29, dtype=np.int32) # FIXME(ycho): hardcoded
         self.mot_from_pin = np.zeros(43, dtype=np.int32) # FIXME(ycho): hardcoded
-        self.mot_from_pin_act = np.zeros(7, dtype=np.int32) # FIXME(ycho): hardcoded
+        self.mot_from_act = np.zeros(7, dtype=np.int32) # FIXME(ycho): hardcoded
         for i_mot, j in enumerate( self.config.motor_joint ):
             i_pin = (self.ikctrl.robot.index(j) - 1)
             self.pin_from_mot[i_mot] = i_pin
             self.mot_from_pin[i_pin] = i_mot
             if j in act_joint:
                 i_act = act_joint.index(j)
-                self.mot_from_pin_act[i_act] = i_mot
+                self.mot_from_act[i_act] = i_mot
 
+        q_mot = np.array(config.default_angles)
+        q_pin = np.zeros_like(self.ikctrl.cfg.q)
+        q_pin[self.pin_from_mot] = q_mot
+
+        if True:
+            default_pose = self.ikctrl.fk(q_pin)
+            xyz = default_pose.translation
+            quat_wxyz = xyzw2wxyz(pin.Quaternion(default_pose.rotation).coeffs())
+            self.default_pose = np.concatenate([xyz, quat_wxyz])
+            self.target_pose = np.copy(self.default_pose)
 
         # Initialize the policy network
         self.policy = torch.jit.load(config.policy_path)
@@ -224,14 +231,24 @@ class Controller:
         for i_mot in range(len(self.config.motor_joint)):
             i_pin = self.pin_from_mot[i_mot]
             self.qj[i_pin] = self.low_state.motor_state[i_mot].q
+
         self.cmd[0] = self.remote_controller.ly
         self.cmd[1] = self.remote_controller.lx * -1
         self.cmd[2] = self.remote_controller.rx * -1
+
+        if False:
             delta = np.concatenate([self.cmd,
                                     [1,0,0,0]])
             res_q = self.ikctrl(self.qj, delta, rel=True)
+        else:
+            # FIXME(ycho): 0.01 --> cmd_scale ?
+            self.target_pose[..., :3] += 0.01 * self.cmd
+            res_q = self.ikctrl(self.qj,
+                                self.target_pose,
+                                rel=False)
+
         for i_act in range(len(res_q)):
-            i_mot = self.mot_from_pin_act[i_act]
+            i_mot = self.mot_from_act[i_act]
             i_pin = self.pin_from_mot[i_mot]
             target_q = (
                     self.low_state.motor_state[i_mot].q + res_q[i_act]

deploy/deploy_real/ikctrl.py

@@ -4,8 +4,10 @@ from contextlib import contextmanager
 from pathlib import Path
 import time
 
+import yaml
 import numpy as np
 import torch as th
+from yourdfpy import URDF
 
 import pinocchio as pin
 import pink
@@ -61,31 +63,42 @@ def dls_ik(
 class IKCtrl:
     def __init__(self,
                  urdf_path: str,
-                 joint_names: Tuple[str, ...],
+                 act_joints: Tuple[str, ...],
+                 frame: str = 'left_hand_palm_link',
                  sqlmda: float = 0.05**2):
         path = Path(urdf_path)
-        print('urdf path', path.parent.resolve())
         with with_dir(path.parent):
             robot = pin.RobotWrapper.BuildFromURDF(filename=path.name,
                                                    package_dirs=["."],
                                                    root_joint=None)
             self.robot = robot
-        # NOTE(ycho): build index map between pin.q and other set(s) of ordered joints.
+            # NOTE(ycho): we skip joint#0(="universe")
-        larm_from_pin = []
+            joint_names = list(self.robot.model.names)
-        for j in joint_names:
+            assert (joint_names[0] == 'universe')
-            larm_from_pin.append(robot.index(j) - 1)
+            self.joint_names = joint_names[1:]
-        self.larm_from_pin = np.asarray(larm_from_pin, dtype=np.int32)
+
-        self.task = FrameTask("left_hand_palm_link",
+        # NOTE(ycho): build index map between pin.q and other set(s) of ordered
-                              position_cost=1.0,
+        # joints.
-                              orientation_cost=0.0)
+        act_from_pin = []
+        for j in act_joints:
+            act_from_pin.append(robot.index(j) - 1)
+        self.frame = frame
+        self.act_from_pin = np.asarray(act_from_pin, dtype=np.int32)
+        self.task = FrameTask(frame, position_cost=1.0, orientation_cost=0.0)
         self.sqlmda = sqlmda
         self.cfg = pink.Configuration(robot.model, robot.data,
                                       np.zeros_like(robot.q0))
 
+    def fk(self, q: np.ndarray):
+        robot = self.robot
+        return pink.Configuration(
+            robot.model, robot.data, q).get_transform_frame_to_world(
+            self.frame)
+
     def __call__(self,
                  q0: np.ndarray,
                  target_pose: np.ndarray,
-                 rel:bool=False
+                 rel: bool = False
                  ):
         """
         Arg:
@@ -100,7 +113,7 @@ class IKCtrl:
 
         # source pose
         self.cfg.update(q0)
-        T0 = self.cfg.get_transform_frame_to_world("left_hand_palm_link")
+        T0 = self.cfg.get_transform_frame_to_world(self.frame)
 
         # target pose
         dst_xyz = target_pose[..., 0:3]
@@ -116,7 +129,7 @@ class IKCtrl:
         # jacobian
         self.task.set_target(T0)
         jac = self.task.compute_jacobian(self.cfg)
-        jac = jac[:, self.larm_from_pin]
+        jac = jac[:, self.act_from_pin]
 
         # error&ik
         dT = T1.actInv(T0)
@@ -124,3 +137,73 @@ class IKCtrl:
         dq = dls_ik(dpose, jac, self.sqlmda)
         return dq
 
+
+def main():
+    urdf_path = '../../resources/robots/g1_description/g1_29dof_with_hand_rev_1_0.urdf'
+
+    # == init yourdfpy robot ==
+    path = Path(urdf_path)
+    with with_dir(path.parent):
+        viz = URDF.load(path.name,
+                        build_collision_scene_graph=True,
+                        load_meshes=False,
+                        load_collision_meshes=True)
+
+    # == populate with defaults ==
+    with open('./configs/ik.yaml', 'r') as fp:
+        data = yaml.safe_load(fp)
+    q_mot = np.asarray(data['default_angles'])
+    q_viz = np.zeros((len(viz.actuated_joint_names)))
+
+    viz_from_mot = np.zeros(len(data['motor_joint']),
+                            dtype=np.int32)
+    for i_mot, j in enumerate(data['motor_joint']):
+        i_viz = viz.actuated_joint_names.index(j)
+        viz_from_mot[i_mot] = i_viz
+    q_viz[viz_from_mot] = q_mot
+
+    ctrl = IKCtrl(urdf_path, data['act_joint'])
+
+    q_pin = np.zeros_like(ctrl.cfg.q)
+    pin_from_mot = np.zeros(len(data['motor_joint']),
+                            dtype=np.int32)
+    for i_mot, j in enumerate(data['motor_joint']):
+        i_pin = ctrl.joint_names.index(j)
+        pin_from_mot[i_mot] = i_pin
+
+    mot_from_act = np.zeros(len(data['act_joint']),
+                            dtype=np.int32)
+    for i_act, j in enumerate(data['act_joint']):
+        i_mot = data['motor_joint'].index(j)
+        mot_from_act[i_act] = i_mot
+
+    viz.update_cfg(q_viz)
+    # viz.show(collision_geometry=True)
+
+    if True:
+        current_pose = viz.get_transform(ctrl.frame)
+        print('curpose (viz)', current_pose)
+        print('curpose (pin)', ctrl.fk(q_pin).homogeneous)
+
+    def callback(scene):
+        if True:
+            current_pose = viz.get_transform(ctrl.frame)
+            T = pin.SE3()
+            T.translation = (current_pose[..., :3, 3]
+                             + [0.01, 0.0, 0.0])
+            T.rotation = current_pose[..., :3, :3]
+            target_pose = pin.SE3ToXYZQUAT(T)
+            target_pose[..., 3:7] = xyzw2wxyz(target_pose[..., 3:7])
+        q_pin[pin_from_mot] = q_mot
+        dq = ctrl(q_pin, target_pose, rel=False)
+        q_mot[mot_from_act] += dq
+
+        q_viz[viz_from_mot] = q_mot
+        viz.update_cfg(q_viz)
+
+    viz.show(collision_geometry=True,
+             callback=callback)
+
+
+if __name__ == '__main__':
+    main()