wip

deploy/deploy_real/configs/g1_eetrack.yaml

@@ -0,0 +1,70 @@
+control_dt: 0.02
+
+msg_type: "hg"     # "hg" or "go"
+imu_type: "pelvis"    # "torso" or "pelvis"
+
+lowcmd_topic: "rt/lowcmd"
+lowstate_topic: "rt/lowstate"
+
+policy_path: "{LEGGED_GYM_ROOT_DIR}/deploy/pre_train/g1/policy.pt"
+
+leg_joint2motor_idx: [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11]
+joint2motor_idx: [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11,
+                  12, 13, 14, 15, 16, 17, 18, 19, 20, 21,
+                  22, 23, 24, 25, 26, 27, 28]
+# kps: [100, 100, 100, 150, 40, 40, 100, 100, 100, 150, 40, 40]
+# kds: [2, 2, 2, 4, 2, 2, 2, 2, 2, 4, 2, 2]
+# default_angles: [-0.1,  0.0,  0.0,  0.3, -0.2, 0.0, 
+#                   -0.1,  0.0,  0.0,  0.3, -0.2, 0.0]
+kps: [
+  100, 100, 100, 150, 40, 40, 100, 100, 100, 150, 40, 40,
+  150, 150, 150,
+  100, 100, 50, 50, 20, 20, 20,
+  100, 100, 50, 50, 20, 20, 20
+  ]
+kds: [
+  2, 2, 2, 4, 2, 2, 2, 2, 2, 4, 2, 2,
+  3, 3, 3, 
+  2, 2, 2, 2, 1, 1, 1,
+  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.35, -0.16, 0., 0.87, 0., 0., 0., 
+  ]
+
+# arm_waist_joint2motor_idx: [12, 13, 14, 
+#                             15, 16, 17, 18, 19, 20, 21, 
+#                             22, 23, 24, 25, 26, 27, 28]
+
+# arm_waist_kps: [300, 300, 300,
+#                 100, 100, 50, 50, 20, 20, 20,
+#                 100, 100, 50, 50, 20, 20, 20]
+
+# arm_waist_kds: [3, 3, 3, 
+#                 2, 2, 2, 2, 1, 1, 1,
+#                 2, 2, 2, 2, 1, 1, 1]
+
+# arm_waist_target: [ 0, 0, 0,
+#                     0, 0, 0, 0, 0, 0, 0,
+#                     0, 0, 0, 0, 0, 0, 0]
+
+# ang_vel_scale: 0.25
+ang_vel_scale: 1.0
+dof_pos_scale: 1.0
+# dof_vel_scale: 0.05
+dof_vel_scale: 1.0
+# action_scale: 0.25
+action_scale: 0.5
+# cmd_scale: [2.0, 2.0, 0.25]
+cmd_scale: [0.0, 0.0, 0.0]
+# num_actions: 12
+num_actions: 29
+# num_obs: 47
+num_obs: 96
+
+# max_cmd: [0.8, 0.5, 1.57]
+max_cmd: [1.0, 1.0, 1.0]

deploy/deploy_real/deploy_real_ros.py

@@ -7,6 +7,9 @@ import torch
 import rclpy as rp
 from unitree_hg.msg import LowCmd as LowCmdHG, LowState as LowStateHG
 from unitree_go.msg import LowCmd as LowCmdGo, LowState as LowStateGo
+from tf2_ros import TransformException
+from tf2_ros.buffer import Buffer
+from tf2_ros.transform_listener import TransformListener
 from common.command_helper_ros import create_damping_cmd, create_zero_cmd, init_cmd_hg, init_cmd_go, MotorMode
 from common.rotation_helper import get_gravity_orientation, transform_imu_data
 from common.remote_controller import RemoteController, KeyMap
@@ -23,6 +26,65 @@ class Mode(Enum):
     policy = 4
     null = 5
 
+def body_pose_axa(frame:str):
+    """ --> tf does not exist """
+    try:
+        t = tf_buffer.lookup_transform(
+            to_frame_rel,
+            from_frame_rel,
+            rclpy.time.Time())
+    except TransformException as ex:
+        print(f'Could not transform {to_frame_rel} to {from_frame_rel}: {ex}')
+        return (np.zeros(3), np.zeros(3))
+
+    txn = t.transform.translation
+    rxn = t.transform.rotation
+
+    xyz = [txn.x, txn.y, txn.z]
+    quat_wxyz = [rxn.w, rxn.x, rxn.y, rxn.z]
+
+    xyz = np.array(xyz)
+    axa = axa_from_quat(quat_wxyz)
+
+    return (xyz, axa)
+
+def make_obs(config,
+             low_state,
+             quat,
+             last_action):
+    # observation terms (order preserved)
+
+    # NOTE(ycho): dummy value
+    base_lin_vel = np.zeros(3)
+    base_ang_vel = np.array([self.low_state.imu_state.gyroscope], dtype=np.float32)
+
+    # FIXME(ycho): check if the convention "q_base^{-1} @ g" holds.
+    projected_gravity = get_gravity_orientation(quat)
+
+    fp_l = body_pose_axa('left_ankle_roll_link')
+    fp_r = body_pose_axa('right_ankle_roll_link')
+    foot_pose = np.concatenate([fp_l[0], fp_r[0], fp_l[1], fp_r[1]])
+
+    hp_l = body_pose_axa('left_hand_palm_link')
+    hp_r = body_pose_axa('right_hand_palm_link')
+    hand_pose = np.concatenate([hp_l[0], hp_r[0], hp_l[1], hp_r[1]])
+
+    projected_com = _
+    projected_zmp = _ # IMPOSSIBLE
+
+    joint_pos = []
+    joint_vel = []
+    for i in range(len(config.leg_joint2motor_idx)):
+        joint_pos[i] = low_state.motor_state[config.leg_joint2motor_idx[i]].q
+        joint_vel[i] = low_state.motor_state[config.leg_joint2motor_idx[i]].dq
+    actions = last_action
+
+    hands_command = _ # goal
+    right_arm_com = _
+    left_arm_com = _
+    pelvis_height = _
+
+
 class Controller:
     def __init__(self, config: Config) -> None:
         self.config = config