diff --git a/MUJOCO_LOG.TXT b/MUJOCO_LOG.TXT new file mode 100644 index 0000000..113d2da --- /dev/null +++ b/MUJOCO_LOG.TXT @@ -0,0 +1,6 @@ +Sat Jan 25 16:14:06 2025 +WARNING: Nan, Inf or huge value in QACC at DOF 0. The simulation is unstable. Time = 0.1650. + +Sat Jan 25 16:23:57 2025 +WARNING: Nan, Inf or huge value in QACC at DOF 18. The simulation is unstable. Time = 0.1150. + diff --git a/deploy/deploy_mujoco/configs/g1.yaml b/deploy/deploy_mujoco/configs/g1.yaml index db18fdf..f0ecac2 100644 --- a/deploy/deploy_mujoco/configs/g1.yaml +++ b/deploy/deploy_mujoco/configs/g1.yaml @@ -1,26 +1,54 @@ # -policy_path: "{LEGGED_GYM_ROOT_DIR}/deploy/pre_train/g1/motion.pt" -xml_path: "{LEGGED_GYM_ROOT_DIR}/resources/robots/g1_description/scene.xml" +# policy_path: "{LEGGED_GYM_ROOT_DIR}/deploy/pre_train/g1/motion.pt" +# policy_path: "{LEGGED_GYM_ROOT_DIR}/logs/g1/walk_with_dr_test_v21/exported/policy.pt" +policy_path: "{LEGGED_GYM_ROOT_DIR}/logs/g1/walk_with_dr_test_v22/exported/policy.pt" +# xml_path: "{LEGGED_GYM_ROOT_DIR}/resources/robots/g1_description/scene.xml" +xml_path: "{LEGGED_GYM_ROOT_DIR}/resources/robots/g1_description/g1_29dof_rev_1_0.xml" # Total simulation time simulation_duration: 60.0 +# simulation_duration: 5. # Simulation time step simulation_dt: 0.002 # Controller update frequency (meets the requirement of simulation_dt * controll_decimation=0.02; 50Hz) control_decimation: 10 -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] +# kps: [100, 100, 100, 150, 40, 40, 100, 100, 100, 150, 40, 40] +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.1, 0.0, 0.0, 0.3, -0.2, 0.0, - -0.1, 0.0, 0.0, 0.3, -0.2, 0.0] +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., + ] -ang_vel_scale: 0.25 +# ang_vel_scale: 0.25 +ang_vel_scale: 1.0 dof_pos_scale: 1.0 -dof_vel_scale: 0.05 -action_scale: 0.25 -cmd_scale: [2.0, 2.0, 0.25] -num_actions: 12 -num_obs: 47 +# dof_vel_scale: 0.05 +dof_vel_scale: 1.0 +# action_scale: 0.25 +# action_scale: 1.0 +action_scale: 0.5 +# cmd_scale: [2.0, 2.0, 0.25] +cmd_scale: [1.0, 1.0, 1.0] +# cmd_scale: [0., 0., 0.] +# num_actions: 12 +num_actions: 29 +# num_obs: 47 +num_obs: 96 cmd_init: [0.5, 0, 0] \ No newline at end of file diff --git a/deploy/deploy_mujoco/configs/g1_nav.yaml b/deploy/deploy_mujoco/configs/g1_nav.yaml new file mode 100644 index 0000000..0c2cf8e --- /dev/null +++ b/deploy/deploy_mujoco/configs/g1_nav.yaml @@ -0,0 +1,120 @@ +# +# policy_path: "{LEGGED_GYM_ROOT_DIR}/deploy/pre_train/g1/motion.pt" +# policy_path: "{LEGGED_GYM_ROOT_DIR}/logs/g1/walk_with_dr_test_v21/exported/policy.pt" +policy_path: "{LEGGED_GYM_ROOT_DIR}/policies/navigation_less_obs/policy.pt" +# xml_path: "{LEGGED_GYM_ROOT_DIR}/resources/robots/g1_description/scene.xml" +xml_path: "{LEGGED_GYM_ROOT_DIR}/resources/robots/g1_description/g1_29dof_rev_1_0.xml" + + +lab_joint: [ + 'left_hip_pitch_joint', + 'right_hip_pitch_joint', + 'waist_yaw_joint', + 'left_hip_roll_joint', + 'right_hip_roll_joint', + 'waist_roll_joint', + 'left_hip_yaw_joint', + 'right_hip_yaw_joint', + 'waist_pitch_joint', + 'left_knee_joint', + 'right_knee_joint', + 'left_shoulder_pitch_joint', + 'right_shoulder_pitch_joint', + 'left_ankle_pitch_joint', + 'right_ankle_pitch_joint', + 'left_shoulder_roll_joint', + 'right_shoulder_roll_joint', + 'left_ankle_roll_joint', + 'right_ankle_roll_joint', + 'left_shoulder_yaw_joint', + 'right_shoulder_yaw_joint', + 'left_elbow_joint', + 'right_elbow_joint', + 'left_wrist_roll_joint', + 'right_wrist_roll_joint', + 'left_wrist_pitch_joint', + 'right_wrist_pitch_joint', + 'left_wrist_yaw_joint', + 'right_wrist_yaw_joint' +] + +motor_joint: [ + 'left_hip_pitch_joint', + 'left_hip_roll_joint', + 'left_hip_yaw_joint', + 'left_knee_joint', + 'left_ankle_pitch_joint', + 'left_ankle_roll_joint', + 'right_hip_pitch_joint', + 'right_hip_roll_joint', + 'right_hip_yaw_joint', + 'right_knee_joint', + 'right_ankle_pitch_joint', + 'right_ankle_roll_joint', + 'waist_yaw_joint', + 'waist_roll_joint', + 'waist_pitch_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', + 'right_shoulder_pitch_joint', + 'right_shoulder_roll_joint', + 'right_shoulder_yaw_joint', + 'right_elbow_joint', + 'right_wrist_roll_joint', + 'right_wrist_pitch_joint', + 'right_wrist_yaw_joint' +] + +# Total simulation time +simulation_duration: 60.0 +# simulation_duration: 5. +# Simulation time step +simulation_dt: 0.002 +# Controller update frequency (meets the requirement of simulation_dt * controll_decimation=0.02; 50Hz) +control_decimation: 10 + +# kps: [100, 100, 100, 150, 40, 40, 100, 100, 100, 150, 40, 40] +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., + ] + +# 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: 1.0 +action_scale: 0.4 +# cmd_scale: [2.0, 2.0, 0.25] +cmd_scale: [1.0, 1.0, 1.0, 1.0] +# cmd_scale: [0., 0., 0.] +# num_actions: 12 +num_actions: 29 +# num_obs: 47 +# num_obs: 96 +num_obs: 97 + +cmd_init: [3, 0, 0, 0] \ No newline at end of file diff --git a/deploy/deploy_mujoco/deploy_mujoco.py b/deploy/deploy_mujoco/deploy_mujoco.py index 94bfb41..5255e14 100644 --- a/deploy/deploy_mujoco/deploy_mujoco.py +++ b/deploy/deploy_mujoco/deploy_mujoco.py @@ -30,6 +30,81 @@ def pd_control(target_q, q, kp, target_dq, dq, kd): if __name__ == "__main__": # get config file name from command line + isaaclab_joint_order = [ + 'left_hip_pitch_joint', + 'right_hip_pitch_joint', + 'waist_yaw_joint', + 'left_hip_roll_joint', + 'right_hip_roll_joint', + 'waist_roll_joint', + 'left_hip_yaw_joint', + 'right_hip_yaw_joint', + 'waist_pitch_joint', + 'left_knee_joint', + 'right_knee_joint', + 'left_shoulder_pitch_joint', + 'right_shoulder_pitch_joint', + 'left_ankle_pitch_joint', + 'right_ankle_pitch_joint', + 'left_shoulder_roll_joint', + 'right_shoulder_roll_joint', + 'left_ankle_roll_joint', + 'right_ankle_roll_joint', + 'left_shoulder_yaw_joint', + 'right_shoulder_yaw_joint', + 'left_elbow_joint', + 'right_elbow_joint', + 'left_wrist_roll_joint', + 'right_wrist_roll_joint', + 'left_wrist_pitch_joint', + 'right_wrist_pitch_joint', + 'left_wrist_yaw_joint', + 'right_wrist_yaw_joint' + ] + + raw_joint_order = [ + 'left_hip_pitch_joint', + 'left_hip_roll_joint', + 'left_hip_yaw_joint', + 'left_knee_joint', + 'left_ankle_pitch_joint', + 'left_ankle_roll_joint', + 'right_hip_pitch_joint', + 'right_hip_roll_joint', + 'right_hip_yaw_joint', + 'right_knee_joint', + 'right_ankle_pitch_joint', + 'right_ankle_roll_joint', + 'waist_yaw_joint', + 'waist_roll_joint', + 'waist_pitch_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', + 'right_shoulder_pitch_joint', + 'right_shoulder_roll_joint', + 'right_shoulder_yaw_joint', + 'right_elbow_joint', + 'right_wrist_roll_joint', + 'right_wrist_pitch_joint', + 'right_wrist_yaw_joint' + ] + + # Create a mapping tensor + # mapping_tensor = torch.zeros((len(sim_b_joints), len(sim_a_joints)), device=env.device) + mapping_tensor = torch.zeros((len(raw_joint_order), len(isaaclab_joint_order))) + + # Fill the mapping tensor + for b_idx, b_joint in enumerate(raw_joint_order): + if b_joint in isaaclab_joint_order: + a_idx = isaaclab_joint_order.index(b_joint) + # mapping_tensor[b_idx, a_idx] = 1.0 + mapping_tensor[a_idx, b_idx] = 1.0 + import argparse parser = argparse.ArgumentParser() @@ -81,6 +156,10 @@ if __name__ == "__main__": start = time.time() while viewer.is_running() and time.time() - start < simulation_duration: step_start = time.time() + from icecream import ic + # ic( + # target_dof_pos, d.qpos[7:], kps, np.zeros_like(kds), d.qvel[6:], kds + # ) tau = pd_control(target_dof_pos, d.qpos[7:], kps, np.zeros_like(kds), d.qvel[6:], kds) d.ctrl[:] = tau # mj_step can be replaced with code that also evaluates @@ -114,12 +193,32 @@ if __name__ == "__main__": obs[9 : 9 + num_actions] = qj obs[9 + num_actions : 9 + 2 * num_actions] = dqj obs[9 + 2 * num_actions : 9 + 3 * num_actions] = action - obs[9 + 3 * num_actions : 9 + 3 * num_actions + 2] = np.array([sin_phase, cos_phase]) + # obs[9 + 3 * num_actions : 9 + 3 * num_actions + 2] = np.array([sin_phase, cos_phase]) obs_tensor = torch.from_numpy(obs).unsqueeze(0) + obs_tensor[..., 9:38] = obs_tensor[..., 9:38] @ mapping_tensor.transpose(0, 1) + obs_tensor[..., 38:67] = obs_tensor[..., 38:67] @ mapping_tensor.transpose(0, 1) + obs_tensor[..., 67:96] = obs_tensor[..., 67:96] @ mapping_tensor.transpose(0, 1) + # from icecream import ic + # ic( + + # obs[..., :9], + # obs[..., 9:38], + # obs[..., 38:67], + # obs[..., 67:96], + # ) # policy inference action = policy(obs_tensor).detach().numpy().squeeze() + # reordered_actions = action @ mapping_tensor.detach().cpu().numpy() + action = action @ mapping_tensor.detach().cpu().numpy() + # ic( + # action + # ) + # action = 0. + # transform action to target_dof_pos + # target_dof_pos = action * action_scale + default_angles target_dof_pos = action * action_scale + default_angles + # raise NotImplementedError # Pick up changes to the physics state, apply perturbations, update options from GUI. viewer.sync() diff --git a/deploy/deploy_mujoco/deploy_mujoco_navigation.py b/deploy/deploy_mujoco/deploy_mujoco_navigation.py new file mode 100644 index 0000000..4c255a8 --- /dev/null +++ b/deploy/deploy_mujoco/deploy_mujoco_navigation.py @@ -0,0 +1,178 @@ +import time + +import mujoco.viewer +import mujoco +import numpy as np +from legged_gym import LEGGED_GYM_ROOT_DIR +import torch +import yaml + +from icecream import ic +import deploy.deploy_real.math_utils as math_utils + + +def get_gravity_orientation(quaternion): + qw = quaternion[0] + qx = quaternion[1] + qy = quaternion[2] + qz = quaternion[3] + + gravity_orientation = np.zeros(3) + + gravity_orientation[0] = 2 * (-qz * qx + qw * qy) + gravity_orientation[1] = -2 * (qz * qy + qw * qx) + gravity_orientation[2] = 1 - 2 * (qw * qw + qz * qz) + + return gravity_orientation + + +def pd_control(target_q, q, kp, target_dq, dq, kd): + """Calculates torques from position commands""" + return (target_q - q) * kp + (target_dq - dq) * kd + + +if __name__ == "__main__": + # get config file name from command line + import argparse + + parser = argparse.ArgumentParser() + parser.add_argument("config_file", type=str, help="config file name in the config folder") + args = parser.parse_args() + config_file = args.config_file + + + + + with open(f"{LEGGED_GYM_ROOT_DIR}/deploy/deploy_mujoco/configs/{config_file}", "r") as f: + config = yaml.load(f, Loader=yaml.FullLoader) + + isaaclab_joint_order = config["lab_joint"] + + raw_joint_order = config["motor_joint"] + mapping_tensor = torch.zeros((len(raw_joint_order), len(isaaclab_joint_order))) + + # Fill the mapping tensor + for b_idx, b_joint in enumerate(raw_joint_order): + if b_joint in isaaclab_joint_order: + a_idx = isaaclab_joint_order.index(b_joint) + mapping_tensor[a_idx, b_idx] = 1.0 + + policy_path = config["policy_path"].replace("{LEGGED_GYM_ROOT_DIR}", LEGGED_GYM_ROOT_DIR) + xml_path = config["xml_path"].replace("{LEGGED_GYM_ROOT_DIR}", LEGGED_GYM_ROOT_DIR) + + simulation_duration = config["simulation_duration"] + simulation_dt = config["simulation_dt"] + control_decimation = config["control_decimation"] + + kps = np.array(config["kps"], dtype=np.float32) + kds = np.array(config["kds"], dtype=np.float32) + + default_angles = np.array(config["default_angles"], dtype=np.float32) + + ang_vel_scale = config["ang_vel_scale"] + dof_pos_scale = config["dof_pos_scale"] + dof_vel_scale = config["dof_vel_scale"] + action_scale = config["action_scale"] + cmd_scale = np.array(config["cmd_scale"], dtype=np.float32) + + num_actions = config["num_actions"] + num_obs = config["num_obs"] + + cmd = np.array(config["cmd_init"], dtype=np.float32) + target_cmd = np.array(config["cmd_init"], dtype=np.float32) + + # define context variables + action = np.zeros(num_actions, dtype=np.float32) + target_dof_pos = default_angles.copy() + obs = np.zeros(num_obs, dtype=np.float32) + + counter = 0 + + # Load robot model + m = mujoco.MjModel.from_xml_path(xml_path) + d = mujoco.MjData(m) + m.opt.timestep = simulation_dt + + # load policy + policy = torch.jit.load(policy_path) + + with mujoco.viewer.launch_passive(m, d) as viewer: + # Close the viewer automatically after simulation_duration wall-seconds. + start = time.time() + while viewer.is_running() and time.time() - start < simulation_duration: + step_start = time.time() + tau = pd_control(target_dof_pos, d.qpos[7:], kps, np.zeros_like(kds), d.qvel[6:], kds) + d.ctrl[:] = tau + # mj_step can be replaced with code that also evaluates + # a policy and applies a control signal before stepping the physics. + mujoco.mj_step(m, d) + + counter += 1 + if counter % control_decimation == 0: + # create observation + + qj = d.qpos[7:] + dqj = d.qvel[6:] + quat = d.qpos[3:7] + omega = d.qvel[3:6] + position = d.qpos[:3] + # position is given on world frame. + # ic(position) + # ic(cmd) + cmd[0] = target_cmd[0] - position[0] + cmd[1] = target_cmd[1] - position[1] + # cmd[2] -= position[2] + cmd[:3] = math_utils.quat_rotate_inverse( + math_utils.yaw_quat(torch.as_tensor(quat).float().to("cpu")), + torch.as_tensor(cmd[:3]).float().to("cpu") + ).float().numpy() + + robot_forward_vec_w = math_utils.quat_apply(torch.as_tensor(quat).float().to("cpu"), torch.tensor([1,0,0]).float().to("cpu")) + robot_heading_w = torch.atan2(robot_forward_vec_w[1], robot_forward_vec_w[0]) + cmd[3] = math_utils.wrap_to_pi(cmd[3] - robot_heading_w) + + ic(cmd) + + + qj = (qj - default_angles) * dof_pos_scale + dqj = dqj * dof_vel_scale + gravity_orientation = get_gravity_orientation(quat) + omega = omega * ang_vel_scale + + period = 0.8 + count = counter * simulation_dt + phase = count % period / period + + obs[:3] = omega + obs[3:6] = gravity_orientation + obs[6:10] = cmd * cmd_scale + obs[10 : 10 + num_actions] = qj + obs[10 + num_actions : 10 + 2 * num_actions] = dqj + obs[10 + 2 * num_actions : 10 + 3 * num_actions] = action + # obs[9 + 3 * num_actions : 9 + 3 * num_actions + 2] = np.array([sin_phase, cos_phase]) + obs_tensor = torch.from_numpy(obs).unsqueeze(0) + obs_tensor[..., 10:39] = obs_tensor[..., 10:39] @ mapping_tensor.transpose(0, 1) + obs_tensor[..., 39:68] = obs_tensor[..., 39:68] @ mapping_tensor.transpose(0, 1) + obs_tensor[..., 68:97] = obs_tensor[..., 68:97] @ mapping_tensor.transpose(0, 1) + # policy inference + action = policy(obs_tensor).detach().numpy().squeeze() + # reordered_actions = action @ mapping_tensor.detach().cpu().numpy() + # applied_action = action @ mapping_tensor.detach().cpu().numpy() + action = action @ mapping_tensor.detach().cpu().numpy() + # ic( + # action + # ) + # action = 0. + + # transform action to target_dof_pos + # target_dof_pos = action * action_scale + default_angles + target_dof_pos = action * action_scale + default_angles + # raise NotImplementedError + + # Pick up changes to the physics state, apply perturbations, update options from GUI. + viewer.sync() + + # Rudimentary time keeping, will drift relative to wall clock. + time_until_next_step = m.opt.timestep - (time.time() - step_start) + if time_until_next_step > 0: + time.sleep(time_until_next_step) diff --git a/deploy/deploy_real/__init__.py b/deploy/deploy_real/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/deploy/deploy_real/act_to_dof.py b/deploy/deploy_real/act_to_dof.py new file mode 100644 index 0000000..f0989b2 --- /dev/null +++ b/deploy/deploy_real/act_to_dof.py @@ -0,0 +1,254 @@ +#!/usr/bin/env python3 + +import numpy as np +import pinocchio as pin + +from common.np_math import (xyzw2wxyz, index_map) +from math_utils import ( + as_np, + quat_from_angle_axis, + quat_mul, + quat_rotate_inverse +) +quat_from_angle_axis = as_np(quat_from_angle_axis) +quat_mul = as_np(quat_mul) +quat_rotate_inverse = as_np(quat_rotate_inverse) + +from config import Config +from ikctrl import IKCtrl + + +class ActToDof: + def __init__(self, + config: Config, + ikctrl: IKCtrl): + self.config = config + self.ikctrl = ikctrl + self.lim_lo_pin = self.ikctrl.robot.model.lowerPositionLimit + self.lim_hi_pin = self.ikctrl.robot.model.upperPositionLimit + + self.mot_from_pin = index_map( + self.config.motor_joint, + self.ikctrl.joint_names) + self.pin_from_mot = index_map( + self.ikctrl.joint_names, + self.config.motor_joint + ) + self.pin_from_lab = index_map( + 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 + ) + self.mot_from_lab = index_map( + 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 + ) + # x = [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 12, 13, 14, 16, 17, 18, 20, 22, 24, 26, 28] + + self.default_nonarm = ( + np.asarray(self.config.lab_joint_offsets)[self.lab_from_nonarm] + ) + + def __call__(self, obs, action, root_quat_wxyz=None): + hands_command_w = obs[..., 119:125] + non_arm_joint_pos = action[..., :22] + left_arm_residual = action[..., 22:29] + + q_lab = obs[..., 32:61] + q_pin = np.zeros_like(self.ikctrl.cfg.q) + q_pin[self.pin_from_lab] = q_lab + q_pin[self.pin_from_lab] += np.asarray(self.config.lab_joint_offsets) + + q_mot = np.zeros(29) + q_mot[self.mot_from_lab] = q_lab + q_mot[self.mot_from_lab] += np.asarray(self.config.lab_joint_offsets) + # print('q_mot (inside)', q_mot) + # print('q0', q_mot[self.mot_from_arm]) + # q_mot[i_mot] = q_lab[ lab_from_mot[i_mot] ] + + if root_quat_wxyz is None: + hands_command_b = hands_command_w + else: + world_from_pelvis_quat = root_quat_wxyz.astype(np.float32) + hands_command_w = hands_command_w.astype(np.float32) + hands_command_b = np.concatenate([ + quat_rotate_inverse(world_from_pelvis_quat, + hands_command_w[..., :3]), + quat_rotate_inverse(world_from_pelvis_quat, + hands_command_w[..., 3:6]) + ], axis=-1) + + axa = hands_command_b[..., 3:] + angle = np.asarray(np.linalg.norm(axa, axis=-1)) + axis = axa / np.maximum(angle, 1e-6) + d_quat = quat_from_angle_axis(angle, axis) + + 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_b[..., :3] + target_quat = quat_mul(d_quat, source_quat) + target = np.concatenate([target_xyz, target_quat]) + res_q_ik = self.ikctrl( + 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) + target_dof_pos += q_mot + target_dof_pos[self.mot_from_arm] += res_q_ik + + if True: + target_dof_pos[self.mot_from_arm] += np.clip( + 0.3 * left_arm_residual, + -0.2, 0.2) + + if True: + # print('default joint pos', self.default_nonarm) + # print('joint order', self.config.non_arm_joint) + # print('mot_from_nonarm', self.mot_from_nonarm) + target_dof_pos[self.mot_from_nonarm] = ( + self.default_nonarm + 0.5 * non_arm_joint_pos + ) + + # target_dof_pos = np.clip( + # target_dof_pos, + # self.lim_lo_pin[self.pin_from_mot], + # self.lim_hi_pin[self.pin_from_mot] + # ) + + 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) + + exps = [] + cals = [] + poss = [] + # nonarm_offset = [-0.2000, -0.2000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000, + # 0.0000, 0.4200, 0.4200, -0.2000, -0.2300, -0.2300, -0.3500, 0.0000, + # 0.0000, 0.0000, 1.0000, 0.0000, 0.0000, 0.0000] + # nonarm_joint_names=['left_hip_pitch_joint', 'right_hip_pitch_joint', 'waist_yaw_joint', 'left_hip_roll_joint', 'right_hip_roll_joint', 'waist_roll_joint', 'left_hip_yaw_joint', 'right_hip_yaw_joint', 'waist_pitch_joint', 'left_knee_joint', 'right_knee_joint', 'right_shoulder_pitch_joint', 'left_ankle_pitch_joint', 'right_ankle_pitch_joint', 'right_shoulder_roll_joint', 'left_ankle_roll_joint', 'right_ankle_roll_joint', 'right_shoulder_yaw_joint', 'right_elbow_joint', 'right_wrist_roll_joint', 'right_wrist_pitch_joint', 'right_wrist_yaw_joint'] + # print( np.asarray(config.lab_joint_offsets)[act_to_dof.lab_from_nonarm] - nonarm_offset) + # print( list(config.non_arm_joint) == list(nonarm_joint_names)) + + mot_from_arm = index_map(d['motor_joint'], d['arm_joint']) + for i in range(61): + obs = np.load(F'/gate/eet6/obs{i:03d}.npy')[0] + # print(obs.shape) + act = np.load(F'/gate/eet6/act{i:03d}.npy')[0] + # print('act', act.shape) + dof_lab = np.load(F'/gate/eet6/dof{i:03d}.npy')[0] + mot_from_lab = index_map(d['motor_joint'], d['lab_joint']) + lab_from_mot = index_map(d['lab_joint'], d['motor_joint']) + target_dof_pos = np.zeros_like(dof_lab) + target_dof_pos[:] = dof_lab[lab_from_mot] + + dof = act_to_dof(obs, act) + + export = target_dof_pos + calc = dof + + # 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]) + + exps.append(target_dof_pos) + cals.append(dof) + + q_lab = obs[..., 32:61] + q_mot = (q_lab + config.lab_joint_offsets)[act_to_dof.lab_from_mot] + q_arm = q_mot[mot_from_arm] + # poss.append(q_arm) + poss.append(q_mot) + + # print('q_mot', q_mot, + # 'sim - q_mot', target_dof_pos - q_mot, + # 'real - q_mot', dof - q_mot) + # break + + 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(29, 1) + fig, axs = plt.subplots(6, 5) + + q_lo = act_to_dof.lim_lo_pin[act_to_dof.pin_from_mot] + q_hi = act_to_dof.lim_hi_pin[act_to_dof.pin_from_mot] + RES = False + for i in range(29): + + ii = i // 5 + jj = i % 5 + ax = axs[ii, jj] + + ax.set_title(config.motor_joint[i]) + if RES: + ax.axhline(0) + else: + ax.axhline(q_lo[i], color='k', linestyle='--') + ax.axhline(q_hi[i], color='k', linestyle='--') + pass + if RES: + ax.plot(exps[:, i] - poss[:, i], label='sim') + ax.plot(cals[:, i] - poss[:, i], label='real') + else: + ax.plot(poss[:, i], label='pos') + ax.plot(exps[:, i], 'x-', label='sim') + ax.plot(cals[:, i], '+-', label='real') + ax.legend() + plt.show() + + +if __name__ == '__main__': + main() diff --git a/deploy/deploy_real/common/__init__.py b/deploy/deploy_real/common/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/deploy/deploy_real/common/command_helper_ros.py b/deploy/deploy_real/common/command_helper_ros.py new file mode 100644 index 0000000..e6a669c --- /dev/null +++ b/deploy/deploy_real/common/command_helper_ros.py @@ -0,0 +1,62 @@ +from unitree_go.msg import LowCmd as LowCmdGo +from unitree_hg.msg import LowCmd as LowCmdHG +from typing import Union + + +class MotorMode: + PR = 0 # Series Control for Pitch/Roll Joints + AB = 1 # Parallel Control for A/B Joints + + +def create_damping_cmd(cmd: Union[LowCmdGo, LowCmdHG]): + size = len(cmd.motor_cmd) + for i in range(size): + cmd.motor_cmd[i].q = 0.0 + cmd.motor_cmd[i].dq = 0.0 + cmd.motor_cmd[i].kp = 0.0 + cmd.motor_cmd[i].kd = 8.0 + cmd.motor_cmd[i].tau = 0.0 + + +def create_zero_cmd(cmd: Union[LowCmdGo, LowCmdHG]): + size = len(cmd.motor_cmd) + for i in range(size): + cmd.motor_cmd[i].q = 0.0 + cmd.motor_cmd[i].dq = 0.0 + cmd.motor_cmd[i].kp = 0.0 + cmd.motor_cmd[i].kd = 0.0 + cmd.motor_cmd[i].tau = 0.0 + + +def init_cmd_hg(cmd: LowCmdHG, mode_machine: int, mode_pr: int): + cmd.mode_machine = mode_machine + cmd.mode_pr = mode_pr + size = len(cmd.motor_cmd) + print(size) + for i in range(size): + cmd.motor_cmd[i].mode = 1 + cmd.motor_cmd[i].q = 0.0 + cmd.motor_cmd[i].dq = 0.0 + cmd.motor_cmd[i].kp = 0.0 + cmd.motor_cmd[i].kd = 0.0 + cmd.motor_cmd[i].tau = 0.0 + + +def init_cmd_go(cmd: LowCmdGo, weak_motor: list): + cmd.head[0] = 0xFE + cmd.head[1] = 0xEF + cmd.level_flag = 0xFF + cmd.gpio = 0 + PosStopF = 2.146e9 + VelStopF = 16000.0 + size = len(cmd.motor_cmd) + for i in range(size): + if i in weak_motor: + cmd.motor_cmd[i].mode = 1 + else: + cmd.motor_cmd[i].mode = 0x0A + cmd.motor_cmd[i].q = PosStopF + cmd.motor_cmd[i].dq = VelStopF + cmd.motor_cmd[i].kp = 0.0 + cmd.motor_cmd[i].kd = 0.0 + cmd.motor_cmd[i].tau = 0.0 diff --git a/deploy/deploy_real/common/crc.py b/deploy/deploy_real/common/crc.py new file mode 100644 index 0000000..77449bd --- /dev/null +++ b/deploy/deploy_real/common/crc.py @@ -0,0 +1,232 @@ +import struct +import ctypes +import os +import platform +from unitree_hg.msg import LowCmd as LowCmdHG, LowState as LowStateHG +from unitree_go.msg import LowCmd as LowCmdGo, LowState as LowStateGo + +class Singleton: + __instance = None + + def __new__(cls, *args, **kwargs): + if cls.__instance is None: + cls.__instance = super(Singleton, cls).__new__(cls) + return cls.__instance + + def __init__(self): + pass + +class CRC(Singleton): + def __init__(self): + #4 bytes aligned, little-endian format. + #size 812 + self.__packFmtLowCmd = '<4B4IH2x' + 'B3x5f3I' * 20 + '4B' + '55Bx2I' + #size 1180 + self.__packFmtLowState = '<4B4IH2x' + '13fb3x' + 'B3x7fb3x3I' * 20 + '4BiH4b15H' + '8hI41B3xf2b2x2f4h2I' + #size 1004 + self.__packFmtHGLowCmd = '<2B2x' + 'B3x5fI' * 35 + '5I' + #size 2092 + self.__packFmtHGLowState = '<2I2B2xI' + '13fh2x' + 'B3x4f2hf7I' * 35 + '40B5I' + + + script_dir = os.path.dirname(os.path.abspath(__file__)) + self.platform = platform.system() + if self.platform == "Linux": + if platform.machine()=="x86_64": + self.crc_lib = ctypes.CDLL(script_dir + '/lib/crc_amd64.so') + elif platform.machine()=="aarch64": + self.crc_lib = ctypes.CDLL(script_dir + '/lib/crc_aarch64.so') + + self.crc_lib.crc32_core.argtypes = (ctypes.POINTER(ctypes.c_uint32), ctypes.c_uint32) + self.crc_lib.crc32_core.restype = ctypes.c_uint32 + + def Crc(self, msg): + if type(msg) == LowCmdGo: + return self.__Crc32(self.__PackLowCmd(msg)) + elif type(msg) == LowStateGo: + return self.__Crc32(self.__PackLowState(msg)) + if type(msg) == LowCmdHG: + return self.__Crc32(self.__PackHGLowCmd(msg)) + elif type(msg) == LowStateHG: + return self.__Crc32(self.__PackHGLowState(msg)) + else: + raise TypeError('unknown message type to crc') + + def __PackLowCmd(self, cmd: LowCmdGo): + origData = [] + origData.extend(cmd.head) + origData.append(cmd.level_flag) + origData.append(cmd.frame_reserve) + origData.extend(cmd.sn) + origData.extend(cmd.version) + origData.append(cmd.bandwidth) + + for i in range(20): + origData.append(cmd.motor_cmd[i].mode) + origData.append(cmd.motor_cmd[i].q) + origData.append(cmd.motor_cmd[i].dq) + origData.append(cmd.motor_cmd[i].tau) + origData.append(cmd.motor_cmd[i].kp) + origData.append(cmd.motor_cmd[i].kd) + origData.extend(cmd.motor_cmd[i].reserve) + + origData.append(cmd.bms_cmd.off) + origData.extend(cmd.bms_cmd.reserve) + + origData.extend(cmd.wireless_remote) + origData.extend(cmd.led) + origData.extend(cmd.fan) + origData.append(cmd.gpio) + origData.append(cmd.reserve) + origData.append(cmd.crc) + + return self.__Trans(struct.pack(self.__packFmtLowCmd, *origData)) + + def __PackLowState(self, state: LowStateGo): + origData = [] + origData.extend(state.head) + origData.append(state.level_flag) + origData.append(state.frame_reserve) + origData.extend(state.sn) + origData.extend(state.version) + origData.append(state.bandwidth) + + origData.extend(state.imu_state.quaternion) + origData.extend(state.imu_state.gyroscope) + origData.extend(state.imu_state.accelerometer) + origData.extend(state.imu_state.rpy) + origData.append(state.imu_state.temperature) + + for i in range(20): + origData.append(state.motor_state[i].mode) + origData.append(state.motor_state[i].q) + origData.append(state.motor_state[i].dq) + origData.append(state.motor_state[i].ddq) + origData.append(state.motor_state[i].tau_est) + origData.append(state.motor_state[i].q_raw) + origData.append(state.motor_state[i].dq_raw) + origData.append(state.motor_state[i].ddq_raw) + origData.append(state.motor_state[i].temperature) + origData.append(state.motor_state[i].lost) + origData.extend(state.motor_state[i].reserve) + + origData.append(state.bms_state.version_high) + origData.append(state.bms_state.version_low) + origData.append(state.bms_state.status) + origData.append(state.bms_state.soc) + origData.append(state.bms_state.current) + origData.append(state.bms_state.cycle) + origData.extend(state.bms_state.bq_ntc) + origData.extend(state.bms_state.mcu_ntc) + origData.extend(state.bms_state.cell_vol) + + origData.extend(state.foot_force) + origData.extend(state.foot_force_est) + origData.append(state.tick) + origData.extend(state.wireless_remote) + origData.append(state.bit_flag) + origData.append(state.adc_reel) + origData.append(state.temperature_ntc1) + origData.append(state.temperature_ntc2) + origData.append(state.power_v) + origData.append(state.power_a) + origData.extend(state.fan_frequency) + origData.append(state.reserve) + origData.append(state.crc) + + return self.__Trans(struct.pack(self.__packFmtLowState, *origData)) + + def __PackHGLowCmd(self, cmd: LowCmdHG): + origData = [] + origData.append(cmd.mode_pr) + origData.append(cmd.mode_machine) + + for i in range(35): + origData.append(cmd.motor_cmd[i].mode) + origData.append(cmd.motor_cmd[i].q) + origData.append(cmd.motor_cmd[i].dq) + origData.append(cmd.motor_cmd[i].tau) + origData.append(cmd.motor_cmd[i].kp) + origData.append(cmd.motor_cmd[i].kd) + origData.append(cmd.motor_cmd[i].reserve) + + origData.extend(cmd.reserve) + origData.append(cmd.crc) + + return self.__Trans(struct.pack(self.__packFmtHGLowCmd, *origData)) + + def __PackHGLowState(self, state: LowStateHG): + origData = [] + origData.extend(state.version) + origData.append(state.mode_pr) + origData.append(state.mode_machine) + origData.append(state.tick) + + origData.extend(state.imu_state.quaternion) + origData.extend(state.imu_state.gyroscope) + origData.extend(state.imu_state.accelerometer) + origData.extend(state.imu_state.rpy) + origData.append(state.imu_state.temperature) + + for i in range(35): + origData.append(state.motor_state[i].mode) + origData.append(state.motor_state[i].q) + origData.append(state.motor_state[i].dq) + origData.append(state.motor_state[i].ddq) + origData.append(state.motor_state[i].tau_est) + origData.extend(state.motor_state[i].temperature) + origData.append(state.motor_state[i].vol) + origData.extend(state.motor_state[i].sensor) + origData.append(state.motor_state[i].motorstate) + origData.extend(state.motor_state[i].reserve) + + origData.extend(state.wireless_remote) + origData.extend(state.reserve) + origData.append(state.crc) + + return self.__Trans(struct.pack(self.__packFmtHGLowState, *origData)) + + def __Trans(self, packData): + calcData = [] + calcLen = ((len(packData)>>2)-1) + + for i in range(calcLen): + d = ((packData[i*4+3] << 24) | (packData[i*4+2] << 16) | (packData[i*4+1] << 8) | (packData[i*4])) + calcData.append(d) + + return calcData + + def _crc_py(self, data): + bit = 0 + crc = 0xFFFFFFFF + polynomial = 0x04c11db7 + + for i in range(len(data)): + bit = 1 << 31 + current = data[i] + + for b in range(32): + if crc & 0x80000000: + crc = (crc << 1) & 0xFFFFFFFF + crc ^= polynomial + else: + crc = (crc << 1) & 0xFFFFFFFF + + if current & bit: + crc ^= polynomial + + bit >>= 1 + + return crc + + def _crc_ctypes(self, data): + uint32_array = (ctypes.c_uint32 * len(data))(*data) + length = len(data) + crc=self.crc_lib.crc32_core(uint32_array, length) + return crc + + def __Crc32(self, data): + if self.platform == "Linux": + return self._crc_ctypes(data) + else: + return self._crc_py(data) \ No newline at end of file diff --git a/deploy/deploy_real/common/np_math.py b/deploy/deploy_real/common/np_math.py new file mode 100644 index 0000000..0f1dc7c --- /dev/null +++ b/deploy/deploy_real/common/np_math.py @@ -0,0 +1,184 @@ +#!/usr/bin/env python3 + +import numpy as np +import torch +import torch as th +from contextlib import contextmanager +import os + +@contextmanager +def with_dir(d): + d0 = os.getcwd() + try: + os.chdir(d) + yield + finally: + os.chdir(d0) + +def axis_angle_from_quat(quat: np.ndarray, eps: float = 1.0e-6) -> np.ndarray: + """Convert rotations given as quaternions to axis/angle. + + Args: + quat: The quaternion orientation in (w, x, y, z). Shape is (..., 4). + eps: The tolerance for Taylor approximation. Defaults to 1.0e-6. + + Returns: + Rotations given as a vector in axis angle form. Shape is (..., 3). + The vector's magnitude is the angle turned anti-clockwise in radians around the vector's direction. + + Reference: + https://github.com/facebookresearch/pytorch3d/blob/main/pytorch3d/transforms/rotation_conversions.py#L526-L554 + """ + # Modified to take in quat as [q_w, q_x, q_y, q_z] + # Quaternion is [q_w, q_x, q_y, q_z] = [cos(theta/2), n_x * sin(theta/2), n_y * sin(theta/2), n_z * sin(theta/2)] + # Axis-angle is [a_x, a_y, a_z] = [theta * n_x, theta * n_y, theta * n_z] + # Thus, axis-angle is [q_x, q_y, q_z] / (sin(theta/2) / theta) + # When theta = 0, (sin(theta/2) / theta) is undefined + # However, as theta --> 0, we can use the Taylor approximation 1/2 - + # theta^2 / 48 + quat = quat * (1.0 - 2.0 * (quat[..., 0:1] < 0.0)) + mag = np.linalg.norm(quat[..., 1:], axis=-1) + half_angle = np.arctan2(mag, quat[..., 0]) + angle = 2.0 * half_angle + # check whether to apply Taylor approximation + sin_half_angles_over_angles = np.where( + np.abs(angle) > eps, + np.sin(half_angle) / angle, + 0.5 - angle * angle / 48 + ) + return quat[..., 1:4] / sin_half_angles_over_angles[..., None] + +def normalize(x: torch.Tensor, eps: float = 1e-9) -> torch.Tensor: + """Normalizes a given input tensor to unit length. + + Args: + x: Input tensor of shape (N, dims). + eps: A small value to avoid division by zero. Defaults to 1e-9. + + Returns: + Normalized tensor of shape (N, dims). + """ + return x / x.norm(p=2, dim=-1).clamp(min=eps, max=None).unsqueeze(-1) + + +def quat_from_angle_axis( + angle: torch.Tensor, + axis: torch.Tensor = None) -> torch.Tensor: + """Convert rotations given as angle-axis to quaternions. + + Args: + angle: The angle turned anti-clockwise in radians around the vector's direction. Shape is (N,). + axis: The axis of rotation. Shape is (N, 3). + + Returns: + The quaternion in (w, x, y, z). Shape is (N, 4). + """ + if axis is None: + axa = angle + angle = torch.linalg.norm(axa, dim=-1) + axis = axa / angle[..., None].clamp_min(1e-6) + theta = (angle / 2).unsqueeze(-1) + xyz = normalize(axis) * theta.sin() + w = theta.cos() + return normalize(torch.cat([w, xyz], dim=-1)) + + +def quat_mul(q1: torch.Tensor, q2: torch.Tensor) -> torch.Tensor: + """Multiply two quaternions together. + + Args: + q1: The first quaternion in (w, x, y, z). Shape is (..., 4). + q2: The second quaternion in (w, x, y, z). Shape is (..., 4). + + Returns: + The product of the two quaternions in (w, x, y, z). Shape is (..., 4). + + Raises: + ValueError: Input shapes of ``q1`` and ``q2`` are not matching. + """ + # check input is correct + if q1.shape != q2.shape: + msg = f"Expected input quaternion shape mismatch: {q1.shape} != {q2.shape}." + raise ValueError(msg) + # reshape to (N, 4) for multiplication + shape = q1.shape + q1 = q1.reshape(-1, 4) + q2 = q2.reshape(-1, 4) + # extract components from quaternions + w1, x1, y1, z1 = q1[:, 0], q1[:, 1], q1[:, 2], q1[:, 3] + w2, x2, y2, z2 = q2[:, 0], q2[:, 1], q2[:, 2], q2[:, 3] + # perform multiplication + ww = (z1 + x1) * (x2 + y2) + yy = (w1 - y1) * (w2 + z2) + zz = (w1 + y1) * (w2 - z2) + xx = ww + yy + zz + qq = 0.5 * (xx + (z1 - x1) * (x2 - y2)) + w = qq - ww + (z1 - y1) * (y2 - z2) + x = qq - xx + (x1 + w1) * (x2 + w2) + y = qq - yy + (w1 - x1) * (y2 + z2) + z = qq - zz + (z1 + y1) * (w2 - x2) + + return torch.stack([w, x, y, z], dim=-1).view(shape) + + +def quat_rotate(q: np.ndarray, v: np.ndarray) -> np.ndarray: + """Rotate a vector by a quaternion along the last dimension of q and v. + + Args: + q: The quaternion in (w, x, y, z). Shape is (..., 4). + v: The vector in (x, y, z). Shape is (..., 3). + + Returns: + The rotated vector in (x, y, z). Shape is (..., 3). + """ + q_w = q[..., 0] + q_vec = q[..., 1:] + a = v * (2.0 * q_w**2 - 1.0)[..., None] + b = np.cross(q_vec, v, axis=-1) * q_w[..., None] * 2.0 + c = q_vec * np.einsum("...i,...i->...", q_vec, v)[..., None] * 2.0 + return a + b + c + + +def quat_rotate_inverse(q: np.ndarray, v: np.ndarray) -> np.ndarray: + """Rotate a vector by the inverse of a quaternion along the last dimension of q and v. + + Args: + q: The quaternion in (w, x, y, z). Shape is (..., 4). + v: The vector in (x, y, z). Shape is (..., 3). + + Returns: + The rotated vector in (x, y, z). Shape is (..., 3). + """ + q_w = q[..., 0] + q_vec = q[..., 1:] + a = v * (2.0 * q_w**2 - 1.0)[..., None] + b = np.cross(q_vec, v, axis=-1) * q_w[..., None] * 2.0 + c = q_vec * np.einsum("...i,...i->...", q_vec, v)[..., None] * 2.0 + return a - b + c + + +def index_map(k_to, k_from): + """ + Returns an index mapping from k_from to k_to. + + Given k_to=a, k_from=b, + returns an index map "a_from_b" such that + array_a[a_from_b] = array_b + + Missing values are set to -1. + """ + index_dict = {k: i for i, k in enumerate(k_to)} # O(len(k_from)) + return [index_dict.get(k, -1) for k in k_from] # O(len(k_to)) + + + +def xyzw2wxyz(q_xyzw: th.Tensor, dim: int = -1): + if isinstance(q_xyzw, np.ndarray): + return np.roll(q_xyzw, 1, axis=dim) + return th.roll(q_xyzw, 1, dims=dim) + + +def wxyz2xyzw(q_wxyz: th.Tensor, dim: int = -1): + if isinstance(q_wxyz, np.ndarray): + return np.roll(q_wxyz, -1, axis=dim) + return th.roll(q_wxyz, -1, dims=dim) diff --git a/deploy/deploy_real/common/step_command.py b/deploy/deploy_real/common/step_command.py new file mode 100644 index 0000000..71468fd --- /dev/null +++ b/deploy/deploy_real/common/step_command.py @@ -0,0 +1,258 @@ +import time +import math +import numpy as np +import matplotlib.pyplot as plt + + +def linear_map(val, in_min, in_max, out_min, out_max): + """Linearly map val from [in_min, in_max] to [out_min, out_max].""" + return out_min + (val - in_min) * (out_max - out_min) / (in_max - in_min) + +def quaternion_multiply(q1, q2): + # q = [w, x, y, z] + w1, x1, y1, z1 = q1 + w2, x2, y2, z2 = q2 + w = w1*w2 - x1*x2 - y1*y2 - z1*z2 + x = w1*x2 + x1*w2 + y1*z2 - z1*y2 + y = w1*y2 - x1*z2 + y1*w2 + z1*x2 + z = w1*z2 + x1*y2 - y1*x2 + z1*w2 + return np.array([w, x, y, z], dtype=np.float32) + +def quaternion_rotate(q, v): + """Rotate vector v by quaternion q.""" + q_conj = np.array([q[0], -q[1], -q[2], -q[3]], dtype=np.float32) + v_q = np.concatenate(([0.0], v)) + rotated = quaternion_multiply(quaternion_multiply(q, v_q), q_conj) + return rotated[1:] + +def yaw_to_quaternion(yaw): + """Convert yaw angle (radians) to a quaternion (w, x, y, z).""" + half_yaw = yaw / 2.0 + return np.array([np.cos(half_yaw), 0.0, 0.0, np.sin(half_yaw)], dtype=np.float32) + +def combine_frame_transforms(pos, quat, rel_pos, rel_quat): + """ + Combine two transforms: + T_new = T * T_rel + where T is given by (pos, quat) and T_rel by (rel_pos, rel_quat). + """ + new_pos = pos + quaternion_rotate(quat, rel_pos) + new_quat = quaternion_multiply(quat, rel_quat) + return new_pos, new_quat + +# ---------------------- +# StepCommand Class +# ---------------------- +class StepCommand: + def __init__(self, current_left_pose, current_right_pose): + """ + Initialize with the current foot poses. + Each pose is a 7-dimensional vector: [x, y, z, qw, qx, qy, qz]. + Both next_ctarget_left and next_ctarget_right are initialized to these values. + Also, store the maximum ranges for x, y, and theta. + - x_range: (-0.2, 0.2) + - y_range: (0.2, 0.4) + - theta_range: (-0.3, 0.3) + """ + self.next_ctarget_left = current_left_pose.copy() + self.next_ctarget_right = current_right_pose.copy() + self.next_ctime_left = 0.4 + self.next_ctime_right = 0.4 + self.delta_ctime = 0.4 # Fixed time delta for a new step + self.max_range = { + 'x_range': (-0.2, 0.2), + 'y_range': (0.2, 0.4), + 'theta_range': (-0.3, 0.3) + } + + def compute_relstep_left(self, lx, ly, rx): + """ + Compute the left foot relative step based on remote controller inputs. + + Mapping: + - x: map ly in [-1,1] to self.max_range['x_range']. + - y: baseline for left is self.max_range['y_range'][0]. If lx > 0, + add an offset mapping lx in [0,1] to [0, self.max_range['y_range'][1]-self.max_range['y_range'][0]]. + - z: fixed at 0. + - rotation: map rx in [-1,1] to self.max_range['theta_range'] and convert to quaternion. + """ + delta_x = linear_map(ly, -1, 1, self.max_range['x_range'][0], self.max_range['x_range'][1]) + baseline_left = self.max_range['y_range'][0] + extra_y = linear_map(lx, 0, 1, 0, self.max_range['y_range'][1] - self.max_range['y_range'][0]) if lx > 0 else 0.0 + delta_y = baseline_left + extra_y + delta_z = 0.0 + theta = linear_map(rx, -1, 1, self.max_range['theta_range'][0], self.max_range['theta_range'][1]) + q = yaw_to_quaternion(theta) + return np.array([delta_x, delta_y, delta_z, q[0], q[1], q[2], q[3]], dtype=np.float32) + + def compute_relstep_right(self, lx, ly, rx): + """ + Compute the right foot relative step based on remote controller inputs. + + Mapping: + - x: map ly in [-1,1] to self.max_range['x_range']. + - y: baseline for right is the negative of self.max_range['y_range'][0]. If lx < 0, + add an offset mapping lx in [-1,0] to [- (self.max_range['y_range'][1]-self.max_range['y_range'][0]), 0]. + - z: fixed at 0. + - rotation: map rx in [-1,1] to self.max_range['theta_range'] and convert to quaternion. + """ + delta_x = linear_map(ly, -1, 1, self.max_range['x_range'][0], self.max_range['x_range'][1]) + baseline_right = -self.max_range['y_range'][0] + extra_y = linear_map(lx, -1, 0, -(self.max_range['y_range'][1] - self.max_range['y_range'][0]), 0) if lx < 0 else 0.0 + delta_y = baseline_right + extra_y + delta_z = 0.0 + theta = linear_map(rx, -1, 1, self.max_range['theta_range'][0], self.max_range['theta_range'][1]) + q = yaw_to_quaternion(theta) + return np.array([delta_x, delta_y, delta_z, q[0], q[1], q[2], q[3]], dtype=np.float32) + + def get_next_ctarget(self, remote_controller, count): + """ + Given the remote controller inputs and elapsed time (count), + compute relative step commands for left and right feet and update + the outdated targets accordingly. + + Update procedure: + - When the left foot is due (count > next_ctime_left), update it by combining + the right foot target with the left relative step. + - Similarly, when the right foot is due (count > next_ctime_right), update it using + the left foot target and the right relative step. + + Returns: + A concatenated 14-dimensional vector: + [left_foot_target (7D), right_foot_target (7D)] + """ + lx = remote_controller.lx + ly = remote_controller.ly + rx = remote_controller.rx + + # Compute relative steps using the internal methods. + relstep_left = self.compute_relstep_left(lx, ly, rx) + relstep_right = self.compute_relstep_right(lx, ly, rx) + # from icecream import ic + + # Update left foot target if its scheduled time has elapsed. + if count > self.next_ctime_left: + self.next_ctime_left = self.next_ctime_right + self.delta_ctime + new_pos, new_quat = combine_frame_transforms( + self.next_ctarget_right[:3], + self.next_ctarget_right[3:7], + relstep_left[:3], + relstep_left[3:7], + ) + self.next_ctarget_left[:3] = new_pos + self.next_ctarget_left[3:7] = new_quat + + # Update right foot target if its scheduled time has elapsed. + if count > self.next_ctime_right: + self.next_ctime_right = self.next_ctime_left + self.delta_ctime + new_pos, new_quat = combine_frame_transforms( + self.next_ctarget_left[:3], + self.next_ctarget_left[3:7], + relstep_right[:3], + relstep_right[3:7], + ) + self.next_ctarget_right[:3] = new_pos + self.next_ctarget_right[3:7] = new_quat + + # Return the concatenated target: left (7D) followed by right (7D). + return (self.next_ctarget_left, self.next_ctarget_right, + (self.next_ctime_left - count), + (self.next_ctime_right - count)) + + + +# For testing purposes, we define a dummy remote controller that mimics the attributes lx, ly, and rx. +class DummyRemoteController: + def __init__(self, lx=0.0, ly=0.0, rx=0.0): + self.lx = lx # lateral command input in range [-1,1] + self.ly = ly # forward/backward command input in range [-1,1] + self.rx = rx # yaw command input in range [-1,1] + +if __name__ == "__main__": + # Initial foot poses (7D each): [x, y, z, qw, qx, qy, qz] + current_left_pose = np.array([0.0, 0.2, 0.0, 1.0, 0.0, 0.0, 0.0], dtype=np.float32) + current_right_pose = np.array([0.0, -0.2, 0.0, 1.0, 0.0, 0.0, 0.0], dtype=np.float32) + + # Create an instance of StepCommand with the initial poses. + step_command = StepCommand(current_left_pose, current_right_pose) + + # Create a dummy remote controller. + dummy_remote = DummyRemoteController() + + # Set up matplotlib for interactive plotting. + plt.ion() + fig, ax = plt.subplots() + ax.set_xlim(-1, 1) + ax.set_ylim(-1, 1) + ax.set_xlabel("X") + ax.set_ylabel("Y") + ax.set_title("Footstep Target Visualization") + + print("Starting test. Press Ctrl+C to exit.") + start_time = time.time() + try: + while True: + elapsed = time.time() - start_time + + # For demonstration, vary the controller inputs over time: + # - ly oscillates between -1 and 1 (forward/backward) + # - lx oscillates between -1 and 1 (lateral left/right) + # - rx is held at 0 (no yaw command) + # dummy_remote.ly = math.sin(elapsed) # forward/backward command + # dummy_remote.lx = math.cos(elapsed) # lateral command + dummy_remote.ly = 0.0 + dummy_remote.lx = 0.0 + dummy_remote.rx = 1. # no yaw + + # Get the current footstep target (14-dimensional) + ctarget = step_command.get_next_ctarget(dummy_remote, elapsed) + print("Time: {:.2f} s, ctarget: {}".format(elapsed, ctarget)) + + # Extract left foot and right foot positions: + # Left foot: indices 0:7 (position: [0:3], quaternion: [3:7]) + left_pos = ctarget[0:3] # [x, y, z] + left_quat = ctarget[3:7] # [qw, qx, qy, qz] + # Right foot: indices 7:14 (position: [7:10], quaternion: [10:14]) + right_pos = ctarget[7:10] + right_quat = ctarget[10:14] + + # For visualization, we use only the x and y components. + left_x, left_y = left_pos[0], left_pos[1] + right_x, right_y = right_pos[0], right_pos[1] + + # Assuming rotation only about z, compute yaw angle from quaternion: + # yaw = 2 * atan2(qz, qw) + left_yaw = 2 * math.atan2(left_quat[3], left_quat[0]) + right_yaw = 2 * math.atan2(right_quat[3], right_quat[0]) + + # Clear and redraw the plot. + ax.cla() + ax.set_xlim(-1, 1) + ax.set_ylim(-1, 1) + ax.set_xlabel("X") + ax.set_ylabel("Y") + ax.set_title("Footstep Target Visualization") + + # Plot the left and right foot positions. + ax.plot(left_x, left_y, 'bo', label='Left Foot') + ax.plot(right_x, right_y, 'ro', label='Right Foot') + + # Draw an arrow for each foot to indicate orientation. + arrow_length = 0.1 + ax.arrow(left_x, left_y, + arrow_length * math.cos(left_yaw), + arrow_length * math.sin(left_yaw), + head_width=0.03, head_length=0.03, fc='b', ec='b') + ax.arrow(right_x, right_y, + arrow_length * math.cos(right_yaw), + arrow_length * math.sin(right_yaw), + head_width=0.03, head_length=0.03, fc='r', ec='r') + + ax.legend() + plt.pause(0.001) + time.sleep(0.1) + except KeyboardInterrupt: + print("Test terminated by user.") + finally: + plt.ioff() + plt.show() \ No newline at end of file diff --git a/deploy/deploy_real/common/utils.py b/deploy/deploy_real/common/utils.py new file mode 100644 index 0000000..d763f6f --- /dev/null +++ b/deploy/deploy_real/common/utils.py @@ -0,0 +1,226 @@ +import numpy as np +from geometry_msgs.msg import Vector3, Quaternion +from typing import Optional, Tuple +def to_array(v): + if isinstance(v, Vector3): + return np.array([v.x, v.y, v.z], dtype=np.float32) + elif isinstance(v, Quaternion): + return np.array([v.x, v.y, v.z, v.w], dtype=np.float32) + + +def normalize(x: np.ndarray, eps: float = 1e-9) -> np.ndarray: + """Normalizes a given input tensor to unit length. + + Args: + x: Input tensor of shape (N, dims). + eps: A small value to avoid division by zero. Defaults to 1e-9. + + Returns: + Normalized tensor of shape (N, dims). + """ + return x / np.linalg.norm(x, ord=2, axis=-1, keepdims=True).clip(min=eps, max=None) + +def yaw_quat(quat: np.ndarray) -> np.ndarray: + """Extract the yaw component of a quaternion. + + Args: + quat: The orientation in (w, x, y, z). Shape is (..., 4) + + Returns: + A quaternion with only yaw component. + """ + shape = quat.shape + quat_yaw = quat.copy().reshape(-1, 4) + qw = quat_yaw[:, 0] + qx = quat_yaw[:, 1] + qy = quat_yaw[:, 2] + qz = quat_yaw[:, 3] + yaw = np.arctan2(2 * (qw * qz + qx * qy), 1 - 2 * (qy * qy + qz * qz)) + quat_yaw[:] = 0.0 + quat_yaw[:, 3] = np.sin(yaw / 2) + quat_yaw[:, 0] = np.cos(yaw / 2) + quat_yaw = normalize(quat_yaw) + return quat_yaw.reshape(shape) + +def quat_conjugate(q: np.ndarray) -> np.ndarray: + """Computes the conjugate of a quaternion. + + Args: + q: The quaternion orientation in (w, x, y, z). Shape is (..., 4). + + Returns: + The conjugate quaternion in (w, x, y, z). Shape is (..., 4). + """ + shape = q.shape + q = q.reshape(-1, 4) + return np.concatenate((q[:, 0:1], -q[:, 1:]), axis=-1).reshape(shape) + + +def quat_inv(q: np.ndarray) -> np.ndarray: + """Compute the inverse of a quaternion. + + Args: + q: The quaternion orientation in (w, x, y, z). Shape is (N, 4). + + Returns: + The inverse quaternion in (w, x, y, z). Shape is (N, 4). + """ + return normalize(quat_conjugate(q)) + +def quat_mul(q1: np.ndarray, q2: np.ndarray) -> np.ndarray: + """Multiply two quaternions together. + + Args: + q1: The first quaternion in (w, x, y, z). Shape is (..., 4). + q2: The second quaternion in (w, x, y, z). Shape is (..., 4). + + Returns: + The product of the two quaternions in (w, x, y, z). Shape is (..., 4). + + Raises: + ValueError: Input shapes of ``q1`` and ``q2`` are not matching. + """ + # check input is correct + if q1.shape != q2.shape: + msg = f"Expected input quaternion shape mismatch: {q1.shape} != {q2.shape}." + raise ValueError(msg) + # reshape to (N, 4) for multiplication + shape = q1.shape + q1 = q1.reshape(-1, 4) + q2 = q2.reshape(-1, 4) + # extract components from quaternions + w1, x1, y1, z1 = q1[:, 0], q1[:, 1], q1[:, 2], q1[:, 3] + w2, x2, y2, z2 = q2[:, 0], q2[:, 1], q2[:, 2], q2[:, 3] + # perform multiplication + ww = (z1 + x1) * (x2 + y2) + yy = (w1 - y1) * (w2 + z2) + zz = (w1 + y1) * (w2 - z2) + xx = ww + yy + zz + qq = 0.5 * (xx + (z1 - x1) * (x2 - y2)) + w = qq - ww + (z1 - y1) * (y2 - z2) + x = qq - xx + (x1 + w1) * (x2 + w2) + y = qq - yy + (w1 - x1) * (y2 + z2) + z = qq - zz + (z1 + y1) * (w2 - x2) + + return np.stack([w, x, y, z], axis=-1).reshape(shape) + +def quat_apply(quat: np.ndarray, vec: np.ndarray) -> np.ndarray: + """Apply a quaternion rotation to a vector. + + Args: + quat: The quaternion in (w, x, y, z). Shape is (..., 4). + vec: The vector in (x, y, z). Shape is (..., 3). + + Returns: + The rotated vector in (x, y, z). Shape is (..., 3). + """ + # store shape + shape = vec.shape + # reshape to (N, 3) for multiplication + quat = quat.reshape(-1, 4) + vec = vec.reshape(-1, 3) + # extract components from quaternions + xyz = quat[:, 1:] + t = np.cross(xyz, vec, axis=-1) * 2 + return (vec + quat[:, 0:1] * t + np.cross(xyz, t, axis=-1)).reshape(shape) + +def subtract_frame_transforms( + t01: np.ndarray, q01: np.ndarray, + t02: Optional[np.ndarray] = None, + q02: Optional[np.ndarray] = None +) -> Tuple[np.ndarray, np.ndarray]: + r"""Subtract transformations between two reference frames into a stationary frame. + + It performs the following transformation operation: :math:`T_{12} = T_{01}^{-1} \times T_{02}`, + where :math:`T_{AB}` is the homogeneous transformation matrix from frame A to B. + + Args: + t01: Position of frame 1 w.r.t. frame 0. Shape is (N, 3). + q01: Quaternion orientation of frame 1 w.r.t. frame 0 in (w, x, y, z). Shape is (N, 4). + t02: Position of frame 2 w.r.t. frame 0. Shape is (N, 3). + Defaults to None, in which case the position is assumed to be zero. + q02: Quaternion orientation of frame 2 w.r.t. frame 0 in (w, x, y, z). Shape is (N, 4). + Defaults to None, in which case the orientation is assumed to be identity. + + Returns: + A tuple containing the position and orientation of frame 2 w.r.t. frame 1. + Shape of the tensors are (N, 3) and (N, 4) respectively. + """ + # compute orientation + q10 = quat_inv(q01) + if q02 is not None: + q12 = quat_mul(q10, q02) + else: + q12 = q10 + # compute translation + if t02 is not None: + t12 = quat_apply(q10, t02 - t01) + else: + t12 = quat_apply(q10, -t01) + return t12, q12 + +def compute_pose_error(t01: np.ndarray, + q01: np.ndarray, + t02: np.ndarray, + q02: np.ndarray, + return_type='axa') -> Tuple[np.ndarray, np.ndarray]: + q10 = quat_inv(q01) + quat_error = quat_mul(q02, q10) + pos_error = t02-t01 + if return_type == 'axa': + quat_error = axis_angle_from_quat(quat_error) + return pos_error, quat_error + + +def axis_angle_from_quat(quat: np.ndarray, eps: float = 1.0e-6) -> np.ndarray: + """Convert rotations given as quaternions to axis/angle. + + Args: + quat: The quaternion orientation in (w, x, y, z). Shape is (..., 4). + eps: The tolerance for Taylor approximation. Defaults to 1.0e-6. + + Returns: + Rotations given as a vector in axis angle form. Shape is (..., 3). + The vector's magnitude is the angle turned anti-clockwise in radians around the vector's direction. + + Reference: + https://github.com/facebookresearch/pytorch3d/blob/main/pytorch3d/transforms/rotation_conversions.py#L526-L554 + """ + # Modified to take in quat as [q_w, q_x, q_y, q_z] + # Quaternion is [q_w, q_x, q_y, q_z] = [cos(theta/2), n_x * sin(theta/2), n_y * sin(theta/2), n_z * sin(theta/2)] + # Axis-angle is [a_x, a_y, a_z] = [theta * n_x, theta * n_y, theta * n_z] + # Thus, axis-angle is [q_x, q_y, q_z] / (sin(theta/2) / theta) + # When theta = 0, (sin(theta/2) / theta) is undefined + # However, as theta --> 0, we can use the Taylor approximation 1/2 - theta^2 / 48 + quat = quat * (1.0 - 2.0 * (quat[..., 0:1] < 0.0)) + mag = np.linalg.norm(quat[..., 1:], axis=-1) + half_angle = np.arctan2(mag, quat[..., 0]) + angle = 2.0 * half_angle + # check whether to apply Taylor approximation + sin_half_angles_over_angles = np.where( + np.abs(angle) > eps, np.sin(half_angle) / angle, 0.5 - angle * angle / 48 + ) + return quat[..., 1:4] / sin_half_angles_over_angles[..., None] + +def wrap_to_pi(angles: np.ndarray) -> np.ndarray: + r"""Wraps input angles (in radians) to the range :math:`[-\pi, \pi]`. + + This function wraps angles in radians to the range :math:`[-\pi, \pi]`, such that + :math:`\pi` maps to :math:`\pi`, and :math:`-\pi` maps to :math:`-\pi`. In general, + odd positive multiples of :math:`\pi` are mapped to :math:`\pi`, and odd negative + multiples of :math:`\pi` are mapped to :math:`-\pi`. + + The function behaves similar to MATLAB's `wrapToPi `_ + function. + + Args: + angles: Input angles of any shape. + + Returns: + Angles in the range :math:`[-\pi, \pi]`. + """ + # wrap to [0, 2*pi) + wrapped_angle = (angles + np.pi) % (2 * np.pi) + # map to [-pi, pi] + # we check for zero in wrapped angle to make it go to pi when input angle is odd multiple of pi + return np.where((wrapped_angle == 0) & (angles > 0), np.pi, wrapped_angle - np.pi) \ No newline at end of file diff --git a/deploy/deploy_real/common/xml_helper.py b/deploy/deploy_real/common/xml_helper.py new file mode 100644 index 0000000..02eed3b --- /dev/null +++ b/deploy/deploy_real/common/xml_helper.py @@ -0,0 +1,36 @@ +import xml.etree.ElementTree as ET +import numpy as np + +def extract_link_data(filename:str="./resources/robots/g1_description/g1_29dof_rev_1_0.xml"): + tree = ET.parse(filename) + root = tree.getroot() + + link_data = {} + + # Iterate over all 'body' elements in 'worldbody' + for body in root.find("worldbody").iter("body"): + name = body.get("name", "") + if "_link" in name or (name == 'pelvis'): # Filter only the link bodies + inertial = body.find("inertial") + if inertial is not None: + mass = float(inertial.get("mass", "0")) + pos = np.array(list(map(float, inertial.get("pos", "0 0 0").split()))) + quat = np.array(list(map(float, inertial.get("quat", "0 0 0 0").split()))) + else: + mass = 0.0 + pos = np.array([0.0, 0.0, 0.0]) + quat = np.array([0.0, 0.0, 0.0, 0.0]) + + link_data[name] = { + "mass": mass, + "pos": pos, + "quat": quat + } + + return link_data + +def main(): + print(extract_link_data()) + +if __name__ == '__main__': + main() diff --git a/deploy/deploy_real/config.py b/deploy/deploy_real/config.py index d316152..9813138 100644 --- a/deploy/deploy_real/config.py +++ b/deploy/deploy_real/config.py @@ -1,4 +1,7 @@ -from legged_gym import LEGGED_GYM_ROOT_DIR +try: + from legged_gym import LEGGED_GYM_ROOT_DIR +except ModuleNotFoundError: + LEGGED_GYM_ROOT_DIR='../..' import numpy as np import yaml @@ -22,16 +25,51 @@ class Config: self.policy_path = config["policy_path"].replace("{LEGGED_GYM_ROOT_DIR}", LEGGED_GYM_ROOT_DIR) - self.leg_joint2motor_idx = config["leg_joint2motor_idx"] + if 'leg_joint2motor_idx' in config: + self.leg_joint2motor_idx = config["leg_joint2motor_idx"] + if 'joint2motor_idx' in config: + self.joint2motor_idx = config["joint2motor_idx"] + self.kps = config["kps"] self.kds = config["kds"] self.default_angles = np.array(config["default_angles"], dtype=np.float32) - self.arm_waist_joint2motor_idx = config["arm_waist_joint2motor_idx"] - self.arm_waist_kps = config["arm_waist_kps"] - self.arm_waist_kds = config["arm_waist_kds"] - self.arm_waist_target = np.array(config["arm_waist_target"], dtype=np.float32) + if 'arm_waist_joint2motor_idx' in config: + self.arm_waist_joint2motor_idx = config["arm_waist_joint2motor_idx"] + self.arm_waist_kps = config["arm_waist_kps"] + self.arm_waist_kds = config["arm_waist_kds"] + self.arm_waist_target = np.array(config["arm_waist_target"], dtype=np.float32) + else: + self.arm_waist_joint2motor_idx = [] + self.arm_waist_kps = [] + self.arm_waist_kds = [] + self.arm_waist_target = [] + if 'motor_joint' in config: + self.motor_joint = config['motor_joint'] + else: + self.motor_joint=[] + + if 'arm_joint' in config: + self.arm_joint = config['arm_joint'] + else: + self.arm_joint=[] + + if 'non_arm_joint' in config: + self.non_arm_joint = config['non_arm_joint'] + else: + self.non_arm_joint=[] + + if 'lab_joint' in config: + self.lab_joint = config['lab_joint'] + else: + self.lab_joint=[] + + if 'lab_joint_offsets' in config: + self.lab_joint_offsets = config['lab_joint_offsets'] + else: + self.lab_joint_offsets=[] + self.ang_vel_scale = config["ang_vel_scale"] self.dof_pos_scale = config["dof_pos_scale"] self.dof_vel_scale = config["dof_vel_scale"] diff --git a/deploy/deploy_real/configs/g1.yaml b/deploy/deploy_real/configs/g1.yaml index 58ede8c..3f1db2a 100644 --- a/deploy/deploy_real/configs/g1.yaml +++ b/deploy/deploy_real/configs/g1.yaml @@ -7,36 +7,97 @@ imu_type: "pelvis" # "torso" or "pelvis" lowcmd_topic: "rt/lowcmd" lowstate_topic: "rt/lowstate" -policy_path: "{LEGGED_GYM_ROOT_DIR}/deploy/pre_train/g1/motion.pt" +policy_path: "{LEGGED_GYM_ROOT_DIR}/deploy/pre_train/g1/step_policy_v2.pt" leg_joint2motor_idx: [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11] -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] +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] +raw_joint_order: [ + 'left_hip_pitch_joint', + 'left_hip_roll_joint', + 'left_hip_yaw_joint', + 'left_knee_joint', + 'left_ankle_pitch_joint', + 'left_ankle_roll_joint', + 'right_hip_pitch_joint', + 'right_hip_roll_joint', + 'right_hip_yaw_joint', + 'right_knee_joint', + 'right_ankle_pitch_joint', + 'right_ankle_roll_joint', + 'waist_yaw_joint', + 'waist_roll_joint', + 'waist_pitch_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', + 'right_shoulder_pitch_joint', + 'right_shoulder_roll_joint', + 'right_shoulder_yaw_joint', + 'right_elbow_joint', + 'right_wrist_roll_joint', + 'right_wrist_pitch_joint', + 'right_wrist_yaw_joint' +] -arm_waist_kps: [300, 300, 300, - 100, 100, 50, 50, 20, 20, 20, - 100, 100, 50, 50, 20, 20, 20] +# arm_waist_joint2motor_idx: [12, 13, 14, +# 15, 16, 17, 18, 19, 20, 21, +# 22, 23, 24, 25, 26, 27, 28] -arm_waist_kds: [3, 3, 3, - 2, 2, 2, 2, 1, 1, 1, - 2, 2, 2, 2, 1, 1, 1] +# arm_waist_kps: [300, 300, 300, +# 100, 100, 50, 50, 20, 20, 20, +# 100, 100, 50, 50, 20, 20, 20] -arm_waist_target: [ 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0] +# arm_waist_kds: [3, 3, 3, +# 2, 2, 2, 2, 1, 1, 1, +# 2, 2, 2, 2, 1, 1, 1] -ang_vel_scale: 0.25 +# 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 -action_scale: 0.25 -cmd_scale: [2.0, 2.0, 0.25] -num_actions: 12 -num_obs: 47 +# 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: 131 -max_cmd: [0.8, 0.5, 1.57] +# max_cmd: [0.8, 0.5, 1.57] +max_cmd: [1.0, 1.0, 1.0] diff --git a/deploy/deploy_real/configs/g1_eetrack.yaml b/deploy/deploy_real/configs/g1_eetrack.yaml new file mode 100644 index 0000000..a80a281 --- /dev/null +++ b/deploy/deploy_real/configs/g1_eetrack.yaml @@ -0,0 +1,176 @@ +# +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_eetrack.pt" + +lab_joint: [ + 'left_hip_pitch_joint', + 'right_hip_pitch_joint', + 'waist_yaw_joint', + 'left_hip_roll_joint', + 'right_hip_roll_joint', + 'waist_roll_joint', + 'left_hip_yaw_joint', + 'right_hip_yaw_joint', + 'waist_pitch_joint', + 'left_knee_joint', + 'right_knee_joint', + 'left_shoulder_pitch_joint', + 'right_shoulder_pitch_joint', + 'left_ankle_pitch_joint', + 'right_ankle_pitch_joint', + 'left_shoulder_roll_joint', + 'right_shoulder_roll_joint', + 'left_ankle_roll_joint', + 'right_ankle_roll_joint', + 'left_shoulder_yaw_joint', + 'right_shoulder_yaw_joint', + 'left_elbow_joint', + 'right_elbow_joint', + 'left_wrist_roll_joint', + 'right_wrist_roll_joint', + 'left_wrist_pitch_joint', + 'right_wrist_pitch_joint', + 'left_wrist_yaw_joint', + 'right_wrist_yaw_joint' +] + + +arm_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" +] + +# non-arm joints; lab-convention +non_arm_joint: ['left_hip_pitch_joint', 'right_hip_pitch_joint', 'waist_yaw_joint', +'left_hip_roll_joint', 'right_hip_roll_joint', 'waist_roll_joint', +'left_hip_yaw_joint', 'right_hip_yaw_joint', 'waist_pitch_joint', +'left_knee_joint', 'right_knee_joint', 'right_shoulder_pitch_joint', +'left_ankle_pitch_joint', 'right_ankle_pitch_joint', 'right_shoulder_roll_joint', +'left_ankle_roll_joint', 'right_ankle_roll_joint', 'right_shoulder_yaw_joint', +'right_elbow_joint', 'right_wrist_roll_joint', 'right_wrist_pitch_joint', +'right_wrist_yaw_joint'] + + +motor_joint: [ + 'left_hip_pitch_joint', + 'left_hip_roll_joint', + 'left_hip_yaw_joint', + 'left_knee_joint', + 'left_ankle_pitch_joint', + 'left_ankle_roll_joint', + 'right_hip_pitch_joint', + 'right_hip_roll_joint', + 'right_hip_yaw_joint', + 'right_knee_joint', + 'right_ankle_pitch_joint', + 'right_ankle_roll_joint', + 'waist_yaw_joint', + 'waist_roll_joint', + 'waist_pitch_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', + 'right_shoulder_pitch_joint', + 'right_shoulder_roll_joint', + 'right_shoulder_yaw_joint', + 'right_elbow_joint', + 'right_wrist_roll_joint', + 'right_wrist_pitch_joint', + 'right_wrist_yaw_joint', + ] +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] +left_arm_joint2motor_idx: [15,16,17,18,19,20,21] +all_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 + 40, 40, 40, 40, 20, 20, 20, + 40, 40, 40, 40, 20, 20, 20, + ] +kds: [ + # left leg + # 2, 2, 2, 4, 2, 2, + 5, 5, 5, 5, 5, 5, + # right leg + # 2, 2, 2, 4, 2, 2, + 5, 5, 5, 5, 5, 5, + # waist + 3, 3, 3, + # left arm + # 2, 2, 2, 2, 1, 1, 1, + 10, 10, 10, 10, 10, 10, 10, + # right arm + # 2, 2, 2, 2, 1, 1, 1 + 10, 10, 10, 10, 10, 10, 10, + ] +lab_joint_offsets: [ + -0.2000, -0.2000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000, + 0.0000, 0.4200, 0.4200, -0.2000, -0.2000, -0.2300, -0.2300, 0.3500, + -0.3500, 0.0000, 0.0000, 0.0000, 0.0000, 1.0000, 1.0000, 0.0000, + 0.0000, 0.0000, 0.0000, 0.0000, 0.0000 +] +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.0, 0.0, 0., 0.0, 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, +# 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: 29 # 22+7 +num_obs: 132 + +# max_cmd: [0.8, 0.5, 1.57] +max_cmd: [1.0, 1.0, 1.0] diff --git a/deploy/deploy_real/configs/g1_nav.yaml b/deploy/deploy_real/configs/g1_nav.yaml new file mode 100644 index 0000000..67e2762 --- /dev/null +++ b/deploy/deploy_real/configs/g1_nav.yaml @@ -0,0 +1,153 @@ +# +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_eetrack.pt" + +lab_joint: [ + 'left_hip_pitch_joint', + 'right_hip_pitch_joint', + 'waist_yaw_joint', + 'left_hip_roll_joint', + 'right_hip_roll_joint', + 'waist_roll_joint', + 'left_hip_yaw_joint', + 'right_hip_yaw_joint', + 'waist_pitch_joint', + 'left_knee_joint', + 'right_knee_joint', + 'left_shoulder_pitch_joint', + 'right_shoulder_pitch_joint', + 'left_ankle_pitch_joint', + 'right_ankle_pitch_joint', + 'left_shoulder_roll_joint', + 'right_shoulder_roll_joint', + 'left_ankle_roll_joint', + 'right_ankle_roll_joint', + 'left_shoulder_yaw_joint', + 'right_shoulder_yaw_joint', + 'left_elbow_joint', + 'right_elbow_joint', + 'left_wrist_roll_joint', + 'right_wrist_roll_joint', + 'left_wrist_pitch_joint', + 'right_wrist_pitch_joint', + 'left_wrist_yaw_joint', + 'right_wrist_yaw_joint' +] + +motor_joint: [ + 'left_hip_pitch_joint', + 'left_hip_roll_joint', + 'left_hip_yaw_joint', + 'left_knee_joint', + 'left_ankle_pitch_joint', + 'left_ankle_roll_joint', + 'right_hip_pitch_joint', + 'right_hip_roll_joint', + 'right_hip_yaw_joint', + 'right_knee_joint', + 'right_ankle_pitch_joint', + 'right_ankle_roll_joint', + 'waist_yaw_joint', + 'waist_roll_joint', + 'waist_pitch_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', + 'right_shoulder_pitch_joint', + 'right_shoulder_roll_joint', + 'right_shoulder_yaw_joint', + 'right_elbow_joint', + 'right_wrist_roll_joint', + 'right_wrist_pitch_joint', + 'right_wrist_yaw_joint', + ] +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] +all_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 + 40, 40, 40, 40, 20, 20, 20, + 40, 40, 40, 40, 20, 20, 20, + ] +kds: [ + # left leg + # 2, 2, 2, 4, 2, 2, + 5, 5, 5, 5, 5, 5, + # right leg + # 2, 2, 2, 4, 2, 2, + 5, 5, 5, 5, 5, 5, + # waist + 3, 3, 3, + # left arm + # 2, 2, 2, 2, 1, 1, 1, + 10, 10, 10, 10, 10, 10, 10, + # right arm + # 2, 2, 2, 2, 1, 1, 1 + 10, 10, 10, 10, 10, 10, 10, + ] +lab_joint_offsets: [ + -0.2000, -0.2000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000, + 0.0000, 0.4200, 0.4200, -0.2000, -0.2000, -0.2300, -0.2300, 0.3500, + -0.3500, 0.0000, 0.0000, 0.0000, 0.0000, 1.0000, 1.0000, 0.0000, + 0.0000, 0.0000, 0.0000, 0.0000, 0.0000 +] +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.0, 0.0, 0., 0.0, 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, +# 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: 29 # 22+7 +num_obs: 97 + +# max_cmd: [0.8, 0.5, 1.57] +max_cmd: [1.0, 1.0, 1.0] diff --git a/deploy/deploy_real/configs/g1_old.yaml b/deploy/deploy_real/configs/g1_old.yaml new file mode 100644 index 0000000..bac133e --- /dev/null +++ b/deploy/deploy_real/configs/g1_old.yaml @@ -0,0 +1,42 @@ +# +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/motion.pt" + +leg_joint2motor_idx: [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11] +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] + +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 +dof_pos_scale: 1.0 +dof_vel_scale: 0.05 +action_scale: 0.25 +cmd_scale: [2.0, 2.0, 0.25] +num_actions: 12 +num_obs: 47 + +max_cmd: [0.8, 0.5, 1.57] \ No newline at end of file diff --git a/deploy/deploy_real/configs/ik.yaml b/deploy/deploy_real/configs/ik.yaml new file mode 100644 index 0000000..aecacf1 --- /dev/null +++ b/deploy/deploy_real/configs/ik.yaml @@ -0,0 +1,121 @@ +# +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" + +arm_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', + 'left_hip_yaw_joint', + 'left_knee_joint', + 'left_ankle_pitch_joint', + 'left_ankle_roll_joint', + 'right_hip_pitch_joint', + 'right_hip_roll_joint', + 'right_hip_yaw_joint', + 'right_knee_joint', + 'right_ankle_pitch_joint', + 'right_ankle_roll_joint', + 'waist_yaw_joint', + 'waist_roll_joint', + 'waist_pitch_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', + 'right_shoulder_pitch_joint', + 'right_shoulder_roll_joint', + 'right_shoulder_yaw_joint', + 'right_elbow_joint', + 'right_wrist_roll_joint', + 'right_wrist_pitch_joint', + 'right_wrist_yaw_joint', + ] +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] +left_arm_joint2motor_idx: [15,16,17,18,19,20,21] +all_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: [ + # 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.0, 0.0, 0., 0.0, 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, +# 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] diff --git a/deploy/deploy_real/deploy_real.py b/deploy/deploy_real/deploy_real.py index e94dce2..cdc3319 100644 --- a/deploy/deploy_real/deploy_real.py +++ b/deploy/deploy_real/deploy_real.py @@ -19,6 +19,79 @@ from common.rotation_helper import get_gravity_orientation, transform_imu_data from common.remote_controller import RemoteController, KeyMap from config import Config +isaaclab_joint_order = [ + 'left_hip_pitch_joint', + 'right_hip_pitch_joint', + 'waist_yaw_joint', + 'left_hip_roll_joint', + 'right_hip_roll_joint', + 'waist_roll_joint', + 'left_hip_yaw_joint', + 'right_hip_yaw_joint', + 'waist_pitch_joint', + 'left_knee_joint', + 'right_knee_joint', + 'left_shoulder_pitch_joint', + 'right_shoulder_pitch_joint', + 'left_ankle_pitch_joint', + 'right_ankle_pitch_joint', + 'left_shoulder_roll_joint', + 'right_shoulder_roll_joint', + 'left_ankle_roll_joint', + 'right_ankle_roll_joint', + 'left_shoulder_yaw_joint', + 'right_shoulder_yaw_joint', + 'left_elbow_joint', + 'right_elbow_joint', + 'left_wrist_roll_joint', + 'right_wrist_roll_joint', + 'left_wrist_pitch_joint', + 'right_wrist_pitch_joint', + 'left_wrist_yaw_joint', + 'right_wrist_yaw_joint' +] + +raw_joint_order = [ + 'left_hip_pitch_joint', + 'left_hip_roll_joint', + 'left_hip_yaw_joint', + 'left_knee_joint', + 'left_ankle_pitch_joint', + 'left_ankle_roll_joint', + 'right_hip_pitch_joint', + 'right_hip_roll_joint', + 'right_hip_yaw_joint', + 'right_knee_joint', + 'right_ankle_pitch_joint', + 'right_ankle_roll_joint', + 'waist_yaw_joint', + 'waist_roll_joint', + 'waist_pitch_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', + 'right_shoulder_pitch_joint', + 'right_shoulder_roll_joint', + 'right_shoulder_yaw_joint', + 'right_elbow_joint', + 'right_wrist_roll_joint', + 'right_wrist_pitch_joint', + 'right_wrist_yaw_joint' +] + +# Create a mapping tensor +# mapping_tensor = torch.zeros((len(sim_b_joints), len(sim_a_joints)), device=env.device) +mapping_tensor = torch.zeros((len(raw_joint_order), len(isaaclab_joint_order))) + +# Fill the mapping tensor +for b_idx, b_joint in enumerate(raw_joint_order): + if b_joint in isaaclab_joint_order: + a_idx = isaaclab_joint_order.index(b_joint) + mapping_tensor[a_idx, b_idx] = 1.0 class Controller: def __init__(self, config: Config) -> None: @@ -104,10 +177,14 @@ class Controller: total_time = 2 num_step = int(total_time / self.config.control_dt) - dof_idx = self.config.leg_joint2motor_idx + self.config.arm_waist_joint2motor_idx - kps = self.config.kps + self.config.arm_waist_kps - kds = self.config.kds + self.config.arm_waist_kds - default_pos = np.concatenate((self.config.default_angles, self.config.arm_waist_target), axis=0) + # dof_idx = self.config.leg_joint2motor_idx + self.config.arm_waist_joint2motor_idx + # kps = self.config.kps + self.config.arm_waist_kps + # kds = self.config.kds + self.config.arm_waist_kds + # default_pos = np.concatenate((self.config.default_angles, self.config.arm_waist_target), axis=0) + dof_idx = self.config.joint2motor_idx + kps = self.config.kps + kds = self.config.kds + default_pos = self.config.default_angles dof_size = len(dof_idx) # record the current pos @@ -133,29 +210,40 @@ class Controller: print("Enter default pos state.") print("Waiting for the Button A signal...") while self.remote_controller.button[KeyMap.A] != 1: - for i in range(len(self.config.leg_joint2motor_idx)): - motor_idx = self.config.leg_joint2motor_idx[i] + # for i in range(len(self.config.leg_joint2motor_idx)): + # motor_idx = self.config.leg_joint2motor_idx[i] + # self.low_cmd.motor_cmd[motor_idx].q = self.config.default_angles[i] + # self.low_cmd.motor_cmd[motor_idx].qd = 0 + # self.low_cmd.motor_cmd[motor_idx].kp = self.config.kps[i] + # self.low_cmd.motor_cmd[motor_idx].kd = self.config.kds[i] + # self.low_cmd.motor_cmd[motor_idx].tau = 0 + # for i in range(len(self.config.arm_waist_joint2motor_idx)): + # motor_idx = self.config.arm_waist_joint2motor_idx[i] + # self.low_cmd.motor_cmd[motor_idx].q = self.config.arm_waist_target[i] + # self.low_cmd.motor_cmd[motor_idx].qd = 0 + # self.low_cmd.motor_cmd[motor_idx].kp = self.config.arm_waist_kps[i] + # self.low_cmd.motor_cmd[motor_idx].kd = self.config.arm_waist_kds[i] + # self.low_cmd.motor_cmd[motor_idx].tau = 0 + for i in range(len(self.config.joint2motor_idx)): + motor_idx = self.config.joint2motor_idx[i] self.low_cmd.motor_cmd[motor_idx].q = self.config.default_angles[i] self.low_cmd.motor_cmd[motor_idx].qd = 0 self.low_cmd.motor_cmd[motor_idx].kp = self.config.kps[i] self.low_cmd.motor_cmd[motor_idx].kd = self.config.kds[i] self.low_cmd.motor_cmd[motor_idx].tau = 0 - for i in range(len(self.config.arm_waist_joint2motor_idx)): - motor_idx = self.config.arm_waist_joint2motor_idx[i] - self.low_cmd.motor_cmd[motor_idx].q = self.config.arm_waist_target[i] - self.low_cmd.motor_cmd[motor_idx].qd = 0 - self.low_cmd.motor_cmd[motor_idx].kp = self.config.arm_waist_kps[i] - self.low_cmd.motor_cmd[motor_idx].kd = self.config.arm_waist_kds[i] - self.low_cmd.motor_cmd[motor_idx].tau = 0 self.send_cmd(self.low_cmd) time.sleep(self.config.control_dt) def run(self): self.counter += 1 # Get the current joint position and velocity - for i in range(len(self.config.leg_joint2motor_idx)): - self.qj[i] = self.low_state.motor_state[self.config.leg_joint2motor_idx[i]].q - self.dqj[i] = self.low_state.motor_state[self.config.leg_joint2motor_idx[i]].dq + # for i in range(len(self.config.leg_joint2motor_idx)): + # self.qj[i] = self.low_state.motor_state[self.config.leg_joint2motor_idx[i]].q + # self.dqj[i] = self.low_state.motor_state[self.config.leg_joint2motor_idx[i]].dq + for i, motor_idx in enumerate(self.config.joint2motor_idx): + self.qj[i] = self.low_state.motor_state[motor_idx].q + self.dqj[i] = self.low_state.motor_state[motor_idx].dq + # imu_state quaternion: w, x, y, z quat = self.low_state.imu_state.quaternion @@ -164,8 +252,11 @@ class Controller: if self.config.imu_type == "torso": # h1 and h1_2 imu is on the torso # imu data needs to be transformed to the pelvis frame - waist_yaw = self.low_state.motor_state[self.config.arm_waist_joint2motor_idx[0]].q - waist_yaw_omega = self.low_state.motor_state[self.config.arm_waist_joint2motor_idx[0]].dq + # waist_yaw = self.low_state.motor_state[self.config.arm_waist_joint2motor_idx[0]].q + # waist_yaw_omega = self.low_state.motor_state[self.config.arm_waist_joint2motor_idx[0]].dq + waist_yaw = self.low_state.motor_state[self.config.joint2motor_idx[12]].q + waist_yaw_omega = self.low_state.motor_state[self.config.joint2motor_idx[12]].dq + quat, ang_vel = transform_imu_data(waist_yaw=waist_yaw, waist_yaw_omega=waist_yaw_omega, imu_quat=quat, imu_omega=ang_vel) # create observation @@ -175,11 +266,11 @@ class Controller: qj_obs = (qj_obs - self.config.default_angles) * self.config.dof_pos_scale dqj_obs = dqj_obs * self.config.dof_vel_scale ang_vel = ang_vel * self.config.ang_vel_scale - period = 0.8 - count = self.counter * self.config.control_dt - phase = count % period / period - sin_phase = np.sin(2 * np.pi * phase) - cos_phase = np.cos(2 * np.pi * phase) + # period = 0.8 + # count = self.counter * self.config.control_dt + # phase = count % period / period + # sin_phase = np.sin(2 * np.pi * phase) + # cos_phase = np.cos(2 * np.pi * phase) self.cmd[0] = self.remote_controller.ly self.cmd[1] = self.remote_controller.lx * -1 @@ -192,32 +283,48 @@ class Controller: self.obs[9 : 9 + num_actions] = qj_obs self.obs[9 + num_actions : 9 + num_actions * 2] = dqj_obs self.obs[9 + num_actions * 2 : 9 + num_actions * 3] = self.action - self.obs[9 + num_actions * 3] = sin_phase - self.obs[9 + num_actions * 3 + 1] = cos_phase + # self.obs[9 + num_actions * 3] = sin_phase + # self.obs[9 + num_actions * 3 + 1] = cos_phase # Get the action from the policy network obs_tensor = torch.from_numpy(self.obs).unsqueeze(0) + + # Reorder the observations + obs_tensor[..., 9:38] = obs_tensor[..., 9:38] @ mapping_tensor.transpose(0, 1) + obs_tensor[..., 38:67] = obs_tensor[..., 38:67] @ mapping_tensor.transpose(0, 1) + obs_tensor[..., 67:96] = obs_tensor[..., 67:96] @ mapping_tensor.transpose(0, 1) + self.action = self.policy(obs_tensor).detach().numpy().squeeze() + + # Reorder the actions + self.action = self.action @ mapping_tensor.detach().cpu().numpy() # transform action to target_dof_pos target_dof_pos = self.config.default_angles + self.action * self.config.action_scale # Build low cmd - for i in range(len(self.config.leg_joint2motor_idx)): - motor_idx = self.config.leg_joint2motor_idx[i] + # for i in range(len(self.config.leg_joint2motor_idx)): + # motor_idx = self.config.leg_joint2motor_idx[i] + # self.low_cmd.motor_cmd[motor_idx].q = target_dof_pos[i] + # self.low_cmd.motor_cmd[motor_idx].qd = 0 + # self.low_cmd.motor_cmd[motor_idx].kp = self.config.kps[i] + # self.low_cmd.motor_cmd[motor_idx].kd = self.config.kds[i] + # self.low_cmd.motor_cmd[motor_idx].tau = 0 + + # for i in range(len(self.config.arm_waist_joint2motor_idx)): + # motor_idx = self.config.arm_waist_joint2motor_idx[i] + # self.low_cmd.motor_cmd[motor_idx].q = self.config.arm_waist_target[i] + # self.low_cmd.motor_cmd[motor_idx].qd = 0 + # self.low_cmd.motor_cmd[motor_idx].kp = self.config.arm_waist_kps[i] + # self.low_cmd.motor_cmd[motor_idx].kd = self.config.arm_waist_kds[i] + # self.low_cmd.motor_cmd[motor_idx].tau = 0 + for i, motor_idx in enumerate(self.config.joint2motor_idx): self.low_cmd.motor_cmd[motor_idx].q = target_dof_pos[i] self.low_cmd.motor_cmd[motor_idx].qd = 0 self.low_cmd.motor_cmd[motor_idx].kp = self.config.kps[i] self.low_cmd.motor_cmd[motor_idx].kd = self.config.kds[i] self.low_cmd.motor_cmd[motor_idx].tau = 0 - for i in range(len(self.config.arm_waist_joint2motor_idx)): - motor_idx = self.config.arm_waist_joint2motor_idx[i] - self.low_cmd.motor_cmd[motor_idx].q = self.config.arm_waist_target[i] - self.low_cmd.motor_cmd[motor_idx].qd = 0 - self.low_cmd.motor_cmd[motor_idx].kp = self.config.arm_waist_kps[i] - self.low_cmd.motor_cmd[motor_idx].kd = self.config.arm_waist_kds[i] - self.low_cmd.motor_cmd[motor_idx].tau = 0 # send the command self.send_cmd(self.low_cmd) diff --git a/deploy/deploy_real/deploy_real_old.py b/deploy/deploy_real/deploy_real_old.py new file mode 100644 index 0000000..575ff1e --- /dev/null +++ b/deploy/deploy_real/deploy_real_old.py @@ -0,0 +1,265 @@ +from legged_gym import LEGGED_GYM_ROOT_DIR +from typing import Union +import numpy as np +import time +import torch + +from unitree_sdk2py.core.channel import ChannelPublisher, ChannelFactoryInitialize +from unitree_sdk2py.core.channel import ChannelSubscriber, ChannelFactoryInitialize +from unitree_sdk2py.idl.default import unitree_hg_msg_dds__LowCmd_, unitree_hg_msg_dds__LowState_ +from unitree_sdk2py.idl.default import unitree_go_msg_dds__LowCmd_, unitree_go_msg_dds__LowState_ +from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowCmd_ as LowCmdHG +from unitree_sdk2py.idl.unitree_go.msg.dds_ import LowCmd_ as LowCmdGo +from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowState_ as LowStateHG +from unitree_sdk2py.idl.unitree_go.msg.dds_ import LowState_ as LowStateGo +from unitree_sdk2py.utils.crc import CRC + +from common.command_helper 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 +from config import Config + + +class Controller: + def __init__(self, config: Config) -> None: + self.config = config + self.remote_controller = RemoteController() + + # Initialize the policy network + self.policy = torch.jit.load(config.policy_path) + # Initializing process variables + self.qj = np.zeros(config.num_actions, dtype=np.float32) + self.dqj = np.zeros(config.num_actions, dtype=np.float32) + self.action = np.zeros(config.num_actions, dtype=np.float32) + self.target_dof_pos = config.default_angles.copy() + self.obs = np.zeros(config.num_obs, dtype=np.float32) + self.cmd = np.array([0.0, 0, 0]) + self.counter = 0 + + if config.msg_type == "hg": + # g1 and h1_2 use the hg msg type + self.low_cmd = unitree_hg_msg_dds__LowCmd_() + self.low_state = unitree_hg_msg_dds__LowState_() + self.mode_pr_ = MotorMode.PR + self.mode_machine_ = 0 + + self.lowcmd_publisher_ = ChannelPublisher(config.lowcmd_topic, LowCmdHG) + self.lowcmd_publisher_.Init() + + self.lowstate_subscriber = ChannelSubscriber(config.lowstate_topic, LowStateHG) + self.lowstate_subscriber.Init(self.LowStateHgHandler, 10) + + elif config.msg_type == "go": + # h1 uses the go msg type + self.low_cmd = unitree_go_msg_dds__LowCmd_() + self.low_state = unitree_go_msg_dds__LowState_() + + self.lowcmd_publisher_ = ChannelPublisher(config.lowcmd_topic, LowCmdGo) + self.lowcmd_publisher_.Init() + + self.lowstate_subscriber = ChannelSubscriber(config.lowstate_topic, LowStateGo) + self.lowstate_subscriber.Init(self.LowStateGoHandler, 10) + + else: + raise ValueError("Invalid msg_type") + + # wait for the subscriber to receive data + self.wait_for_low_state() + + # Initialize the command msg + if config.msg_type == "hg": + init_cmd_hg(self.low_cmd, self.mode_machine_, self.mode_pr_) + elif config.msg_type == "go": + init_cmd_go(self.low_cmd, weak_motor=self.config.weak_motor) + + def LowStateHgHandler(self, msg: LowStateHG): + self.low_state = msg + self.mode_machine_ = self.low_state.mode_machine + self.remote_controller.set(self.low_state.wireless_remote) + + def LowStateGoHandler(self, msg: LowStateGo): + self.low_state = msg + self.remote_controller.set(self.low_state.wireless_remote) + + def send_cmd(self, cmd: Union[LowCmdGo, LowCmdHG]): + cmd.crc = CRC().Crc(cmd) + self.lowcmd_publisher_.Write(cmd) + + def wait_for_low_state(self): + while self.low_state.tick == 0: + time.sleep(self.config.control_dt) + print("Successfully connected to the robot.") + + def zero_torque_state(self): + print("Enter zero torque state.") + print("Waiting for the start signal...") + while self.remote_controller.button[KeyMap.start] != 1: + create_zero_cmd(self.low_cmd) + self.send_cmd(self.low_cmd) + time.sleep(self.config.control_dt) + + def move_to_default_pos(self): + print("Moving to default pos.") + # move time 2s + total_time = 2 + num_step = int(total_time / self.config.control_dt) + + dof_idx = self.config.leg_joint2motor_idx + self.config.arm_waist_joint2motor_idx + kps = self.config.kps + self.config.arm_waist_kps + kds = self.config.kds + self.config.arm_waist_kds + default_pos = np.concatenate((self.config.default_angles, self.config.arm_waist_target), axis=0) + dof_size = len(dof_idx) + + # record the current pos + init_dof_pos = np.zeros(dof_size, dtype=np.float32) + for i in range(dof_size): + init_dof_pos[i] = self.low_state.motor_state[dof_idx[i]].q + + # move to default pos + for i in range(num_step): + alpha = i / num_step + for j in range(dof_size): + motor_idx = dof_idx[j] + target_pos = default_pos[j] + self.low_cmd.motor_cmd[motor_idx].q = init_dof_pos[j] * (1 - alpha) + target_pos * alpha + self.low_cmd.motor_cmd[motor_idx].qd = 0 + self.low_cmd.motor_cmd[motor_idx].kp = kps[j] + self.low_cmd.motor_cmd[motor_idx].kd = kds[j] + self.low_cmd.motor_cmd[motor_idx].tau = 0 + self.send_cmd(self.low_cmd) + time.sleep(self.config.control_dt) + + def default_pos_state(self): + print("Enter default pos state.") + print("Waiting for the Button A signal...") + while self.remote_controller.button[KeyMap.A] != 1: + for i in range(len(self.config.leg_joint2motor_idx)): + motor_idx = self.config.leg_joint2motor_idx[i] + self.low_cmd.motor_cmd[motor_idx].q = self.config.default_angles[i] + self.low_cmd.motor_cmd[motor_idx].qd = 0 + self.low_cmd.motor_cmd[motor_idx].kp = self.config.kps[i] + self.low_cmd.motor_cmd[motor_idx].kd = self.config.kds[i] + self.low_cmd.motor_cmd[motor_idx].tau = 0 + for i in range(len(self.config.arm_waist_joint2motor_idx)): + motor_idx = self.config.arm_waist_joint2motor_idx[i] + self.low_cmd.motor_cmd[motor_idx].q = self.config.arm_waist_target[i] + self.low_cmd.motor_cmd[motor_idx].qd = 0 + self.low_cmd.motor_cmd[motor_idx].kp = self.config.arm_waist_kps[i] + self.low_cmd.motor_cmd[motor_idx].kd = self.config.arm_waist_kds[i] + self.low_cmd.motor_cmd[motor_idx].tau = 0 + self.send_cmd(self.low_cmd) + time.sleep(self.config.control_dt) + + def run(self): + self.counter += 1 + # Get the current joint position and velocity + for i in range(len(self.config.leg_joint2motor_idx)): + self.qj[i] = self.low_state.motor_state[self.config.leg_joint2motor_idx[i]].q + self.dqj[i] = self.low_state.motor_state[self.config.leg_joint2motor_idx[i]].dq + + # imu_state quaternion: w, x, y, z + quat = self.low_state.imu_state.quaternion + ang_vel = np.array([self.low_state.imu_state.gyroscope], dtype=np.float32) + + if self.config.imu_type == "torso": + # h1 and h1_2 imu is on the torso + # imu data needs to be transformed to the pelvis frame + waist_yaw = self.low_state.motor_state[self.config.arm_waist_joint2motor_idx[0]].q + waist_yaw_omega = self.low_state.motor_state[self.config.arm_waist_joint2motor_idx[0]].dq + quat, ang_vel = transform_imu_data(waist_yaw=waist_yaw, waist_yaw_omega=waist_yaw_omega, imu_quat=quat, imu_omega=ang_vel) + + # create observation + gravity_orientation = get_gravity_orientation(quat) + qj_obs = self.qj.copy() + dqj_obs = self.dqj.copy() + qj_obs = (qj_obs - self.config.default_angles) * self.config.dof_pos_scale + dqj_obs = dqj_obs * self.config.dof_vel_scale + ang_vel = ang_vel * self.config.ang_vel_scale + period = 0.8 + count = self.counter * self.config.control_dt + phase = count % period / period + sin_phase = np.sin(2 * np.pi * phase) + cos_phase = np.cos(2 * np.pi * phase) + + self.cmd[0] = self.remote_controller.ly + self.cmd[1] = self.remote_controller.lx * -1 + self.cmd[2] = self.remote_controller.rx * -1 + + num_actions = self.config.num_actions + self.obs[:3] = ang_vel + self.obs[3:6] = gravity_orientation + self.obs[6:9] = self.cmd * self.config.cmd_scale * self.config.max_cmd + self.obs[9 : 9 + num_actions] = qj_obs + self.obs[9 + num_actions : 9 + num_actions * 2] = dqj_obs + self.obs[9 + num_actions * 2 : 9 + num_actions * 3] = self.action + self.obs[9 + num_actions * 3] = sin_phase + self.obs[9 + num_actions * 3 + 1] = cos_phase + + # 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() + + # transform action to target_dof_pos + target_dof_pos = self.config.default_angles + self.action * self.config.action_scale + + # Build low cmd + for i in range(len(self.config.leg_joint2motor_idx)): + motor_idx = self.config.leg_joint2motor_idx[i] + self.low_cmd.motor_cmd[motor_idx].q = target_dof_pos[i] + self.low_cmd.motor_cmd[motor_idx].qd = 0 + self.low_cmd.motor_cmd[motor_idx].kp = self.config.kps[i] + self.low_cmd.motor_cmd[motor_idx].kd = self.config.kds[i] + self.low_cmd.motor_cmd[motor_idx].tau = 0 + + for i in range(len(self.config.arm_waist_joint2motor_idx)): + motor_idx = self.config.arm_waist_joint2motor_idx[i] + self.low_cmd.motor_cmd[motor_idx].q = self.config.arm_waist_target[i] + self.low_cmd.motor_cmd[motor_idx].qd = 0 + self.low_cmd.motor_cmd[motor_idx].kp = self.config.arm_waist_kps[i] + self.low_cmd.motor_cmd[motor_idx].kd = self.config.arm_waist_kds[i] + self.low_cmd.motor_cmd[motor_idx].tau = 0 + + # send the command + self.send_cmd(self.low_cmd) + + time.sleep(self.config.control_dt) + + +if __name__ == "__main__": + import argparse + + parser = argparse.ArgumentParser() + parser.add_argument("net", type=str, help="network interface") + parser.add_argument("config", type=str, help="config file name in the configs folder", default="g1.yaml") + args = parser.parse_args() + + # Load config + config_path = f"{LEGGED_GYM_ROOT_DIR}/deploy/deploy_real/configs/{args.config}" + config = Config(config_path) + + # Initialize DDS communication + ChannelFactoryInitialize(0, args.net) + + controller = Controller(config) + + # Enter the zero torque state, press the start key to continue executing + controller.zero_torque_state() + + # Move to the default position + controller.move_to_default_pos() + + # Enter the default position state, press the A key to continue executing + controller.default_pos_state() + + while True: + try: + controller.run() + # Press the select key to exit + if controller.remote_controller.button[KeyMap.select] == 1: + break + except KeyboardInterrupt: + break + # Enter the damping state + create_damping_cmd(controller.low_cmd) + controller.send_cmd(controller.low_cmd) + print("Exit") \ No newline at end of file diff --git a/deploy/deploy_real/deploy_real_ros.py b/deploy/deploy_real/deploy_real_ros.py new file mode 100644 index 0000000..01cfc58 --- /dev/null +++ b/deploy/deploy_real/deploy_real_ros.py @@ -0,0 +1,322 @@ +from legged_gym import LEGGED_GYM_ROOT_DIR +from typing import Union +import numpy as np +import time +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 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 +from config import Config +from common.crc import CRC + +from enum import Enum + +class Mode(Enum): + wait = 0 + zero_torque = 1 + default_pos = 2 + damping = 3 + policy = 4 + null = 5 + +class Controller: + def __init__(self, config: Config) -> None: + self.config = config + self.remote_controller = RemoteController() + + # Initialize the policy network + self.policy = torch.jit.load(config.policy_path) + # Initializing process variables + self.qj = np.zeros(config.num_actions, dtype=np.float32) + self.dqj = np.zeros(config.num_actions, dtype=np.float32) + self.action = np.zeros(config.num_actions, dtype=np.float32) + self.target_dof_pos = config.default_angles.copy() + self.obs = np.zeros(config.num_obs, dtype=np.float32) + self.cmd = np.array([0.0, 0, 0]) + self.counter = 0 + + rp.init() + self._node = rp.create_node("low_level_cmd_sender") + + if config.msg_type == "hg": + # g1 and h1_2 use the hg msg type + + self.low_cmd = LowCmdHG() + self.low_state = LowStateHG() + + self.lowcmd_publisher_ = self._node.create_publisher(LowCmdHG, + 'lowcmd', 10) + self.lowstate_subscriber = self._node.create_subscription(LowStateHG, + 'lowstate', self.LowStateHgHandler, 10) + self.mode_pr_ = MotorMode.PR + self.mode_machine_ = 0 + + # self.lowcmd_publisher_ = ChannelPublisher(config.lowcmd_topic, LowCmdHG) + # self.lowcmd_publisher_.Init() + + # self.lowstate_subscriber = ChannelSubscriber(config.lowstate_topic, LowStateHG) + # self.lowstate_subscriber.Init(self.LowStateHgHandler, 10) + + elif config.msg_type == "go": + raise ValueError(f"{config.msg_type} is not implemented yet.") + + else: + raise ValueError("Invalid msg_type") + + # wait for the subscriber to receive data + # self.wait_for_low_state() + + # Initialize the command msg + if config.msg_type == "hg": + init_cmd_hg(self.low_cmd, self.mode_machine_, self.mode_pr_) + elif config.msg_type == "go": + init_cmd_go(self.low_cmd, weak_motor=self.config.weak_motor) + + self.mode = Mode.wait + self._mode_change = True + self._timer = self._node.create_timer(self.config.control_dt, self.run_wrapper) + self._terminate = False + try: + rp.spin(self._node) + except KeyboardInterrupt: + print("KeyboardInterrupt") + finally: + self._node.destroy_timer(self._timer) + create_damping_cmd(self.low_cmd) + self.send_cmd(self.low_cmd) + self._node.destroy_node() + rp.shutdown() + print("Exit") + + def LowStateHgHandler(self, msg: LowStateHG): + self.low_state = msg + self.mode_machine_ = self.low_state.mode_machine + self.remote_controller.set(self.low_state.wireless_remote) + + def LowStateGoHandler(self, msg: LowStateGo): + self.low_state = msg + self.remote_controller.set(self.low_state.wireless_remote) + + def send_cmd(self, cmd: Union[LowCmdGo, LowCmdHG]): + cmd.mode_machine = self.mode_machine_ + cmd.crc = CRC().Crc(cmd) + size = len(cmd.motor_cmd) + # print(cmd.mode_machine) + # for i in range(size): + # print(i, cmd.motor_cmd[i].q, + # cmd.motor_cmd[i].dq, + # cmd.motor_cmd[i].kp, + # cmd.motor_cmd[i].kd, + # cmd.motor_cmd[i].tau) + self.lowcmd_publisher_.publish(cmd) + + def wait_for_low_state(self): + while self.low_state.crc == 0: + print(self.low_state) + time.sleep(self.config.control_dt) + print("Successfully connected to the robot.") + + def zero_torque_state(self): + if self.remote_controller.button[KeyMap.start] == 1: + self._mode_change = True + self.mode = Mode.default_pos + else: + create_zero_cmd(self.low_cmd) + self.send_cmd(self.low_cmd) + + def prepare_default_pos(self): + # move time 2s + total_time = 2 + self.counter = 0 + self._num_step = int(total_time / self.config.control_dt) + + dof_idx = self.config.leg_joint2motor_idx + self.config.arm_waist_joint2motor_idx + kps = self.config.kps + self.config.arm_waist_kps + kds = self.config.kds + self.config.arm_waist_kds + self._kps = [float(kp) for kp in kps] + self._kds = [float(kd) for kd in kds] + self._default_pos = np.concatenate((self.config.default_angles, self.config.arm_waist_target), axis=0) + self._dof_size = len(dof_idx) + self._dof_idx = dof_idx + + # record the current pos + self._init_dof_pos = np.zeros(self._dof_size, + dtype=np.float32) + for i in range(self._dof_size): + self._init_dof_pos[i] = self.low_state.motor_state[dof_idx[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): + motor_idx = self._dof_idx[j] + target_pos = self._default_pos[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 + self.low_cmd.motor_cmd[motor_idx].kp = self._kps[j] + self.low_cmd.motor_cmd[motor_idx].kd = self._kds[j] + self.low_cmd.motor_cmd[motor_idx].tau = 0.0 + self.send_cmd(self.low_cmd) + self.counter += 1 + else: + self._mode_change = True + self.mode = Mode.damping + + def default_pos_state(self): + if self.remote_controller.button[KeyMap.A] != 1: + for i in range(len(self.config.leg_joint2motor_idx)): + motor_idx = self.config.leg_joint2motor_idx[i] + self.low_cmd.motor_cmd[motor_idx].q = float(self.config.default_angles[i]) + self.low_cmd.motor_cmd[motor_idx].dq = 0.0 + self.low_cmd.motor_cmd[motor_idx].kp = self._kps[i] + self.low_cmd.motor_cmd[motor_idx].kd = self._kds[i] + self.low_cmd.motor_cmd[motor_idx].tau = 0.0 + for i in range(len(self.config.arm_waist_joint2motor_idx)): + motor_idx = self.config.arm_waist_joint2motor_idx[i] + self.low_cmd.motor_cmd[motor_idx].q = float(self.config.arm_waist_target[i]) + self.low_cmd.motor_cmd[motor_idx].dq = 0.0 + self.low_cmd.motor_cmd[motor_idx].kp = self._kps[i] + self.low_cmd.motor_cmd[motor_idx].kd = self._kds[i] + self.low_cmd.motor_cmd[motor_idx].tau = 0.0 + self.send_cmd(self.low_cmd) + else: + self._mode_change = True + self.mode = Mode.policy + + def run_policy(self): + if self.remote_controller.button[KeyMap.select] == 1: + self._mode_change = True + self.mode = Mode.null + return + self.counter += 1 + # Get the current joint position and velocity + for i in range(len(self.config.leg_joint2motor_idx)): + self.qj[i] = self.low_state.motor_state[self.config.leg_joint2motor_idx[i]].q + self.dqj[i] = self.low_state.motor_state[self.config.leg_joint2motor_idx[i]].dq + + # imu_state quaternion: w, x, y, z + quat = self.low_state.imu_state.quaternion + ang_vel = np.array([self.low_state.imu_state.gyroscope], dtype=np.float32) + + if self.config.imu_type == "torso": + # h1 and h1_2 imu is on the torso + # imu data needs to be transformed to the pelvis frame + waist_yaw = self.low_state.motor_state[self.config.arm_waist_joint2motor_idx[0]].q + waist_yaw_omega = self.low_state.motor_state[self.config.arm_waist_joint2motor_idx[0]].dq + quat, ang_vel = transform_imu_data(waist_yaw=waist_yaw, waist_yaw_omega=waist_yaw_omega, imu_quat=quat, imu_omega=ang_vel) + + # create observation + gravity_orientation = get_gravity_orientation(quat) + qj_obs = self.qj.copy() + dqj_obs = self.dqj.copy() + qj_obs = (qj_obs - self.config.default_angles) * self.config.dof_pos_scale + dqj_obs = dqj_obs * self.config.dof_vel_scale + ang_vel = ang_vel * self.config.ang_vel_scale + period = 0.8 + count = self.counter * self.config.control_dt + phase = count % period / period + sin_phase = np.sin(2 * np.pi * phase) + cos_phase = np.cos(2 * np.pi * phase) + + self.cmd[0] = self.remote_controller.ly + self.cmd[1] = self.remote_controller.lx * -1 + self.cmd[2] = self.remote_controller.rx * -1 + # print(self.remote_controller.ly, + # self.remote_controller.lx, + # self.remote_controller.rx) + # self.cmd[0] = 0.0 + # self.cmd[1] = 0.0 + # self.cmd[2] = 0.0 + + num_actions = self.config.num_actions + self.obs[:3] = ang_vel + self.obs[3:6] = gravity_orientation + self.obs[6:9] = self.cmd * self.config.cmd_scale * self.config.max_cmd + self.obs[9 : 9 + num_actions] = qj_obs + self.obs[9 + num_actions : 9 + num_actions * 2] = dqj_obs + self.obs[9 + num_actions * 2 : 9 + num_actions * 3] = self.action + self.obs[9 + num_actions * 3] = sin_phase + self.obs[9 + num_actions * 3 + 1] = cos_phase + + # 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() + + # transform action to target_dof_pos + target_dof_pos = self.config.default_angles + self.action * self.config.action_scale + + # Build low cmd + for i in range(len(self.config.leg_joint2motor_idx)): + motor_idx = self.config.leg_joint2motor_idx[i] + self.low_cmd.motor_cmd[motor_idx].q = float(target_dof_pos[i]) + self.low_cmd.motor_cmd[motor_idx].dq = 0.0 + self.low_cmd.motor_cmd[motor_idx].kp = float(self.config.kps[i]) + self.low_cmd.motor_cmd[motor_idx].kd = float(self.config.kds[i]) + self.low_cmd.motor_cmd[motor_idx].tau = 0.0 + + for i in range(len(self.config.arm_waist_joint2motor_idx)): + motor_idx = self.config.arm_waist_joint2motor_idx[i] + self.low_cmd.motor_cmd[motor_idx].q = float(self.config.arm_waist_target[i]) + self.low_cmd.motor_cmd[motor_idx].dq = 0.0 + self.low_cmd.motor_cmd[motor_idx].kp = float(self.config.arm_waist_kps[i]) + self.low_cmd.motor_cmd[motor_idx].kd = float(self.config.arm_waist_kds[i]) + self.low_cmd.motor_cmd[motor_idx].tau = 0.0 + + # send the command + self.send_cmd(self.low_cmd) + + def run_wrapper(self): + # print("hello", self.mode, + # self.mode == Mode.zero_torque) + if self.mode == Mode.wait: + if self.low_state.crc != 0: + self.mode = Mode.zero_torque + self.low_cmd.mode_machine = self.mode_machine_ + print("Successfully connected to the robot.") + elif self.mode == Mode.zero_torque: + if self._mode_change: + print("Enter zero torque state.") + print("Waiting for the start signal...") + self._mode_change = False + self.zero_torque_state() + elif self.mode == Mode.default_pos: + if self._mode_change: + print("Moving to default pos.") + self._mode_change = False + self.prepare_default_pos() + self.move_to_default_pos() + elif self.mode == Mode.damping: + if self._mode_change: + print("Enter default pos state.") + print("Waiting for the Button A signal...") + self._mode_change = False + self.default_pos_state() + elif self.mode == Mode.policy: + if self._mode_change: + print("Run policy.") + self._mode_change = False + self.counter = 0 + self.run_policy() + elif self.mode == Mode.null: + self._terminate = True + + # time.sleep(self.config.control_dt) + +if __name__ == "__main__": + import argparse + + parser = argparse.ArgumentParser() + parser.add_argument("config", type=str, help="config file name in the configs folder", default="g1.yaml") + args = parser.parse_args() + + # Load config + config_path = f"{LEGGED_GYM_ROOT_DIR}/deploy/deploy_real/configs/{args.config}" + config = Config(config_path) + + controller = Controller(config) diff --git a/deploy/deploy_real/deploy_real_ros_eetrack.py b/deploy/deploy_real/deploy_real_ros_eetrack.py new file mode 100644 index 0000000..9cada00 --- /dev/null +++ b/deploy/deploy_real/deploy_real_ros_eetrack.py @@ -0,0 +1,943 @@ +from legged_gym import LEGGED_GYM_ROOT_DIR +from typing import Union, List +import numpy as np +import time +import torch +import torch as th +from pathlib import Path + +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 tf2_ros import TransformBroadcaster, TransformStamped +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 +from config import Config +from common.crc import CRC +from enum import Enum +import pinocchio as pin +from ikctrl import IKCtrl, xyzw2wxyz +from yourdfpy import URDF + +import math_utils +import random as rd +from act_to_dof import ActToDof + + +class Mode(Enum): + wait = 0 + zero_torque = 1 + default_pos = 2 + damping = 3 + policy = 4 + null = 5 + + +axis_angle_from_quat = math_utils.as_np(math_utils.axis_angle_from_quat) +quat_conjugate = math_utils.as_np(math_utils.quat_conjugate) +quat_mul = math_utils.as_np(math_utils.quat_mul) +quat_rotate = math_utils.as_np(math_utils.quat_rotate) +quat_rotate_inverse = math_utils.as_np(math_utils.quat_rotate_inverse) +wrap_to_pi = math_utils.as_np(math_utils.wrap_to_pi) +combine_frame_transforms = math_utils.as_np( + math_utils.combine_frame_transforms) + +class GlobalClock: + def __init__(self, node): + self.node = node + + def get_time(self): + return self.node.get_clock().now() +clock = None + +def body_pose( + tf_buffer, + frame: str, + ref_frame: str = 'pelvis', + stamp=None, + rot_type: str = 'axa'): + """ --> tf does not exist """ + if stamp is None: + stamp = rp.time.Time() + #stamp = clock.get_time() + try: + # t = "ref{=pelvis}_from_frame" transform + t = tf_buffer.lookup_transform( + ref_frame, # to + frame, # from + stamp) + except TransformException as ex: + print(f'Could not transform {frame} to {ref_frame}: {ex}') + raise + + txn = t.transform.translation + rxn = t.transform.rotation + + xyz = np.array([txn.x, txn.y, txn.z]) + quat_wxyz = np.array([rxn.w, rxn.x, rxn.y, rxn.z]) + + xyz = np.array(xyz) + if rot_type == 'axa': + axa = axis_angle_from_quat(quat_wxyz) + axa = wrap_to_pi(axa) + return (xyz, axa) + elif rot_type == 'quat': + return (xyz, quat_wxyz) + raise ValueError(f"Unknown rot_type: {rot_type}") + + +from common.xml_helper import extract_link_data + + +def compute_com(tf_buffer, body_frames: List[str]): + """compute com of body frames""" + mass_list = [] + com_list = [] + + # bring default values + com_data = extract_link_data( + '../../resources/robots/g1_description/g1_29dof_rev_1_0.xml') + + # iterate for frames + for frame in body_frames: + try: + frame_data = com_data[frame] + except KeyError: + continue + + try: + link_pos, link_wxyz = body_pose(tf_buffer, + frame, rot_type='quat') + except TransformException: + continue + + com_pos_b, com_wxyz = frame_data['pos'], frame_data['quat'] + + # compute com from world coordinates + # NOTE 'math_utils' package will be brought from isaaclab + com_pos = link_pos + quat_rotate(link_wxyz, com_pos_b) + com_list.append(com_pos) + + # get math + mass = frame_data['mass'] + mass_list.append(mass) + + com = sum([m * pos for m, pos in zip(mass_list, com_list)]) / sum(mass_list) + return com + + +def index_map(k_to, k_from): + """ + Returns an index mapping from k_from to k_to. + + Given k_to=a, k_from=b, + returns an index map "a_from_b" such that + array_a[a_from_b] = array_b + + Missing values are set to -1. + """ + index_dict = {k: i for i, k in enumerate(k_to)} # O(len(k_from)) + return [index_dict.get(k, -1) for k in k_from] # O(len(k_to)) + + +def interpolate_position(pos1, pos2, n_segments): + increments = (pos2 - pos1) / n_segments + interp_pos = [pos1 + increments * p for p in range(n_segments)] + interp_pos.append(pos2) + return interp_pos + + +class eetrack: + def __init__(self, root_state_w, tf_buffer): + self.tf_buffer = tf_buffer + # self.eetrack_midpt = root_state_w.clone() + # self.eetrack_midpt[..., 1] += 0.3 + self.eetrack_midpt = ( + root_state_w[..., :3] + + quat_rotate(root_state_w[0, 3:7].detach().cpu().numpy(), + np.array([0.3, 0.0, 0.0]))[None] + ) + self.eetrack_end = None + self.eetrack_subgoal = None + self.number_of_subgoals = 30 + self.eetrack_line_length = 0.3 + self.device = "cpu" + self.create_eetrack(root_state_w) + self.eetrack_subgoal = self.create_subgoal() + self.sg_idx = 0 + # first subgoal sampling time = 1.0s + # self.init_time = rp.time.Time()#.nanoseconds / 1e9 + 1.0 + self.init_time = clock.get_time() + + def create_eetrack(self, root_state_w): + self.eetrack_start = self.eetrack_midpt.clone() + self.eetrack_end = self.eetrack_midpt.clone() + is_hor = rd.choice([True, False]) + eetrack_offset = rd.uniform(-0.5, 0.5) + # For testing + is_hor = True + eetrack_offset = 0.0 + if is_hor: + dx = (self.eetrack_line_length) / 2. + dz = eetrack_offset + delta_body0 = [0, +dx, dz] + delta_body1 = [0, -dx, dz] + + self.eetrack_start += math_utils.quat_rotate( + root_state_w[..., 3:7].float(), + th.as_tensor(delta_body0, dtype=th.float32)[None] + ) + self.eetrack_end += math_utils.quat_rotate( + root_state_w[..., 3:7].float(), + th.as_tensor(delta_body1, dtype=th.float32)[None] + ) + # self.eetrack_start[..., 2] += eetrack_offset + # self.eetrack_end[..., 2] += eetrack_offset + # self.eetrack_start[..., 0] -= (self.eetrack_line_length) / 2. + # self.eetrack_end[..., 0] += (self.eetrack_line_length) / 2. + else: + # self.eetrack_start[..., 0] += eetrack_offset + # self.eetrack_end[..., 0] += eetrack_offset + # self.eetrack_start[..., 2] += (self.eetrack_line_length) / 2. + # self.eetrack_end[..., 2] -= (self.eetrack_line_length) / 2. + dx = eetrack_offset + dz = (self.eetrack_line_length) / 2. + delta_body0 = [0, dx, +dz] + delta_body1 = [0, dx, -dz] + self.eetrack_start += math_utils.quat_rotate( + root_state_w[..., 3:7], + th.as_tensor(delta_body0)[None] + ) + self.eetrack_end += math_utils.quat_rotate( + root_state_w[..., 3:7], + th.as_tensor(delta_body1)[None] + ) + + return self.eetrack_start, self.eetrack_end + + def create_direction(self): + angle_from_eetrack_line = torch.rand(1, device=self.device) * np.pi + angle_from_xyplane_in_global_frame = torch.rand( + 1, device=self.device) * np.pi - np.pi / 2 + # For testing + angle_from_eetrack_line = torch.rand(1, device=self.device) * np.pi / 2 + angle_from_xyplane_in_global_frame = torch.rand( + 1, device=self.device) * 0 + roll = torch.zeros(1, device=self.device) + pitch = angle_from_xyplane_in_global_frame + yaw = angle_from_eetrack_line + euler = torch.stack([roll, pitch, yaw], dim=1) + quat = math_utils.quat_from_euler_xyz( + euler[:, 0], euler[:, 1], euler[:, 2]) + return quat + + def create_subgoal(self): + eetrack_subgoals = interpolate_position( + self.eetrack_start, self.eetrack_end, self.number_of_subgoals) + eetrack_subgoals = [ + ( + l.clone().to(self.device, dtype=torch.float32) + if isinstance(l, torch.Tensor) + else torch.tensor(l, device=self.device, dtype=torch.float32) + ) + for l in eetrack_subgoals + ] + eetrack_subgoals = torch.stack(eetrack_subgoals, axis=1) + eetrack_ori = self.create_direction().unsqueeze( + 1).repeat(1, self.number_of_subgoals + 1, 1) + # welidng_subgoals -> Nenv x Npoints x (3 + 4) + return torch.cat([eetrack_subgoals, eetrack_ori], dim=2) + + def update_command(self): + # print(rp.time.Time().nanoseconds) + time = (clock.get_time() - self.init_time).nanoseconds / 1e9 + if (time >= 1.0): + self.sg_idx = int((time-1)/ 0.1 + 1) + print(time, self.sg_idx) + # self.sg_idx.clamp_(0, self.number_of_subgoals + 1) + self.sg_idx = min(self.sg_idx, self.number_of_subgoals) + self.next_command_s_left = self.eetrack_subgoal[..., + self.sg_idx, :] + + def get_command(self, root_state_w): + self.update_command() + + pos_hand_b_left, quat_hand_b_left = body_pose( + self.tf_buffer, + "left_hand_palm_link", + rot_type='quat' + ) + + lerp_command_w_left = self.next_command_s_left + + (lerp_command_b_left_pos, + lerp_command_b_left_quat) = math_utils.subtract_frame_transforms( + root_state_w[..., 0:3], + root_state_w[..., 3:7], + lerp_command_w_left[:, 0:3], + lerp_command_w_left[:, 3:7], + ) + + # lerp_command_b_left = lerp_command_w_left + + pos_delta_b_left, rot_delta_b_left = math_utils.compute_pose_error( + torch.from_numpy(pos_hand_b_left)[None], + torch.from_numpy(quat_hand_b_left)[None], + lerp_command_b_left_pos, + lerp_command_b_left_quat, + ) + axa_delta_b_left = math_utils.wrap_to_pi(rot_delta_b_left) + + hand_command = torch.cat((pos_delta_b_left, axa_delta_b_left), dim=-1) + return hand_command + + +class Observation: + def __init__(self, + urdf_path: str, + config, + tf_buffer: Buffer): + self.links = list(URDF.load(urdf_path).link_map.keys()) + self.config = config + self.num_lab_joint = len(config.lab_joint) + self.tf_buffer = tf_buffer + self.lab_from_mot = index_map(config.lab_joint, + config.motor_joint) + + def __call__(self, + low_state: LowStateHG, + last_action: np.ndarray, + hands_command: np.ndarray + ): + lab_from_mot = self.lab_from_mot + num_lab_joint = self.num_lab_joint + # observation terms (order preserved) + + # FIXME(ycho): dummy value + # base_lin_vel = np.zeros(3) + ang_vel = np.array([low_state.imu_state.gyroscope], + dtype=np.float32) + + if True: + # NOTE(ycho): requires running `fake_world_tf_pub.py`. + world_from_pelvis = self.tf_buffer.lookup_transform( + 'world', + 'pelvis', + rp.time.Time() + #clock.get_time() + ) + rxn = world_from_pelvis.transform.rotation + quat = np.array([rxn.w, rxn.x, rxn.y, rxn.z]) + else: + quat = low_state.imu_state.quaternion + + if self.config.imu_type == "torso": + waist_yaw = low_state.motor_state[self.config.arm_waist_joint2motor_idx[0]].q + waist_yaw_omega = low_state.motor_state[self.config.arm_waist_joint2motor_idx[0]].dq + quat, ang_vel = transform_imu_data( + waist_yaw=waist_yaw, + waist_yaw_omega=waist_yaw_omega, + imu_quat=quat, + imu_omega=ang_vel) + + # NOTE(ycho): `ang_vel` is _probably_ in the pelvis frame, + # since otherwise transform_imu_data() would be unnecessary for + # `ang_vel`. + base_ang_vel = ang_vel.squeeze(0) + + # TODO(ycho): check if the convention "q_base^{-1} @ g" holds. + projected_gravity = get_gravity_orientation(quat) + + fp_l = body_pose(self.tf_buffer, 'left_ankle_roll_link') + fp_r = body_pose(self.tf_buffer, 'right_ankle_roll_link') + foot_pose = np.concatenate([fp_l[0], fp_r[0], fp_l[1], fp_r[1]]) + + hp_l = body_pose(self.tf_buffer, 'left_hand_palm_link') + hp_r = body_pose(self.tf_buffer, 'right_hand_palm_link') + hand_pose = np.concatenate([hp_l[0], hp_r[0], hp_l[1], hp_r[1]]) + + # FIXME(ycho): implement com_pos_wrt_pelvis + projected_com = compute_com(self.tf_buffer, self.links)[..., :2] + # projected_zmp = _ # IMPOSSIBLE + + # Map `low_state` to index-mapped joint_{pos,vel} + joint_pos = np.zeros(num_lab_joint, + dtype=np.float32) + joint_vel = np.zeros(num_lab_joint, + dtype=np.float32) + joint_pos[lab_from_mot] = [low_state.motor_state[i_mot].q for i_mot in + range(len(lab_from_mot))] + joint_pos -= config.lab_joint_offsets + joint_vel[lab_from_mot] = [low_state.motor_state[i_mot].dq for i_mot in + range(len(lab_from_mot))] + actions = last_action + + # Given as delta_pos {xyz,axa}; i.e. 6D vector + # hands_command = self.eetrack.get_command() + + right_arm_com = compute_com(self.tf_buffer, [ + "right_shoulder_pitch_link", + "right_shoulder_roll_link", + "right_shoulder_yaw_link", + "right_elbow_link", + "right_wrist_pitch_link" + "right_wrist_roll_link", + "right_wrist_yaw_link" + ]) + left_arm_com = compute_com(self.tf_buffer, [ + "left_shoulder_pitch_link", + "left_shoulder_roll_link", + "left_shoulder_yaw_link", + "left_elbow_link", + "left_wrist_pitch_link" + "left_wrist_roll_link", + "left_wrist_yaw_link" + ]) + world_from_pelvis = self.tf_buffer.lookup_transform( + 'world', + 'pelvis', + rp.time.Time() + #clock.get_time() + ) + pelvis_height = [world_from_pelvis.transform.translation.z] + + obs = [ + base_ang_vel, + projected_gravity, + foot_pose, + hand_pose, + projected_com, + joint_pos, + joint_vel, + actions, + hands_command, + right_arm_com, + left_arm_com, + pelvis_height + ] + # print([np.shape(o) for o in obs]) + return np.concatenate(obs, axis=-1) + + +class Controller: + def __init__(self, config: Config) -> None: + self.config = config + self.remote_controller = RemoteController() + + # Initialize the policy network + self.policy = torch.jit.load(config.policy_path) + self.action = np.zeros(config.num_actions, dtype=np.float32) + 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 + + # == build index map == + arm_joint = config.arm_joint + self.mot_from_pin = index_map( + self.config.motor_joint, + self.ikctrl.joint_names) + self.pin_from_mot = index_map( + self.ikctrl.joint_names, + self.config.motor_joint + ) + self.mot_from_arm = index_map( + self.config.motor_joint, + self.config.arm_joint + ) + self.mot_from_nonarm = index_map( + 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) + self.cmd = np.array([0.0, 0, 0]) + self.counter = 0 + + # ROS handles & helpers + rp.init() + self._node = rp.create_node("low_level_cmd_sender") + + global clock + clock = GlobalClock(self._node) + + self.tf_buffer = Buffer() + self.tf_listener = TransformListener(self.tf_buffer, self._node) + self.tf_broadcaster = TransformBroadcaster(self._node) + self.obsmap = Observation( + '../../resources/robots/g1_description/g1_29dof_with_hand_rev_1_0.urdf', + config, self.tf_buffer) + # FIXME(ycho): give `root_state_w` + self.eetrack = None + + if True: + q_mot = np.array(config.default_angles) + q_pin = np.zeros_like(self.ikctrl.cfg.q) + q_pin[self.pin_from_mot] = q_mot + default_pose = self.ikctrl.fk(q_pin) + xyz = default_pose.translation + quat_wxyz = xyzw2wxyz( + pin.Quaternion( + default_pose.rotation).coeffs()) + self.default_pose_b = np.concatenate([xyz, quat_wxyz]) + # self.target_pose = np.copy(self.default_pose_b) + self.target_pose = None + # print('default_pose', self.default_pose_b) + + if config.msg_type == "hg": + # g1 and h1_2 use the hg msg type + + self.low_cmd = LowCmdHG() + self.low_state = LowStateHG() + + self.lowcmd_publisher_ = self._node.create_publisher(LowCmdHG, + 'lowcmd', 10) + self.lowstate_subscriber = self._node.create_subscription( + LowStateHG, 'lowstate', self.LowStateHgHandler, 10) + self.mode_pr_ = MotorMode.PR + self.mode_machine_ = 0 + + elif config.msg_type == "go": + raise ValueError(f"{config.msg_type} is not implemented yet.") + + else: + raise ValueError("Invalid msg_type") + + # wait for the subscriber to receive data + # self.wait_for_low_state() + + # Initialize the command msg + if config.msg_type == "hg": + init_cmd_hg(self.low_cmd, self.mode_machine_, self.mode_pr_) + elif config.msg_type == "go": + init_cmd_go(self.low_cmd, weak_motor=self.config.weak_motor) + + # NOTE(ycho): + # if running from real robot: + self.mode = Mode.wait + # if running from rosbag: + # self.mode = Mode.policy + + self._mode_change = True + self._timer = self._node.create_timer( + self.config.control_dt, self.run_wrapper) + self._terminate = False + try: + rp.spin(self._node) + except KeyboardInterrupt: + print("KeyboardInterrupt") + finally: + self._node.destroy_timer(self._timer) + create_damping_cmd(self.low_cmd) + self.send_cmd(self.low_cmd) + self._node.destroy_node() + rp.shutdown() + print("Exit") + + def LowStateHgHandler(self, msg: LowStateHG): + self.low_state = msg + self.mode_machine_ = self.low_state.mode_machine + self.remote_controller.set(self.low_state.wireless_remote) + + def LowStateGoHandler(self, msg: LowStateGo): + self.low_state = msg + self.remote_controller.set(self.low_state.wireless_remote) + + def send_cmd(self, cmd: Union[LowCmdGo, LowCmdHG]): + cmd.mode_machine = self.mode_machine_ + cmd.crc = CRC().Crc(cmd) + size = len(cmd.motor_cmd) + self.lowcmd_publisher_.publish(cmd) + + def wait_for_low_state(self): + while self.low_state.crc == 0: + print(self.low_state) + time.sleep(self.config.control_dt) + print("Successfully connected to the robot.") + + def zero_torque_state(self): + if self.remote_controller.button[KeyMap.start] == 1: + self._mode_change = True + self.mode = Mode.default_pos + else: + create_zero_cmd(self.low_cmd) + self.send_cmd(self.low_cmd) + + def prepare_default_pos(self): + # move time 2s + total_time = 2 + self.counter = 0 + self._num_step = int(total_time / self.config.control_dt) + + dof_idx = self.config.leg_joint2motor_idx + self.config.arm_waist_joint2motor_idx + kps = self.config.kps + self.config.arm_waist_kps + kds = self.config.kds + self.config.arm_waist_kds + self._kps = [float(kp) for kp in kps] + self._kds = [float(kd) for kd in kds] + self._default_pos = np.concatenate( + (self.config.default_angles, self.config.arm_waist_target), axis=0) + self._dof_size = len(dof_idx) + self._dof_idx = dof_idx + + # record the current pos + # self._init_dof_pos = np.zeros(self._dof_size, + # dtype=np.float32) + # 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 = 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(29): + # 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 + self.low_cmd.motor_cmd[motor_idx].kp = self._kps[j] + self.low_cmd.motor_cmd[motor_idx].kd = self._kds[j] + self.low_cmd.motor_cmd[motor_idx].tau = 0.0 + self.send_cmd(self.low_cmd) + self.counter += 1 + else: + self._mode_change = True + self.mode = Mode.damping + + def default_pos_state(self): + if self.remote_controller.button[KeyMap.A] != 1: + for i in range(len(self.config.leg_joint2motor_idx)): + motor_idx = self.config.leg_joint2motor_idx[i] + self.low_cmd.motor_cmd[motor_idx].q = float( + self.config.default_angles[i]) + self.low_cmd.motor_cmd[motor_idx].dq = 0.0 + self.low_cmd.motor_cmd[motor_idx].kp = self._kps[i] + self.low_cmd.motor_cmd[motor_idx].kd = self._kds[i] + self.low_cmd.motor_cmd[motor_idx].tau = 0.0 + for i in range(len(self.config.arm_waist_joint2motor_idx)): + motor_idx = self.config.arm_waist_joint2motor_idx[i] + self.low_cmd.motor_cmd[motor_idx].q = float( + self.config.arm_waist_target[i]) + self.low_cmd.motor_cmd[motor_idx].dq = 0.0 + self.low_cmd.motor_cmd[motor_idx].kp = self._kps[i] + self.low_cmd.motor_cmd[motor_idx].kd = self._kds[i] + self.low_cmd.motor_cmd[motor_idx].tau = 0.0 + self.send_cmd(self.low_cmd) + else: + self._mode_change = True + self.mode = Mode.policy + + def publish_hand_target(self): + t = TransformStamped() + + # Read message content and assign it to + # corresponding tf variables + t.header.stamp = self._node.get_clock().now().to_msg() + t.header.frame_id = 'world' + t.child_frame_id = 'target' + + # Turtle only exists in 2D, thus we get x and y translation + # coordinates from the message and set the z coordinate to 0 + t.transform.translation.x = float(self.target_pose[0]) + t.transform.translation.y = float(self.target_pose[1]) + t.transform.translation.z = float(self.target_pose[2]) + + # Set world_from_pelvis quaternion based on IMU state + # TODO(ycho): consider applying 90-deg offset? + qw, qx, qy, qz = [float(x) for x in self.target_pose[3:7] ] + t.transform.rotation.x = qx + t.transform.rotation.y = qy + t.transform.rotation.z = qz + t.transform.rotation.w = qw + + # Send the transformation + self.tf_broadcaster.sendTransform(t) + + def run_policy(self): + if self.remote_controller.button[KeyMap.select] == 1: + self._mode_change = True + self.mode = Mode.null + return + self.counter += 1 + + # TODO(ycho): consider using `cmd` for `hands_command` + self.cmd[0] = self.remote_controller.ly + self.cmd[1] = self.remote_controller.lx * -1 + self.cmd[2] = self.remote_controller.rx * -1 + + if self.target_pose is None: + # NOTE(ycho): ensure target pose is defined in world frame! + if False: + world_from_pelvis = body_pose( + self.tf_buffer, + 'pelvis', + 'world', + rot_type='quat' + ) + xyz0, quat_wxyz0 = world_from_pelvis + xyz1, quat_wxyz1 = self.default_pose_b[0:3], self.default_pose_b[3:7] + + xyz, quat = combine_frame_transforms( + xyz0, quat_wxyz0, + xyz1, quat_wxyz1) + else: + xyz, quat = body_pose( + self.tf_buffer, + 'left_hand_palm_link', + 'world', + rot_type='quat' + ) + self.target_pose = np.concatenate([xyz, quat]) + #print('validation...', + # self.target_pose, + # body_pose(self.tf_buffer, + # 'left_hand_palm_link', + # 'world', rot_type='quat')) + + if False: + self.target_pose[..., :3] += 0.01 * self.cmd + self.publish_hand_target() + + # FIXME(ycho): implement `_hands_command_` + # to use the output of `eetrack`. + if True: + # NOTE(ycho): requires running `fake_world_tf_pub.py`. + world_from_pelvis = body_pose( + self.tf_buffer, + 'pelvis', + 'world', + rot_type='quat' + ) + xyz, quat_wxyz = world_from_pelvis + root_state_w = np.zeros(7) + root_state_w[0:3] = xyz + root_state_w[3:7] = quat_wxyz + + if self.eetrack is None: + self.eetrack = eetrack(torch.from_numpy(root_state_w)[None], + self.tf_buffer) + + + if True: + _hands_command_ = self.eetrack.get_command( + torch.from_numpy(root_state_w)[None])[0].detach().cpu().numpy() + + # NOTE(ycho) EETRACK version + if True: + self.target_pose = np.copy( + self.eetrack.next_command_s_left.squeeze().detach().cpu().numpy() + ) + self.publish_hand_target() + else: + _hands_command_ = np.zeros(6) + + # TODO(ycho): restore updating to `hands_command` + # from `target_pose`. + if False: + if False: + q_mot = [self.low_state.motor_state[i_mot].q + for i_mot in range(29)] + # print('q_mot (out)', q_mot) + q_pin = np.zeros_like(self.ikctrl.cfg.q) + q_pin[self.pin_from_mot] = q_mot + current_pose = self.ikctrl.fk(q_pin) + + cur_xyz = current_pose.translation + cur_quat = xyzw2wxyz(pin.Quaternion( + current_pose.rotation).coeffs()) + else: + cur_xyz, cur_quat = body_pose( + self.tf_buffer, + 'left_hand_palm_link', + 'world', + rot_type='quat' + ) + _hands_command_ = np.zeros(6) + _hands_command_[0:3] = (self.target_pose[:3] - cur_xyz) + + # q_target @ q_current^{-1} + d_quat = quat_mul( + self.target_pose[3:7], + quat_conjugate(cur_quat) + ) + d_axa = axis_angle_from_quat(d_quat) + _hands_command_[3:6] = d_axa + print('hands_command', _hands_command_) + else: + pelvis_from_world = body_pose( + self.tf_buffer, + 'world', + 'pelvis', + rot_type='quat' + ) + dst_xyz, dst_quat = combine_frame_transforms( + pelvis_from_world[0], + pelvis_from_world[1], + self.target_pose[..., :3], + self.target_pose[..., 3:7] + ) + + cur_xyz, cur_quat = body_pose( + self.tf_buffer, + 'left_hand_palm_link', + rot_type='quat') + + _hands_command_ = np.zeros(6) + _hands_command_[0:3] = (dst_xyz - cur_xyz) + # q_target @ q_current^{-1} + d_quat = quat_mul( + dst_quat, + quat_conjugate(cur_quat) + ) + d_axa = axis_angle_from_quat(d_quat) + _hands_command_[3:6] = d_axa + print('hands_command', _hands_command_) + + # print(_hands_command_) + # _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_) + # logpath = Path('/tmp/eet8/') + # logpath.mkdir(parents=True, exist_ok=True) + # np.save(F'{logpath}/obs{self.counter:03d}.npy', + # self.obs) + + # Get the action from the policy network + obs_tensor = torch.from_numpy(self.obs).unsqueeze(0) + obs_tensor = obs_tensor.detach().clone() + + # hands_command = obs[..., 119:125] + if False: + world_from_body_quat = math_utils.quat_from_euler_xyz( + th.as_tensor([0], dtype=th.float32), + th.as_tensor([0], dtype=th.float32), + th.as_tensor([1.57], dtype=th.float32)).reshape(4) + obs_tensor[..., 119:122] = math_utils.quat_rotate( + world_from_body_quat[None], + obs_tensor[..., 119:122]) + obs_tensor[..., 122:125] = math_utils.quat_rotate( + world_from_body_quat[None], + obs_tensor[..., 122:125]) + if True: + self.action = self.policy(obs_tensor).detach().numpy().squeeze() + + if False: + non_arm_target = np.load('/tmp/eet5/act064.npy')[0][:22] + self.action[..., :22] = non_arm_target + + # np.save(F'{logpath}/act{self.counter:03d}.npy', + # self.action) + + target_dof_pos = self.actmap( + self.obs, + self.action, + #root_state_w[3:7] + ) + + q_mot = np.asarray( + [self.low_state.motor_state[i_mot].q for i_mot in range(29)] + ) + target_dof_pos = ( + 0.7 * q_mot + + 0.3 * target_dof_pos + ) + # print('??', + # target_dof_pos, + # [self.low_state.motor_state[i_mot].q for i_mot in range(29)]) + + # np.save(F'{logpath}/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.5 * float(self.config.kps[i]) + self.low_cmd.motor_cmd[i].kd = 0.5 * float(self.config.kds[i]) + self.low_cmd.motor_cmd[i].tau = 0.0 + + # reduce KP for non-arm joints + for i in self.mot_from_nonarm: + self.low_cmd.motor_cmd[i].kp = 0.2 * float(self.config.kps[i]) + self.low_cmd.motor_cmd[i].kd = 0.2 * float(self.config.kds[i]) + + # send the command + self.send_cmd(self.low_cmd) + + def run_wrapper(self): + # print("hello", self.mode, + # self.mode == Mode.zero_torque) + if self.mode == Mode.wait: + if self.low_state.crc != 0: + self.mode = Mode.zero_torque + self.low_cmd.mode_machine = self.mode_machine_ + print("Successfully connected to the robot.") + elif self.mode == Mode.zero_torque: + if self._mode_change: + print("Enter zero torque state.") + print("Waiting for the start signal...") + self._mode_change = False + self.zero_torque_state() + elif self.mode == Mode.default_pos: + if self._mode_change: + print("Moving to default pos.") + self._mode_change = False + self.prepare_default_pos() + self.move_to_default_pos() + elif self.mode == Mode.damping: + if self._mode_change: + print("Enter default pos state.") + print("Waiting for the Button A signal...") + self._mode_change = False + self.default_pos_state() + elif self.mode == Mode.policy: + if self._mode_change: + print("Run policy.") + self._mode_change = False + self.counter = 0 + self.run_policy() + elif self.mode == Mode.null: + self._terminate = True + + # time.sleep(self.config.control_dt) + + +if __name__ == "__main__": + import argparse + + parser = argparse.ArgumentParser() + parser.add_argument( + "config", + type=str, + help="config file name in the configs folder", + default="g1.yaml") + args = parser.parse_args() + + # Load config + config_path = f"{LEGGED_GYM_ROOT_DIR}/deploy/deploy_real/configs/{args.config}" + config = Config(config_path) + + controller = Controller(config) diff --git a/deploy/deploy_real/deploy_real_ros_ikctrl.py b/deploy/deploy_real/deploy_real_ros_ikctrl.py new file mode 100644 index 0000000..3f8647e --- /dev/null +++ b/deploy/deploy_real/deploy_real_ros_ikctrl.py @@ -0,0 +1,319 @@ +from legged_gym import LEGGED_GYM_ROOT_DIR +from typing import Union +import numpy as np +import time +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 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 +from config import Config +from common.crc import CRC + +from enum import Enum +import pinocchio as pin +from ikctrl import IKCtrl, xyzw2wxyz + +class Mode(Enum): + wait = 0 + zero_torque = 1 + default_pos = 2 + damping = 3 + policy = 4 + null = 5 + +class Controller: + def __init__(self, config: Config) -> None: + self.config = config + self.remote_controller = RemoteController() + + act_joint = config.arm_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_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_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) + # Initializing process variables + self.qj = np.zeros(43, dtype=np.float32) + self.dqj = np.zeros(43, dtype=np.float32) + self.action = np.zeros(7, dtype=np.float32) + self.target_dof_pos = config.default_angles.copy() + self.obs = np.zeros(config.num_obs, dtype=np.float32) + self.cmd = np.array([0.0, 0, 0]) + self.counter = 0 + + rp.init() + self._node = rp.create_node("low_level_cmd_sender") + + if config.msg_type == "hg": + # g1 and h1_2 use the hg msg type + + self.low_cmd = LowCmdHG() + self.low_state = LowStateHG() + + self.lowcmd_publisher_ = self._node.create_publisher(LowCmdHG, + 'lowcmd', 10) + self.lowstate_subscriber = self._node.create_subscription(LowStateHG, + 'lowstate', self.LowStateHgHandler, 10) + self.mode_pr_ = MotorMode.PR + self.mode_machine_ = 0 + + # self.lowcmd_publisher_ = ChannelPublisher(config.lowcmd_topic, LowCmdHG) + # self.lowcmd_publisher_.Init() + + # self.lowstate_subscriber = ChannelSubscriber(config.lowstate_topic, LowStateHG) + # self.lowstate_subscriber.Init(self.LowStateHgHandler, 10) + + elif config.msg_type == "go": + raise ValueError(f"{config.msg_type} is not implemented yet.") + + else: + raise ValueError("Invalid msg_type") + + # wait for the subscriber to receive data + # self.wait_for_low_state() + + # Initialize the command msg + if config.msg_type == "hg": + init_cmd_hg(self.low_cmd, self.mode_machine_, self.mode_pr_) + elif config.msg_type == "go": + init_cmd_go(self.low_cmd, weak_motor=self.config.weak_motor) + + self.mode = Mode.wait + self._mode_change = True + self._timer = self._node.create_timer(self.config.control_dt, self.run_wrapper) + self._terminate = False + try: + rp.spin(self._node) + except KeyboardInterrupt: + print("KeyboardInterrupt") + finally: + self._node.destroy_timer(self._timer) + create_damping_cmd(self.low_cmd) + self.send_cmd(self.low_cmd) + self._node.destroy_node() + rp.shutdown() + print("Exit") + + def LowStateHgHandler(self, msg: LowStateHG): + self.low_state = msg + self.mode_machine_ = self.low_state.mode_machine + self.remote_controller.set(self.low_state.wireless_remote) + + def LowStateGoHandler(self, msg: LowStateGo): + self.low_state = msg + self.remote_controller.set(self.low_state.wireless_remote) + + def send_cmd(self, cmd: Union[LowCmdGo, LowCmdHG]): + cmd.mode_machine = self.mode_machine_ + cmd.crc = CRC().Crc(cmd) + size = len(cmd.motor_cmd) + # print(cmd.mode_machine) + # for i in range(size): + # print(i, cmd.motor_cmd[i].q, + # cmd.motor_cmd[i].dq, + # cmd.motor_cmd[i].kp, + # cmd.motor_cmd[i].kd, + # cmd.motor_cmd[i].tau) + self.lowcmd_publisher_.publish(cmd) + + def wait_for_low_state(self): + while self.low_state.crc == 0: + print(self.low_state) + time.sleep(self.config.control_dt) + print("Successfully connected to the robot.") + + def zero_torque_state(self): + if self.remote_controller.button[KeyMap.start] == 1: + self._mode_change = True + self.mode = Mode.default_pos + else: + create_zero_cmd(self.low_cmd) + self.send_cmd(self.low_cmd) + + def prepare_default_pos(self): + # move time 2s + total_time = 2 + self.counter = 0 + self._num_step = int(total_time / self.config.control_dt) + + dof_idx = self.config.leg_joint2motor_idx + self.config.arm_waist_joint2motor_idx + kps = self.config.kps + self.config.arm_waist_kps + kds = self.config.kds + self.config.arm_waist_kds + self._kps = [float(kp) for kp in kps] + self._kds = [float(kd) for kd in kds] + self._default_pos = np.concatenate((self.config.default_angles, self.config.arm_waist_target), axis=0) + self._dof_size = len(dof_idx) + self._dof_idx = dof_idx + + # record the current pos + 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(29): + # 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 + self.low_cmd.motor_cmd[motor_idx].kp = self._kps[j] + self.low_cmd.motor_cmd[motor_idx].kd = self._kds[j] + self.low_cmd.motor_cmd[motor_idx].tau = 0.0 + self.send_cmd(self.low_cmd) + self.counter += 1 + else: + self._mode_change = True + self.mode = Mode.damping + + def default_pos_state(self): + if self.remote_controller.button[KeyMap.A] != 1: + for i in range(len(self.config.leg_joint2motor_idx)): + motor_idx = self.config.leg_joint2motor_idx[i] + self.low_cmd.motor_cmd[motor_idx].q = float(self.config.default_angles[i]) + self.low_cmd.motor_cmd[motor_idx].dq = 0.0 + self.low_cmd.motor_cmd[motor_idx].kp = self._kps[i] + self.low_cmd.motor_cmd[motor_idx].kd = self._kds[i] + self.low_cmd.motor_cmd[motor_idx].tau = 0.0 + for i in range(len(self.config.arm_waist_joint2motor_idx)): + motor_idx = self.config.arm_waist_joint2motor_idx[i] + self.low_cmd.motor_cmd[motor_idx].q = float(self.config.arm_waist_target[i]) + self.low_cmd.motor_cmd[motor_idx].dq = 0.0 + self.low_cmd.motor_cmd[motor_idx].kp = self._kps[i] + self.low_cmd.motor_cmd[motor_idx].kd = self._kds[i] + self.low_cmd.motor_cmd[motor_idx].tau = 0.0 + self.send_cmd(self.low_cmd) + else: + self._mode_change = True + self.mode = Mode.policy + + def run_policy(self): + if self.remote_controller.button[KeyMap.select] == 1: + self._mode_change = True + self.mode = Mode.null + return + self.counter += 1 + + # Get current joint positions + 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_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] + ) + target_q = np.clip(target_q, + self.lim_lo_pin[i_pin], + self.lim_hi_pin[i_pin]) + self.low_cmd.motor_cmd[i_mot].q = target_q + self.low_cmd.motor_cmd[i_mot].dq = 0.0 + # FIXME(ycho): arbitrary scaling + self.low_cmd.motor_cmd[i_mot].kp = 0.2*float(self.config.kps[i_mot]) + self.low_cmd.motor_cmd[i_mot].kd = 0.2*float(self.config.kds[i_mot]) + self.low_cmd.motor_cmd[i_mot].tau = 0.0 + # send the command + self.send_cmd(self.low_cmd) + + def run_wrapper(self): + # print("hello", self.mode, + # self.mode == Mode.zero_torque) + if self.mode == Mode.wait: + if self.low_state.crc != 0: + self.mode = Mode.zero_torque + self.low_cmd.mode_machine = self.mode_machine_ + print("Successfully connected to the robot.") + elif self.mode == Mode.zero_torque: + if self._mode_change: + print("Enter zero torque state.") + print("Waiting for the start signal...") + self._mode_change = False + self.zero_torque_state() + elif self.mode == Mode.default_pos: + if self._mode_change: + print("Moving to default pos.") + self._mode_change = False + self.prepare_default_pos() + self.move_to_default_pos() + elif self.mode == Mode.damping: + if self._mode_change: + print("Enter default pos state.") + print("Waiting for the Button A signal...") + self._mode_change = False + self.default_pos_state() + elif self.mode == Mode.policy: + if self._mode_change: + print("Run policy.") + self._mode_change = False + self.counter = 0 + self.run_policy() + elif self.mode == Mode.null: + self._terminate = True + + # time.sleep(self.config.control_dt) + +if __name__ == "__main__": + import argparse + + parser = argparse.ArgumentParser() + parser.add_argument("config", type=str, help="config file name in the configs folder", default="g1.yaml") + args = parser.parse_args() + + # Load config + config_path = f"{LEGGED_GYM_ROOT_DIR}/deploy/deploy_real/configs/{args.config}" + config = Config(config_path) + + controller = Controller(config) diff --git a/deploy/deploy_real/deploy_real_ros_navigation.py b/deploy/deploy_real/deploy_real_ros_navigation.py new file mode 100644 index 0000000..f544a3e --- /dev/null +++ b/deploy/deploy_real/deploy_real_ros_navigation.py @@ -0,0 +1,461 @@ +from legged_gym import LEGGED_GYM_ROOT_DIR +from typing import Union, List +import numpy as np +import time +import torch +import torch as th +from pathlib import Path + +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 tf2_ros import TransformBroadcaster, TransformStamped +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 +from config import Config +from common.crc import CRC +from enum import Enum +import pinocchio as pin +from ikctrl import IKCtrl, xyzw2wxyz +from yourdfpy import URDF + +import math_utils +import random as rd +from act_to_dof import ActToDof +from common.utils import (to_array, normalize, yaw_quat, + axis_angle_from_quat, + subtract_frame_transforms, + wrap_to_pi, + compute_pose_error, + quat_apply + ) + +class Mode(Enum): + wait = 0 + zero_torque = 1 + default_pos = 2 + damping = 3 + policy = 4 + null = 5 + + +def index_map(k_to, k_from): + """ + Returns an index mapping from k_from to k_to. + + Given k_to=a, k_from=b, + returns an index map "a_from_b" such that + array_a[a_from_b] = array_b + + Missing values are set to -1. + """ + index_dict = {k: i for i, k in enumerate(k_to)} # O(len(k_from)) + return [index_dict.get(k, -1) for k in k_from] # O(len(k_to)) + + +class Observation: + def __init__(self, + urdf_path: str, + config, + tf_buffer: Buffer): + self.links = list(URDF.load(urdf_path).link_map.keys()) + self.config = config + self.num_lab_joint = len(config.lab_joint) + self.tf_buffer = tf_buffer + self.lab_from_mot = index_map(config.lab_joint, + config.motor_joint) + + def __call__(self, + low_state: LowStateHG, + last_action: np.ndarray, + hands_command: np.ndarray + ): + lab_from_mot = self.lab_from_mot + num_lab_joint = self.num_lab_joint + + # FIXME(ycho): dummy value + # base_lin_vel = np.zeros(3) + ang_vel = np.array([low_state.imu_state.gyroscope], + dtype=np.float32) + + if True: + # NOTE(ycho): requires running `fake_world_tf_pub.py`. + world_from_pelvis = self.tf_buffer.lookup_transform( + 'world', + 'pelvis', + rp.time.Time() + ) + rxn = world_from_pelvis.transform.rotation + quat = np.array([rxn.w, rxn.x, rxn.y, rxn.z]) + else: + quat = low_state.imu_state.quaternion + + if self.config.imu_type == "torso": + waist_yaw = low_state.motor_state[self.config.arm_waist_joint2motor_idx[0]].q + waist_yaw_omega = low_state.motor_state[self.config.arm_waist_joint2motor_idx[0]].dq + quat, ang_vel = transform_imu_data( + waist_yaw=waist_yaw, + waist_yaw_omega=waist_yaw_omega, + imu_quat=quat, + imu_omega=ang_vel) + + # NOTE(ycho): `ang_vel` is _probably_ in the pelvis frame, + # since otherwise transform_imu_data() would be unnecessary for + # `ang_vel`. + base_ang_vel = ang_vel.squeeze(0) + + # TODO(ycho): check if the convention "q_base^{-1} @ g" holds. + projected_gravity = get_gravity_orientation(quat) + + # Map `low_state` to index-mapped joint_{pos,vel} + joint_pos = np.zeros(num_lab_joint, + dtype=np.float32) + joint_vel = np.zeros(num_lab_joint, + dtype=np.float32) + joint_pos[lab_from_mot] = [low_state.motor_state[i_mot].q for i_mot in + range(len(lab_from_mot))] + joint_pos -= config.lab_joint_offsets + joint_vel[lab_from_mot] = [low_state.motor_state[i_mot].dq for i_mot in + range(len(lab_from_mot))] + actions = last_action + + obs = [ + base_ang_vel, + projected_gravity, + hands_command, + joint_pos, + joint_vel, + actions, + ] + # print([np.shape(o) for o in obs]) + return np.concatenate(obs, axis=-1) + + +class Controller: + def __init__(self, config: Config) -> None: + self.config = config + # init mapping tensor for joint order. + self.mapping_tensor = torch.zeros((len(config.lab_joint), len(config.motor_joint))) + for b_idx, b_joint in enumerate(config.motor_joint): + if b_joint in config.lab_joint: + a_idx = config.lab_joint.index(b_joint) + self.mapping_tensor[a_idx, b_idx] = 1.0 + + self.mot_from_lab = index_map(config.motor_joint, config.lab_joint) + + self.remote_controller = RemoteController() + + # Initialize the policy network + self.policy = torch.jit.load(config.policy_path) + self.action = np.zeros(config.num_actions, dtype=np.float32) + + # Data buffers + self.obs = np.zeros(config.num_obs, dtype=np.float32) + # command : x[m] y[m] z[m] heading[rad] + self.cmd = np.array([0., 0., 0., 0.]) + self.given_cmd = np.array([0., 0., 0., 0.]) + self.counter = 0 + + # ROS handles & helpers + rp.init() + self._node = rp.create_node("low_level_cmd_sender") + self.tf_buffer = Buffer() + self.tf_listener = TransformListener(self.tf_buffer, self._node) + self.tf_broadcaster = TransformBroadcaster(self._node) + self.obsmap = Observation( + '../../resources/robots/g1_description/g1_29dof_with_hand_rev_1_0.urdf', + config, self.tf_buffer) + + if config.msg_type == "hg": + # g1 and h1_2 use the hg msg type + + self.low_cmd = LowCmdHG() + self.low_state = LowStateHG() + + self.lowcmd_publisher_ = self._node.create_publisher(LowCmdHG, + 'lowcmd', 10) + self.lowstate_subscriber = self._node.create_subscription( + LowStateHG, 'lowstate', self.LowStateHgHandler, 10) + self.mode_pr_ = MotorMode.PR + self.mode_machine_ = 0 + + + + elif config.msg_type == "go": + raise ValueError(f"{config.msg_type} is not implemented yet.") + + else: + raise ValueError("Invalid msg_type") + + # wait for the subscriber to receive data + # self.wait_for_low_state() + + # Initialize the command msg + if config.msg_type == "hg": + init_cmd_hg(self.low_cmd, self.mode_machine_, self.mode_pr_) + elif config.msg_type == "go": + init_cmd_go(self.low_cmd, weak_motor=self.config.weak_motor) + + # NOTE(ycho): + # if running from real robot: + self.mode = Mode.wait + # if running from rosbag: + # self.mode = Mode.policy + + self._mode_change = True + self._timer = self._node.create_timer( + self.config.control_dt, self.run_wrapper) + self._terminate = False + try: + rp.spin(self._node) + except KeyboardInterrupt: + print("KeyboardInterrupt") + finally: + self._node.destroy_timer(self._timer) + create_damping_cmd(self.low_cmd) + self.send_cmd(self.low_cmd) + self._node.destroy_node() + rp.shutdown() + print("Exit") + + def LowStateHgHandler(self, msg: LowStateHG): + self.low_state = msg + self.mode_machine_ = self.low_state.mode_machine + self.remote_controller.set(self.low_state.wireless_remote) + + def LowStateGoHandler(self, msg: LowStateGo): + self.low_state = msg + self.remote_controller.set(self.low_state.wireless_remote) + + def send_cmd(self, cmd: Union[LowCmdGo, LowCmdHG]): + cmd.mode_machine = self.mode_machine_ + cmd.crc = CRC().Crc(cmd) + size = len(cmd.motor_cmd) + self.lowcmd_publisher_.publish(cmd) + + def wait_for_low_state(self): + while self.low_state.crc == 0: + print(self.low_state) + time.sleep(self.config.control_dt) + print("Successfully connected to the robot.") + + def zero_torque_state(self): + if self.remote_controller.button[KeyMap.start] == 1: + self._mode_change = True + self.mode = Mode.default_pos + else: + create_zero_cmd(self.low_cmd) + self.send_cmd(self.low_cmd) + + def prepare_default_pos(self): + # move time 2s + total_time = 2 + self.counter = 0 + self._num_step = int(total_time / self.config.control_dt) + + dof_idx = self.config.leg_joint2motor_idx + self.config.arm_waist_joint2motor_idx + kps = self.config.kps + self.config.arm_waist_kps + kds = self.config.kds + self.config.arm_waist_kds + self._kps = [float(kp) for kp in kps] + self._kds = [float(kd) for kd in kds] + self._default_pos = np.concatenate( + (self.config.default_angles, self.config.arm_waist_target), axis=0) + self._dof_size = len(dof_idx) + self._dof_idx = dof_idx + + # record the current pos + # self._init_dof_pos = np.zeros(self._dof_size, + # dtype=np.float32) + # 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 = 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(29): + # 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 + self.low_cmd.motor_cmd[motor_idx].kp = self._kps[j] + self.low_cmd.motor_cmd[motor_idx].kd = self._kds[j] + self.low_cmd.motor_cmd[motor_idx].tau = 0.0 + self.send_cmd(self.low_cmd) + self.counter += 1 + else: + self._mode_change = True + self.mode = Mode.damping + + def default_pos_state(self): + if self.remote_controller.button[KeyMap.A] != 1: + for i in range(len(self.config.leg_joint2motor_idx)): + motor_idx = self.config.leg_joint2motor_idx[i] + self.low_cmd.motor_cmd[motor_idx].q = float( + self.config.default_angles[i]) + self.low_cmd.motor_cmd[motor_idx].dq = 0.0 + self.low_cmd.motor_cmd[motor_idx].kp = self._kps[i] + self.low_cmd.motor_cmd[motor_idx].kd = self._kds[i] + self.low_cmd.motor_cmd[motor_idx].tau = 0.0 + for i in range(len(self.config.arm_waist_joint2motor_idx)): + motor_idx = self.config.arm_waist_joint2motor_idx[i] + self.low_cmd.motor_cmd[motor_idx].q = float( + self.config.arm_waist_target[i]) + self.low_cmd.motor_cmd[motor_idx].dq = 0.0 + self.low_cmd.motor_cmd[motor_idx].kp = self._kps[i] + self.low_cmd.motor_cmd[motor_idx].kd = self._kds[i] + self.low_cmd.motor_cmd[motor_idx].tau = 0.0 + self.send_cmd(self.low_cmd) + else: + self._mode_change = True + self.mode = Mode.policy + + def tf_to_pose(self, tf, order='xyzw'): + pos = to_array(tf.transform.translation) + quat = to_array(tf.transform.rotation) + if order == 'wxyz': + quat = np.roll(quat, 1, axis=-1) + return np.concatenate((pos, quat), axis=0) + + def update_command(self): + """update command """ + # Get controller input, update the target navigation command from the world coordinate + pass + + def get_command(self, base_pose_w): + self.update_command() + # Get the target navigation command, which is converted to the body coordinate system + x_w, y_w, _, heading_w = self.given_cmd + + # Compute xyz navigation goal from body coordinate system + """ + target_vec = self.pos_command_w - self.robot.data.root_pos_w[:, :3] + self.pos_command_b[:] = quat_rotate_inverse( + yaw_quat(self.robot.data.root_quat_w), target_vec + ) + """ + xyz_w , quat_w = base_pose_w[:3], base_pose_w[3:] + xyz_cmd_w = np.array(x_w - xyz_w[0], y_w - xyz_w[1], 0) + xyz_cmd_b = math_utils.quat_rotate_inverse( + math_utils.yaw_quat(torch.as_tensor(quat_w).float().to("cpu")), + torch.as_tensor(xyz_cmd_w).float().to("cpu") + ).float().numpy() + + # Compute heading direction from body coordinate system + """ + forward_w = math_utils.quat_apply(self.root_quat_w, self.FORWARD_VEC_B) + return torch.atan2(forward_w[:, 1], forward_w[:, 0]) + + self.heading_command_b[:] = wrap_to_pi( + self.heading_command_w - self.robot.data.heading_w + ) + """ + robot_forward_vec_w = quat_apply(quat_w, torch.tensor([1,0,0]).float().to("cpu")) + robot_heading_w = torch.atan2(robot_forward_vec_w[:, 1], robot_forward_vec_w[:, 0]) + heading_command_b = wrap_to_pi(heading_w - robot_heading_w) + return np.append(xyz_cmd_b, heading_command_b) + + def run_policy(self): + if self.remote_controller.button[KeyMap.select] == 1: + self._mode_change = True + self.mode = Mode.null + return + self.counter += 1 + + base_pose_w = self.tf_to_pose(self.tf_buffer.lookup_transform( + "world", "pelvis", + rp.time.Time()), 'wxyz') + + self.cmd = self.get_command(base_pose_w) + + self.obs[:] = self.obsmap(self.low_state, + self.action, + self.cmd) + + # Get the action from the policy network + obs_tensor = torch.from_numpy(self.obs).unsqueeze(0) + obs_tensor = obs_tensor.detach().clone() + + self.action = self.policy(obs_tensor).detach().numpy().squeeze() + + action = np.zeros_like(self.action, + dtype=np.float32) + action[self.mot_from_lab] = self.action # index_map works ? + + target_dof_pos = self.config.default_angles + action * self.config.action_scale + + # 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 = 1.0 * float(self.config.kps[i]) + self.low_cmd.motor_cmd[i].kd = 1.0 * float(self.config.kds[i]) + self.low_cmd.motor_cmd[i].tau = 0.0 + + # send the command + self.send_cmd(self.low_cmd) + + def run_wrapper(self): + # print("hello", self.mode, + # self.mode == Mode.zero_torque) + if self.mode == Mode.wait: + if self.low_state.crc != 0: + self.mode = Mode.zero_torque + self.low_cmd.mode_machine = self.mode_machine_ + print("Successfully connected to the robot.") + elif self.mode == Mode.zero_torque: + if self._mode_change: + print("Enter zero torque state.") + print("Waiting for the start signal...") + self._mode_change = False + self.zero_torque_state() + elif self.mode == Mode.default_pos: + if self._mode_change: + print("Moving to default pos.") + self._mode_change = False + self.prepare_default_pos() + self.move_to_default_pos() + elif self.mode == Mode.damping: + if self._mode_change: + print("Enter default pos state.") + print("Waiting for the Button A signal...") + self._mode_change = False + self.default_pos_state() + elif self.mode == Mode.policy: + if self._mode_change: + print("Run policy.") + self._mode_change = False + self.counter = 0 + self.run_policy() + elif self.mode == Mode.null: + self._terminate = True + + +if __name__ == "__main__": + import argparse + + parser = argparse.ArgumentParser() + parser.add_argument( + "config", + type=str, + help="config file name in the configs folder", + default="g1_nav.yaml") + args = parser.parse_args() + + # Load config + config_path = f"{LEGGED_GYM_ROOT_DIR}/deploy/deploy_real/configs/{args.config}" + config = Config(config_path) + + controller = Controller(config) diff --git a/deploy/deploy_real/deploy_real_step.py b/deploy/deploy_real/deploy_real_step.py new file mode 100644 index 0000000..32a961e --- /dev/null +++ b/deploy/deploy_real/deploy_real_step.py @@ -0,0 +1,556 @@ +from legged_gym import LEGGED_GYM_ROOT_DIR +from typing import Union +import numpy as np +import time +import torch + +import rclpy as rp + +from unitree_sdk2py.core.channel import ChannelPublisher, ChannelFactoryInitialize +from unitree_sdk2py.core.channel import ChannelSubscriber, ChannelFactoryInitialize +from unitree_sdk2py.idl.default import unitree_hg_msg_dds__LowCmd_, unitree_hg_msg_dds__LowState_ +from unitree_sdk2py.idl.default import unitree_go_msg_dds__LowCmd_, unitree_go_msg_dds__LowState_ +from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowCmd_ as LowCmdHG +from unitree_sdk2py.idl.unitree_go.msg.dds_ import LowCmd_ as LowCmdGo +from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowState_ as LowStateHG +from unitree_sdk2py.idl.unitree_go.msg.dds_ import LowState_ as LowStateGo +from unitree_sdk2py.utils.crc import CRC + +from common.command_helper 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 +from common.step_command import StepCommand +from common.utils import (to_array, normalize, yaw_quat, + axis_angle_from_quat, + subtract_frame_transforms, + wrap_to_pi, + compute_pose_error + ) +from config import Config + +from tf2_ros.buffer import Buffer +from tf2_ros.transform_listener import TransformListener +from tf2_ros import TransformBroadcaster, TransformStamped, StaticTransformBroadcaster + +isaaclab_joint_order = [ + 'left_hip_pitch_joint', + 'right_hip_pitch_joint', + 'waist_yaw_joint', + 'left_hip_roll_joint', + 'right_hip_roll_joint', + 'waist_roll_joint', + 'left_hip_yaw_joint', + 'right_hip_yaw_joint', + 'waist_pitch_joint', + 'left_knee_joint', + 'right_knee_joint', + 'left_shoulder_pitch_joint', + 'right_shoulder_pitch_joint', + 'left_ankle_pitch_joint', + 'right_ankle_pitch_joint', + 'left_shoulder_roll_joint', + 'right_shoulder_roll_joint', + 'left_ankle_roll_joint', + 'right_ankle_roll_joint', + 'left_shoulder_yaw_joint', + 'right_shoulder_yaw_joint', + 'left_elbow_joint', + 'right_elbow_joint', + 'left_wrist_roll_joint', + 'right_wrist_roll_joint', + 'left_wrist_pitch_joint', + 'right_wrist_pitch_joint', + 'left_wrist_yaw_joint', + 'right_wrist_yaw_joint' +] + +raw_joint_order = [ + 'left_hip_pitch_joint', + 'left_hip_roll_joint', + 'left_hip_yaw_joint', + 'left_knee_joint', + 'left_ankle_pitch_joint', + 'left_ankle_roll_joint', + 'right_hip_pitch_joint', + 'right_hip_roll_joint', + 'right_hip_yaw_joint', + 'right_knee_joint', + 'right_ankle_pitch_joint', + 'right_ankle_roll_joint', + 'waist_yaw_joint', + 'waist_roll_joint', + 'waist_pitch_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', + 'right_shoulder_pitch_joint', + 'right_shoulder_roll_joint', + 'right_shoulder_yaw_joint', + 'right_elbow_joint', + 'right_wrist_roll_joint', + 'right_wrist_pitch_joint', + 'right_wrist_yaw_joint' +] + +# Create a mapping tensor +# mapping_tensor = torch.zeros((len(sim_b_joints), len(sim_a_joints)), device=env.device) +mapping_tensor = torch.zeros((len(raw_joint_order), len(isaaclab_joint_order))) + +# Fill the mapping tensor +for b_idx, b_joint in enumerate(raw_joint_order): + if b_joint in isaaclab_joint_order: + a_idx = isaaclab_joint_order.index(b_joint) + mapping_tensor[a_idx, b_idx] = 1.0 + +class Controller: + def __init__(self, config: Config) -> None: + self.config = config + self.remote_controller = RemoteController() + + # Initialize the policy network + self.policy = torch.jit.load(config.policy_path) + # Initializing process variables + self.qj = np.zeros(config.num_actions, dtype=np.float32) + self.dqj = np.zeros(config.num_actions, dtype=np.float32) + self.action = np.zeros(config.num_actions, dtype=np.float32) + self.target_dof_pos = config.default_angles.copy() + self.obs = np.zeros(config.num_obs, dtype=np.float32) + self.cmd = np.array([0.0, 0, 0]) + self.counter = 0 + + rp.init() + self._node = rp.create_node("low_level_cmd_sender") + + self.tf_buffer = Buffer() + self.tf_listener = TransformListener(self.tf_buffer, self._node) + self.tf_broadcaster = TransformBroadcaster(self._node) + + self._step_command = None + self._saved = False + + self._cur_time = None + + if config.msg_type == "hg": + # g1 and h1_2 use the hg msg type + self.low_cmd = unitree_hg_msg_dds__LowCmd_() + self.low_state = unitree_hg_msg_dds__LowState_() + self.mode_pr_ = MotorMode.PR + self.mode_machine_ = 0 + + self.lowcmd_publisher_ = ChannelPublisher(config.lowcmd_topic, LowCmdHG) + self.lowcmd_publisher_.Init() + + self.lowstate_subscriber = ChannelSubscriber(config.lowstate_topic, LowStateHG) + self.lowstate_subscriber.Init(self.LowStateHgHandler, 10) + + elif config.msg_type == "go": + # h1 uses the go msg type + self.low_cmd = unitree_go_msg_dds__LowCmd_() + self.low_state = unitree_go_msg_dds__LowState_() + + self.lowcmd_publisher_ = ChannelPublisher(config.lowcmd_topic, LowCmdGo) + self.lowcmd_publisher_.Init() + + self.lowstate_subscriber = ChannelSubscriber(config.lowstate_topic, LowStateGo) + self.lowstate_subscriber.Init(self.LowStateGoHandler, 10) + + else: + raise ValueError("Invalid msg_type") + + # wait for the subscriber to receive data + self.wait_for_low_state() + + # Initialize the command msg + if config.msg_type == "hg": + init_cmd_hg(self.low_cmd, self.mode_machine_, self.mode_pr_) + elif config.msg_type == "go": + init_cmd_go(self.low_cmd, weak_motor=self.config.weak_motor) + + def LowStateHgHandler(self, msg: LowStateHG): + self.low_state = msg + self.mode_machine_ = self.low_state.mode_machine + self.remote_controller.set(self.low_state.wireless_remote) + + def LowStateGoHandler(self, msg: LowStateGo): + self.low_state = msg + self.remote_controller.set(self.low_state.wireless_remote) + + def send_cmd(self, cmd: Union[LowCmdGo, LowCmdHG]): + cmd.crc = CRC().Crc(cmd) + self.lowcmd_publisher_.Write(cmd) + + def wait_for_low_state(self): + while self.low_state.tick == 0: + time.sleep(self.config.control_dt) + print("Successfully connected to the robot.") + + def zero_torque_state(self): + print("Enter zero torque state.") + print("Waiting for the start signal...") + while self.remote_controller.button[KeyMap.start] != 1: + create_zero_cmd(self.low_cmd) + self.send_cmd(self.low_cmd) + time.sleep(self.config.control_dt) + + def move_to_default_pos(self): + print("Moving to default pos.") + # move time 2s + total_time = 2 + num_step = int(total_time / self.config.control_dt) + + # dof_idx = self.config.leg_joint2motor_idx + self.config.arm_waist_joint2motor_idx + # kps = self.config.kps + self.config.arm_waist_kps + # kds = self.config.kds + self.config.arm_waist_kds + # default_pos = np.concatenate((self.config.default_angles, self.config.arm_waist_target), axis=0) + dof_idx = self.config.joint2motor_idx + kps = self.config.kps + kds = self.config.kds + default_pos = self.config.default_angles + dof_size = len(dof_idx) + + # record the current pos + init_dof_pos = np.zeros(dof_size, dtype=np.float32) + for i in range(dof_size): + init_dof_pos[i] = self.low_state.motor_state[dof_idx[i]].q + + # move to default pos + for i in range(num_step): + alpha = i / num_step + for j in range(dof_size): + motor_idx = dof_idx[j] + target_pos = default_pos[j] + self.low_cmd.motor_cmd[motor_idx].q = init_dof_pos[j] * (1 - alpha) + target_pos * alpha + self.low_cmd.motor_cmd[motor_idx].qd = 0 + self.low_cmd.motor_cmd[motor_idx].kp = kps[j] + self.low_cmd.motor_cmd[motor_idx].kd = kds[j] + self.low_cmd.motor_cmd[motor_idx].tau = 0 + self.send_cmd(self.low_cmd) + time.sleep(self.config.control_dt) + + def default_pos_state(self): + print("Enter default pos state.") + print("Waiting for the Button A signal...") + while self.remote_controller.button[KeyMap.A] != 1: + # for i in range(len(self.config.leg_joint2motor_idx)): + # motor_idx = self.config.leg_joint2motor_idx[i] + # self.low_cmd.motor_cmd[motor_idx].q = self.config.default_angles[i] + # self.low_cmd.motor_cmd[motor_idx].qd = 0 + # self.low_cmd.motor_cmd[motor_idx].kp = self.config.kps[i] + # self.low_cmd.motor_cmd[motor_idx].kd = self.config.kds[i] + # self.low_cmd.motor_cmd[motor_idx].tau = 0 + # for i in range(len(self.config.arm_waist_joint2motor_idx)): + # motor_idx = self.config.arm_waist_joint2motor_idx[i] + # self.low_cmd.motor_cmd[motor_idx].q = self.config.arm_waist_target[i] + # self.low_cmd.motor_cmd[motor_idx].qd = 0 + # self.low_cmd.motor_cmd[motor_idx].kp = self.config.arm_waist_kps[i] + # self.low_cmd.motor_cmd[motor_idx].kd = self.config.arm_waist_kds[i] + # self.low_cmd.motor_cmd[motor_idx].tau = 0 + for i in range(len(self.config.joint2motor_idx)): + motor_idx = self.config.joint2motor_idx[i] + self.low_cmd.motor_cmd[motor_idx].q = self.config.default_angles[i] + self.low_cmd.motor_cmd[motor_idx].qd = 0 + self.low_cmd.motor_cmd[motor_idx].kp = self.config.kps[i] + self.low_cmd.motor_cmd[motor_idx].kd = self.config.kds[i] + self.low_cmd.motor_cmd[motor_idx].tau = 0 + self.send_cmd(self.low_cmd) + time.sleep(self.config.control_dt) + + def tf_to_pose(self, tf, order='xyzw'): + pos = to_array(tf.transform.translation) + quat = to_array(tf.transform.rotation) + if order == 'wxyz': + quat = np.roll(quat, 1, axis=-1) + return np.concatenate((pos, quat), axis=0) + + def publish_step_command(self, next_ctarget_left, next_ctarget_right): + left_tf = TransformStamped() + left_tf.header.stamp = self._node.get_clock().now().to_msg() + left_tf.header.frame_id = 'world' + left_tf.child_frame_id = 'left_ctarget' + left_tf.transform.translation.x = float(next_ctarget_left[0]) + left_tf.transform.translation.y = float(next_ctarget_left[1]) + left_tf.transform.translation.z = float(next_ctarget_left[2]) + + left_tf.transform.rotation.x = float(next_ctarget_left[4]) + left_tf.transform.rotation.y = float(next_ctarget_left[5]) + left_tf.transform.rotation.z = float(next_ctarget_left[6]) + left_tf.transform.rotation.w = float(next_ctarget_left[3]) + + right_tf = TransformStamped() + right_tf.header.stamp = left_tf.header.stamp + right_tf.header.frame_id = 'world' + right_tf.child_frame_id = 'right_ctarget' + right_tf.transform.translation.x = float(next_ctarget_right[0]) + right_tf.transform.translation.y = float(next_ctarget_right[1]) + right_tf.transform.translation.z = float(next_ctarget_right[2]) + + right_tf.transform.rotation.x = float(next_ctarget_right[4]) + right_tf.transform.rotation.y = float(next_ctarget_right[5]) + right_tf.transform.rotation.z = float(next_ctarget_right[6]) + right_tf.transform.rotation.w = float(next_ctarget_right[3]) + + self.tf_broadcaster.sendTransform(left_tf) + self.tf_broadcaster.sendTransform(right_tf) + + def get_command(self, pelvis_w, + foot_left_b, + foot_right_b, + ctarget_left_w, + ctarget_right_w): + ctarget_left_b_pos, ctarget_left_b_quat = subtract_frame_transforms(pelvis_w[:3], + pelvis_w[3:7], + ctarget_left_w[:3], + ctarget_left_w[3:7]) + ctarget_right_b_pos, ctarget_right_b_quat = subtract_frame_transforms(pelvis_w[:3], + pelvis_w[3:7], + ctarget_right_w[:3], + ctarget_right_w[3:7]) + pos_delta_left, axa_delta_left = compute_pose_error(foot_left_b[:3], + foot_left_b[3:7], + ctarget_left_b_pos, + ctarget_left_b_quat) + pos_delta_right, axa_delta_right = compute_pose_error(foot_right_b[:3], + foot_right_b[3:7], + ctarget_right_b_pos, + ctarget_right_b_quat) + return np.concatenate((pos_delta_left, axa_delta_left, pos_delta_right, axa_delta_right), axis=0) + + def run_wrapper(self): + t = time.time() + if self._cur_time is None: + self._cur_time = 0.0 + if not self._saved: + while True: + rp.spin_once(self._node) + try: + current_left_tf = self.tf_buffer.lookup_transform( + "world", + "left_ankle_roll_link", + rp.time.Time(), + rp.duration.Duration(seconds=0.02)) + break + except Exception as ex: + print(ex) + time.sleep(0.05) + if t- self._cur_time > self.config.control_dt: + self.run() + self._cur_time = t + + time.sleep(self.config.control_dt/10) + + + def run(self): + if self._step_command is None: + + current_left_tf = self.tf_buffer.lookup_transform( + "world", + "left_ankle_roll_link", + rp.time.Time(), + rp.duration.Duration(seconds=0.02)) + current_left_pose = self.tf_to_pose(current_left_tf, 'wxyz') + current_left_pose[2] = 0.0 + current_left_pose[3:7] = yaw_quat(current_left_pose[3:7]) + current_right_tf = self.tf_buffer.lookup_transform( + "world", + "right_ankle_roll_link", + rp.time.Time(), + rp.duration.Duration(seconds=0.02)) + current_right_pose = self.tf_to_pose(current_right_tf, 'wxyz') + current_right_pose[2] = 0.0 + current_right_pose[3:7] = yaw_quat(current_right_pose[3:7]) + self._step_command = StepCommand(current_left_pose, current_right_pose) + + self.counter += 1 + next_ctarget = self._step_command.get_next_ctarget( + self.remote_controller, + self.counter * self.config.control_dt) + next_ctarget_left, next_ctarget_right, dt_left, dt_right = next_ctarget + self.publish_step_command(next_ctarget_left, next_ctarget_right) + + # Get the current joint position and velocity + # for i in range(len(self.config.leg_joint2motor_idx)): + # self.qj[i] = self.low_state.motor_state[self.config.leg_joint2motor_idx[i]].q + # self.dqj[i] = self.low_state.motor_state[self.config.leg_joint2motor_idx[i]].dq + for i, motor_idx in enumerate(self.config.joint2motor_idx): + self.qj[i] = self.low_state.motor_state[motor_idx].q + self.dqj[i] = self.low_state.motor_state[motor_idx].dq + + + # imu_state quaternion: w, x, y, z + quat = self.low_state.imu_state.quaternion + ang_vel = np.array([self.low_state.imu_state.gyroscope], dtype=np.float32) + + if self.config.imu_type == "torso": + # h1 and h1_2 imu is on the torso + # imu data needs to be transformed to the pelvis frame + # waist_yaw = self.low_state.motor_state[self.config.arm_waist_joint2motor_idx[0]].q + # waist_yaw_omega = self.low_state.motor_state[self.config.arm_waist_joint2motor_idx[0]].dq + waist_yaw = self.low_state.motor_state[self.config.joint2motor_idx[12]].q + waist_yaw_omega = self.low_state.motor_state[self.config.joint2motor_idx[12]].dq + + quat, ang_vel = transform_imu_data(waist_yaw=waist_yaw, waist_yaw_omega=waist_yaw_omega, imu_quat=quat, imu_omega=ang_vel) + + # create observation + gravity_orientation = get_gravity_orientation(quat) + qj_obs = self.qj.copy() + dqj_obs = self.dqj.copy() + qj_obs = (qj_obs - self.config.default_angles) * self.config.dof_pos_scale + dqj_obs = dqj_obs * self.config.dof_vel_scale + ang_vel = ang_vel * self.config.ang_vel_scale + + # foot pose + left_foot_from_base_tf = self.tf_buffer.lookup_transform( + "pelvis", + "left_ankle_roll_link", + rp.time.Time()) + right_foot_from_base_tf = self.tf_buffer.lookup_transform( + "pelvis", + "right_ankle_roll_link", + rp.time.Time()) + print(left_foot_from_base_tf, right_foot_from_base_tf) + lf_b = self.tf_to_pose(left_foot_from_base_tf, 'wxyz') + rf_b = self.tf_to_pose(right_foot_from_base_tf, 'wxyz') + left_foot_axa = wrap_to_pi(axis_angle_from_quat(lf_b[3:7])) + right_foot_axa = wrap_to_pi(axis_angle_from_quat(rf_b[3:7])) + rel_foot = np.concatenate((lf_b[:3], + rf_b[:3], + left_foot_axa, + right_foot_axa), axis=0) + # hand pose + left_hand_from_base_tf = self.tf_buffer.lookup_transform( + "pelvis", + "left_rubber_hand", + rp.time.Time()) + right_hand_from_base_tf = self.tf_buffer.lookup_transform( + "pelvis", + "right_rubber_hand", + rp.time.Time()) + left_hand_from_base = self.tf_to_pose(left_hand_from_base_tf, 'wxyz') + right_hand_from_base = self.tf_to_pose(right_hand_from_base_tf, 'wxyz') + left_hand_axa = wrap_to_pi(axis_angle_from_quat(left_hand_from_base[3:7])) + right_hand_axa = wrap_to_pi(axis_angle_from_quat(right_hand_from_base[3:7])) + rel_hand = np.concatenate((left_hand_from_base[:3], + right_hand_from_base[:3], + left_hand_axa, + right_hand_axa), axis=0) + # foot command + base_pose_w = self.tf_to_pose(self.tf_buffer.lookup_transform( + "world", "pelvis", + rp.time.Time()), 'wxyz') + step_command = self.get_command(base_pose_w, + lf_b, + rf_b, + next_ctarget_left, + next_ctarget_right) + step_command = np.concatenate((step_command, + np.asarray([dt_left, dt_right])), axis=0) + + num_actions = self.config.num_actions + self.obs[:3] = ang_vel + self.obs[3:6] = gravity_orientation + # self.obs[6:9] = self.cmd * self.config.cmd_scale * self.config.max_cmd + self.obs[6:18] = rel_foot + self.obs[18:30] = rel_hand + self.obs[30 : 30 + num_actions] = qj_obs + self.obs[30 + num_actions : 30 + num_actions * 2] = dqj_obs + self.obs[30 + num_actions * 2 : 30 + num_actions * 3] = self.action + self.obs[30 + num_actions * 3 : 30 + num_actions * 3 + 14] = step_command + # self.obs[9 + num_actions * 3] = sin_phase + # self.obs[9 + num_actions * 3 + 1] = cos_phase + + # Get the action from the policy network + obs_tensor = torch.from_numpy(self.obs).unsqueeze(0) + + # Reorder the observations + obs_tensor[..., 30:30+num_actions] = obs_tensor[..., 30:30+num_actions] @ mapping_tensor.transpose(0, 1) + obs_tensor[..., 30 + num_actions : 30 + num_actions * 2] = obs_tensor[..., 30 + num_actions : 30 + num_actions * 2] @ mapping_tensor.transpose(0, 1) + obs_tensor[..., 30 + num_actions * 2 : 30 + num_actions * 3] = obs_tensor[..., 30 + num_actions * 2 : 30 + num_actions * 3] @ mapping_tensor.transpose(0, 1) + + if not self._saved: + torch.save(obs_tensor, "obs.pt") + self._saved = True + + self.action = self.policy(obs_tensor).detach().numpy().squeeze() + + # Reorder the actions + self.action = self.action @ mapping_tensor.detach().cpu().numpy() + + # transform action to target_dof_pos + target_dof_pos = self.config.default_angles + self.action * self.config.action_scale + + # Build low cmd + # for i in range(len(self.config.leg_joint2motor_idx)): + # motor_idx = self.config.leg_joint2motor_idx[i] + # self.low_cmd.motor_cmd[motor_idx].q = target_dof_pos[i] + # self.low_cmd.motor_cmd[motor_idx].qd = 0 + # self.low_cmd.motor_cmd[motor_idx].kp = self.config.kps[i] + # self.low_cmd.motor_cmd[motor_idx].kd = self.config.kds[i] + # self.low_cmd.motor_cmd[motor_idx].tau = 0 + + # for i in range(len(self.config.arm_waist_joint2motor_idx)): + # motor_idx = self.config.arm_waist_joint2motor_idx[i] + # self.low_cmd.motor_cmd[motor_idx].q = self.config.arm_waist_target[i] + # self.low_cmd.motor_cmd[motor_idx].qd = 0 + # self.low_cmd.motor_cmd[motor_idx].kp = self.config.arm_waist_kps[i] + # self.low_cmd.motor_cmd[motor_idx].kd = self.config.arm_waist_kds[i] + # self.low_cmd.motor_cmd[motor_idx].tau = 0 + if False: + for i, motor_idx in enumerate(self.config.joint2motor_idx): + self.low_cmd.motor_cmd[motor_idx].q = target_dof_pos[i] + self.low_cmd.motor_cmd[motor_idx].qd = 0 + self.low_cmd.motor_cmd[motor_idx].kp = self.config.kps[i] + self.low_cmd.motor_cmd[motor_idx].kd = self.config.kds[i] + self.low_cmd.motor_cmd[motor_idx].tau = 0 + + + # send the command + self.send_cmd(self.low_cmd) + + def clear(self): + self._node.destroy_node() + rp.shutdown() + + +if __name__ == "__main__": + import argparse + + parser = argparse.ArgumentParser() + parser.add_argument("net", type=str, help="network interface") + parser.add_argument("config", type=str, help="config file name in the configs folder", default="g1.yaml") + args = parser.parse_args() + + # Load config + config_path = f"{LEGGED_GYM_ROOT_DIR}/deploy/deploy_real/configs/{args.config}" + config = Config(config_path) + + # Initialize DDS communication + ChannelFactoryInitialize(0, args.net) + + controller = Controller(config) + + # Enter the zero torque state, press the start key to continue executing + controller.zero_torque_state() + + # Move to the default position + controller.move_to_default_pos() + + # Enter the default position state, press the A key to continue executing + controller.default_pos_state() + + while True: + try: + controller.run_wrapper() + # Press the select key to exit + if controller.remote_controller.button[KeyMap.select] == 1: + break + except KeyboardInterrupt: + break + # Enter the damping state + create_damping_cmd(controller.low_cmd) + controller.send_cmd(controller.low_cmd) + controller.clear() + print("Exit") diff --git a/deploy/deploy_real/deploy_real_step_ros.py b/deploy/deploy_real/deploy_real_step_ros.py new file mode 100644 index 0000000..594baf8 --- /dev/null +++ b/deploy/deploy_real/deploy_real_step_ros.py @@ -0,0 +1,552 @@ +from legged_gym import LEGGED_GYM_ROOT_DIR +from typing import Union +import numpy as np +import time +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 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 +from config import Config +from common.crc import CRC + +from enum import Enum + +from common.step_command import StepCommand +from common.utils import (to_array, normalize, yaw_quat, + axis_angle_from_quat, + subtract_frame_transforms, + wrap_to_pi, + compute_pose_error + ) +from config import Config + +from tf2_ros.buffer import Buffer +from tf2_ros.transform_listener import TransformListener +from tf2_ros import TransformBroadcaster, TransformStamped, StaticTransformBroadcaster + +class Mode(Enum): + wait = 0 + zero_torque = 1 + default_pos = 2 + damping = 3 + policy = 4 + null = 5 + +isaaclab_joint_order = [ + 'left_hip_pitch_joint', + 'right_hip_pitch_joint', + 'waist_yaw_joint', + 'left_hip_roll_joint', + 'right_hip_roll_joint', + 'waist_roll_joint', + 'left_hip_yaw_joint', + 'right_hip_yaw_joint', + 'waist_pitch_joint', + 'left_knee_joint', + 'right_knee_joint', + 'left_shoulder_pitch_joint', + 'right_shoulder_pitch_joint', + 'left_ankle_pitch_joint', + 'right_ankle_pitch_joint', + 'left_shoulder_roll_joint', + 'right_shoulder_roll_joint', + 'left_ankle_roll_joint', + 'right_ankle_roll_joint', + 'left_shoulder_yaw_joint', + 'right_shoulder_yaw_joint', + 'left_elbow_joint', + 'right_elbow_joint', + 'left_wrist_roll_joint', + 'right_wrist_roll_joint', + 'left_wrist_pitch_joint', + 'right_wrist_pitch_joint', + 'left_wrist_yaw_joint', + 'right_wrist_yaw_joint' +] + +raw_joint_order = [ + 'left_hip_pitch_joint', + 'left_hip_roll_joint', + 'left_hip_yaw_joint', + 'left_knee_joint', + 'left_ankle_pitch_joint', + 'left_ankle_roll_joint', + 'right_hip_pitch_joint', + 'right_hip_roll_joint', + 'right_hip_yaw_joint', + 'right_knee_joint', + 'right_ankle_pitch_joint', + 'right_ankle_roll_joint', + 'waist_yaw_joint', + 'waist_roll_joint', + 'waist_pitch_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', + 'right_shoulder_pitch_joint', + 'right_shoulder_roll_joint', + 'right_shoulder_yaw_joint', + 'right_elbow_joint', + 'right_wrist_roll_joint', + 'right_wrist_pitch_joint', + 'right_wrist_yaw_joint' +] + +# Create a mapping tensor +mapping_tensor = torch.zeros((len(raw_joint_order), len(isaaclab_joint_order))) + +# Fill the mapping tensor +for b_idx, b_joint in enumerate(raw_joint_order): + if b_joint in isaaclab_joint_order: + a_idx = isaaclab_joint_order.index(b_joint) + # if 'shoulder' in b_joint or 'elbow' in b_joint or 'wrist' in b_joint: + # mapping_tensor[a_idx, b_idx] = 0.1 + # else: + mapping_tensor[a_idx, b_idx] = 1.0 + +mask = torch.ones(len(isaaclab_joint_order)) +for b_idx, b_joint in enumerate(isaaclab_joint_order): + if 'shoulder' in b_joint or 'elbow' in b_joint or 'wrist' in b_joint: + mask[b_idx] = 0 + +class Controller: + def __init__(self, config: Config) -> None: + self.config = config + self.remote_controller = RemoteController() + + # Initialize the policy network + self.policy = torch.jit.load(config.policy_path) + # Initializing process variables + self.qj = np.zeros(config.num_actions, dtype=np.float32) + self.dqj = np.zeros(config.num_actions, dtype=np.float32) + self.action = np.zeros(config.num_actions, dtype=np.float32) + self.target_dof_pos = config.default_angles.copy() + self.obs = np.zeros(config.num_obs, dtype=np.float32) + self.cmd = np.array([0.0, 0, 0]) + self.counter = 0 + + rp.init() + self._node = rp.create_node("low_level_cmd_sender") + + self.tf_buffer = Buffer() + self.tf_listener = TransformListener(self.tf_buffer, self._node) + self.tf_broadcaster = TransformBroadcaster(self._node) + + self._step_command = None + self._saved = False + + self._cur_time = None + + if config.msg_type == "hg": + # g1 and h1_2 use the hg msg type + + self.low_cmd = LowCmdHG() + self.low_state = LowStateHG() + + self.lowcmd_publisher_ = self._node.create_publisher(LowCmdHG, + 'lowcmd', 10) + self.lowstate_subscriber = self._node.create_subscription(LowStateHG, + 'lowstate', self.LowStateHgHandler, 10) + self.mode_pr_ = MotorMode.PR + self.mode_machine_ = 0 + + # self.lowcmd_publisher_ = ChannelPublisher(config.lowcmd_topic, LowCmdHG) + # self.lowcmd_publisher_.Init() + + # self.lowstate_subscriber = ChannelSubscriber(config.lowstate_topic, LowStateHG) + # self.lowstate_subscriber.Init(self.LowStateHgHandler, 10) + + elif config.msg_type == "go": + raise ValueError(f"{config.msg_type} is not implemented yet.") + + else: + raise ValueError("Invalid msg_type") + + # wait for the subscriber to receive data + # self.wait_for_low_state() + + # Initialize the command msg + if config.msg_type == "hg": + init_cmd_hg(self.low_cmd, self.mode_machine_, self.mode_pr_) + elif config.msg_type == "go": + init_cmd_go(self.low_cmd, weak_motor=self.config.weak_motor) + + self.mode = Mode.wait + self._mode_change = True + self._timer = self._node.create_timer(self.config.control_dt, self.run_wrapper) + self._terminate = False + self._obs_buf = [] + try: + rp.spin(self._node) + except KeyboardInterrupt: + print("KeyboardInterrupt") + finally: + self._node.destroy_timer(self._timer) + create_damping_cmd(self.low_cmd) + self.send_cmd(self.low_cmd) + self._node.destroy_node() + rp.shutdown() + torch.save(torch.cat(self._obs_buf, dim=0), "obs6.pt") + print("Exit") + + def LowStateHgHandler(self, msg: LowStateHG): + self.low_state = msg + self.mode_machine_ = self.low_state.mode_machine + self.remote_controller.set(self.low_state.wireless_remote) + + def LowStateGoHandler(self, msg: LowStateGo): + self.low_state = msg + self.remote_controller.set(self.low_state.wireless_remote) + + def send_cmd(self, cmd: Union[LowCmdGo, LowCmdHG]): + cmd.mode_machine = self.mode_machine_ + cmd.crc = CRC().Crc(cmd) + size = len(cmd.motor_cmd) + # print(cmd.mode_machine) + # for i in range(size): + # print(i, cmd.motor_cmd[i].q, + # cmd.motor_cmd[i].dq, + # cmd.motor_cmd[i].kp, + # cmd.motor_cmd[i].kd, + # cmd.motor_cmd[i].tau) + self.lowcmd_publisher_.publish(cmd) + + def wait_for_low_state(self): + while self.low_state.crc == 0: + print(self.low_state) + time.sleep(self.config.control_dt) + print("Successfully connected to the robot.") + + def zero_torque_state(self): + if self.remote_controller.button[KeyMap.start] == 1: + self._mode_change = True + self.mode = Mode.default_pos + else: + create_zero_cmd(self.low_cmd) + self.send_cmd(self.low_cmd) + + def prepare_default_pos(self): + # move time 2s + total_time = 2 + self.counter = 0 + self._num_step = int(total_time / self.config.control_dt) + + dof_idx = self.config.joint2motor_idx + kps = self.config.kps + kds = self.config.kds + self._kps = [float(kp) for kp in kps] + self._kds = [float(kd) for kd in kds] + self._default_pos = np.asarray(self.config.default_angles) + # np.concatenate((self.config.default_angles), axis=0) + self._dof_size = len(dof_idx) + self._dof_idx = dof_idx + + # record the current pos + self._init_dof_pos = np.zeros(self._dof_size, + dtype=np.float32) + for i in range(self._dof_size): + self._init_dof_pos[i] = self.low_state.motor_state[dof_idx[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): + motor_idx = self._dof_idx[j] + target_pos = self._default_pos[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 + self.low_cmd.motor_cmd[motor_idx].kp = self._kps[j] + self.low_cmd.motor_cmd[motor_idx].kd = self._kds[j] + self.low_cmd.motor_cmd[motor_idx].tau = 0.0 + self.send_cmd(self.low_cmd) + self.counter += 1 + else: + self._mode_change = True + self.mode = Mode.damping + + def default_pos_state(self): + if self.remote_controller.button[KeyMap.A] != 1: + for i in range(len(self.config.joint2motor_idx)): + motor_idx = self.config.joint2motor_idx[i] + self.low_cmd.motor_cmd[motor_idx].q = float(self.config.default_angles[i]) + self.low_cmd.motor_cmd[motor_idx].dq = 0.0 + self.low_cmd.motor_cmd[motor_idx].kp = self._kps[i] + self.low_cmd.motor_cmd[motor_idx].kd = self._kds[i] + self.low_cmd.motor_cmd[motor_idx].tau = 0.0 + self.send_cmd(self.low_cmd) + else: + self._mode_change = True + self.mode = Mode.policy + + def tf_to_pose(self, tf, order='xyzw'): + pos = to_array(tf.transform.translation) + quat = to_array(tf.transform.rotation) + if order == 'wxyz': + quat = np.roll(quat, 1, axis=-1) + return np.concatenate((pos, quat), axis=0) + + def publish_step_command(self, next_ctarget_left, next_ctarget_right): + left_tf = TransformStamped() + left_tf.header.stamp = self._node.get_clock().now().to_msg() + left_tf.header.frame_id = 'world' + left_tf.child_frame_id = 'left_ctarget' + left_tf.transform.translation.x = float(next_ctarget_left[0]) + left_tf.transform.translation.y = float(next_ctarget_left[1]) + left_tf.transform.translation.z = float(next_ctarget_left[2]) + + left_tf.transform.rotation.x = float(next_ctarget_left[4]) + left_tf.transform.rotation.y = float(next_ctarget_left[5]) + left_tf.transform.rotation.z = float(next_ctarget_left[6]) + left_tf.transform.rotation.w = float(next_ctarget_left[3]) + + right_tf = TransformStamped() + right_tf.header.stamp = left_tf.header.stamp + right_tf.header.frame_id = 'world' + right_tf.child_frame_id = 'right_ctarget' + right_tf.transform.translation.x = float(next_ctarget_right[0]) + right_tf.transform.translation.y = float(next_ctarget_right[1]) + right_tf.transform.translation.z = float(next_ctarget_right[2]) + + right_tf.transform.rotation.x = float(next_ctarget_right[4]) + right_tf.transform.rotation.y = float(next_ctarget_right[5]) + right_tf.transform.rotation.z = float(next_ctarget_right[6]) + right_tf.transform.rotation.w = float(next_ctarget_right[3]) + + self.tf_broadcaster.sendTransform(left_tf) + self.tf_broadcaster.sendTransform(right_tf) + + def get_command(self, pelvis_w, + foot_left_b, + foot_right_b, + ctarget_left_w, + ctarget_right_w): + ctarget_left_b_pos, ctarget_left_b_quat = subtract_frame_transforms(pelvis_w[:3], + pelvis_w[3:7], + ctarget_left_w[:3], + ctarget_left_w[3:7]) + ctarget_right_b_pos, ctarget_right_b_quat = subtract_frame_transforms(pelvis_w[:3], + pelvis_w[3:7], + ctarget_right_w[:3], + ctarget_right_w[3:7]) + pos_delta_left, axa_delta_left = compute_pose_error(foot_left_b[:3], + foot_left_b[3:7], + ctarget_left_b_pos, + ctarget_left_b_quat) + pos_delta_right, axa_delta_right = compute_pose_error(foot_right_b[:3], + foot_right_b[3:7], + ctarget_right_b_pos, + ctarget_right_b_quat) + return np.concatenate((pos_delta_left, axa_delta_left, pos_delta_right, axa_delta_right), axis=0) + + def run_policy(self): + if self._step_command is None: + + current_left_tf = self.tf_buffer.lookup_transform( + "world", + "left_ankle_roll_link", + rp.time.Time()) + # rp.duration.Duration(seconds=0.02)) + current_left_pose = self.tf_to_pose(current_left_tf, 'wxyz') + current_left_pose[2] = 0.0 + current_left_pose[3:7] = yaw_quat(current_left_pose[3:7]) + current_right_tf = self.tf_buffer.lookup_transform( + "world", + "right_ankle_roll_link", + rp.time.Time()) + # rp.duration.Duration(seconds=0.02)) + current_right_pose = self.tf_to_pose(current_right_tf, 'wxyz') + current_right_pose[2] = 0.0 + current_right_pose[3:7] = yaw_quat(current_right_pose[3:7]) + self._step_command = StepCommand(current_left_pose, current_right_pose) + + if self.remote_controller.button[KeyMap.select] == 1: + self._mode_change = True + self.mode = Mode.null + return + self.counter += 1 + # Get the current joint position and velocity + next_ctarget = self._step_command.get_next_ctarget( + self.remote_controller, + self.counter * self.config.control_dt) + print(next_ctarget) + next_ctarget_left, next_ctarget_right, dt_left, dt_right = next_ctarget + self.publish_step_command(next_ctarget_left, next_ctarget_right) + + for i in range(len(self.config.joint2motor_idx)): + self.qj[i] = self.low_state.motor_state[self.config.joint2motor_idx[i]].q + self.dqj[i] = self.low_state.motor_state[self.config.joint2motor_idx[i]].dq + + # imu_state quaternion: w, x, y, z + quat = self.low_state.imu_state.quaternion + ang_vel = np.array([self.low_state.imu_state.gyroscope], dtype=np.float32) + + if self.config.imu_type == "torso": + # h1 and h1_2 imu is on the torso + # imu data needs to be transformed to the pelvis frame + waist_yaw = self.low_state.motor_state[self.config.arm_waist_joint2motor_idx[0]].q + waist_yaw_omega = self.low_state.motor_state[self.config.arm_waist_joint2motor_idx[0]].dq + quat, ang_vel = transform_imu_data(waist_yaw=waist_yaw, waist_yaw_omega=waist_yaw_omega, imu_quat=quat, imu_omega=ang_vel) + + # create observation + gravity_orientation = get_gravity_orientation(quat) + qj_obs = self.qj.copy() + dqj_obs = self.dqj.copy() + qj_obs = (qj_obs - self.config.default_angles) * self.config.dof_pos_scale + dqj_obs = dqj_obs * self.config.dof_vel_scale + ang_vel = ang_vel * self.config.ang_vel_scale + + # foot pose + left_foot_from_base_tf = self.tf_buffer.lookup_transform( + "pelvis", + "left_ankle_roll_link", + rp.time.Time()) + right_foot_from_base_tf = self.tf_buffer.lookup_transform( + "pelvis", + "right_ankle_roll_link", + rp.time.Time()) + + lf_b = self.tf_to_pose(left_foot_from_base_tf, 'wxyz') + rf_b = self.tf_to_pose(right_foot_from_base_tf, 'wxyz') + left_foot_axa = wrap_to_pi(axis_angle_from_quat(lf_b[3:7])) + right_foot_axa = wrap_to_pi(axis_angle_from_quat(rf_b[3:7])) + rel_foot = np.concatenate((lf_b[:3], + rf_b[:3], + left_foot_axa, + right_foot_axa), axis=0) + # hand pose + left_hand_from_base_tf = self.tf_buffer.lookup_transform( + "pelvis", + "left_rubber_hand", + rp.time.Time()) + right_hand_from_base_tf = self.tf_buffer.lookup_transform( + "pelvis", + "right_rubber_hand", + rp.time.Time()) + lh_b = self.tf_to_pose(left_hand_from_base_tf, 'wxyz') + rh_b = self.tf_to_pose(right_hand_from_base_tf, 'wxyz') + left_hand_axa = wrap_to_pi(axis_angle_from_quat(lh_b[3:7])) + right_hand_axa = wrap_to_pi(axis_angle_from_quat(rh_b[3:7])) + rel_hand = np.concatenate((lh_b[:3], + rh_b[:3], + left_hand_axa, + right_hand_axa), axis=0) + + # foot command + base_pose_w = self.tf_to_pose(self.tf_buffer.lookup_transform( + "world", "pelvis", + rp.time.Time()), 'wxyz') + dt_left = dt_right = 0.0 + step_command = self.get_command(base_pose_w, + lf_b, + rf_b, + next_ctarget_left, + next_ctarget_right) + step_command = np.concatenate((step_command, + np.asarray([dt_left, dt_right])), axis=0) + + num_actions = self.config.num_actions + self.obs[:3] = ang_vel + self.obs[3:6] = gravity_orientation + self.obs[6:18] = rel_foot + self.obs[18:30] = rel_hand + self.obs[30 : 30 + num_actions] = qj_obs + self.obs[30 + num_actions : 30 + num_actions * 2] = dqj_obs + self.obs[30 + num_actions * 2 : 30 + num_actions * 3] = self.action + self.obs[30 + num_actions * 3 : 30 + num_actions * 3 + 14] = step_command + + # Get the action from the policy network + obs_tensor = torch.from_numpy(self.obs).unsqueeze(0) + obs_tensor[..., 30:30+num_actions] = obs_tensor[..., 30:30+num_actions] @ mapping_tensor.transpose(0, 1) + obs_tensor[..., 30 + num_actions : 30 + num_actions * 2] = obs_tensor[..., 30 + num_actions : 30 + num_actions * 2] @ mapping_tensor.transpose(0, 1) + obs_tensor[..., 30 + num_actions * 2 : 30 + num_actions * 3] = obs_tensor[..., 30 + num_actions * 2 : 30 + num_actions * 3] @ mapping_tensor.transpose(0, 1) + + # if not self._saved: + # torch.save(obs_tensor, "obs.pt") + # self._saved = True + self._obs_buf.append(obs_tensor.clone()) + + self.action = self.policy(obs_tensor).detach().numpy().squeeze() + # self.action = self.action * mask.numpy() + # Reorder the actions + self.action = self.action @ mapping_tensor.detach().cpu().numpy() + + # transform action to target_dof_pos + target_dof_pos = self.config.default_angles + self.action * self.config.action_scale *0.8 + + # Build low cmd + if True: + for i, motor_idx in enumerate(self.config.joint2motor_idx): + self.low_cmd.motor_cmd[motor_idx].q = float(target_dof_pos[i]) + self.low_cmd.motor_cmd[motor_idx].dq = 0.0 + self.low_cmd.motor_cmd[motor_idx].kp = float(self.config.kps[i]) + self.low_cmd.motor_cmd[motor_idx].kd = float(self.config.kds[i]) + self.low_cmd.motor_cmd[motor_idx].tau = 0.0 + # send the command + self.send_cmd(self.low_cmd) + + def run_wrapper(self): + # print("hello", self.mode, + # self.mode == Mode.zero_torque) + if self.mode == Mode.wait: + if self.low_state.crc != 0: + self.mode = Mode.zero_torque + self.low_cmd.mode_machine = self.mode_machine_ + print("Successfully connected to the robot.") + elif self.mode == Mode.zero_torque: + if self._mode_change: + print("Enter zero torque state.") + print("Waiting for the start signal...") + self._mode_change = False + self.zero_torque_state() + elif self.mode == Mode.default_pos: + if self._mode_change: + print("Moving to default pos.") + self._mode_change = False + self.prepare_default_pos() + self.move_to_default_pos() + elif self.mode == Mode.damping: + if self._mode_change: + print("Enter default pos state.") + print("Waiting for the Button A signal...") + try: + current_left_tf = self.tf_buffer.lookup_transform( + "world", + "left_ankle_roll_link", + rp.time.Time()) + self._mode_change = False + except Exception as ex: + print(ex) + self.default_pos_state() + elif self.mode == Mode.policy: + if self._mode_change: + print("Run policy.") + self._mode_change = False + self.counter = 0 + self.run_policy() + elif self.mode == Mode.null: + self._terminate = True + + # time.sleep(self.config.control_dt) + +if __name__ == "__main__": + import argparse + + parser = argparse.ArgumentParser() + parser.add_argument("config", type=str, help="config file name in the configs folder", default="g1.yaml") + args = parser.parse_args() + + # Load config + config_path = f"{LEGGED_GYM_ROOT_DIR}/deploy/deploy_real/configs/{args.config}" + config = Config(config_path) + + controller = Controller(config) diff --git a/deploy/deploy_real/fake_world_tf_pub.py b/deploy/deploy_real/fake_world_tf_pub.py new file mode 100755 index 0000000..c816f5d --- /dev/null +++ b/deploy/deploy_real/fake_world_tf_pub.py @@ -0,0 +1,119 @@ +#!/usr/bin/env python3 + +from pathlib import Path + +import rclpy +from rclpy.node import Node +from rclpy.qos import QoSProfile +from unitree_hg.msg import LowState as LowStateHG +from tf2_ros import TransformBroadcaster, TransformStamped + +import numpy as np +import pinocchio as pin +import pink +import yaml +from common.np_math import (index_map, with_dir) +from math_utils import (as_np, quat_rotate) + +quat_rotate = as_np(quat_rotate) + + +class FakeWorldPublisher(Node): + def __init__(self): + super().__init__('fake_world_publisher') + + + urdf_path = '../../resources/robots/g1_description/g1_29dof_with_hand_rev_1_0.urdf' + path = Path(urdf_path) + with with_dir(path.parent): + robot = pin.RobotWrapper.BuildFromURDF(filename=path.name, + package_dirs=["."], + root_joint=None) + self.robot = robot + + self.low_state = LowStateHG() + self.low_state_subscriber = self.create_subscription( + LowStateHG, + 'lowstate', + self.on_low_state, + 10) + self.tf_broadcaster = TransformBroadcaster(self) + + pin_joint = self.robot.model.names[1:] + with open('./configs/ik.yaml', 'r') as fp: + motor_joint = yaml.safe_load(fp)['motor_joint'] + + self.pin_from_mot = index_map( + pin_joint, motor_joint + ) + + def on_low_state(self, + msg: LowStateHG): + self.low_state = msg + + t = TransformStamped() + + # Read message content and assign it to + # corresponding tf variables + t.header.stamp = self.get_clock().now().to_msg() + t.header.frame_id = 'world' + t.child_frame_id = 'pelvis' + + # Turtle only exists in 2D, thus we get x and y translation + # coordinates from the message and set the z coordinate to 0 + t.transform.translation.x = 0.0 + t.transform.translation.y = 0.0 + t.transform.translation.z = self.pelvis_height(self.low_state) + + # Set world_from_pelvis quaternion based on IMU state + # TODO(ycho): consider applying 90-deg offset? + qw, qx, qy, qz = [ + float(x) for x in + self.low_state.imu_state.quaternion + ] + t.transform.rotation.x = qx + t.transform.rotation.y = qy + t.transform.rotation.z = qz + t.transform.rotation.w = qw + + # Send the transformation + self.tf_broadcaster.sendTransform(t) + + def pelvis_height(self, low_state: LowStateHG): + robot = self.robot + q_mot = [self.low_state.motor_state[i_mot].q for i_mot in range(29)] + q_pin = np.zeros_like(self.robot.q0) + q_pin[self.pin_from_mot] = q_mot + cfg = pink.Configuration(robot.model, robot.data, q_pin) + + pelvis_from_rf = cfg.get_transform_frame_to_world( + 'right_ankle_roll_link') + pelvis_from_lf = cfg.get_transform_frame_to_world( + 'left_ankle_roll_link') + + # NOTE(ycho): 0.02 = "roll_link" height (approx) + world_from_pelvis_quat = np.asarray(low_state.imu_state.quaternion, + dtype=np.float32) + xyz_rf = np.asarray( pelvis_from_rf.translation, + dtype=np.float32) + xyz_lf = np.asarray( pelvis_from_lf.translation, + dtype=np.float32) + + pelvis_z_rf = -quat_rotate( + world_from_pelvis_quat, xyz_rf)[2] + 0.02 + pelvis_z_lf = -quat_rotate( + world_from_pelvis_quat, xyz_lf)[2] + 0.02 + return 0.5 * pelvis_z_lf + 0.5 * pelvis_z_rf + + +def main(): + rclpy.init() + node = FakeWorldPublisher() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + rclpy.shutdown() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/deploy/deploy_real/ikctrl.py b/deploy/deploy_real/ikctrl.py new file mode 100644 index 0000000..beeb488 --- /dev/null +++ b/deploy/deploy_real/ikctrl.py @@ -0,0 +1,210 @@ +import os +from typing import Tuple +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 +from pink.tasks import FrameTask + + +@contextmanager +def with_dir(d): + d0 = os.getcwd() + try: + os.chdir(d) + yield + finally: + os.chdir(d0) + + +def xyzw2wxyz(q_xyzw: th.Tensor, dim: int = -1): + if isinstance(q_xyzw, np.ndarray): + return np.roll(q_xyzw, 1, axis=dim) + return th.roll(q_xyzw, 1, dims=dim) + + +def wxyz2xyzw(q_wxyz: th.Tensor, dim: int = -1): + if isinstance(q_wxyz, np.ndarray): + return np.roll(q_wxyz, -1, axis=dim) + return th.roll(q_wxyz, -1, dims=dim) + + +def dls_ik( + dpose: np.ndarray, + jac: np.ndarray, + sqlmda: float +): + """ + Arg: + dpose: task-space error (A[..., err]) + jac: jacobian (A[..., err, dof]) + sqlmda: DLS damping factor. + Return: + joint residual (A[..., dof]) + """ + if isinstance(dpose, tuple): + dpose = np.concatenate([dpose[0], dpose[1]], axis=-1) + J = jac + A = J @ J.T + # NOTE(ycho): add to view of diagonal + a = np.einsum('...ii->...i', A) + a += sqlmda + dq = (J.T @ np.linalg.solve(A, dpose[..., None]))[..., 0] + return dq + + +class IKCtrl: + def __init__(self, + urdf_path: str, + act_joints: Tuple[str, ...], + frame: str = 'left_hand_palm_link', + sqlmda: float = 0.05**2): + path = Path(urdf_path) + with with_dir(path.parent): + robot = pin.RobotWrapper.BuildFromURDF(filename=path.name, + package_dirs=["."], + root_joint=None) + self.robot = robot + # NOTE(ycho): we skip joint#0(="universe") + joint_names = list(self.robot.model.names) + assert (joint_names[0] == 'universe') + self.joint_names = joint_names[1:] + + # NOTE(ycho): build index map between pin.q and other set(s) of ordered + # joints. + 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 + ): + """ + Arg: + q0: Current robot joints; A[..., 43?] + target_pose: + Policy output. A[..., 7] formatted as (xyz, q_{wxyz}) + Given as world frame absolute pose, for some reason. + Return: + joint residual: A[..., 7] + """ + robot = self.robot + + # source pose + self.cfg.update(q0) + T0 = self.cfg.get_transform_frame_to_world(self.frame) + + # target pose + dst_xyz = target_pose[..., 0:3] + dst_quat = pin.Quaternion(wxyz2xyzw(target_pose[..., 3:7])) + T1 = pin.SE3(dst_quat, dst_xyz) + if rel: + TL = pin.SE3.Identity() + TL.translation = dst_xyz + TR = pin.SE3.Identity() + TR.rotation = dst_quat.toRotationMatrix() + T1 = TL * T0 * TR + + # jacobian + self.task.set_target(T0) + jac = self.task.compute_jacobian(self.cfg) + jac = jac[:, self.act_from_pin] + + # error&ik + dT = T1.actInv(T0) + dpose = pin.log(dT).vector + dq = dls_ik(dpose, jac, self.sqlmda) + return dq + + +def main(): + from yourdfpy import URDF + 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() diff --git a/deploy/deploy_real/localization/__init__.py b/deploy/deploy_real/localization/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/deploy/deploy_real/localization/pelvis_pub.py b/deploy/deploy_real/localization/pelvis_pub.py new file mode 100644 index 0000000..aec1e8c --- /dev/null +++ b/deploy/deploy_real/localization/pelvis_pub.py @@ -0,0 +1,205 @@ +#!/usr/bin/env python3 + +from pathlib import Path + +import rclpy +from rclpy.node import Node +from rclpy.qos import QoSProfile +from unitree_hg.msg import LowState as LowStateHG +from tf2_ros.buffer import Buffer +from tf2_ros.transform_listener import TransformListener +from tf2_ros import TransformBroadcaster, TransformStamped, StaticTransformBroadcaster + +import numpy as np +import yaml +from geometry_msgs.msg import Vector3, Quaternion + +from scipy.spatial.transform import Rotation as R + +def index_map(k_to, k_from): + """ + Returns an index mapping from k_from to k_to. + + Given k_to=a, k_from=b, + returns an index map "a_from_b" such that + array_a[a_from_b] = array_b + + Missing values are set to -1. + """ + index_dict = {k: i for i, k in enumerate(k_to)} # O(len(k_from)) + return [index_dict.get(k, -1) for k in k_from] # O(len(k_to)) + +def quat_rotate(q: np.ndarray, v: np.ndarray) -> np.ndarray: + """Rotate a vector by a quaternion along the last dimension of q and v. + + Args: + q: The quaternion in (w, x, y, z). Shape is (..., 4). + v: The vector in (x, y, z). Shape is (..., 3). + + Returns: + The rotated vector in (x, y, z). Shape is (..., 3). + """ + q_w = q[..., 0] + q_vec = q[..., 1:] + a = v * (2.0 * q_w**2 - 1.0)[..., None] + b = np.cross(q_vec, v, axis=-1) * q_w[..., None] * 2.0 + c = q_vec * np.einsum("...i,...i->...", q_vec, v)[..., None] * 2.0 + return a + b + c + +def to_array(v): + if isinstance(v, Vector3): + return np.array([v.x, v.y, v.z], dtype=np.float32) + elif isinstance(v, Quaternion): + return np.array([v.x, v.y, v.z, v.w], dtype=np.float32) + +class PelvistoTrack(Node): + def __init__(self): + super().__init__('pelvis_track_publisher') + + self.tf_buffer = Buffer() + self.tf_listener = TransformListener(self.tf_buffer, self) + self.tf_broadcaster = TransformBroadcaster(self) + + self.low_state = LowStateHG() + self.low_state_subscriber = self.create_subscription( + LowStateHG, + 'lowstate', + self.on_low_state, + 10) + + self.static_tf_broadcaster = StaticTransformBroadcaster(self) + + # Timer for dynamic transform broadcasting (e.g., pelvis tracking) + self.timer = self.create_timer(0.01, self.on_timer) + # One-shot timer to check & publish the static transform after a short delay + self.static_tf_timer = self.create_timer(1.0, self.publish_static_tf) + + def on_low_state(self, + msg: LowStateHG): + self.low_state = msg + + def on_timer(self): + try: + self.tf_buffer.lookup_transform( + "world", "camera_init", rclpy.time.Time() + ) + except Exception as ex: + print(f'Could not transform mid360_link_IMU to pelvis as world to camera_init is yet published: {ex}') + try: + t = TransformStamped() + + # Read message content and assign it to + # corresponding tf variables + + t_lidar_pelvis = self.tf_buffer.lookup_transform( + 'mid360_link_IMU', + # 'zed2_camera_center', + 'pelvis', rclpy.time.Time() + ) + + t.header.stamp = self.get_clock().now().to_msg() + # t.header.frame_id = 'zed_camera_center' + t.header.frame_id = 'body' + t.child_frame_id = 'pelvis' + + # Turtle only exists in 2D, thus we get x and y translation + # coordinates from the message and set the z coordinate to 0 + t.transform.translation.x = t_lidar_pelvis.transform.translation.x + t.transform.translation.y = t_lidar_pelvis.transform.translation.y + t.transform.translation.z = t_lidar_pelvis.transform.translation.z + + t.transform.rotation.x = t_lidar_pelvis.transform.rotation.x + t.transform.rotation.y = t_lidar_pelvis.transform.rotation.y + t.transform.rotation.z = t_lidar_pelvis.transform.rotation.z + t.transform.rotation.w = t_lidar_pelvis.transform.rotation.w + + # Send the transformation + self.tf_broadcaster.sendTransform(t) + except Exception as ex: + print(f'Could not transform mid360_link_IMU to pelvis: {ex}') + + def publish_static_tf(self): + """Check if a static transform from 'world' to 'camera_init' exists. + If not, publish it using the parameter 'camera_init_z' for the z-value. + This method is designed to run only once. + """ + # Cancel the timer so this callback runs only one time. + if self.low_state.crc == 0: + return + self.static_tf_timer.cancel() + + try: + # Try to look up an existing transform from "world" to "camera_init". + # Here, rclpy.time.Time() (i.e. time=0) means "the latest available". + self.tf_buffer.lookup_transform( + "world", "camera_init", rclpy.time.Time() + ) + self.get_logger().info( + "Static transform from 'world' to 'camera_init' already exists. Not publishing a new one." + ) + except Exception as ex: + # If the transform isn't found, declare (or get) the parameter for z and publish the static transform. + z_value, rot = self.lidar_height_rot(self.low_state) + static_tf = TransformStamped() + static_tf.header.stamp = self.get_clock().now().to_msg() + static_tf.header.frame_id = "world" + static_tf.child_frame_id = "camera_init" + # static_tf.child_frame_id = "pelvis" + + static_tf.transform.translation.x = 0.0 + static_tf.transform.translation.y = 0.0 + static_tf.transform.translation.z = z_value + static_tf.transform.rotation.x = float(rot[0]) + static_tf.transform.rotation.y = float(rot[1]) + static_tf.transform.rotation.z = float(rot[2]) + static_tf.transform.rotation.w = float(rot[3]) + + self.static_tf_broadcaster.sendTransform(static_tf) + self.get_logger().info( + f"Published static transform from 'world' to 'camera_init' with z = {z_value} quat = {rot}" + ) + + def lidar_height_rot(self, low_state: LowStateHG): + print(self.tf_buffer.lookup_transform('pelvis', + 'left_ankle_roll_link', rclpy.time.Time())) + + world_from_pelvis_quat = np.asarray(low_state.imu_state.quaternion, + dtype=np.float32) + pelvis_from_rf = self.tf_buffer.lookup_transform('pelvis', + 'right_ankle_roll_link', rclpy.time.Time()) + pelvis_from_lf = self.tf_buffer.lookup_transform('pelvis', + 'left_ankle_roll_link', rclpy.time.Time()) + xyz_rf = to_array(pelvis_from_rf.transform.translation) + xyz_lf = to_array(pelvis_from_rf.transform.translation) + + pelvis_z_rf = -quat_rotate( + world_from_pelvis_quat, xyz_rf)[2] + 0.028531 + pelvis_z_lf = -quat_rotate( + world_from_pelvis_quat, xyz_lf)[2] + 0.028531 + # print(xyz_lf) + lidar_from_pelvis = self.tf_buffer.lookup_transform('pelvis', + 'mid360_link_frame', rclpy.time.Time()) + # print(to_array(lidar_from_pelvis.transform.rotation), + # world_from_pelvis_quat) + lidar_z_pevlis = quat_rotate(world_from_pelvis_quat, + to_array(lidar_from_pelvis.transform.translation))[2] + lidar_rot = (R.from_quat(np.roll(world_from_pelvis_quat, -1)) * + R.from_quat(to_array(lidar_from_pelvis.transform.rotation))) + return (0.5 * pelvis_z_lf + 0.5 * pelvis_z_rf + lidar_z_pevlis, + # lidar_rot.as_quat()) + # np.roll(world_from_pelvis_quat, -1)) + # to_array(lidar_from_pelvis.transform.rotation)) + lidar_rot.as_quat()) + + +def main(): + rclpy.init() + node = PelvistoTrack() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + rclpy.shutdown() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/deploy/deploy_real/localization/state_pub.launch.py b/deploy/deploy_real/localization/state_pub.launch.py new file mode 100644 index 0000000..42f3de0 --- /dev/null +++ b/deploy/deploy_real/localization/state_pub.launch.py @@ -0,0 +1,27 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + +def generate_launch_description(): + + use_sim_time = LaunchConfiguration('use_sim_time', default='false') + + urdf = '../../resources/robots/g1_description/g1_29dof_rev_1_0_lidar.urdf' + with open(urdf, 'r') as infp: + robot_desc = infp.read() + return LaunchDescription([ + DeclareLaunchArgument( + 'use_sim_time', + default_value='false', + description='Use simulation (Gazebo) clock if true'), + Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + output='screen', + parameters=[{'use_sim_time': use_sim_time, 'robot_description': robot_desc}], + arguments=[urdf]), + ]) \ No newline at end of file diff --git a/deploy/deploy_real/localization/state_pub.py b/deploy/deploy_real/localization/state_pub.py new file mode 100644 index 0000000..ee31811 --- /dev/null +++ b/deploy/deploy_real/localization/state_pub.py @@ -0,0 +1,64 @@ +from math import sin, cos, pi +import rclpy +from rclpy.node import Node +from rclpy.qos import QoSProfile +from geometry_msgs.msg import Quaternion +from sensor_msgs.msg import JointState +# from tf2_ros import TransformBroadcaster, TransformStamped +from unitree_hg.msg import LowCmd as LowCmdHG, LowState as LowStateHG +import yaml + +class StatePublisher(Node): + + def __init__(self): + super().__init__('state_publisher') + qos_profile = QoSProfile(depth=10) + + with open('./configs/g1.yaml', 'r') as fp: + self.joint_names = yaml.safe_load(fp)['raw_joint_order'] + + self.low_state = LowStateHG() + self.low_state_subscriber = self.create_subscription(LowStateHG, + 'lowstate', self.on_low_state, 10) + self.joint_pub = self.create_publisher(JointState, + 'joint_states', qos_profile) + self.nodeName = self.get_name() + self.get_logger().info("{0} started".format(self.nodeName)) + self.joint_state = JointState() + + def on_low_state(self, msg: LowStateHG): + self.low_state = msg + joint_state = self.joint_state + now = self.get_clock().now() + joint_state.header.stamp = now.to_msg() + joint_state.name = self.joint_names + joint_state.position = [0.0 for _ in self.joint_names] + joint_state.velocity = [0.0 for _ in self.joint_names] + + n:int = min(len(self.joint_names), len(self.low_state.motor_state)) + for i in range(n): + joint_state.position[i] = self.low_state.motor_state[i].q + joint_state.velocity[i] = self.low_state.motor_state[i].dq + self.joint_pub.publish(joint_state) + + def run(self): + loop_rate = self.create_rate(10) + try: + # rclpy.spin() + while rclpy.ok(): + rclpy.spin_once(self) + loop_rate.sleep() + except KeyboardInterrupt: + pass + + + +def main(): + rclpy.init() + node = StatePublisher() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/deploy/deploy_real/math_utils.py b/deploy/deploy_real/math_utils.py new file mode 100644 index 0000000..e48f82b --- /dev/null +++ b/deploy/deploy_real/math_utils.py @@ -0,0 +1,1626 @@ +# Copyright (c) 2022-2024, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Sub-module containing utilities for various math operations.""" + +# needed to import for allowing type-hinting: torch.Tensor | np.ndarray +from __future__ import annotations + +import numpy as np +import torch +import torch.nn.functional +import torch as th +from typing import Literal, Mapping, Iterable +from functools import wraps + +""" +General +""" + +def th2np(x): + if isinstance(x, np.ndarray): + return x + + if isinstance(x, th.Tensor): + return x.numpy() + + if isinstance(x, Mapping): + return {k: th2np(v) for (k,v) in x.items()} + + if isinstance(x, Iterable): + return [th2np(e) for e in x] + return x + +def np2th(x): + if isinstance(x, th.Tensor): + return x + + if isinstance(x, np.ndarray): + return th.from_numpy(x) + + if isinstance(x, Mapping): + return {k: np2th(v) for (k,v) in x.items()} + + if isinstance(x, Iterable): + return [np2th(e) for e in x] + + return x + +def as_np(func): + @wraps(func) + def np_func(*args, **kwds): + th_args = np2th(args) + th_kwds = np2th(kwds) + th_out = func(*th_args, **th_kwds) + # TODO(ycho): consider interoperable functions, + # i.e., numpy in -> numpy out; torch in -> torch out + np_out = th2np(th_out) + return np_out + return np_func + +@torch.jit.script +def scale_transform( + x: torch.Tensor, lower: torch.Tensor, upper: torch.Tensor +) -> torch.Tensor: + """Normalizes a given input tensor to a range of [-1, 1]. + + .. note:: + It uses pytorch broadcasting functionality to deal with batched input. + + Args: + x: Input tensor of shape (N, dims). + lower: The minimum value of the tensor. Shape is (N, dims) or (dims,). + upper: The maximum value of the tensor. Shape is (N, dims) or (dims,). + + Returns: + Normalized transform of the tensor. Shape is (N, dims). + """ + # default value of center + offset = (lower + upper) * 0.5 + # return normalized tensor + return 2 * (x - offset) / (upper - lower) + + +@torch.jit.script +def unscale_transform( + x: torch.Tensor, lower: torch.Tensor, upper: torch.Tensor +) -> torch.Tensor: + """De-normalizes a given input tensor from range of [-1, 1] to (lower, upper). + + .. note:: + It uses pytorch broadcasting functionality to deal with batched input. + + Args: + x: Input tensor of shape (N, dims). + lower: The minimum value of the tensor. Shape is (N, dims) or (dims,). + upper: The maximum value of the tensor. Shape is (N, dims) or (dims,). + + Returns: + De-normalized transform of the tensor. Shape is (N, dims). + """ + # default value of center + offset = (lower + upper) * 0.5 + # return normalized tensor + return x * (upper - lower) * 0.5 + offset + + +@torch.jit.script +def saturate(x: torch.Tensor, lower: torch.Tensor, upper: torch.Tensor) -> torch.Tensor: + """Clamps a given input tensor to (lower, upper). + + It uses pytorch broadcasting functionality to deal with batched input. + + Args: + x: Input tensor of shape (N, dims). + lower: The minimum value of the tensor. Shape is (N, dims) or (dims,). + upper: The maximum value of the tensor. Shape is (N, dims) or (dims,). + + Returns: + Clamped transform of the tensor. Shape is (N, dims). + """ + return torch.max(torch.min(x, upper), lower) + + +@torch.jit.script +def normalize(x: torch.Tensor, eps: float = 1e-9) -> torch.Tensor: + """Normalizes a given input tensor to unit length. + + Args: + x: Input tensor of shape (N, dims). + eps: A small value to avoid division by zero. Defaults to 1e-9. + + Returns: + Normalized tensor of shape (N, dims). + """ + return x / x.norm(p=2, dim=-1).clamp(min=eps, max=None).unsqueeze(-1) + + +@torch.jit.script +def wrap_to_pi(angles: torch.Tensor) -> torch.Tensor: + r"""Wraps input angles (in radians) to the range :math:`[-\pi, \pi]`. + + This function wraps angles in radians to the range :math:`[-\pi, \pi]`, such that + :math:`\pi` maps to :math:`\pi`, and :math:`-\pi` maps to :math:`-\pi`. In general, + odd positive multiples of :math:`\pi` are mapped to :math:`\pi`, and odd negative + multiples of :math:`\pi` are mapped to :math:`-\pi`. + + The function behaves similar to MATLAB's `wrapToPi `_ + function. + + Args: + angles: Input angles of any shape. + + Returns: + Angles in the range :math:`[-\pi, \pi]`. + """ + # wrap to [0, 2*pi) + wrapped_angle = (angles + torch.pi) % (2 * torch.pi) + # map to [-pi, pi] + # we check for zero in wrapped angle to make it go to pi when input angle is odd multiple of pi + return torch.where( + (wrapped_angle == 0) & (angles > 0), torch.pi, wrapped_angle - torch.pi + ) + + +@torch.jit.script +def copysign(mag: float, other: torch.Tensor) -> torch.Tensor: + """Create a new floating-point tensor with the magnitude of input and the sign of other, element-wise. + + Note: + The implementation follows from `torch.copysign`. The function allows a scalar magnitude. + + Args: + mag: The magnitude scalar. + other: The tensor containing values whose signbits are applied to magnitude. + + Returns: + The output tensor. + """ + mag_torch = torch.tensor(mag, device=other.device, dtype=torch.float).repeat( + other.shape[0] + ) + return torch.abs(mag_torch) * torch.sign(other) + + +""" +Rotation +""" + + +@torch.jit.script +def matrix_from_quat(quaternions: torch.Tensor) -> torch.Tensor: + """Convert rotations given as quaternions to rotation matrices. + + Args: + quaternions: The quaternion orientation in (w, x, y, z). Shape is (..., 4). + + Returns: + Rotation matrices. The shape is (..., 3, 3). + + Reference: + https://github.com/facebookresearch/pytorch3d/blob/main/pytorch3d/transforms/rotation_conversions.py#L41-L70 + """ + r, i, j, k = torch.unbind(quaternions, -1) + # pyre-fixme[58]: `/` is not supported for operand types `float` and `Tensor`. + two_s = 2.0 / (quaternions * quaternions).sum(-1) + + o = torch.stack( + ( + 1 - two_s * (j * j + k * k), + two_s * (i * j - k * r), + two_s * (i * k + j * r), + two_s * (i * j + k * r), + 1 - two_s * (i * i + k * k), + two_s * (j * k - i * r), + two_s * (i * k - j * r), + two_s * (j * k + i * r), + 1 - two_s * (i * i + j * j), + ), + -1, + ) + return o.reshape(quaternions.shape[:-1] + (3, 3)) + + +def convert_quat( + quat: torch.Tensor | np.ndarray, to: Literal["xyzw", "wxyz"] = "xyzw" +) -> torch.Tensor | np.ndarray: + """Converts quaternion from one convention to another. + + The convention to convert TO is specified as an optional argument. If to == 'xyzw', + then the input is in 'wxyz' format, and vice-versa. + + Args: + quat: The quaternion of shape (..., 4). + to: Convention to convert quaternion to.. Defaults to "xyzw". + + Returns: + The converted quaternion in specified convention. + + Raises: + ValueError: Invalid input argument `to`, i.e. not "xyzw" or "wxyz". + ValueError: Invalid shape of input `quat`, i.e. not (..., 4,). + """ + # check input is correct + if quat.shape[-1] != 4: + msg = f"Expected input quaternion shape mismatch: {quat.shape} != (..., 4)." + raise ValueError(msg) + if to not in ["xyzw", "wxyz"]: + msg = f"Expected input argument `to` to be 'xyzw' or 'wxyz'. Received: {to}." + raise ValueError(msg) + # check if input is numpy array (we support this backend since some classes use numpy) + if isinstance(quat, np.ndarray): + # use numpy functions + if to == "xyzw": + # wxyz -> xyzw + return np.roll(quat, -1, axis=-1) + else: + # xyzw -> wxyz + return np.roll(quat, 1, axis=-1) + else: + # convert to torch (sanity check) + if not isinstance(quat, torch.Tensor): + quat = torch.tensor(quat, dtype=float) + # convert to specified quaternion type + if to == "xyzw": + # wxyz -> xyzw + return quat.roll(-1, dims=-1) + else: + # xyzw -> wxyz + return quat.roll(1, dims=-1) + + +@torch.jit.script +def quat_conjugate(q: torch.Tensor) -> torch.Tensor: + """Computes the conjugate of a quaternion. + + Args: + q: The quaternion orientation in (w, x, y, z). Shape is (..., 4). + + Returns: + The conjugate quaternion in (w, x, y, z). Shape is (..., 4). + """ + shape = q.shape + q = q.reshape(-1, 4) + return torch.cat((q[:, 0:1], -q[:, 1:]), dim=-1).view(shape) + + +@torch.jit.script +def quat_inv(q: torch.Tensor) -> torch.Tensor: + """Compute the inverse of a quaternion. + + Args: + q: The quaternion orientation in (w, x, y, z). Shape is (N, 4). + + Returns: + The inverse quaternion in (w, x, y, z). Shape is (N, 4). + """ + return normalize(quat_conjugate(q)) + + +@torch.jit.script +def quat_from_euler_xyz( + roll: torch.Tensor, pitch: torch.Tensor, yaw: torch.Tensor +) -> torch.Tensor: + """Convert rotations given as Euler angles in radians to Quaternions. + + Note: + The euler angles are assumed in XYZ convention. + + Args: + roll: Rotation around x-axis (in radians). Shape is (N,). + pitch: Rotation around y-axis (in radians). Shape is (N,). + yaw: Rotation around z-axis (in radians). Shape is (N,). + + Returns: + The quaternion in (w, x, y, z). Shape is (N, 4). + """ + cy = torch.cos(yaw * 0.5) + sy = torch.sin(yaw * 0.5) + cr = torch.cos(roll * 0.5) + sr = torch.sin(roll * 0.5) + cp = torch.cos(pitch * 0.5) + sp = torch.sin(pitch * 0.5) + # compute quaternion + qw = cy * cr * cp + sy * sr * sp + qx = cy * sr * cp - sy * cr * sp + qy = cy * cr * sp + sy * sr * cp + qz = sy * cr * cp - cy * sr * sp + + return torch.stack([qw, qx, qy, qz], dim=-1) + + +@torch.jit.script +def _sqrt_positive_part(x: torch.Tensor) -> torch.Tensor: + """Returns torch.sqrt(torch.max(0, x)) but with a zero sub-gradient where x is 0. + + Reference: + https://github.com/facebookresearch/pytorch3d/blob/main/pytorch3d/transforms/rotation_conversions.py#L91-L99 + """ + ret = torch.zeros_like(x) + positive_mask = x > 0 + ret[positive_mask] = torch.sqrt(x[positive_mask]) + return ret + + +@torch.jit.script +def quat_from_matrix(matrix: torch.Tensor) -> torch.Tensor: + """Convert rotations given as rotation matrices to quaternions. + + Args: + matrix: The rotation matrices. Shape is (..., 3, 3). + + Returns: + The quaternion in (w, x, y, z). Shape is (..., 4). + + Reference: + https://github.com/facebookresearch/pytorch3d/blob/main/pytorch3d/transforms/rotation_conversions.py#L102-L161 + """ + if matrix.size(-1) != 3 or matrix.size(-2) != 3: + raise ValueError(f"Invalid rotation matrix shape {matrix.shape}.") + + batch_dim = matrix.shape[:-2] + m00, m01, m02, m10, m11, m12, m20, m21, m22 = torch.unbind( + matrix.reshape(batch_dim + (9,)), dim=-1 + ) + + q_abs = _sqrt_positive_part( + torch.stack( + [ + 1.0 + m00 + m11 + m22, + 1.0 + m00 - m11 - m22, + 1.0 - m00 + m11 - m22, + 1.0 - m00 - m11 + m22, + ], + dim=-1, + ) + ) + + # we produce the desired quaternion multiplied by each of r, i, j, k + quat_by_rijk = torch.stack( + [ + # pyre-fixme[58]: `**` is not supported for operand types `Tensor` and `int`. + torch.stack([q_abs[..., 0] ** 2, m21 - m12, m02 - m20, m10 - m01], dim=-1), + # pyre-fixme[58]: `**` is not supported for operand types `Tensor` and `int`. + torch.stack([m21 - m12, q_abs[..., 1] ** 2, m10 + m01, m02 + m20], dim=-1), + # pyre-fixme[58]: `**` is not supported for operand types `Tensor` and `int`. + torch.stack([m02 - m20, m10 + m01, q_abs[..., 2] ** 2, m12 + m21], dim=-1), + # pyre-fixme[58]: `**` is not supported for operand types `Tensor` and `int`. + torch.stack([m10 - m01, m20 + m02, m21 + m12, q_abs[..., 3] ** 2], dim=-1), + ], + dim=-2, + ) + + # We floor here at 0.1 but the exact level is not important; if q_abs is small, + # the candidate won't be picked. + flr = torch.tensor(0.1).to(dtype=q_abs.dtype, device=q_abs.device) + quat_candidates = quat_by_rijk / (2.0 * q_abs[..., None].max(flr)) + + # if not for numerical problems, quat_candidates[i] should be same (up to a sign), + # forall i; we pick the best-conditioned one (with the largest denominator) + return quat_candidates[ + torch.nn.functional.one_hot(q_abs.argmax(dim=-1), num_classes=4) > 0.5, : + ].reshape(batch_dim + (4,)) + + +def _axis_angle_rotation( + axis: Literal["X", "Y", "Z"], angle: torch.Tensor +) -> torch.Tensor: + """Return the rotation matrices for one of the rotations about an axis of which Euler angles describe, + for each value of the angle given. + + Args: + axis: Axis label "X" or "Y or "Z". + angle: Euler angles in radians of any shape. + + Returns: + Rotation matrices. Shape is (..., 3, 3). + + Reference: + https://github.com/facebookresearch/pytorch3d/blob/main/pytorch3d/transforms/rotation_conversions.py#L164-L191 + """ + cos = torch.cos(angle) + sin = torch.sin(angle) + one = torch.ones_like(angle) + zero = torch.zeros_like(angle) + + if axis == "X": + R_flat = (one, zero, zero, zero, cos, -sin, zero, sin, cos) + elif axis == "Y": + R_flat = (cos, zero, sin, zero, one, zero, -sin, zero, cos) + elif axis == "Z": + R_flat = (cos, -sin, zero, sin, cos, zero, zero, zero, one) + else: + raise ValueError("letter must be either X, Y or Z.") + + return torch.stack(R_flat, -1).reshape(angle.shape + (3, 3)) + + +def matrix_from_euler(euler_angles: torch.Tensor, convention: str) -> torch.Tensor: + """ + Convert rotations given as Euler angles in radians to rotation matrices. + + Args: + euler_angles: Euler angles in radians. Shape is (..., 3). + convention: Convention string of three uppercase letters from {"X", "Y", and "Z"}. + For example, "XYZ" means that the rotations should be applied first about x, + then y, then z. + + Returns: + Rotation matrices. Shape is (..., 3, 3). + + Reference: + https://github.com/facebookresearch/pytorch3d/blob/main/pytorch3d/transforms/rotation_conversions.py#L194-L220 + """ + if euler_angles.dim() == 0 or euler_angles.shape[-1] != 3: + raise ValueError("Invalid input euler angles.") + if len(convention) != 3: + raise ValueError("Convention must have 3 letters.") + if convention[1] in (convention[0], convention[2]): + raise ValueError(f"Invalid convention {convention}.") + for letter in convention: + if letter not in ("X", "Y", "Z"): + raise ValueError(f"Invalid letter {letter} in convention string.") + matrices = [ + _axis_angle_rotation(c, e) + for c, e in zip(convention, torch.unbind(euler_angles, -1)) + ] + # return functools.reduce(torch.matmul, matrices) + return torch.matmul(torch.matmul(matrices[0], matrices[1]), matrices[2]) + + +@torch.jit.script +def euler_xyz_from_quat( + quat: torch.Tensor, +) -> tuple[torch.Tensor, torch.Tensor, torch.Tensor]: + """Convert rotations given as quaternions to Euler angles in radians. + + Note: + The euler angles are assumed in XYZ convention. + + Args: + quat: The quaternion orientation in (w, x, y, z). Shape is (N, 4). + + Returns: + A tuple containing roll-pitch-yaw. Each element is a tensor of shape (N,). + + Reference: + https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles + """ + q_w, q_x, q_y, q_z = quat[:, 0], quat[:, 1], quat[:, 2], quat[:, 3] + # roll (x-axis rotation) + sin_roll = 2.0 * (q_w * q_x + q_y * q_z) + cos_roll = 1 - 2 * (q_x * q_x + q_y * q_y) + roll = torch.atan2(sin_roll, cos_roll) + + # pitch (y-axis rotation) + sin_pitch = 2.0 * (q_w * q_y - q_z * q_x) + pitch = torch.where( + torch.abs(sin_pitch) >= 1, + copysign(torch.pi / 2.0, sin_pitch), + torch.asin(sin_pitch), + ) + + # yaw (z-axis rotation) + sin_yaw = 2.0 * (q_w * q_z + q_x * q_y) + cos_yaw = 1 - 2 * (q_y * q_y + q_z * q_z) + yaw = torch.atan2(sin_yaw, cos_yaw) + + return ( + roll % (2 * torch.pi), + pitch % (2 * torch.pi), + yaw % (2 * torch.pi), + ) # TODO: why not wrap_to_pi here ? + + +@torch.jit.script +def quat_unique(q: torch.Tensor) -> torch.Tensor: + """Convert a unit quaternion to a standard form where the real part is non-negative. + + Quaternion representations have a singularity since ``q`` and ``-q`` represent the same + rotation. This function ensures the real part of the quaternion is non-negative. + + Args: + q: The quaternion orientation in (w, x, y, z). Shape is (..., 4). + + Returns: + Standardized quaternions. Shape is (..., 4). + """ + return torch.where(q[..., 0:1] < 0, -q, q) + + +@torch.jit.script +def quat_mul(q1: torch.Tensor, q2: torch.Tensor) -> torch.Tensor: + """Multiply two quaternions together. + + Args: + q1: The first quaternion in (w, x, y, z). Shape is (..., 4). + q2: The second quaternion in (w, x, y, z). Shape is (..., 4). + + Returns: + The product of the two quaternions in (w, x, y, z). Shape is (..., 4). + + Raises: + ValueError: Input shapes of ``q1`` and ``q2`` are not matching. + """ + # check input is correct + if q1.shape != q2.shape: + msg = f"Expected input quaternion shape mismatch: {q1.shape} != {q2.shape}." + raise ValueError(msg) + # reshape to (N, 4) for multiplication + shape = q1.shape + q1 = q1.reshape(-1, 4) + q2 = q2.reshape(-1, 4) + # extract components from quaternions + w1, x1, y1, z1 = q1[:, 0], q1[:, 1], q1[:, 2], q1[:, 3] + w2, x2, y2, z2 = q2[:, 0], q2[:, 1], q2[:, 2], q2[:, 3] + # perform multiplication + ww = (z1 + x1) * (x2 + y2) + yy = (w1 - y1) * (w2 + z2) + zz = (w1 + y1) * (w2 - z2) + xx = ww + yy + zz + qq = 0.5 * (xx + (z1 - x1) * (x2 - y2)) + w = qq - ww + (z1 - y1) * (y2 - z2) + x = qq - xx + (x1 + w1) * (x2 + w2) + y = qq - yy + (w1 - x1) * (y2 + z2) + z = qq - zz + (z1 + y1) * (w2 - x2) + + return torch.stack([w, x, y, z], dim=-1).view(shape) + + +@torch.jit.script +def quat_box_minus(q1: torch.Tensor, q2: torch.Tensor) -> torch.Tensor: + """The box-minus operator (quaternion difference) between two quaternions. + + Args: + q1: The first quaternion in (w, x, y, z). Shape is (N, 4). + q2: The second quaternion in (w, x, y, z). Shape is (N, 4). + + Returns: + The difference between the two quaternions. Shape is (N, 3). + """ + quat_diff = quat_mul(q1, quat_conjugate(q2)) # q1 * q2^-1 + re = quat_diff[:, 0] # real part, q = [w, x, y, z] = [re, im] + im = quat_diff[:, 1:] # imaginary part + norm_im = torch.norm(im, dim=1) + scale = 2.0 * torch.where( + norm_im > 1.0e-7, torch.atan2(norm_im, re) / norm_im, torch.sign(re) + ) + return scale.unsqueeze(-1) * im + + +@torch.jit.script +def yaw_quat(quat: torch.Tensor) -> torch.Tensor: + """Extract the yaw component of a quaternion. + + Args: + quat: The orientation in (w, x, y, z). Shape is (..., 4) + + Returns: + A quaternion with only yaw component. + """ + shape = quat.shape + quat_yaw = quat.clone().view(-1, 4) + qw = quat_yaw[:, 0] + qx = quat_yaw[:, 1] + qy = quat_yaw[:, 2] + qz = quat_yaw[:, 3] + yaw = torch.atan2(2 * (qw * qz + qx * qy), 1 - 2 * (qy * qy + qz * qz)) + quat_yaw[:] = 0.0 + quat_yaw[:, 3] = torch.sin(yaw / 2) + quat_yaw[:, 0] = torch.cos(yaw / 2) + quat_yaw = normalize(quat_yaw) + return quat_yaw.view(shape) + + +@torch.jit.script +def quat_apply(quat: torch.Tensor, vec: torch.Tensor) -> torch.Tensor: + """Apply a quaternion rotation to a vector. + + Args: + quat: The quaternion in (w, x, y, z). Shape is (..., 4). + vec: The vector in (x, y, z). Shape is (..., 3). + + Returns: + The rotated vector in (x, y, z). Shape is (..., 3). + """ + # store shape + shape = vec.shape + # reshape to (N, 3) for multiplication + quat = quat.reshape(-1, 4) + vec = vec.reshape(-1, 3) + # extract components from quaternions + xyz = quat[:, 1:] + t = xyz.cross(vec, dim=-1) * 2 + return (vec + quat[:, 0:1] * t + xyz.cross(t, dim=-1)).view(shape) + + +@torch.jit.script +def quat_apply_yaw(quat: torch.Tensor, vec: torch.Tensor) -> torch.Tensor: + """Rotate a vector only around the yaw-direction. + + Args: + quat: The orientation in (w, x, y, z). Shape is (N, 4). + vec: The vector in (x, y, z). Shape is (N, 3). + + Returns: + The rotated vector in (x, y, z). Shape is (N, 3). + """ + quat_yaw = yaw_quat(quat) + return quat_apply(quat_yaw, vec) + + +@torch.jit.script +def quat_rotate(q: torch.Tensor, v: torch.Tensor) -> torch.Tensor: + """Rotate a vector by a quaternion along the last dimension of q and v. + + Args: + q: The quaternion in (w, x, y, z). Shape is (..., 4). + v: The vector in (x, y, z). Shape is (..., 3). + + Returns: + The rotated vector in (x, y, z). Shape is (..., 3). + """ + q_w = q[..., 0] + q_vec = q[..., 1:] + a = v * (2.0 * q_w**2 - 1.0).unsqueeze(-1) + b = torch.cross(q_vec, v, dim=-1) * q_w.unsqueeze(-1) * 2.0 + # for two-dimensional tensors, bmm is faster than einsum + if q_vec.dim() == 2: + c = ( + q_vec + * torch.bmm(q_vec.view(q.shape[0], 1, 3), v.view(q.shape[0], 3, 1)).squeeze( + -1 + ) + * 2.0 + ) + else: + c = q_vec * torch.einsum("...i,...i->...", q_vec, v).unsqueeze(-1) * 2.0 + return a + b + c + + +@torch.jit.script +def quat_rotate_inverse(q: torch.Tensor, v: torch.Tensor) -> torch.Tensor: + """Rotate a vector by the inverse of a quaternion along the last dimension of q and v. + + Args: + q: The quaternion in (w, x, y, z). Shape is (..., 4). + v: The vector in (x, y, z). Shape is (..., 3). + + Returns: + The rotated vector in (x, y, z). Shape is (..., 3). + """ + q_w = q[..., 0] + q_vec = q[..., 1:] + a = v * (2.0 * q_w**2 - 1.0).unsqueeze(-1) + b = torch.cross(q_vec, v, dim=-1) * q_w.unsqueeze(-1) * 2.0 + # for two-dimensional tensors, bmm is faster than einsum + if q_vec.dim() == 2: + c = ( + q_vec + * torch.bmm(q_vec.view(q.shape[0], 1, 3), v.view(q.shape[0], 3, 1)).squeeze( + -1 + ) + * 2.0 + ) + else: + c = q_vec * torch.einsum("...i,...i->...", q_vec, v).unsqueeze(-1) * 2.0 + return a - b + c + + +@torch.jit.script +def quat_from_angle_axis(angle: torch.Tensor, axis: torch.Tensor) -> torch.Tensor: + """Convert rotations given as angle-axis to quaternions. + + Args: + angle: The angle turned anti-clockwise in radians around the vector's direction. Shape is (N,). + axis: The axis of rotation. Shape is (N, 3). + + Returns: + The quaternion in (w, x, y, z). Shape is (N, 4). + """ + theta = (angle / 2).unsqueeze(-1) + xyz = normalize(axis) * theta.sin() + w = theta.cos() + return normalize(torch.cat([w, xyz], dim=-1)) + + +@torch.jit.script +def axis_angle_from_quat(quat: torch.Tensor, eps: float = 1.0e-6) -> torch.Tensor: + """Convert rotations given as quaternions to axis/angle. + + Args: + quat: The quaternion orientation in (w, x, y, z). Shape is (..., 4). + eps: The tolerance for Taylor approximation. Defaults to 1.0e-6. + + Returns: + Rotations given as a vector in axis angle form. Shape is (..., 3). + The vector's magnitude is the angle turned anti-clockwise in radians around the vector's direction. + + Reference: + https://github.com/facebookresearch/pytorch3d/blob/main/pytorch3d/transforms/rotation_conversions.py#L526-L554 + """ + # Modified to take in quat as [q_w, q_x, q_y, q_z] + # Quaternion is [q_w, q_x, q_y, q_z] = [cos(theta/2), n_x * sin(theta/2), n_y * sin(theta/2), n_z * sin(theta/2)] + # Axis-angle is [a_x, a_y, a_z] = [theta * n_x, theta * n_y, theta * n_z] + # Thus, axis-angle is [q_x, q_y, q_z] / (sin(theta/2) / theta) + # When theta = 0, (sin(theta/2) / theta) is undefined + # However, as theta --> 0, we can use the Taylor approximation 1/2 - theta^2 / 48 + quat = quat * (1.0 - 2.0 * (quat[..., 0:1] < 0.0)) + mag = torch.linalg.norm(quat[..., 1:], dim=-1) + half_angle = torch.atan2(mag, quat[..., 0]) + angle = 2.0 * half_angle + # check whether to apply Taylor approximation + sin_half_angles_over_angles = torch.where( + angle.abs() > eps, torch.sin(half_angle) / angle, 0.5 - angle * angle / 48 + ) + return quat[..., 1:4] / sin_half_angles_over_angles.unsqueeze(-1) + + +@torch.jit.script +def quat_error_magnitude(q1: torch.Tensor, q2: torch.Tensor) -> torch.Tensor: + """Computes the rotation difference between two quaternions. + + Args: + q1: The first quaternion in (w, x, y, z). Shape is (..., 4). + q2: The second quaternion in (w, x, y, z). Shape is (..., 4). + + Returns: + Angular error between input quaternions in radians. + """ + quat_diff = quat_mul(q1, quat_conjugate(q2)) + return torch.norm(axis_angle_from_quat(quat_diff), dim=-1) + + +@torch.jit.script +def skew_symmetric_matrix(vec: torch.Tensor) -> torch.Tensor: + """Computes the skew-symmetric matrix of a vector. + + Args: + vec: The input vector. Shape is (3,) or (N, 3). + + Returns: + The skew-symmetric matrix. Shape is (1, 3, 3) or (N, 3, 3). + + Raises: + ValueError: If input tensor is not of shape (..., 3). + """ + # check input is correct + if vec.shape[-1] != 3: + raise ValueError( + f"Expected input vector shape mismatch: {vec.shape} != (..., 3)." + ) + # unsqueeze the last dimension + if vec.ndim == 1: + vec = vec.unsqueeze(0) + # create a skew-symmetric matrix + skew_sym_mat = torch.zeros(vec.shape[0], 3, 3, device=vec.device, dtype=vec.dtype) + skew_sym_mat[:, 0, 1] = -vec[:, 2] + skew_sym_mat[:, 0, 2] = vec[:, 1] + skew_sym_mat[:, 1, 2] = -vec[:, 0] + skew_sym_mat[:, 1, 0] = vec[:, 2] + skew_sym_mat[:, 2, 0] = -vec[:, 1] + skew_sym_mat[:, 2, 1] = vec[:, 0] + + return skew_sym_mat + + +""" +Transformations +""" + + +def is_identity_pose(pos: torch.tensor, rot: torch.tensor) -> bool: + """Checks if input poses are identity transforms. + + The function checks if the input position and orientation are close to zero and + identity respectively using L2-norm. It does NOT check the error in the orientation. + + Args: + pos: The cartesian position. Shape is (N, 3). + rot: The quaternion in (w, x, y, z). Shape is (N, 4). + + Returns: + True if all the input poses result in identity transform. Otherwise, False. + """ + # create identity transformations + pos_identity = torch.zeros_like(pos) + rot_identity = torch.zeros_like(rot) + rot_identity[..., 0] = 1 + # compare input to identity + return torch.allclose(pos, pos_identity) and torch.allclose(rot, rot_identity) + + +# @torch.jit.script +# require new name: multiply_transform +def combine_frame_transforms( + t01: torch.Tensor, + q01: torch.Tensor, + t12: torch.Tensor | None = None, + q12: torch.Tensor | None = None, +) -> tuple[torch.Tensor, torch.Tensor]: + r"""Combine transformations between two reference frames into a stationary frame. + + It performs the following transformation operation: :math:`T_{02} = T_{01} \times T_{12}`, + where :math:`T_{AB}` is the homogeneous transformation matrix from frame A to B. + + Args: + t01: Position of frame 1 w.r.t. frame 0. Shape is (N, 3). + q01: Quaternion orientation of frame 1 w.r.t. frame 0 in (w, x, y, z). Shape is (N, 4). + t12: Position of frame 2 w.r.t. frame 1. Shape is (N, 3). + Defaults to None, in which case the position is assumed to be zero. + q12: Quaternion orientation of frame 2 w.r.t. frame 1 in (w, x, y, z). Shape is (N, 4). + Defaults to None, in which case the orientation is assumed to be identity. + + Returns: + A tuple containing the position and orientation of frame 2 w.r.t. frame 0. + Shape of the tensors are (N, 3) and (N, 4) respectively. + """ + # compute orientation + if q12 is not None: + q02 = quat_mul(q01, q12) + else: + q02 = q01 + # compute translation + if t12 is not None: + t02 = t01 + quat_apply(q01, t12) + else: + t02 = t01 + + return t02, q02 + + +# @torch.jit.script +def subtract_frame_transforms( + t01: torch.Tensor, + q01: torch.Tensor, + t02: torch.Tensor | None = None, + q02: torch.Tensor | None = None, +) -> tuple[torch.Tensor, torch.Tensor]: + r"""Subtract transformations between two reference frames into a stationary frame. + + It performs the following transformation operation: :math:`T_{12} = T_{01}^{-1} \times T_{02}`, + where :math:`T_{AB}` is the homogeneous transformation matrix from frame A to B. + + Args: + t01: Position of frame 1 w.r.t. frame 0. Shape is (N, 3). + q01: Quaternion orientation of frame 1 w.r.t. frame 0 in (w, x, y, z). Shape is (N, 4). + t02: Position of frame 2 w.r.t. frame 0. Shape is (N, 3). + Defaults to None, in which case the position is assumed to be zero. + q02: Quaternion orientation of frame 2 w.r.t. frame 0 in (w, x, y, z). Shape is (N, 4). + Defaults to None, in which case the orientation is assumed to be identity. + + Returns: + A tuple containing the position and orientation of frame 2 w.r.t. frame 1. + Shape of the tensors are (N, 3) and (N, 4) respectively. + """ + # compute orientation + q10 = quat_inv(q01) + if q02 is not None: + q12 = quat_mul(q10, q02) + else: + q12 = q10 + # compute translation + if t02 is not None: + t12 = quat_apply(q10, t02 - t01) + else: + t12 = quat_apply(q10, -t01) + return t12, q12 + + +# @torch.jit.script +def compute_pose_error( + t01: torch.Tensor, + q01: torch.Tensor, + t02: torch.Tensor, + q02: torch.Tensor, + rot_error_type: Literal["quat", "axis_angle"] = "axis_angle", +) -> tuple[torch.Tensor, torch.Tensor]: + """Compute the position and orientation error between source and target frames. + + Args: + t01: Position of source frame. Shape is (N, 3). + q01: Quaternion orientation of source frame in (w, x, y, z). Shape is (N, 4). + t02: Position of target frame. Shape is (N, 3). + q02: Quaternion orientation of target frame in (w, x, y, z). Shape is (N, 4). + rot_error_type: The rotation error type to return: "quat", "axis_angle". + Defaults to "axis_angle". + + Returns: + A tuple containing position and orientation error. Shape of position error is (N, 3). + Shape of orientation error depends on the value of :attr:`rot_error_type`: + + - If :attr:`rot_error_type` is "quat", the orientation error is returned + as a quaternion. Shape is (N, 4). + - If :attr:`rot_error_type` is "axis_angle", the orientation error is + returned as an axis-angle vector. Shape is (N, 3). + + Raises: + ValueError: Invalid rotation error type. + """ + # Compute quaternion error (i.e., difference quaternion) + # Reference: https://personal.utdallas.edu/~sxb027100/dock/quaternion.html + # q_current_norm = q_current * q_current_conj + source_quat_norm = quat_mul(q01, quat_conjugate(q01))[:, 0] + # q_current_inv = q_current_conj / q_current_norm + source_quat_inv = quat_conjugate(q01) / source_quat_norm.unsqueeze(-1) + # q_error = q_target * q_current_inv + quat_error = quat_mul(q02, source_quat_inv) + + # Compute position error + pos_error = t02 - t01 + + # return error based on specified type + if rot_error_type == "quat": + return pos_error, quat_error + elif rot_error_type == "axis_angle": + # Convert to axis-angle error + axis_angle_error = axis_angle_from_quat(quat_error) + return pos_error, axis_angle_error + else: + raise ValueError( + f"Unsupported orientation error type: {rot_error_type}. Valid: 'quat', 'axis_angle'." + ) + + +@torch.jit.script +def apply_delta_pose( + source_pos: torch.Tensor, + source_rot: torch.Tensor, + delta_pose: torch.Tensor, + eps: float = 1.0e-6, +) -> tuple[torch.Tensor, torch.Tensor]: + """Applies delta pose transformation on source pose. + + The first three elements of `delta_pose` are interpreted as cartesian position displacement. + The remaining three elements of `delta_pose` are interpreted as orientation displacement + in the angle-axis format. + + Args: + source_pos: Position of source frame. Shape is (N, 3). + source_rot: Quaternion orientation of source frame in (w, x, y, z). Shape is (N, 4).. + delta_pose: Position and orientation displacements. Shape is (N, 6). + eps: The tolerance to consider orientation displacement as zero. Defaults to 1.0e-6. + + Returns: + A tuple containing the displaced position and orientation frames. + Shape of the tensors are (N, 3) and (N, 4) respectively. + """ + # number of poses given + num_poses = source_pos.shape[0] + device = source_pos.device + + # interpret delta_pose[:, 0:3] as target position displacements + target_pos = source_pos + delta_pose[:, 0:3] + # interpret delta_pose[:, 3:6] as target rotation displacements + rot_actions = delta_pose[:, 3:6] + angle = torch.linalg.vector_norm(rot_actions, dim=1) + axis = rot_actions / angle.unsqueeze(-1) + # change from axis-angle to quat convention + identity_quat = torch.tensor([1.0, 0.0, 0.0, 0.0], device=device).repeat( + num_poses, 1 + ) + rot_delta_quat = torch.where( + angle.unsqueeze(-1).repeat(1, 4) > eps, + quat_from_angle_axis(angle, axis), + identity_quat, + ) + # TODO: Check if this is the correct order for this multiplication. + target_rot = quat_mul(rot_delta_quat, source_rot) + + return target_pos, target_rot + + +# @torch.jit.script +def transform_points( + points: torch.Tensor, + pos: torch.Tensor | None = None, + quat: torch.Tensor | None = None, +) -> torch.Tensor: + r"""Transform input points in a given frame to a target frame. + + This function transform points from a source frame to a target frame. The transformation is defined by the + position :math:`t` and orientation :math:`R` of the target frame in the source frame. + + .. math:: + p_{target} = R_{target} \times p_{source} + t_{target} + + If the input `points` is a batch of points, the inputs `pos` and `quat` must be either a batch of + positions and quaternions or a single position and quaternion. If the inputs `pos` and `quat` are + a single position and quaternion, the same transformation is applied to all points in the batch. + + If either the inputs :attr:`pos` and :attr:`quat` are None, the corresponding transformation is not applied. + + Args: + points: Points to transform. Shape is (N, P, 3) or (P, 3). + pos: Position of the target frame. Shape is (N, 3) or (3,). + Defaults to None, in which case the position is assumed to be zero. + quat: Quaternion orientation of the target frame in (w, x, y, z). Shape is (N, 4) or (4,). + Defaults to None, in which case the orientation is assumed to be identity. + + Returns: + Transformed points in the target frame. Shape is (N, P, 3) or (P, 3). + + Raises: + ValueError: If the inputs `points` is not of shape (N, P, 3) or (P, 3). + ValueError: If the inputs `pos` is not of shape (N, 3) or (3,). + ValueError: If the inputs `quat` is not of shape (N, 4) or (4,). + """ + points_batch = points.clone() + # check if inputs are batched + is_batched = points_batch.dim() == 3 + # -- check inputs + if points_batch.dim() == 2: + points_batch = points_batch[None] # (P, 3) -> (1, P, 3) + if points_batch.dim() != 3: + raise ValueError( + f"Expected points to have dim = 2 or dim = 3: got shape {points.shape}" + ) + if not (pos is None or pos.dim() == 1 or pos.dim() == 2): + raise ValueError( + f"Expected pos to have dim = 1 or dim = 2: got shape {pos.shape}" + ) + if not (quat is None or quat.dim() == 1 or quat.dim() == 2): + raise ValueError( + f"Expected quat to have dim = 1 or dim = 2: got shape {quat.shape}" + ) + # -- rotation + if quat is not None: + # convert to batched rotation matrix + rot_mat = matrix_from_quat(quat) + if rot_mat.dim() == 2: + rot_mat = rot_mat[None] # (3, 3) -> (1, 3, 3) + # convert points to matching batch size (N, P, 3) -> (N, 3, P) + # and apply rotation + points_batch = torch.matmul(rot_mat, points_batch.transpose_(1, 2)) + # (N, 3, P) -> (N, P, 3) + points_batch = points_batch.transpose_(1, 2) + # -- translation + if pos is not None: + # convert to batched translation vector + if pos.dim() == 1: + pos = pos[None, None, :] # (3,) -> (1, 1, 3) + else: + pos = pos[:, None, :] # (N, 3) -> (N, 1, 3) + # apply translation + points_batch += pos + # -- return points in same shape as input + if not is_batched: + points_batch = points_batch.squeeze(0) # (1, P, 3) -> (P, 3) + + return points_batch + + +""" +Projection operations. +""" + + +@torch.jit.script +def orthogonalize_perspective_depth( + depth: torch.Tensor, intrinsics: torch.Tensor +) -> torch.Tensor: + """Converts perspective depth image to orthogonal depth image. + + Perspective depth images contain distances measured from the camera's optical center. + Meanwhile, orthogonal depth images provide the distance from the camera's image plane. + This method uses the camera geometry to convert perspective depth to orthogonal depth image. + + The function assumes that the width and height are both greater than 1. + + Args: + depth: The perspective depth images. Shape is (H, W) or or (H, W, 1) or (N, H, W) or (N, H, W, 1). + intrinsics: The camera's calibration matrix. If a single matrix is provided, the same + calibration matrix is used across all the depth images in the batch. + Shape is (3, 3) or (N, 3, 3). + + Returns: + The orthogonal depth images. Shape matches the input shape of depth images. + + Raises: + ValueError: When depth is not of shape (H, W) or (H, W, 1) or (N, H, W) or (N, H, W, 1). + ValueError: When intrinsics is not of shape (3, 3) or (N, 3, 3). + """ + # Clone inputs to avoid in-place modifications + perspective_depth_batch = depth.clone() + intrinsics_batch = intrinsics.clone() + + # Check if inputs are batched + is_batched = perspective_depth_batch.dim() == 4 or ( + perspective_depth_batch.dim() == 3 and perspective_depth_batch.shape[-1] != 1 + ) + + # Track whether the last dimension was singleton + add_last_dim = False + if perspective_depth_batch.dim() == 4 and perspective_depth_batch.shape[-1] == 1: + add_last_dim = True + perspective_depth_batch = perspective_depth_batch.squeeze( + dim=3 + ) # (N, H, W, 1) -> (N, H, W) + if perspective_depth_batch.dim() == 3 and perspective_depth_batch.shape[-1] == 1: + add_last_dim = True + perspective_depth_batch = perspective_depth_batch.squeeze( + dim=2 + ) # (H, W, 1) -> (H, W) + + if perspective_depth_batch.dim() == 2: + perspective_depth_batch = perspective_depth_batch[None] # (H, W) -> (1, H, W) + + if intrinsics_batch.dim() == 2: + intrinsics_batch = intrinsics_batch[None] # (3, 3) -> (1, 3, 3) + + if is_batched and intrinsics_batch.shape[0] == 1: + intrinsics_batch = intrinsics_batch.expand( + perspective_depth_batch.shape[0], -1, -1 + ) # (1, 3, 3) -> (N, 3, 3) + + # Validate input shapes + if perspective_depth_batch.dim() != 3: + raise ValueError( + f"Expected depth images to have 2, 3, or 4 dimensions; got {depth.shape}." + ) + if intrinsics_batch.dim() != 3: + raise ValueError( + f"Expected intrinsics to have shape (3, 3) or (N, 3, 3); got {intrinsics.shape}." + ) + + # Image dimensions + im_height, im_width = perspective_depth_batch.shape[1:] + + # Get the intrinsics parameters + fx = intrinsics_batch[:, 0, 0].view(-1, 1, 1) + fy = intrinsics_batch[:, 1, 1].view(-1, 1, 1) + cx = intrinsics_batch[:, 0, 2].view(-1, 1, 1) + cy = intrinsics_batch[:, 1, 2].view(-1, 1, 1) + + # Create meshgrid of pixel coordinates + u_grid = torch.arange(im_width, device=depth.device, dtype=depth.dtype) + v_grid = torch.arange(im_height, device=depth.device, dtype=depth.dtype) + u_grid, v_grid = torch.meshgrid(u_grid, v_grid, indexing="xy") + + # Expand the grids for batch processing + u_grid = u_grid.unsqueeze(0).expand(perspective_depth_batch.shape[0], -1, -1) + v_grid = v_grid.unsqueeze(0).expand(perspective_depth_batch.shape[0], -1, -1) + + # Compute the squared terms for efficiency + x_term = ((u_grid - cx) / fx) ** 2 + y_term = ((v_grid - cy) / fy) ** 2 + + # Calculate the orthogonal (normal) depth + orthogonal_depth = perspective_depth_batch / torch.sqrt(1 + x_term + y_term) + + # Restore the last dimension if it was present in the input + if add_last_dim: + orthogonal_depth = orthogonal_depth.unsqueeze(-1) + + # Return to original shape if input was not batched + if not is_batched: + orthogonal_depth = orthogonal_depth.squeeze(0) + + return orthogonal_depth + + +@torch.jit.script +def unproject_depth( + depth: torch.Tensor, intrinsics: torch.Tensor, is_ortho: bool = True +) -> torch.Tensor: + r"""Un-project depth image into a pointcloud. + + This function converts orthogonal or perspective depth images into points given the calibration matrix + of the camera. It uses the following transformation based on camera geometry: + + .. math:: + p_{3D} = K^{-1} \times [u, v, 1]^T \times d + + where :math:`p_{3D}` is the 3D point, :math:`d` is the depth value (measured from the image plane), + :math:`u` and :math:`v` are the pixel coordinates and :math:`K` is the intrinsic matrix. + + The function assumes that the width and height are both greater than 1. This makes the function + deal with many possible shapes of depth images and intrinsics matrices. + + .. note:: + If :attr:`is_ortho` is False, the input depth images are transformed to orthogonal depth images + by using the :meth:`orthogonalize_perspective_depth` method. + + Args: + depth: The depth measurement. Shape is (H, W) or or (H, W, 1) or (N, H, W) or (N, H, W, 1). + intrinsics: The camera's calibration matrix. If a single matrix is provided, the same + calibration matrix is used across all the depth images in the batch. + Shape is (3, 3) or (N, 3, 3). + is_ortho: Whether the input depth image is orthogonal or perspective depth image. If True, the input + depth image is considered as the *orthogonal* type, where the measurements are from the camera's + image plane. If False, the depth image is considered as the *perspective* type, where the + measurements are from the camera's optical center. Defaults to True. + + Returns: + The 3D coordinates of points. Shape is (P, 3) or (N, P, 3). + + Raises: + ValueError: When depth is not of shape (H, W) or (H, W, 1) or (N, H, W) or (N, H, W, 1). + ValueError: When intrinsics is not of shape (3, 3) or (N, 3, 3). + """ + # clone inputs to avoid in-place modifications + intrinsics_batch = intrinsics.clone() + # convert depth image to orthogonal if needed + if not is_ortho: + depth_batch = orthogonalize_perspective_depth(depth, intrinsics) + else: + depth_batch = depth.clone() + + # check if inputs are batched + is_batched = depth_batch.dim() == 4 or ( + depth_batch.dim() == 3 and depth_batch.shape[-1] != 1 + ) + # make sure inputs are batched + if depth_batch.dim() == 3 and depth_batch.shape[-1] == 1: + depth_batch = depth_batch.squeeze(dim=2) # (H, W, 1) -> (H, W) + if depth_batch.dim() == 2: + depth_batch = depth_batch[None] # (H, W) -> (1, H, W) + if depth_batch.dim() == 4 and depth_batch.shape[-1] == 1: + depth_batch = depth_batch.squeeze(dim=3) # (N, H, W, 1) -> (N, H, W) + if intrinsics_batch.dim() == 2: + intrinsics_batch = intrinsics_batch[None] # (3, 3) -> (1, 3, 3) + # check shape of inputs + if depth_batch.dim() != 3: + raise ValueError( + f"Expected depth images to have dim = 2 or 3 or 4: got shape {depth.shape}" + ) + if intrinsics_batch.dim() != 3: + raise ValueError( + f"Expected intrinsics to have shape (3, 3) or (N, 3, 3): got shape {intrinsics.shape}" + ) + + # get image height and width + im_height, im_width = depth_batch.shape[1:] + # create image points in homogeneous coordinates (3, H x W) + indices_u = torch.arange(im_width, device=depth.device, dtype=depth.dtype) + indices_v = torch.arange(im_height, device=depth.device, dtype=depth.dtype) + img_indices = torch.stack( + torch.meshgrid([indices_u, indices_v], indexing="ij"), dim=0 + ).reshape(2, -1) + pixels = torch.nn.functional.pad( + img_indices, (0, 0, 0, 1), mode="constant", value=1.0 + ) + pixels = pixels.unsqueeze(0) # (3, H x W) -> (1, 3, H x W) + + # unproject points into 3D space + points = torch.matmul(torch.inverse(intrinsics_batch), pixels) # (N, 3, H x W) + points = points / points[:, -1, :].unsqueeze(1) # normalize by last coordinate + # flatten depth image (N, H, W) -> (N, H x W) + depth_batch = ( + depth_batch.transpose_(1, 2).reshape(depth_batch.shape[0], -1).unsqueeze(2) + ) + depth_batch = depth_batch.expand(-1, -1, 3) + # scale points by depth + points_xyz = points.transpose_(1, 2) * depth_batch # (N, H x W, 3) + + # return points in same shape as input + if not is_batched: + points_xyz = points_xyz.squeeze(0) + + return points_xyz + + +@torch.jit.script +def project_points(points: torch.Tensor, intrinsics: torch.Tensor) -> torch.Tensor: + r"""Projects 3D points into 2D image plane. + + This project 3D points into a 2D image plane. The transformation is defined by the intrinsic + matrix of the camera. + + .. math:: + + \begin{align} + p &= K \times p_{3D} = \\ + p_{2D} &= \begin{pmatrix} u \\ v \\ d \end{pmatrix} + = \begin{pmatrix} p[0] / p[2] \\ p[1] / p[2] \\ Z \end{pmatrix} + \end{align} + + where :math:`p_{2D} = (u, v, d)` is the projected 3D point, :math:`p_{3D} = (X, Y, Z)` is the + 3D point and :math:`K \in \mathbb{R}^{3 \times 3}` is the intrinsic matrix. + + If `points` is a batch of 3D points and `intrinsics` is a single intrinsic matrix, the same + calibration matrix is applied to all points in the batch. + + Args: + points: The 3D coordinates of points. Shape is (P, 3) or (N, P, 3). + intrinsics: Camera's calibration matrix. Shape is (3, 3) or (N, 3, 3). + + Returns: + Projected 3D coordinates of points. Shape is (P, 3) or (N, P, 3). + """ + # clone the inputs to avoid in-place operations modifying the original data + points_batch = points.clone() + intrinsics_batch = intrinsics.clone() + + # check if inputs are batched + is_batched = points_batch.dim() == 2 + # make sure inputs are batched + if points_batch.dim() == 2: + points_batch = points_batch[None] # (P, 3) -> (1, P, 3) + if intrinsics_batch.dim() == 2: + intrinsics_batch = intrinsics_batch[None] # (3, 3) -> (1, 3, 3) + # check shape of inputs + if points_batch.dim() != 3: + raise ValueError(f"Expected points to have dim = 3: got shape {points.shape}.") + if intrinsics_batch.dim() != 3: + raise ValueError( + f"Expected intrinsics to have shape (3, 3) or (N, 3, 3): got shape {intrinsics.shape}." + ) + + # project points into 2D image plane + points_2d = torch.matmul(intrinsics_batch, points_batch.transpose(1, 2)) + points_2d = points_2d / points_2d[:, -1, :].unsqueeze( + 1 + ) # normalize by last coordinate + points_2d = points_2d.transpose_(1, 2) # (N, 3, P) -> (N, P, 3) + # replace last coordinate with depth + points_2d[:, :, -1] = points_batch[:, :, -1] + + # return points in same shape as input + if not is_batched: + points_2d = points_2d.squeeze(0) # (1, 3, P) -> (3, P) + + return points_2d + + +""" +Sampling +""" + + +@torch.jit.script +def default_orientation(num: int, device: str) -> torch.Tensor: + """Returns identity rotation transform. + + Args: + num: The number of rotations to sample. + device: Device to create tensor on. + + Returns: + Identity quaternion in (w, x, y, z). Shape is (num, 4). + """ + quat = torch.zeros((num, 4), dtype=torch.float, device=device) + quat[..., 0] = 1.0 + + return quat + + +@torch.jit.script +def random_orientation(num: int, device: str) -> torch.Tensor: + """Returns sampled rotation in 3D as quaternion. + + Args: + num: The number of rotations to sample. + device: Device to create tensor on. + + Returns: + Sampled quaternion in (w, x, y, z). Shape is (num, 4). + + Reference: + https://docs.scipy.org/doc/scipy/reference/generated/scipy.spatial.transform.Rotation.random.html + """ + # sample random orientation from normal distribution + quat = torch.randn((num, 4), dtype=torch.float, device=device) + # normalize the quaternion + return torch.nn.functional.normalize(quat, p=2.0, dim=-1, eps=1e-12) + + +@torch.jit.script +def random_yaw_orientation(num: int, device: str) -> torch.Tensor: + """Returns sampled rotation around z-axis. + + Args: + num: The number of rotations to sample. + device: Device to create tensor on. + + Returns: + Sampled quaternion in (w, x, y, z). Shape is (num, 4). + """ + roll = torch.zeros(num, dtype=torch.float, device=device) + pitch = torch.zeros(num, dtype=torch.float, device=device) + yaw = 2 * torch.pi * torch.rand(num, dtype=torch.float, device=device) + + return quat_from_euler_xyz(roll, pitch, yaw) + + +def sample_triangle( + lower: float, upper: float, size: int | tuple[int, ...], device: str +) -> torch.Tensor: + """Randomly samples tensor from a triangular distribution. + + Args: + lower: The lower range of the sampled tensor. + upper: The upper range of the sampled tensor. + size: The shape of the tensor. + device: Device to create tensor on. + + Returns: + Sampled tensor. Shape is based on :attr:`size`. + """ + # convert to tuple + if isinstance(size, int): + size = (size,) + # create random tensor in the range [-1, 1] + r = 2 * torch.rand(*size, device=device) - 1 + # convert to triangular distribution + r = torch.where(r < 0.0, -torch.sqrt(-r), torch.sqrt(r)) + # rescale back to [0, 1] + r = (r + 1.0) / 2.0 + # rescale to range [lower, upper] + return (upper - lower) * r + lower + + +def sample_uniform( + lower: torch.Tensor | float, + upper: torch.Tensor | float, + size: int | tuple[int, ...], + device: str, +) -> torch.Tensor: + """Sample uniformly within a range. + + Args: + lower: Lower bound of uniform range. + upper: Upper bound of uniform range. + size: The shape of the tensor. + device: Device to create tensor on. + + Returns: + Sampled tensor. Shape is based on :attr:`size`. + """ + # convert to tuple + if isinstance(size, int): + size = (size,) + # return tensor + return torch.rand(*size, device=device) * (upper - lower) + lower + + +def sample_log_uniform( + lower: torch.Tensor | float, + upper: torch.Tensor | float, + size: int | tuple[int, ...], + device: str, +) -> torch.Tensor: + r"""Sample using log-uniform distribution within a range. + + The log-uniform distribution is defined as a uniform distribution in the log-space. It + is useful for sampling values that span several orders of magnitude. The sampled values + are uniformly distributed in the log-space and then exponentiated to get the final values. + + .. math:: + + x = \exp(\text{uniform}(\log(\text{lower}), \log(\text{upper}))) + + Args: + lower: Lower bound of uniform range. + upper: Upper bound of uniform range. + size: The shape of the tensor. + device: Device to create tensor on. + + Returns: + Sampled tensor. Shape is based on :attr:`size`. + """ + # cast to tensor if not already + if not isinstance(lower, torch.Tensor): + lower = torch.tensor(lower, dtype=torch.float, device=device) + if not isinstance(upper, torch.Tensor): + upper = torch.tensor(upper, dtype=torch.float, device=device) + # sample in log-space and exponentiate + return torch.exp(sample_uniform(torch.log(lower), torch.log(upper), size, device)) + + +def sample_gaussian( + mean: torch.Tensor | float, + std: torch.Tensor | float, + size: int | tuple[int, ...], + device: str, +) -> torch.Tensor: + """Sample using gaussian distribution. + + Args: + mean: Mean of the gaussian. + std: Std of the gaussian. + size: The shape of the tensor. + device: Device to create tensor on. + + Returns: + Sampled tensor. + """ + if isinstance(mean, float): + if isinstance(size, int): + size = (size,) + return torch.normal(mean=mean, std=std, size=size).to(device=device) + else: + return torch.normal(mean=mean, std=std).to(device=device) + + +def sample_cylinder( + radius: float, + h_range: tuple[float, float], + size: int | tuple[int, ...], + device: str, +) -> torch.Tensor: + """Sample 3D points uniformly on a cylinder's surface. + + The cylinder is centered at the origin and aligned with the z-axis. The height of the cylinder is + sampled uniformly from the range :obj:`h_range`, while the radius is fixed to :obj:`radius`. + + The sampled points are returned as a tensor of shape :obj:`(*size, 3)`, i.e. the last dimension + contains the x, y, and z coordinates of the sampled points. + + Args: + radius: The radius of the cylinder. + h_range: The minimum and maximum height of the cylinder. + size: The shape of the tensor. + device: Device to create tensor on. + + Returns: + Sampled tensor. Shape is :obj:`(*size, 3)`. + """ + # sample angles + angles = (torch.rand(size, device=device) * 2 - 1) * torch.pi + h_min, h_max = h_range + # add shape + if isinstance(size, int): + size = (size, 3) + else: + size += (3,) + # allocate a tensor + xyz = torch.zeros(size, device=device) + xyz[..., 0] = radius * torch.cos(angles) + xyz[..., 1] = radius * torch.sin(angles) + xyz[..., 2].uniform_(h_min, h_max) + # return positions + return xyz + + +""" + interpolation +""" + + +def safe_rotvec2quat(rotvec: torch.Tensor, form: Literal["xyzw", "wxyz"] = "wxyz"): + angle = torch.linalg.vector_norm(rotvec, dim=1) + small_angle = angle <= 1e-3 + large_angle = ~small_angle + scale = torch.empty_like(angle) + scale[small_angle] = ( + 0.5 - angle[small_angle] ** 2 / 48 + angle[small_angle] ** 4 / 3840 + ) + scale[large_angle] = torch.sin(angle[large_angle] / 2) / angle[large_angle] + + xyz = scale[:, None] * rotvec + w = torch.cos(angle / 2)[..., None] + if form == "wxyz": + return torch.cat([w, xyz], dim=-1) + else: + return torch.cat([xyz, w], dim=-1) + + +def slerp(q0: torch.Tensor, q1: torch.Tensor, steps: torch.Tensor): + + quat_diff = quat_mul(quat_conjugate(q0), q1) + quat_diff = quat_unique(quat_diff) + rot_vec = axis_angle_from_quat(quat_diff) # (batch, 3) + diff = steps[..., None] * rot_vec + + diff_quat = safe_rotvec2quat(diff) + + return quat_mul(q0, diff_quat) + +def main(): + quat = np.random.normal(size=4) + quat /= np.linalg.norm(quat) + axa = as_np(axis_angle_from_quat)(quat) + print(axa) + + axa_th = axis_angle_from_quat(th.as_tensor(quat, + device='cpu')) + print(axa_th.detach().cpu().numpy()) + print( quat_from_angle_axis( + th.zeros(1), + th.zeros(1,3))) + +if __name__ == '__main__': + main() diff --git a/deploy/deploy_real/state_pub.launch.py b/deploy/deploy_real/state_pub.launch.py new file mode 100644 index 0000000..d709ff4 --- /dev/null +++ b/deploy/deploy_real/state_pub.launch.py @@ -0,0 +1,28 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + +def generate_launch_description(): + + use_sim_time = LaunchConfiguration('use_sim_time', default='false') + + urdf = '../../resources/robots/g1_description/g1_29dof_with_hand_rev_1_0.urdf' + with open(urdf, 'r') as infp: + robot_desc = infp.read() + + return LaunchDescription([ + DeclareLaunchArgument( + 'use_sim_time', + default_value='false', + description='Use simulation (Gazebo) clock if true'), + Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + output='screen', + parameters=[{'use_sim_time': use_sim_time, 'robot_description': robot_desc}], + arguments=[urdf]), + ]) \ No newline at end of file diff --git a/deploy/deploy_real/state_pub.py b/deploy/deploy_real/state_pub.py new file mode 100644 index 0000000..ab7fe4e --- /dev/null +++ b/deploy/deploy_real/state_pub.py @@ -0,0 +1,64 @@ +from math import sin, cos, pi +import rclpy +from rclpy.node import Node +from rclpy.qos import QoSProfile +from geometry_msgs.msg import Quaternion +from sensor_msgs.msg import JointState +# from tf2_ros import TransformBroadcaster, TransformStamped +from unitree_hg.msg import LowCmd as LowCmdHG, LowState as LowStateHG +import yaml + +class StatePublisher(Node): + + def __init__(self): + super().__init__('state_publisher') + qos_profile = QoSProfile(depth=10) + + with open('./configs/ik.yaml', 'r') as fp: + self.joint_names = yaml.safe_load(fp)['motor_joint'] + + self.low_state = LowStateHG() + self.low_state_subscriber = self.create_subscription(LowStateHG, + 'lowstate', self.on_low_state, 10) + self.joint_pub = self.create_publisher(JointState, + 'joint_states', qos_profile) + self.nodeName = self.get_name() + self.get_logger().info("{0} started".format(self.nodeName)) + self.joint_state = JointState() + + def on_low_state(self, msg: LowStateHG): + self.low_state = msg + joint_state = self.joint_state + now = self.get_clock().now() + joint_state.header.stamp = now.to_msg() + joint_state.name = self.joint_names + joint_state.position = [0.0 for _ in self.joint_names] + joint_state.velocity = [0.0 for _ in self.joint_names] + + n:int = min(len(self.joint_names), len(self.low_state.motor_state)) + for i in range(n): + joint_state.position[i] = self.low_state.motor_state[i].q + joint_state.velocity[i] = self.low_state.motor_state[i].dq + self.joint_pub.publish(joint_state) + + def run(self): + loop_rate = self.create_rate(30) + try: + # rclpy.spin() + while rclpy.ok(): + rclpy.spin_once(self) + loop_rate.sleep() + except KeyboardInterrupt: + pass + + + +def main(): + rclpy.init() + node = StatePublisher() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/deploy/deploy_real/test.py b/deploy/deploy_real/test.py new file mode 100644 index 0000000..22c356c --- /dev/null +++ b/deploy/deploy_real/test.py @@ -0,0 +1,50 @@ +import rclpy +from rclpy.node import Node + +from tf2_ros.buffer import Buffer +from tf2_ros.transform_listener import TransformListener +from tf2_ros import TransformBroadcaster, TransformStamped, StaticTransformBroadcaster + +class Tester(Node): + def __init__(self): + super().__init__('tester') + self.tf_buffer = Buffer() + self.tf_listener = TransformListener(self.tf_buffer, self) + self.tf_broadcaster = TransformBroadcaster(self) + + self.timer = self.create_timer(0.01, self.on_timer) + + def on_timer(self): + try: + self.tf_buffer.lookup_transform( + "world", "camera_init", rclpy.time.Time() + ) + except Exception as ex: + print(f'Could not transform mid360_link_IMU to pelvis as world to camera_init is yet published: {ex}') + return + try: + current_left_tf = self.tf_buffer.lookup_transform( + "world", + "left_ankle_roll_link", + rclpy.time.Time(), + rclpy.duration.Duration(seconds=0.02)) + current_right_tf = self.tf_buffer.lookup_transform( + "world", + "right_ankle_roll_link", + rclpy.time.Time(), + rclpy.duration.Duration(seconds=0.02)) + print(current_left_tf, current_right_tf) + except Exception as ex: + print(f'Could not transform mid360_link_IMU to pelvis: {ex}') + +def main(): + rclpy.init() + node = Tester() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + rclpy.shutdown() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/deploy/deploy_real/test_policy.py b/deploy/deploy_real/test_policy.py new file mode 100644 index 0000000..af54a12 --- /dev/null +++ b/deploy/deploy_real/test_policy.py @@ -0,0 +1,12 @@ +#!/usr/bin/env python3 +import torch +import torch as th +import numpy as np +policy = torch.jit.load('../pre_train/g1/policy_eetrack.pt') +obs=np.load('/tmp/eet5/obs002.npy') +print('obs', obs.shape) +act=policy(torch.from_numpy(obs)) +act_sim=np.load('/tmp/eet5/act002.npy') +act_rec=act.detach().cpu().numpy() +delta= (act_sim - act_rec) +print(np.abs(delta).max()) diff --git a/deploy/pre_train/g1/policy.pt b/deploy/pre_train/g1/policy.pt new file mode 100644 index 0000000..7a29523 Binary files /dev/null and b/deploy/pre_train/g1/policy.pt differ diff --git a/legged_gym/envs/base/legged_robot_config.py b/legged_gym/envs/base/legged_robot_config.py index 03f09a2..5838d11 100644 --- a/legged_gym/envs/base/legged_robot_config.py +++ b/legged_gym/envs/base/legged_robot_config.py @@ -41,7 +41,8 @@ class LeggedRobotCfg(BaseConfig): max_curriculum = 1. num_commands = 4 # default: lin_vel_x, lin_vel_y, ang_vel_yaw, heading (in heading mode ang_vel_yaw is recomputed from heading error) resampling_time = 10. # time before command are changed[s] - heading_command = True # if true: compute ang vel command from heading error + # heading_command = True # if true: compute ang vel command from heading error + heading_command = False # if true: compute ang vel command from heading error class ranges: lin_vel_x = [-1.0, 1.0] # min max [m/s] lin_vel_y = [-1.0, 1.0] # min max [m/s] @@ -86,7 +87,8 @@ class LeggedRobotCfg(BaseConfig): linear_damping = 0. max_angular_velocity = 1000. max_linear_velocity = 1000. - armature = 0. + # armature = 0. + armature = 0.01 thickness = 0.01 class domain_rand: @@ -126,10 +128,13 @@ class LeggedRobotCfg(BaseConfig): class normalization: class obs_scales: - lin_vel = 2.0 - ang_vel = 0.25 + # lin_vel = 2.0 + lin_vel = 1.0 + # ang_vel = 0.25 + ang_vel = 1.0 dof_pos = 1.0 - dof_vel = 0.05 + # dof_vel = 0.05 + dof_vel = 1.0 height_measurements = 5.0 clip_observations = 100. clip_actions = 100. diff --git a/legged_gym/envs/g1/g1_config.py b/legged_gym/envs/g1/g1_config.py index 6b88cc8..7f6d517 100644 --- a/legged_gym/envs/g1/g1_config.py +++ b/legged_gym/envs/g1/g1_config.py @@ -6,23 +6,43 @@ class G1RoughCfg( LeggedRobotCfg ): default_joint_angles = { # = target angles [rad] when action = 0.0 'left_hip_yaw_joint' : 0. , 'left_hip_roll_joint' : 0, - 'left_hip_pitch_joint' : -0.1, - 'left_knee_joint' : 0.3, - 'left_ankle_pitch_joint' : -0.2, + 'left_hip_pitch_joint' : -0.2, + 'left_knee_joint' : 0.42, + 'left_ankle_pitch_joint' : -0.23, 'left_ankle_roll_joint' : 0, 'right_hip_yaw_joint' : 0., 'right_hip_roll_joint' : 0, - 'right_hip_pitch_joint' : -0.1, - 'right_knee_joint' : 0.3, - 'right_ankle_pitch_joint': -0.2, + 'right_hip_pitch_joint' : -0.2, + 'right_knee_joint' : 0.42, + 'right_ankle_pitch_joint': -0.23, 'right_ankle_roll_joint' : 0, - 'torso_joint' : 0. + 'left_elbow_joint': 0.87, + 'right_elbow_joint': 0.87, + 'left_shoulder_roll_joint': 0.16, + 'left_shoulder_pitch_joint': 0.35, + 'left_shoulder_yaw_joint': 0., + 'right_shoulder_roll_joint': -0.16, + 'right_shoulder_pitch_joint': 0.35, + 'right_shoulder_yaw_joint': 0., + 'waist_roll_joint' : 0, + 'waist_pitch_joint' : 0, + 'waist_yaw_joint' : 0, + 'left_wrist_roll_joint' : 0, + 'left_wrist_pitch_joint' : 0, + 'left_wrist_yaw_joint' : 0, + 'right_wrist_roll_joint' : 0, + 'right_wrist_pitch_joint' : 0, + 'right_wrist_yaw_joint' : 0, + } class env(LeggedRobotCfg.env): - num_observations = 47 - num_privileged_obs = 50 - num_actions = 12 + # num_observations = 47 + num_observations = 96 + # num_privileged_obs = 50 + num_privileged_obs = 96 + # num_actions = 12 + num_actions = 29 class domain_rand(LeggedRobotCfg.domain_rand): @@ -44,24 +64,40 @@ class G1RoughCfg( LeggedRobotCfg ): 'hip_pitch': 100, 'knee': 150, 'ankle': 40, + 'shoulder_pitch': 100, + 'shoulder_roll': 100, + 'shoulder_yaw': 50, + 'elbow': 50, + 'wrist': 20, + 'waist': 150, } # [N*m/rad] damping = { 'hip_yaw': 2, 'hip_roll': 2, 'hip_pitch': 2, 'knee': 4, 'ankle': 2, + 'shoulder_pitch': 2, + 'shoulder_roll': 2, + 'shoulder_yaw': 2, + 'elbow': 2, + 'wrist': 1, + 'waist': 3, } # [N*m/rad] # [N*m*s/rad] # action scale: target angle = actionScale * action + defaultAngle - action_scale = 0.25 + # action_scale = 0.25 + action_scale = 0.5 # decimation: Number of control action updates @ sim DT per policy DT decimation = 4 class asset( LeggedRobotCfg.asset ): - file = '{LEGGED_GYM_ROOT_DIR}/resources/robots/g1_description/g1_12dof.urdf' + # file = '{LEGGED_GYM_ROOT_DIR}/resources/robots/g1_description/g1_12dof.urdf' + file = '{LEGGED_GYM_ROOT_DIR}/resources/robots/g1_description/g1_29dof_rev_1_0.urdf' + # file = '{LEGGED_GYM_ROOT_DIR}/resources/robots/g1_description/g1_29dof_rev_1_0.urdf' name = "g1" foot_name = "ankle_roll" penalize_contacts_on = ["hip", "knee"] - terminate_after_contacts_on = ["pelvis"] + # terminate_after_contacts_on = ["pelvis"] + terminate_after_contacts_on = [] self_collisions = 0 # 1 to disable, 0 to enable...bitwise filter flip_visual_attachments = False @@ -91,8 +127,10 @@ class G1RoughCfg( LeggedRobotCfg ): class G1RoughCfgPPO( LeggedRobotCfgPPO ): class policy: init_noise_std = 0.8 - actor_hidden_dims = [32] - critic_hidden_dims = [32] + # actor_hidden_dims = [32] + actor_hidden_dims = [256, 128, 128] + # critic_hidden_dims = [32] + critic_hidden_dims = [256, 128, 128] activation = 'elu' # can be elu, relu, selu, crelu, lrelu, tanh, sigmoid # only for 'ActorCriticRecurrent': rnn_type = 'lstm' @@ -102,7 +140,8 @@ class G1RoughCfgPPO( LeggedRobotCfgPPO ): class algorithm( LeggedRobotCfgPPO.algorithm ): entropy_coef = 0.01 class runner( LeggedRobotCfgPPO.runner ): - policy_class_name = "ActorCriticRecurrent" + # policy_class_name = "ActorCriticRecurrent" + policy_class_name = "ActorCritic" max_iterations = 10000 run_name = '' experiment_name = 'g1' diff --git a/legged_gym/envs/g1/g1_env.py b/legged_gym/envs/g1/g1_env.py index b91b9d6..edad8b5 100644 --- a/legged_gym/envs/g1/g1_env.py +++ b/legged_gym/envs/g1/g1_env.py @@ -27,7 +27,7 @@ class G1Robot(LeggedRobot): noise_vec[9:9+self.num_actions] = noise_scales.dof_pos * noise_level * self.obs_scales.dof_pos noise_vec[9+self.num_actions:9+2*self.num_actions] = noise_scales.dof_vel * noise_level * self.obs_scales.dof_vel noise_vec[9+2*self.num_actions:9+3*self.num_actions] = 0. # previous actions - noise_vec[9+3*self.num_actions:9+3*self.num_actions+2] = 0. # sin/cos phase + # noise_vec[9+3*self.num_actions:9+3*self.num_actions+2] = 0. # sin/cos phase return noise_vec @@ -68,26 +68,55 @@ class G1Robot(LeggedRobot): def compute_observations(self): """ Computes observations """ - sin_phase = torch.sin(2 * np.pi * self.phase ).unsqueeze(1) - cos_phase = torch.cos(2 * np.pi * self.phase ).unsqueeze(1) - self.obs_buf = torch.cat(( self.base_ang_vel * self.obs_scales.ang_vel, + # sin_phase = torch.sin(2 * np.pi * self.phase ).unsqueeze(1) + # cos_phase = torch.cos(2 * np.pi * self.phase ).unsqueeze(1) + self.gym.refresh_rigid_body_state_tensor(self.sim) + self.pelvis_states = self.rigid_body_states_view[:, 0, :] + from icecream import ic + # ic(self.pelvis_states) + # ic(self.commands[:, :3] * self.commands_scale) + self.pelvis_ang_vel = quat_rotate_inverse(self.pelvis_states[..., 3:7], self.pelvis_states[:, 10:13]) + self.projected_gravity = quat_rotate_inverse(self.pelvis_states[..., 3:7], self.gravity_vec) + # self.commands[..., :3] = 0. + + # if self.episode_length_buf == 0: + # self.pelvis_ang_vel[..., :] = 0. + # self.projected_gravity[..., 0:2] = 0. + # self.projected_gravity[..., 2] = -1. + + self.obs_buf = torch.cat(( + # self.base_ang_vel * self.obs_scales.ang_vel, + self.pelvis_ang_vel* self.obs_scales.ang_vel, self.projected_gravity, self.commands[:, :3] * self.commands_scale, (self.dof_pos - self.default_dof_pos) * self.obs_scales.dof_pos, self.dof_vel * self.obs_scales.dof_vel, self.actions, - sin_phase, - cos_phase + # sin_phase, + # cos_phase ),dim=-1) - self.privileged_obs_buf = torch.cat(( self.base_lin_vel * self.obs_scales.lin_vel, + from icecream import ic + # ic( + # self.episode_length_buf, + # self.base_ang_vel, + # self.projected_gravity, + # self.commands, + # (self.dof_pos - self.default_dof_pos) * self.obs_scales.dof_pos, + # self.dof_vel, + # self.actions, + + + # ) + self.privileged_obs_buf = torch.cat(( + # self.base_lin_vel * self.obs_scales.lin_vel, self.base_ang_vel * self.obs_scales.ang_vel, self.projected_gravity, self.commands[:, :3] * self.commands_scale, (self.dof_pos - self.default_dof_pos) * self.obs_scales.dof_pos, self.dof_vel * self.obs_scales.dof_vel, self.actions, - sin_phase, - cos_phase + # sin_phase, + # cos_phase ),dim=-1) # add perceptive inputs if not blind # add noise if needed diff --git a/legged_gym/scripts/play.py b/legged_gym/scripts/play.py index fc24a88..732d1a6 100644 --- a/legged_gym/scripts/play.py +++ b/legged_gym/scripts/play.py @@ -33,6 +33,80 @@ def play(args): ppo_runner, train_cfg = task_registry.make_alg_runner(env=env, name=args.task, args=args, train_cfg=train_cfg) policy = ppo_runner.get_inference_policy(device=env.device) + # Define the joint orders for sim A and sim B + sim_a_joints = [ + 'left_hip_pitch_joint', + 'right_hip_pitch_joint', + 'waist_yaw_joint', + 'left_hip_roll_joint', + 'right_hip_roll_joint', + 'waist_roll_joint', + 'left_hip_yaw_joint', + 'right_hip_yaw_joint', + 'waist_pitch_joint', + 'left_knee_joint', + 'right_knee_joint', + 'left_shoulder_pitch_joint', + 'right_shoulder_pitch_joint', + 'left_ankle_pitch_joint', + 'right_ankle_pitch_joint', + 'left_shoulder_roll_joint', + 'right_shoulder_roll_joint', + 'left_ankle_roll_joint', + 'right_ankle_roll_joint', + 'left_shoulder_yaw_joint', + 'right_shoulder_yaw_joint', + 'left_elbow_joint', + 'right_elbow_joint', + 'left_wrist_roll_joint', + 'right_wrist_roll_joint', + 'left_wrist_pitch_joint', + 'right_wrist_pitch_joint', + 'left_wrist_yaw_joint', + 'right_wrist_yaw_joint' + ] + + sim_b_joints = [ + 'left_hip_pitch_joint', + 'left_hip_roll_joint', + 'left_hip_yaw_joint', + 'left_knee_joint', + 'left_ankle_pitch_joint', + 'left_ankle_roll_joint', + 'right_hip_pitch_joint', + 'right_hip_roll_joint', + 'right_hip_yaw_joint', + 'right_knee_joint', + 'right_ankle_pitch_joint', + 'right_ankle_roll_joint', + 'waist_yaw_joint', + 'waist_roll_joint', + 'waist_pitch_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', + 'right_shoulder_pitch_joint', + 'right_shoulder_roll_joint', + 'right_shoulder_yaw_joint', + 'right_elbow_joint', + 'right_wrist_roll_joint', + 'right_wrist_pitch_joint', + 'right_wrist_yaw_joint' + ] + + # Create a mapping tensor + mapping_tensor = torch.zeros((len(sim_b_joints), len(sim_a_joints)), device=env.device) + + # Fill the mapping tensor + for b_idx, b_joint in enumerate(sim_b_joints): + if b_joint in sim_a_joints: + a_idx = sim_a_joints.index(b_joint) + # mapping_tensor[b_idx, a_idx] = 1.0 + mapping_tensor[a_idx, b_idx] = 1.0 # export policy as a jit module (used to run it from C++) if EXPORT_POLICY: path = os.path.join(LEGGED_GYM_ROOT_DIR, 'logs', train_cfg.runner.experiment_name, 'exported', 'policies') @@ -40,11 +114,28 @@ def play(args): print('Exported policy as jit script to: ', path) for i in range(10*int(env.max_episode_length)): + obs[..., 9:38] = obs[..., 9:38] @ mapping_tensor.transpose(0, 1) + obs[..., 38:67] = obs[..., 38:67] @ mapping_tensor.transpose(0, 1) + obs[..., 67:96] = obs[..., 67:96] @ mapping_tensor.transpose(0, 1) + # from icecream import ic + # ic( + # obs[..., :9], + # obs[..., 9:38], + # obs[..., 38:67], + # obs[..., 67:96], + + # ) actions = policy(obs.detach()) - obs, _, rews, dones, infos = env.step(actions.detach()) + # ic( + # actions + # ) + reordered_actions = actions @ mapping_tensor + # obs, _, rews, dones, infos = env.step(actions.detach()) + obs, _, rews, dones, infos = env.step(reordered_actions.detach()) if __name__ == '__main__': EXPORT_POLICY = True + # EXPORT_POLICY = False RECORD_FRAMES = False MOVE_CAMERA = False args = get_args() diff --git a/resources/robots/g1_description/g1_29dof_rev_1_0.urdf b/resources/robots/g1_description/g1_29dof_rev_1_0.urdf index e7a551d..9e6248e 100644 --- a/resources/robots/g1_description/g1_29dof_rev_1_0.urdf +++ b/resources/robots/g1_description/g1_29dof_rev_1_0.urdf @@ -1,3 +1,4 @@ + @@ -215,6 +216,12 @@ + + + + + + @@ -407,6 +414,12 @@ + + + + + + @@ -830,6 +843,12 @@ + + + + + + @@ -1054,5 +1073,11 @@ + + + + + + \ No newline at end of file diff --git a/resources/robots/g1_description/g1_29dof_rev_1_0.xml b/resources/robots/g1_description/g1_29dof_rev_1_0.xml index 16b799d..9590593 100644 --- a/resources/robots/g1_description/g1_29dof_rev_1_0.xml +++ b/resources/robots/g1_description/g1_29dof_rev_1_0.xml @@ -1,6 +1,11 @@ + + + + + diff --git a/resources/robots/g1_description/g1_29dof_rev_1_0_lidar.urdf b/resources/robots/g1_description/g1_29dof_rev_1_0_lidar.urdf new file mode 100755 index 0000000..ff28318 --- /dev/null +++ b/resources/robots/g1_description/g1_29dof_rev_1_0_lidar.urdf @@ -0,0 +1,1191 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/resources/robots/g1_description/meshes/zed2.obj b/resources/robots/g1_description/meshes/zed2.obj new file mode 100644 index 0000000..23a82fc --- /dev/null +++ b/resources/robots/g1_description/meshes/zed2.obj @@ -0,0 +1,1964 @@ +#### +# +# OBJ File Generated by Meshlab +# +#### +# Object zed2.obj +# +# Vertices: 488 +# Faces: 972 +# +#### +vn 4.2667179 -1.0842435 -4.4520340 +v -0.0028573 0.0007336 -0.0132939 +vn 5.4522343 -0.0000000 -2.3478677 +v -0.0027618 0.0000000 -0.0131057 +vn 4.4069467 0.0058815 -4.4465456 +v -0.0029500 0.0000000 -0.0132939 +vn 5.2635460 -1.4182539 -2.3493881 +v -0.0026677 0.0007148 -0.0131057 +vn 4.7117510 -2.7412300 -2.3503785 +v -0.0023918 0.0013809 -0.0131057 +vn 3.8734643 -2.1138744 -4.4412055 +v -0.0025851 0.0014212 -0.0132939 +vn 3.8377247 -3.8744259 -2.3487175 +v -0.0019529 0.0019529 -0.0131057 +vn 3.2372143 -3.0006638 -4.4401107 +v -0.0021505 0.0020194 -0.0132939 +vn 2.7017858 -4.7416682 -2.3450508 +v -0.0013809 0.0023918 -0.0131057 +vn 2.3871369 -3.7019913 -4.4506421 +v -0.0015807 0.0024908 -0.0132939 +vn 1.3814642 -5.2848654 -2.3401642 +v -0.0007148 0.0026677 -0.0131057 +vn 1.3878894 -4.1725440 -4.4591355 +v -0.0009116 0.0028056 -0.0132939 +vn -0.0333861 -5.4678030 -2.3347163 +v -0.0000000 0.0027618 -0.0131057 +vn 0.3045112 -4.3830032 -4.4629927 +v -0.0001852 0.0029442 -0.0132939 +vn -1.4466727 -5.2786622 -2.3292408 +v 0.0007148 0.0026677 -0.0131057 +vn -0.7989537 -4.3202262 -4.4624677 +v 0.0005528 0.0028977 -0.0132939 +vn -2.7625468 -4.7306366 -2.3242304 +v 0.0013809 0.0023918 -0.0131057 +vn -1.8580587 -3.9868612 -4.4557538 +v 0.0012561 0.0026692 -0.0132939 +vn -2.7825520 -3.4047866 -4.4569321 +v 0.0018804 0.0022730 -0.0132939 +vn -3.8918056 -3.8612761 -2.3199790 +v 0.0019529 0.0019529 -0.0131057 +vn -3.5383985 -2.5926490 -4.4689913 +v 0.0023866 0.0017340 -0.0132939 +vn -4.7579727 -2.7297015 -2.3167596 +v 0.0023918 0.0013809 -0.0131057 +vn -4.0688705 -1.6214995 -4.4764190 +v 0.0027428 0.0010860 -0.0132939 +vn -5.3023076 -1.4127825 -2.3147752 +v 0.0026677 0.0007148 -0.0131057 +vn -4.3420801 -0.5519311 -4.4799042 +v 0.0029267 0.0003697 -0.0132939 +vn -5.4879060 0.0000000 -2.3141246 +v 0.0027618 0.0000000 -0.0131057 +vn -4.3393044 0.5549886 -4.4820485 +v 0.0029267 -0.0003697 -0.0132939 +vn -5.3023071 1.4127824 -2.3147752 +v 0.0026677 -0.0007148 -0.0131057 +vn -4.0717573 1.6317419 -4.4684772 +v 0.0027428 -0.0010860 -0.0132939 +vn -4.7579727 2.7297015 -2.3167596 +v 0.0023918 -0.0013809 -0.0131057 +vn -3.5463109 2.5880985 -4.4644918 +v 0.0023866 -0.0017340 -0.0132939 +vn -3.8918052 3.8612761 -2.3199790 +v 0.0019529 -0.0019529 -0.0131057 +vn -2.7816224 3.3904831 -4.4706168 +v 0.0018804 -0.0022730 -0.0132939 +vn -2.7625468 4.7306366 -2.3242304 +v 0.0013809 -0.0023918 -0.0131057 +vn -1.8457437 3.9779205 -4.4715128 +v 0.0012561 -0.0026692 -0.0132939 +vn -1.4466727 5.2786627 -2.3292408 +v 0.0007148 -0.0026677 -0.0131057 +vn -0.7942672 4.3165979 -4.4676852 +v 0.0005528 -0.0028977 -0.0132939 +vn -0.0333861 5.4678030 -2.3347163 +v -0.0000000 -0.0027618 -0.0131057 +vn 0.3088832 4.3864594 -4.4586220 +v -0.0001852 -0.0029442 -0.0132939 +vn 1.3814642 5.2848649 -2.3401642 +v -0.0007148 -0.0026677 -0.0131057 +vn 1.3986024 4.1830173 -4.4436226 +v -0.0009116 -0.0028056 -0.0132939 +vn 2.7017860 4.7416682 -2.3450506 +v -0.0013809 -0.0023918 -0.0131057 +vn 2.3791125 3.7146778 -4.4430442 +v -0.0015807 -0.0024908 -0.0132939 +vn 3.8377247 3.8744259 -2.3487175 +v -0.0019529 -0.0019529 -0.0131057 +vn 3.2262006 2.9983628 -4.4511919 +v -0.0021505 -0.0020194 -0.0132939 +vn 4.7117505 2.7412300 -2.3503785 +v -0.0023918 -0.0013809 -0.0131057 +vn 3.8692253 2.1037841 -4.4511256 +v -0.0025851 -0.0014212 -0.0132939 +vn 5.2635460 1.4182539 -2.3493881 +v -0.0026677 -0.0007148 -0.0131057 +vn 4.2737584 1.0795176 -4.4459105 +v -0.0028573 -0.0007336 -0.0132939 +vn 2.3347449 -0.2850048 5.2252979 +v 0.0140073 -0.0755000 0.0138973 +vn -2.0770531 0.1781371 5.3203745 +v -0.0165034 0.0755000 0.0149628 +vn -2.0451996 -0.3053545 5.2868061 +v -0.0165034 -0.0755000 0.0149628 +vn 2.3542242 0.2530862 5.2270083 +v 0.0140073 0.0755000 0.0138973 +vn 5.3886805 0.1223951 2.1899123 +v 0.0150000 0.0755000 0.0128329 +vn 5.3951125 -0.1003829 2.1852632 +v 0.0150000 -0.0755000 0.0128329 +vn 2.2912090 -3.1370435 4.1011858 +v 0.0140067 -0.0810210 0.0119884 +vn -2.2033987 -2.3226578 4.3951845 +v -0.0165034 -0.0800002 0.0138728 +vn -2.1475306 -3.2597244 3.8842211 +v -0.0165034 -0.0819994 0.0125774 +vn 2.3058128 -5.1393929 -0.6035562 +v 0.0140068 -0.0862775 -0.0020571 +vn -2.0229964 -5.0520453 -1.4136003 +v -0.0165034 -0.0868043 -0.0049130 +vn 2.2989426 -4.9442739 -1.5583651 +v 0.0140073 -0.0856206 -0.0051625 +vn -2.0845268 -4.6755419 -2.1921430 +v -0.0165034 -0.0857024 -0.0078368 +vn -2.1827717 -4.0534792 -2.9697375 +v -0.0165034 -0.0841501 -0.0103588 +vn 2.2939403 -4.5305333 -2.4951818 +v 0.0140067 -0.0844487 -0.0079431 +vn 2.2884345 -2.2177038 4.6698341 +v 0.0140073 -0.0793299 0.0130150 +vn 2.2799110 -3.8943074 -3.3923204 +v 0.0140073 -0.0828214 -0.0103037 +vn -2.2810960 -3.2504134 -3.6424046 +v -0.0165034 -0.0822554 -0.0123644 +vn 2.2811606 -3.9439359 3.3267565 +v 0.0140072 -0.0828922 0.0102215 +vn -2.0420656 -3.9413967 3.3878403 +v -0.0165034 -0.0837422 0.0108666 +vn -2.0584602 -4.5114565 2.5330896 +v -0.0165034 -0.0853963 0.0084307 +vn 2.2629406 -3.0283086 -4.1869569 +v 0.0140074 -0.0808814 -0.0120902 +vn -2.1792305 -2.4507246 -4.3669987 +v -0.0165034 -0.0802652 -0.0137344 +vn 2.2815797 -1.9951400 -4.7695303 +v 0.0140074 -0.0788063 -0.0132456 +vn -2.2020543 -1.5100051 -4.8038225 +v -0.0165034 -0.0780664 -0.0146174 +vn 2.3040624 -0.9866552 -5.0784588 +v 0.0140073 -0.0771830 -0.0137317 +vn -2.4622509 -0.6105934 -4.6882915 +v -0.0165034 -0.0767952 -0.0148755 +vn -2.0155876 -5.0151939 1.4868627 +v -0.0165034 -0.0866797 0.0053433 +vn 2.2835999 -4.5748343 2.4072437 +v 0.0140067 -0.0845430 0.0077670 +vn 2.3542438 -0.2531228 -5.2271409 +v 0.0140073 -0.0755000 -0.0138973 +vn -2.0771096 -0.1778600 -5.3202662 +v -0.0165034 -0.0755000 -0.0149628 +vn -2.1944125 -5.0101781 0.4016878 +v -0.0165034 -0.0873675 0.0018907 +vn 2.2913294 -4.9782109 1.4191155 +v 0.0140073 -0.0857320 0.0047903 +vn 2.2618058 -1.1587151 5.0459585 +v 0.0140073 -0.0774610 0.0136718 +vn -2.0601656 -1.2909945 4.9646177 +v -0.0165034 -0.0778071 0.0146843 +vn -2.1947198 -5.0079541 -0.5514156 +v -0.0165034 -0.0873830 -0.0017304 +vn 2.2931287 -5.1648445 0.4098687 +v 0.0140070 -0.0863384 0.0014443 +vn 2.2912407 3.1370616 -4.1011691 +v 0.0140067 0.0810210 -0.0119883 +vn -2.0269940 2.3878150 -4.6044698 +v -0.0165034 0.0800003 -0.0138728 +vn -2.0229428 3.2270308 -4.0988913 +v -0.0165034 0.0819995 -0.0125773 +vn 2.2989116 4.9440413 1.5590687 +v 0.0140073 0.0856197 0.0051653 +vn 2.3058484 5.1393156 0.6040951 +v 0.0140069 0.0862773 0.0020585 +vn -2.0233860 5.0508199 1.4161327 +v -0.0165034 0.0868039 0.0049148 +vn -2.1853149 4.0508575 2.9677706 +v -0.0165034 0.0841493 0.0103598 +vn 2.2940252 4.5302992 2.4956379 +v 0.0140068 0.0844482 0.0079439 +vn -2.0842104 4.6757779 2.1927705 +v -0.0165034 0.0857019 0.0078375 +vn 2.2884116 2.2177281 -4.6698189 +v 0.0140073 0.0793299 -0.0130150 +vn -2.2816613 3.2500167 3.6420183 +v -0.0165034 0.0822560 0.0123639 +vn 2.2798481 3.8939421 3.3927088 +v 0.0140073 0.0828205 0.0103048 +vn 2.2811146 3.9439769 -3.3267169 +v 0.0140072 0.0828924 -0.0102214 +vn -2.2367718 3.7344198 -3.2594681 +v -0.0165034 0.0837422 -0.0108665 +vn -2.2437932 4.3007135 -2.4341371 +v -0.0165034 0.0853964 -0.0084303 +vn 2.2629588 3.0277648 4.1873674 +v 0.0140074 0.0808803 0.0120909 +vn -2.1803882 2.4509306 4.3654418 +v -0.0165034 0.0802673 0.0137333 +vn 2.2815540 1.9945194 4.7698016 +v 0.0140074 0.0788050 0.0132461 +vn -2.2046862 1.5103827 4.8001227 +v -0.0165034 0.0780703 0.0146164 +vn 2.3041809 0.9862630 5.0785880 +v 0.0140074 0.0771823 0.0137318 +vn -2.4618573 0.6112890 4.6885157 +v -0.0165034 0.0767971 0.0148753 +vn -2.1614907 4.8126626 -1.5643429 +v -0.0165034 0.0866799 -0.0053427 +vn 2.2835996 4.5748940 -2.4071405 +v 0.0140067 0.0845430 -0.0077668 +vn -2.0102975 5.2152252 -0.5496980 +v -0.0165034 0.0873676 -0.0018893 +vn 2.2912874 4.9782815 -1.4188802 +v 0.0140073 0.0857322 -0.0047896 +vn 2.3347433 0.2850032 -5.2252388 +v 0.0140073 0.0755000 -0.0138973 +vn -2.0482903 0.3120315 -5.2829351 +v -0.0165034 0.0755000 -0.0149628 +vn 2.2618113 1.1587100 -5.0459571 +v 0.0140073 0.0774610 -0.0136718 +vn -1.9916006 1.2504903 -5.0538845 +v -0.0165034 0.0778071 -0.0146843 +vn -2.1948085 5.0078545 0.5516355 +v -0.0165034 0.0873829 0.0017315 +vn 2.2931113 5.1648464 -0.4093500 +v 0.0140070 0.0863386 -0.0014422 +vn 0.9560990 -1.2832589 -0.6649004 +v 0.0150000 0.0370386 0.0008564 +vn 4.7123899 -1.5707965 -1.5707966 +v 0.0150000 0.0353378 0.0008564 +vn 3.7562783 -2.8540566 0.9058969 +v 0.0150000 0.0352573 -0.0016669 +vn 4.7124023 1.5707964 1.5707963 +v 0.0150000 0.0375817 -0.0016848 +vn 3.7595015 2.8511384 -0.9100140 +v 0.0150000 0.0375370 0.0008497 +vn 0.9528863 1.2803421 0.6607824 +v 0.0150000 0.0357356 -0.0016848 +vn 4.7123890 1.5707964 1.5707971 +v 0.0150000 0.0375370 0.0012497 +vn 4.7123833 1.5707964 1.5707967 +v 0.0150000 0.0401006 -0.0016848 +vn 1.5707963 1.5707965 -1.5707964 +v 0.0150000 0.0384623 -0.0004489 +vn 1.5707964 1.5707964 1.5707964 +v 0.0150000 0.0384623 -0.0016848 +vn 4.7123890 1.5707964 -1.5707964 +v 0.0150000 0.0399888 -0.0004489 +vn 4.7123880 1.5707965 1.5707964 +v 0.0150000 0.0399888 -0.0000555 +vn 5.3518405 -2.1511018 -0.5794978 +v 0.0150000 -0.0847390 -0.0043896 +vn 5.3477931 -2.2147098 0.2775192 +v 0.0150000 -0.0852236 0.0018998 +vn 5.3410969 -2.2345028 -0.1393410 +v 0.0150000 -0.0852911 -0.0011742 +vn 5.3451209 -2.1231556 0.6944491 +v 0.0150000 -0.0845841 0.0049099 +vn 5.3461637 -2.0050960 -0.9827067 +v 0.0150000 -0.0837590 -0.0069600 +vn 5.3405032 -1.9425631 1.1099166 +v 0.0150000 -0.0834382 0.0075691 +vn 5.3378167 -1.7566755 -1.3895705 +v 0.0150000 -0.0822920 -0.0092769 +vn 5.3357420 -1.6501462 1.5159013 +v 0.0150000 -0.0818325 0.0098140 +vn 5.3382497 -1.4150696 -1.7363232 +v 0.0150000 -0.0805986 -0.0109693 +vn 5.3455753 -1.2419951 1.8536012 +v 0.0150000 -0.0799536 0.0114387 +vn 5.3335066 -1.0212219 -1.9993399 +v 0.0150000 -0.0790510 -0.0119660 +vn 5.3479185 -0.7882748 2.0873485 +v 0.0150000 -0.0781593 0.0123543 +vn 5.3474874 -0.5283639 -2.1656969 +v 0.0150000 -0.0773333 -0.0126077 +vn 5.3503332 -0.3773690 2.1973858 +v 0.0150000 -0.0768502 0.0127112 +vn 5.3886900 -0.1224143 -2.1899250 +v 0.0150000 -0.0755000 -0.0128329 +vn 3.3667257 2.9001756 1.1486397 +v 0.0150000 -0.0672755 -0.0028806 +vn 3.3674760 2.5232153 1.8339376 +v 0.0150000 -0.0663306 -0.0045994 +vn 3.3685710 1.9876227 2.4038849 +v 0.0150000 -0.0649878 -0.0060293 +vn 3.3700435 1.3271137 2.8226247 +v 0.0150000 -0.0633317 -0.0070803 +vn 3.3718894 0.5832403 3.0638499 +v 0.0150000 -0.0614663 -0.0076864 +vn 3.3741016 -0.1971928 3.1124187 +v 0.0150000 -0.0595087 -0.0078096 +vn 5.3950772 0.1003830 -2.1852636 +v 0.0150000 0.0755000 -0.0128329 +vn 3.3663719 3.0947766 0.3910846 +v 0.0150000 -0.0677633 -0.0009807 +vn 3.3766904 -0.9650931 2.9653139 +v 0.0150000 -0.0575819 -0.0074420 +vn 3.3796468 -1.6721681 2.6318376 +v 0.0150000 -0.0558072 -0.0066069 +vn 3.3829892 -2.2739735 2.1330099 +v 0.0150000 -0.0542958 -0.0053566 +vn 3.3867068 -2.7327065 1.5002437 +v 0.0150000 -0.0531429 -0.0037697 +vn 3.3907976 -3.0195665 0.7733817 +v 0.0150000 -0.0524208 -0.0019460 +vn 3.3929205 -3.1168199 0.0000000 +v 0.0150000 -0.0521750 0.0000000 +vn 4.7123895 -1.5707964 -1.5707970 +v 0.0150000 0.0352573 -0.0020782 +vn 4.7123890 1.5707964 -1.5707964 +v 0.0150000 0.0375817 -0.0020782 +vn 4.7123871 -1.5707966 -1.5707967 +v 0.0150000 0.0380198 -0.0020782 +vn 3.3718860 0.5832404 3.0638499 +v 0.0150000 0.0585337 -0.0076864 +vn 4.7123890 1.5707964 -1.5707964 +v 0.0150000 0.0401006 -0.0020782 +vn 4.7124562 -1.5707966 -1.5707960 +v 0.0150000 0.0405878 -0.0020782 +vn 4.7123890 1.5707964 -1.5707964 +v 0.0150000 0.0401006 0.0008564 +vn 4.7123890 1.5707965 1.5707967 +v 0.0150000 0.0401006 0.0012497 +vn 3.3700676 1.3271148 2.8226237 +v 0.0150000 0.0566683 -0.0070803 +vn 3.1779675 0.0571250 -3.1405535 +v 0.0150000 0.0413544 -0.0020782 +vn 3.2401154 0.2683797 -3.1262832 +v 0.0150000 0.0417229 -0.0020648 +vn 3.3065584 0.6752520 -3.0572283 +v 0.0150000 0.0420193 -0.0020245 +vn 3.3214934 1.1890295 -2.8941619 +v 0.0150000 0.0422777 -0.0019446 +vn 3.3839791 1.7633286 -2.5721629 +v 0.0150000 0.0425322 -0.0018122 +vn 3.4056046 2.3482060 -2.0456166 +v 0.0150000 0.0428113 -0.0015664 +vn 3.3845682 2.7899148 -1.3932080 +v 0.0150000 0.0430273 -0.0012400 +vn 3.3773341 3.0419989 -0.6923575 +v 0.0150000 0.0431656 -0.0008512 +vn 3.3445518 3.1253996 -0.0138646 +v 0.0150000 0.0432117 -0.0004176 +vn 3.3685679 1.9876239 2.4038842 +v 0.0150000 0.0550122 -0.0060293 +vn 3.3674717 2.5232131 1.8339423 +v 0.0150000 0.0536694 -0.0045994 +vn 3.3667326 2.9001744 1.1486405 +v 0.0150000 0.0527245 -0.0028806 +vn 3.3663712 3.0947771 0.3910827 +v 0.0150000 0.0522367 -0.0009807 +vn 3.3741024 -0.1971936 3.1124187 +v 0.0150000 0.0604913 -0.0078096 +vn 3.3766897 -0.9650937 2.9653134 +v 0.0150000 0.0624181 -0.0074420 +vn 3.3796477 -1.6721693 2.6318367 +v 0.0150000 0.0641928 -0.0066069 +vn 3.3829858 -2.2739739 2.1330106 +v 0.0150000 0.0657042 -0.0053566 +vn 3.3867054 -2.7327037 1.5002502 +v 0.0150000 0.0668571 -0.0037697 +vn 3.3908060 -3.0195649 0.7733815 +v 0.0150000 0.0675792 -0.0019460 +vn 3.3929150 -3.1168211 0.0000000 +v 0.0150000 0.0678250 0.0000000 +vn 3.3741016 -0.1971928 -3.1124187 +v 0.0150000 -0.0595087 0.0078096 +vn 3.3718894 0.5832403 -3.0638499 +v 0.0150000 -0.0614663 0.0076864 +vn 3.3700435 1.3271139 -2.8226247 +v 0.0150000 -0.0633317 0.0070803 +vn 3.3685710 1.9876227 -2.4038846 +v 0.0150000 -0.0649878 0.0060293 +vn 3.3674760 2.5232151 -1.8339376 +v 0.0150000 -0.0663306 0.0045994 +vn 3.3667259 2.9001756 -1.1486397 +v 0.0150000 -0.0672755 0.0028806 +vn 3.3663716 3.0947766 -0.3910846 +v 0.0150000 -0.0677633 0.0009807 +vn 3.3766904 -0.9650931 -2.9653139 +v 0.0150000 -0.0575819 0.0074420 +vn 3.3796468 -1.6721681 -2.6318374 +v 0.0150000 -0.0558072 0.0066069 +vn 3.3829892 -2.2739735 -2.1330099 +v 0.0150000 -0.0542958 0.0053566 +vn 3.3867068 -2.7327065 -1.5002437 +v 0.0150000 -0.0531429 0.0037697 +vn 3.3907976 -3.0195665 -0.7733817 +v 0.0150000 -0.0524208 0.0019460 +vn 4.7123804 -1.5707964 1.5707964 +v 0.0150000 0.0353378 0.0012497 +vn 3.3718977 0.5832404 -3.0638499 +v 0.0150000 0.0585337 0.0076864 +vn 4.7125177 -1.5707964 1.5707964 +v 0.0150000 0.0380198 0.0012497 +vn 3.3700445 1.3271148 -2.8226237 +v 0.0150000 0.0566683 0.0070803 +vn 4.7123890 -1.5707964 1.5707964 +v 0.0150000 0.0405878 0.0012497 +vn 3.1835961 0.0659593 3.1402073 +v 0.0150000 0.0413455 0.0012497 +vn 3.2643104 0.3235337 3.1189451 +v 0.0150000 0.0417642 0.0012321 +vn 3.3026829 0.7603508 3.0376930 +v 0.0150000 0.0420819 0.0011793 +vn 3.3023133 1.2372227 2.8766799 +v 0.0150000 0.0423291 0.0010958 +vn 3.3687403 1.7627208 2.5759773 +v 0.0150000 0.0425367 0.0009860 +vn 3.4074638 2.3324237 2.0630119 +v 0.0150000 0.0428222 0.0007388 +vn 3.3362441 2.7530231 1.4823208 +v 0.0150000 0.0430351 0.0004217 +vn 3.2707372 2.9638255 1.0218844 +v 0.0150000 0.0431124 0.0002372 +vn 3.3119979 3.0783567 0.5673166 +v 0.0150000 0.0431675 0.0000359 +vn 3.3685687 1.9876239 -2.4038842 +v 0.0150000 0.0550122 0.0060293 +vn 3.3674717 2.5232131 -1.8339422 +v 0.0150000 0.0536694 0.0045994 +vn 3.3667326 2.9001744 -1.1486405 +v 0.0150000 0.0527245 0.0028806 +vn 3.3663716 3.0947771 -0.3910827 +v 0.0150000 0.0522367 0.0009807 +vn 3.3741024 -0.1971936 -3.1124187 +v 0.0150000 0.0604913 0.0078096 +vn 3.3766897 -0.9650937 -2.9653134 +v 0.0150000 0.0624181 0.0074420 +vn 3.3796477 -1.6721693 -2.6318367 +v 0.0150000 0.0641928 0.0066069 +vn 3.3829858 -2.2739739 -2.1330106 +v 0.0150000 0.0657042 0.0053566 +vn 3.3867054 -2.7327037 -1.5002503 +v 0.0150000 0.0668571 0.0037697 +vn 3.3908060 -3.0195649 -0.7733815 +v 0.0150000 0.0675792 0.0019460 +vn 5.3503351 0.3773670 -2.1973841 +v 0.0150000 0.0768502 -0.0127112 +vn 5.3475156 0.5283507 2.1656728 +v 0.0150000 0.0773333 0.0126077 +vn 5.3479218 0.7882749 -2.0873451 +v 0.0150000 0.0781593 -0.0123543 +vn 5.3334990 1.0211935 1.9993658 +v 0.0150000 0.0790510 0.0119660 +vn 5.3455777 1.2419924 -1.8536000 +v 0.0150000 0.0799536 -0.0114387 +vn 5.3382487 1.4150726 1.7363276 +v 0.0150000 0.0805986 0.0109693 +vn 5.3357325 1.6501513 -1.5159101 +v 0.0150000 0.0818325 -0.0098140 +vn 5.3378077 1.7567073 1.3895506 +v 0.0150000 0.0822920 0.0092769 +vn 5.3404980 1.9425647 -1.1099255 +v 0.0150000 0.0834382 -0.0075691 +vn 5.3462152 2.0050349 0.9827251 +v 0.0150000 0.0837590 0.0069600 +vn 5.3450933 2.1231749 -0.6944827 +v 0.0150000 0.0845841 -0.0049099 +vn 5.3517809 2.1511865 0.5794379 +v 0.0150000 0.0847390 0.0043896 +vn 5.3477440 2.2147536 -0.2775936 +v 0.0150000 0.0852236 -0.0018998 +vn 5.3411007 2.2345014 0.1393692 +v 0.0150000 0.0852911 0.0011742 +vn 1.5707964 1.5707964 -1.5707967 +v 0.0150000 0.0384623 0.0008564 +vn 1.5707964 1.5707964 1.5707964 +v 0.0150000 0.0384623 -0.0000555 +vn -0.5063803 -0.9041732 -6.0828876 +v 0.0026585 0.0034880 -0.0142936 +vn -1.4405509 -3.0331955 -5.2042575 +v 0.0016340 0.0032983 -0.0140250 +vn -0.2731597 -1.0422812 -6.0769901 +v 0.0017677 0.0040398 -0.0143248 +vn -0.7420222 -3.2908340 -5.1936636 +v 0.0008870 0.0036045 -0.0140561 +vn 1.8809973 0.1329953 -5.9208651 +v -0.0043362 -0.0000000 -0.0145068 +vn 0.5900944 0.0071821 -6.2441683 +v -0.0045825 -0.0000000 -0.0145465 +vn 1.1969373 0.0194996 -6.0835171 +v -0.0045509 0.0005313 -0.0145454 +vn -2.0607071 -2.6415770 -5.2083635 +v 0.0022974 0.0028404 -0.0139972 +vn -0.7041086 -0.7107143 -6.0878730 +v 0.0034127 0.0027222 -0.0142673 +vn 2.9822991 0.1197275 -5.4635549 +v -0.0041073 -0.0000000 -0.0144073 +vn -2.5968373 -2.1196213 -5.2042837 +v 0.0028470 0.0022521 -0.0139742 +vn -0.8467394 -0.4867120 -6.0905576 +v 0.0039385 0.0018514 -0.0142490 +vn -3.0022438 -1.4851092 -5.2036848 +v 0.0032583 0.0015610 -0.0139569 +vn 3.4621029 -0.4331464 -5.1278572 +v -0.0038571 0.0006280 -0.0142521 +vn 1.2778080 -0.2826374 -6.0414147 +v -0.0044565 0.0010527 -0.0145421 +vn 4.1366835 0.0456726 -4.7176023 +v -0.0039103 -0.0000000 -0.0142542 +vn -0.8688855 -0.1185320 -6.1177759 +v 0.0042530 0.0008799 -0.0142380 +vn -3.3486826 -0.6604121 -5.1682987 +v 0.0035124 0.0007987 -0.0139463 +vn 3.2602701 -1.1065484 -5.1580429 +v -0.0037000 0.0012381 -0.0142456 +vn 1.1822110 -0.4943806 -6.0495596 +v -0.0040904 0.0020370 -0.0145293 +vn -0.1880862 -0.0077537 -6.2670240 +v 0.0043407 0.0000000 -0.0142349 +vn -1.6263337 -0.1575790 -5.9761891 +v 0.0040695 0.0000000 -0.0142072 +vn -2.8120527 -0.1171274 -5.5377812 +v 0.0038159 0.0000000 -0.0141073 +vn 2.9636388 -1.7112486 -5.1733027 +v -0.0033496 0.0019722 -0.0142312 +vn 1.0581572 -0.7011238 -6.0503135 +v -0.0035350 0.0028705 -0.0145099 +vn -4.1175399 -0.0283688 -4.7319865 +v 0.0035987 0.0000000 -0.0139426 +vn 2.5496244 -2.2628145 -5.1802578 +v -0.0028498 0.0026135 -0.0142107 +vn 0.9039624 -0.8638521 -6.0528216 +v -0.0029099 0.0034803 -0.0144881 +vn 0.7116355 -0.9950880 -6.0568919 +v -0.0021204 0.0039860 -0.0144605 +vn 2.0279963 -2.7405124 -5.1777000 +v -0.0022239 0.0031314 -0.0141849 +vn 0.4835829 -1.0882639 -6.0616403 +v -0.0011944 0.0043279 -0.0144282 +vn 1.4081038 -3.1026649 -5.1769762 +v -0.0015021 0.0035027 -0.0141552 +vn 0.2319530 -1.1309389 -6.0666986 +v -0.0001848 0.0044584 -0.0143929 +vn 0.7165380 -3.3244596 -5.1790857 +v -0.0007183 0.0037097 -0.0141228 +vn -0.0255141 -1.1165851 -6.0716128 +v 0.0008277 0.0043569 -0.0143576 +vn -0.0124862 -3.3889339 -5.1851487 +v 0.0000907 0.0037441 -0.0140892 +vn 1.7938832 2.8947654 -5.1817484 +v -0.0019557 -0.0032928 -0.0141739 +vn 0.6860059 1.0066189 -6.0579500 +v -0.0020095 -0.0040398 -0.0144567 +vn 0.8991339 0.8588377 -6.0551758 +v -0.0029003 -0.0034880 -0.0144878 +vn 1.1655117 3.1845810 -5.1892967 +v -0.0012163 -0.0035993 -0.0141434 +vn -0.8384452 -0.0320381 -6.1304159 +v 0.0043091 -0.0005313 -0.0142360 +vn 2.3663375 2.4762948 -5.1687274 +v -0.0026140 -0.0028361 -0.0142010 +vn 1.0742940 0.6653025 -6.0530415 +v -0.0036545 -0.0027222 -0.0145141 +vn 2.8261986 1.9512177 -5.1631384 +v -0.0031606 -0.0022494 -0.0142235 +vn 1.1956441 0.4468452 -6.0521445 +v -0.0041803 -0.0018514 -0.0145325 +vn -3.3344905 0.5553043 -5.1813416 +v 0.0035345 -0.0006895 -0.0139453 +vn 3.1663208 1.3439131 -5.1612935 +v -0.0035705 -0.0015597 -0.0142403 +vn -0.9433799 0.3091175 -6.0823784 +v 0.0042147 -0.0010527 -0.0142393 +vn 3.4454079 0.5973402 -5.1310682 +v -0.0038241 -0.0007982 -0.0142507 +vn 1.2132491 0.1170590 -6.0747948 +v -0.0044949 -0.0008799 -0.0145435 +vn -0.8338714 0.5369745 -6.0862169 +v 0.0038486 -0.0020370 -0.0142521 +vn -3.0744567 1.2996287 -5.2107844 +v 0.0033450 -0.0013558 -0.0139533 +vn -0.6890918 0.7534941 -6.0818939 +v 0.0032932 -0.0028705 -0.0142715 +vn -2.7223091 1.9281619 -5.2129097 +v 0.0029815 -0.0020607 -0.0139686 +vn -0.5135253 0.9161606 -6.0782423 +v 0.0026681 -0.0034803 -0.0142933 +vn -2.2340384 2.4880943 -5.2066617 +v 0.0024777 -0.0026744 -0.0139897 +vn -0.3016946 1.0373205 -6.0751352 +v 0.0018786 -0.0039860 -0.0143209 +vn -1.6429985 2.9312539 -5.1975322 +v 0.0018550 -0.0031685 -0.0140157 +vn -0.0594355 1.1135110 -6.0710959 +v 0.0009526 -0.0043279 -0.0143532 +vn -0.9711951 3.2312877 -5.1911268 +v 0.0011414 -0.0035212 -0.0140455 +vn 0.1980200 1.1346053 -6.0665455 +v -0.0000570 -0.0044584 -0.0143885 +vn -0.2537896 3.3755047 -5.1866856 +v 0.0003681 -0.0037154 -0.0140777 +vn 0.4513988 1.0983906 -6.0617838 +v -0.0010695 -0.0043569 -0.0144238 +vn 0.4709952 3.3578696 -5.1861186 +v -0.0004297 -0.0037422 -0.0141108 +vn -5.3278913 0.0809663 2.2536426 +v -0.0166000 0.0755000 0.0148662 +vn -4.7141085 2.4959602 -1.0755442 +v -0.0166000 0.0855216 -0.0079833 +vn -4.8033772 2.1805642 -1.5300900 +v -0.0166000 0.0839219 -0.0104944 +vn -5.1648335 0.5167835 2.3447719 +v -0.0166000 0.0777641 0.0145953 +vn -5.2431688 1.5012926 -1.7797538 +v -0.0166000 0.0819796 -0.0124691 +vn -4.8412333 1.0282636 2.4309423 +v -0.0166000 0.0799128 0.0138086 +vn -4.7420640 1.5605650 2.2049410 +v -0.0166000 0.0818346 0.0125845 +vn -5.2026567 0.9712850 -2.1546752 +v -0.0166000 0.0798838 -0.0138230 +vn -4.9494853 1.9036092 1.7208806 +v -0.0166000 0.0838876 0.0105373 +vn -5.1650081 2.0932453 1.1712698 +v -0.0166000 0.0855499 0.0079268 +vn -5.2311540 0.5006062 -2.2826474 +v -0.0166000 0.0777527 -0.0145981 +vn -5.2661595 2.2302456 0.5964065 +v -0.0166000 0.0867160 0.0048697 +vn -4.9438095 2.5704851 0.0386907 +v -0.0166000 0.0873135 0.0014018 +vn -4.9706321 2.4916420 -0.5501109 +v -0.0166000 0.0866716 -0.0050285 +vn -4.9354882 -2.5359523 0.4560342 +v -0.0166000 -0.0872351 0.0022099 +vn -5.3279343 -0.0808706 -2.2536063 +v -0.0166000 -0.0755000 -0.0148662 +vn -5.1695142 -2.1649861 1.0223632 +v -0.0166000 -0.0853729 0.0082702 +vn -5.2052970 -1.8743727 1.4385403 +v -0.0166000 -0.0837294 0.0107313 +vn -5.1672735 -0.5164185 -2.3427207 +v -0.0166000 -0.0777644 -0.0145952 +vn -5.0472722 -1.4162412 2.0525010 +v -0.0166000 -0.0817557 0.0126468 +vn -4.8440161 -1.0272036 -2.4294214 +v -0.0166000 -0.0799127 -0.0138086 +vn -4.8397412 -0.9627649 2.4583879 +v -0.0166000 -0.0796411 0.0139392 +vn -4.7431583 -1.5600566 -2.2044919 +v -0.0166000 -0.0818353 -0.0125840 +vn -4.9538713 -1.9017990 -1.7182239 +v -0.0166000 -0.0838905 -0.0105337 +vn -5.0888987 -0.3950288 2.4280217 +v -0.0166000 -0.0776208 0.0146289 +vn -5.1645346 -2.0935116 -1.1716186 +v -0.0166000 -0.0855502 -0.0079265 +vn -5.2667804 -2.2289994 -0.5988075 +v -0.0166000 -0.0867158 -0.0048707 +vn -4.9443421 -2.5701039 -0.0383084 +v -0.0166000 -0.0873136 -0.0014012 +vn -4.4427543 0.0000000 -4.4430118 +v -0.0165517 0.0000000 -0.0149145 +vn -5.2995667 0.1387187 -2.2765405 +v -0.0166000 0.0755000 -0.0148662 +vn 2.8843794 -3.1151803 -0.0000000 +v 0.0095000 -0.0521750 -0.0000000 +vn 2.8865266 -3.0096271 -0.8048670 +v 0.0095000 -0.0524416 0.0020253 +vn 2.8905704 -2.6989861 -1.5568016 +v 0.0095000 -0.0532233 0.0039125 +vn 2.8942237 -2.2041905 -2.2027102 +v 0.0095000 -0.0544669 0.0055331 +vn 2.8974895 -1.5589793 -2.6985078 +v 0.0095000 -0.0560875 0.0067766 +vn 2.9003632 -0.8073486 -3.0103610 +v 0.0095000 -0.0579747 0.0075584 +vn 2.9028506 -0.0005592 -3.1169806 +v 0.0095000 -0.0600000 0.0078250 +vn 2.9049511 0.8063653 -3.0110776 +v 0.0095000 -0.0620253 0.0075584 +vn 2.9066720 1.5583861 -2.6998637 +v 0.0095000 -0.0639125 0.0067766 +vn 2.9080057 2.2042141 -2.2045519 +v 0.0095000 -0.0655331 0.0055331 +vn 2.9089589 2.6998086 -1.5589108 +v 0.0095000 -0.0667766 0.0039125 +vn 2.9095271 3.0113626 -0.8069688 +v 0.0095000 -0.0675584 0.0020253 +vn 2.9097273 3.1176310 0.0000000 +v 0.0095000 -0.0678250 0.0000000 +vn 2.9095273 3.0113626 0.8069688 +v 0.0095000 -0.0675584 -0.0020253 +vn 2.9089584 2.6998086 1.5589108 +v 0.0095000 -0.0667766 -0.0039125 +vn 2.9080055 2.2042141 2.2045519 +v 0.0095000 -0.0655331 -0.0055331 +vn 2.9066725 1.5583861 2.6998637 +v 0.0095000 -0.0639125 -0.0067766 +vn 2.9049509 0.8063653 3.0110776 +v 0.0095000 -0.0620253 -0.0075584 +vn 2.9028511 -0.0005592 3.1169806 +v 0.0095000 -0.0600000 -0.0078250 +vn 2.9003625 -0.8073486 3.0103610 +v 0.0095000 -0.0579747 -0.0075584 +vn 2.8974895 -1.5589793 2.6985078 +v 0.0095000 -0.0560875 -0.0067766 +vn 2.8942242 -2.2041905 2.2027102 +v 0.0095000 -0.0544669 -0.0055331 +vn 2.8905706 -2.6989861 1.5568016 +v 0.0095000 -0.0532233 -0.0039125 +vn 2.8865263 -3.0096273 0.8048671 +v 0.0095000 -0.0524416 -0.0020253 +vn 2.8843846 -3.1151807 0.0000000 +v 0.0095000 0.0678250 -0.0000000 +vn 2.8865242 -3.0096269 -0.8048676 +v 0.0095000 0.0675584 0.0020253 +vn 2.8905711 -2.6989849 -1.5568045 +v 0.0095000 0.0667766 0.0039125 +vn 2.8942227 -2.2041907 -2.2027097 +v 0.0095000 0.0655331 0.0055331 +vn 2.8974900 -1.5589801 -2.6985075 +v 0.0095000 0.0639125 0.0067766 +vn 2.9003613 -0.8073496 -3.0103605 +v 0.0095000 0.0620253 0.0075584 +vn 2.9028511 -0.0005585 -3.1169808 +v 0.0095000 0.0600000 0.0078250 +vn 2.9049523 0.8063647 -3.0110779 +v 0.0095000 0.0579747 0.0075584 +vn 2.9066715 1.5583856 -2.6998639 +v 0.0095000 0.0560875 0.0067766 +vn 2.9080043 2.2042143 -2.2045519 +v 0.0095000 0.0544669 0.0055331 +vn 2.9089580 2.6998096 -1.5589099 +v 0.0095000 0.0532233 0.0039125 +vn 2.9095333 3.0113633 -0.8069686 +v 0.0095000 0.0524416 0.0020253 +vn 2.9097247 3.1176305 0.0000000 +v 0.0095000 0.0521750 0.0000000 +vn 2.9095333 3.0113635 0.8069686 +v 0.0095000 0.0524416 -0.0020253 +vn 2.9089575 2.6998096 1.5589100 +v 0.0095000 0.0532233 -0.0039125 +vn 2.9080038 2.2042143 2.2045519 +v 0.0095000 0.0544669 -0.0055331 +vn 2.9066713 1.5583855 2.6998639 +v 0.0095000 0.0560875 -0.0067766 +vn 2.9049530 0.8063646 3.0110779 +v 0.0095000 0.0579747 -0.0075584 +vn 2.9028509 -0.0005585 3.1169808 +v 0.0095000 0.0600000 -0.0078250 +vn 2.9003615 -0.8073496 3.0103605 +v 0.0095000 0.0620253 -0.0075584 +vn 2.8974895 -1.5589801 2.6985075 +v 0.0095000 0.0639125 -0.0067766 +vn 2.8942232 -2.2041907 2.2027097 +v 0.0095000 0.0655331 -0.0055331 +vn 2.8905716 -2.6989851 1.5568044 +v 0.0095000 0.0667766 -0.0039125 +vn 2.8865237 -3.0096266 0.8048675 +v 0.0095000 0.0675584 -0.0020253 +vn -5.2461357 -2.2060246 0.7402620 +v -0.0166000 -0.0865710 0.0053686 +vn -5.3035555 -0.1301166 2.2735324 +v -0.0166000 -0.0755000 0.0148662 +vn -5.2654552 2.3010962 -0.1989178 +v -0.0166000 0.0872746 -0.0018483 +vn 3.0085869 -0.8061351 -2.8797884 +v -0.0026677 0.0007148 -0.0094199 +vn 3.0085869 0.8061351 -2.8797879 +v -0.0026677 -0.0007148 -0.0094199 +vn 3.1147187 0.0000000 -2.8798065 +v -0.0027618 0.0000000 -0.0094199 +vn 2.6974194 -1.5573616 -2.8797865 +v -0.0023918 0.0013809 -0.0094199 +vn 2.6974196 1.5573617 -2.8797872 +v -0.0023918 -0.0013809 -0.0094199 +vn 2.2024307 -2.2024438 -2.8797975 +v -0.0019529 0.0019529 -0.0094199 +vn 2.2024305 2.2024436 -2.8797965 +v -0.0019529 -0.0019529 -0.0094199 +vn 1.5573609 -2.6974220 -2.8797970 +v -0.0013809 0.0023918 -0.0094199 +vn 1.5573610 2.6974220 -2.8797972 +v -0.0013809 -0.0023918 -0.0094199 +vn 0.8061494 -3.0085831 -2.8797879 +v -0.0007148 0.0026677 -0.0094199 +vn 0.8061494 3.0085831 -2.8797884 +v -0.0007148 -0.0026677 -0.0094199 +vn -0.0000000 -3.1147165 -2.8797970 +v -0.0000000 0.0027618 -0.0094199 +vn 0.0000000 3.1147165 -2.8797970 +v -0.0000000 -0.0027618 -0.0094199 +vn -0.8061494 -3.0085831 -2.8797884 +v 0.0007148 0.0026677 -0.0094199 +vn -0.8061494 3.0085831 -2.8797879 +v 0.0007148 -0.0026677 -0.0094199 +vn -1.5573621 -2.6974213 -2.8797965 +v 0.0013809 0.0023918 -0.0094199 +vn -1.5573620 2.6974213 -2.8797965 +v 0.0013809 -0.0023918 -0.0094199 +vn -2.2024317 -2.2024424 -2.8797972 +v 0.0019529 0.0019529 -0.0094199 +vn -2.2024319 2.2024426 -2.8797975 +v 0.0019529 -0.0019529 -0.0094199 +vn -2.6974196 -1.5573612 -2.8797870 +v 0.0023918 0.0013809 -0.0094199 +vn -2.6974196 1.5573614 -2.8797867 +v 0.0023918 -0.0013809 -0.0094199 +vn -3.0085869 -0.8061336 -2.8797865 +v 0.0026677 0.0007148 -0.0094199 +vn -3.0085871 0.8061335 -2.8797870 +v 0.0026677 -0.0007148 -0.0094199 +vn -3.1147189 -0.0000000 -2.8798089 +v 0.0027618 0.0000000 -0.0094199 +vn 3.0991936 -0.0665800 3.1401811 +v 0.0150000 0.0413678 -0.0016982 +vn 1.5707963 1.5707955 -1.5707963 +v 0.0150000 0.0410303 0.0008698 +vn 1.5707964 1.5707964 1.5707964 +v 0.0150000 0.0410303 -0.0016982 +vn 3.0989361 -0.0669850 -3.1401639 +v 0.0150000 0.0413678 0.0008698 +vn 3.0381813 -0.2948075 3.1235125 +v 0.0150000 0.0416444 -0.0016865 +vn 3.0295568 -0.3090076 -3.1214063 +v 0.0150000 0.0416427 0.0008581 +vn 2.9883475 -0.7181232 -3.0489457 +v 0.0150000 0.0418684 0.0008229 +vn 2.9807696 -0.7023739 3.0516546 +v 0.0150000 0.0418841 -0.0016513 +vn 2.9822216 -1.1837242 -2.8992870 +v 0.0150000 0.0420634 0.0007608 +vn 2.9403827 -1.2380146 2.8700814 +v 0.0150000 0.0420959 -0.0015842 +vn 2.9171822 -1.7095773 -2.6121283 +v 0.0150000 0.0422462 0.0006686 +vn 2.9141798 -1.8175212 2.5375509 +v 0.0150000 0.0422886 -0.0014770 +vn 2.8419278 -2.3163733 -2.0698075 +v 0.0150000 0.0424657 0.0004868 +vn 2.8591702 -2.3902290 1.9902036 +v 0.0150000 0.0424903 -0.0012945 +vn 2.8707359 -2.8110332 -1.3370804 +v 0.0150000 0.0426239 0.0002451 +vn 2.8780890 -2.8419547 1.2737994 +v 0.0150000 0.0426350 -0.0010601 +vn 2.9222114 -3.0611691 -0.6169053 +v 0.0150000 0.0427194 -0.0000547 +vn 2.9332366 -3.0709925 0.5760911 +v 0.0150000 0.0427222 -0.0007676 +vn 2.9711635 -3.1301687 -0.0123857 +v 0.0150000 0.0427513 -0.0004109 +vn 1.5707963 -1.5707964 -1.5707964 +v 0.0151901 0.0405878 -0.0020782 +vn 4.7123890 1.5707955 1.5707963 +v 0.0151901 0.0410303 -0.0016982 +vn 1.5707964 -1.5707966 1.5707965 +v 0.0151901 0.0405878 0.0012497 +vn 3.1052175 0.0571250 -3.1405535 +v 0.0151901 0.0413544 -0.0020782 +vn 3.1839914 -0.0665800 3.1401811 +v 0.0151901 0.0413678 -0.0016982 +vn 3.0430703 0.2683797 -3.1262832 +v 0.0151901 0.0417229 -0.0020648 +vn 3.2450039 -0.2948075 3.1235125 +v 0.0151901 0.0416444 -0.0016865 +vn 3.3024163 -0.7023739 3.0516546 +v 0.0151901 0.0418841 -0.0016513 +vn 2.9766269 0.6752520 -3.0572283 +v 0.0151901 0.0420193 -0.0020245 +vn 3.3428020 -1.2380147 2.8700814 +v 0.0151901 0.0420959 -0.0015842 +vn 2.9616921 1.1890295 -2.8941622 +v 0.0151901 0.0422777 -0.0019446 +vn 3.3690047 -1.8175213 2.5375509 +v 0.0151901 0.0422886 -0.0014770 +vn 2.8992057 1.7633287 -2.5721631 +v 0.0151901 0.0425322 -0.0018122 +vn 3.4240155 -2.3902287 1.9902034 +v 0.0151901 0.0424903 -0.0012945 +vn 3.4050965 -2.8419547 1.2737994 +v 0.0151901 0.0426350 -0.0010601 +vn 2.8775814 2.3482060 -2.0456164 +v 0.0151901 0.0428113 -0.0015664 +vn 3.3499494 -3.0709925 0.5760911 +v 0.0151901 0.0427222 -0.0007676 +vn 3.3120215 -3.1301689 -0.0123857 +v 0.0151901 0.0427513 -0.0004109 +vn 4.7123890 1.5707964 -1.5707964 +v 0.0151901 0.0410303 0.0008698 +vn 3.0995893 0.0659593 3.1402073 +v 0.0151901 0.0413455 0.0012497 +vn 3.1842496 -0.0669850 -3.1401637 +v 0.0151901 0.0413678 0.0008698 +vn 3.0188751 0.3235337 3.1189451 +v 0.0151901 0.0417642 0.0012321 +vn 3.2536283 -0.3090077 -3.1214066 +v 0.0151901 0.0416427 0.0008581 +vn 3.2948387 -0.7181232 -3.0489459 +v 0.0151901 0.0418684 0.0008229 +vn 2.9805021 0.7603508 3.0376933 +v 0.0151901 0.0420819 0.0011793 +vn 3.3009648 -1.1837240 -2.8992867 +v 0.0151901 0.0420634 0.0007608 +vn 3.3660026 -1.7095773 -2.6121283 +v 0.0151901 0.0422462 0.0006686 +vn 2.9808717 1.2372227 2.8766797 +v 0.0151901 0.0423291 0.0010958 +vn 3.4412570 -2.3163733 -2.0698075 +v 0.0151901 0.0424657 0.0004868 +vn 2.9144454 1.7627208 2.5759776 +v 0.0151901 0.0425367 0.0009860 +vn 3.4124498 -2.8110332 -1.3370804 +v 0.0151901 0.0426239 0.0002451 +vn 2.8757236 2.3324234 2.0630116 +v 0.0151901 0.0428222 0.0007388 +vn 3.3609750 -3.0611691 -0.6169054 +v 0.0151901 0.0427194 -0.0000547 +vn 2.8986166 2.7899148 -1.3932080 +v 0.0151901 0.0430273 -0.0012400 +vn 2.9469414 2.7530231 1.4823208 +v 0.0151901 0.0430351 0.0004217 +vn 2.9058511 3.0419991 -0.6923574 +v 0.0151901 0.0431656 -0.0008512 +vn 3.0124483 2.9638257 1.0218844 +v 0.0151901 0.0431124 0.0002372 +vn 2.9711874 3.0783570 0.5673166 +v 0.0151901 0.0431675 0.0000359 +vn 2.9386313 3.1253996 -0.0138646 +v 0.0151901 0.0432117 -0.0004176 +vn 1.5707964 -1.5707964 -1.5707964 +v 0.0151901 0.0380198 -0.0020782 +vn 4.7123885 1.5707965 1.5707967 +v 0.0151901 0.0384623 -0.0016848 +vn 1.5707963 -1.5707966 1.5707967 +v 0.0151901 0.0380198 0.0012497 +vn 1.5707963 1.5707964 -1.5707967 +v 0.0151901 0.0401006 -0.0020782 +vn 1.5707964 1.5707964 1.5707964 +v 0.0151901 0.0401006 -0.0016848 +vn 1.5707964 1.5707965 -1.5707964 +v 0.0151901 0.0399888 -0.0004489 +vn 1.5707964 1.5707964 1.5707964 +v 0.0151901 0.0384623 -0.0000555 +vn 1.5707964 1.5707964 -1.5707964 +v 0.0151901 0.0384623 -0.0004489 +vn 1.5707964 1.5707964 1.5707964 +v 0.0151901 0.0399888 -0.0000555 +vn 4.7123885 1.5707964 -1.5707964 +v 0.0151901 0.0384623 0.0008564 +vn 1.5707963 1.5707964 1.5707964 +v 0.0151901 0.0401006 0.0012497 +vn 1.5707964 1.5707965 -1.5707967 +v 0.0151901 0.0401006 0.0008564 +vn 1.5707974 1.5707964 -1.5707970 +v 0.0151901 0.0375817 -0.0020782 +vn 2.5268955 -2.8540554 0.9058960 +v 0.0151901 0.0352573 -0.0016669 +vn 1.5707964 -1.5707964 -1.5707964 +v 0.0151901 0.0352573 -0.0020782 +vn 5.3302989 1.2803421 0.6607822 +v 0.0151901 0.0357356 -0.0016848 +vn 1.5707964 1.5707964 1.5707964 +v 0.0151901 0.0375817 -0.0016848 +vn 5.3270869 -1.2832602 -0.6648997 +v 0.0151901 0.0370386 0.0008564 +vn 2.5236826 2.8511386 -0.9100140 +v 0.0151901 0.0375370 0.0008497 +vn 1.5707963 -1.5707965 1.5707971 +v 0.0151901 0.0353378 0.0012497 +vn 1.5707964 -1.5707964 -1.5707964 +v 0.0151901 0.0353378 0.0008564 +vn 1.5707965 1.5707964 1.5707964 +v 0.0151901 0.0375370 0.0012497 +# 488 vertices, 0 vertices normals + +f 1//1 2//2 3//3 +f 4//4 2//2 1//1 +f 5//5 4//4 6//6 +f 6//6 4//4 1//1 +f 7//7 5//5 8//8 +f 8//8 5//5 6//6 +f 9//9 7//7 10//10 +f 10//10 7//7 8//8 +f 11//11 9//9 12//12 +f 12//12 9//9 10//10 +f 13//13 11//11 14//14 +f 11//11 12//12 14//14 +f 15//15 13//13 16//16 +f 13//13 14//14 16//16 +f 17//17 15//15 18//18 +f 15//15 16//16 18//18 +f 19//19 17//17 18//18 +f 20//20 17//17 19//19 +f 21//21 20//20 19//19 +f 22//22 20//20 21//21 +f 23//23 22//22 21//21 +f 23//23 24//24 22//22 +f 23//23 25//25 24//24 +f 25//25 26//26 24//24 +f 25//25 27//27 26//26 +f 27//27 28//28 26//26 +f 27//27 29//29 28//28 +f 29//29 30//30 28//28 +f 29//29 31//31 30//30 +f 30//30 31//31 32//32 +f 31//31 33//33 32//32 +f 32//32 33//33 34//34 +f 33//33 35//35 34//34 +f 34//34 35//35 36//36 +f 36//36 35//35 37//37 +f 36//36 37//37 38//38 +f 38//38 37//37 39//39 +f 38//38 39//39 40//40 +f 40//40 39//39 41//41 +f 40//40 41//41 42//42 +f 41//41 43//43 42//42 +f 42//42 43//43 44//44 +f 43//43 45//45 44//44 +f 44//44 45//45 46//46 +f 45//45 47//47 46//46 +f 46//46 47//47 48//48 +f 47//47 49//49 48//48 +f 48//48 49//49 2//2 +f 3//3 2//2 49//49 +f 50//50 51//51 52//52 +f 51//51 50//50 53//53 +f 54//54 50//50 55//55 +f 50//50 54//54 53//53 +f 56//56 57//57 58//58 +f 59//59 60//60 61//61 +f 62//62 63//63 64//64 +f 65//65 57//57 56//56 +f 61//61 62//62 64//64 +f 66//66 63//63 67//67 +f 68//68 58//58 69//69 +f 64//64 63//63 66//66 +f 69//69 70//70 68//68 +f 71//71 67//67 72//72 +f 71//71 66//66 67//67 +f 73//73 72//72 74//74 +f 68//68 56//56 58//58 +f 73//73 71//71 72//72 +f 75//75 74//74 76//76 +f 70//70 77//77 78//78 +f 73//73 74//74 75//75 +f 79//79 76//76 80//80 +f 79//79 75//75 76//76 +f 68//68 70//70 78//78 +f 77//77 81//81 82//82 +f 50//50 52//52 83//83 +f 83//83 52//52 84//84 +f 78//78 77//77 82//82 +f 81//81 85//85 86//86 +f 65//65 84//84 57//57 +f 82//82 81//81 86//86 +f 85//85 60//60 59//59 +f 86//86 85//85 59//59 +f 83//83 84//84 65//65 +f 60//60 62//62 61//61 +f 87//87 88//88 89//89 +f 90//90 91//91 92//92 +f 93//93 94//94 95//95 +f 96//96 88//88 87//87 +f 94//94 90//90 95//95 +f 97//97 98//98 93//93 +f 99//99 89//89 100//100 +f 93//93 98//98 94//94 +f 101//101 99//99 100//100 +f 102//102 97//97 103//103 +f 102//102 98//98 97//97 +f 104//104 103//103 105//105 +f 99//99 87//87 89//89 +f 102//102 103//103 104//104 +f 106//106 105//105 107//107 +f 108//108 109//109 101//101 +f 104//104 105//105 106//106 +f 53//53 107//107 51//51 +f 106//106 107//107 53//53 +f 109//109 99//99 101//101 +f 110//110 111//111 108//108 +f 112//112 113//113 114//114 +f 114//114 113//113 115//115 +f 111//111 109//109 108//108 +f 116//116 117//117 110//110 +f 96//96 115//115 88//88 +f 117//117 111//111 110//110 +f 92//92 91//91 116//116 +f 91//91 117//117 116//116 +f 114//114 115//115 96//96 +f 95//95 90//90 92//92 +f 118//118 119//119 120//120 +f 121//121 122//122 123//123 +f 122//122 121//121 124//124 +f 125//125 126//126 127//127 +f 126//126 125//125 128//128 +f 128//128 125//125 129//129 +f 130//130 131//131 132//132 +f 131//131 130//130 133//133 +f 133//133 130//130 134//134 +f 133//133 134//134 135//135 +f 135//135 134//134 136//136 +f 135//135 136//136 137//137 +f 137//137 136//136 138//138 +f 137//137 138//138 139//139 +f 139//139 138//138 140//140 +f 139//139 140//140 141//141 +f 141//141 140//140 142//142 +f 141//141 142//142 143//143 +f 143//143 142//142 144//144 +f 143//143 144//144 55//55 +f 55//55 144//144 145//145 +f 145//145 144//144 146//146 +f 146//146 144//144 147//147 +f 147//147 144//144 148//148 +f 148//148 144//144 149//149 +f 149//149 144//144 150//150 +f 150//150 144//144 151//151 +f 55//55 145//145 152//152 +f 150//150 151//151 153//153 +f 153//153 151//151 154//154 +f 154//154 151//151 155//155 +f 155//155 151//151 156//156 +f 156//156 151//151 157//157 +f 157//157 151//151 158//158 +f 158//158 151//151 159//159 +f 159//159 151//151 160//160 +f 160//160 151//151 161//161 +f 160//160 161//161 121//121 +f 161//161 151//151 162//162 +f 161//161 162//162 163//163 +f 163//163 162//162 164//164 +f 163//163 164//164 125//125 +f 125//125 164//164 165//165 +f 165//165 164//164 166//166 +f 164//164 162//162 167//167 +f 164//164 167//167 168//168 +f 168//168 167//167 169//169 +f 169//169 167//167 170//170 +f 170//170 167//167 171//171 +f 171//171 167//167 172//172 +f 172//172 167//167 173//173 +f 173//173 167//167 174//174 +f 174//174 167//167 175//175 +f 175//175 167//167 176//176 +f 176//176 167//167 177//177 +f 176//176 177//177 178//178 +f 176//176 178//178 179//179 +f 176//176 179//179 180//180 +f 162//162 151//151 181//181 +f 181//181 151//151 182//182 +f 182//182 151//151 183//183 +f 183//183 151//151 184//184 +f 184//184 151//151 185//185 +f 185//185 151//151 186//186 +f 186//186 151//151 187//187 +f 55//55 188//188 54//54 +f 188//188 55//55 189//189 +f 189//189 55//55 190//190 +f 190//190 55//55 191//191 +f 191//191 55//55 192//192 +f 192//192 55//55 193//193 +f 193//193 55//55 194//194 +f 194//194 55//55 152//152 +f 54//54 188//188 195//195 +f 54//54 195//195 196//196 +f 54//54 196//196 197//197 +f 54//54 197//197 198//198 +f 54//54 198//198 199//199 +f 54//54 199//199 158//158 +f 54//54 158//158 200//200 +f 200//200 158//158 120//120 +f 120//120 158//158 159//159 +f 200//200 120//120 119//119 +f 54//54 200//200 201//201 +f 201//201 200//200 124//124 +f 201//201 124//124 202//202 +f 202//202 124//124 121//121 +f 202//202 121//121 161//161 +f 201//201 202//202 203//203 +f 203//203 202//202 166//166 +f 203//203 166//166 204//204 +f 204//204 166//166 164//164 +f 203//203 204//204 205//205 +f 203//203 205//205 206//206 +f 203//203 206//206 207//207 +f 203//203 207//207 208//208 +f 203//203 208//208 209//209 +f 203//203 209//209 210//210 +f 203//203 210//210 211//211 +f 203//203 211//211 212//212 +f 203//203 212//212 213//213 +f 203//203 213//213 176//176 +f 203//203 176//176 214//214 +f 214//214 176//176 215//215 +f 215//215 176//176 216//216 +f 216//216 176//176 217//217 +f 217//217 176//176 180//180 +f 54//54 201//201 218//218 +f 54//54 218//218 219//219 +f 54//54 219//219 220//220 +f 54//54 220//220 221//221 +f 54//54 221//221 222//222 +f 54//54 222//222 223//223 +f 54//54 223//223 187//187 +f 54//54 187//187 151//151 +f 54//54 151//151 224//224 +f 54//54 224//224 225//225 +f 225//225 224//224 226//226 +f 225//225 226//226 227//227 +f 227//227 226//226 228//228 +f 227//227 228//228 229//229 +f 229//229 228//228 230//230 +f 229//229 230//230 231//231 +f 231//231 230//230 232//232 +f 231//231 232//232 233//233 +f 233//233 232//232 234//234 +f 233//233 234//234 235//235 +f 235//235 234//234 236//236 +f 235//235 236//236 237//237 +f 129//129 238//238 239//239 +f 238//238 129//129 165//165 +f 165//165 129//129 125//125 +f 136//136 64//64 66//66 +f 138//138 136//136 66//66 +f 50//50 143//143 55//55 +f 138//138 66//66 71//71 +f 140//140 138//138 71//71 +f 143//143 83//83 141//141 +f 50//50 83//83 143//143 +f 142//142 140//140 73//73 +f 73//73 140//140 71//71 +f 75//75 142//142 73//73 +f 141//141 65//65 139//139 +f 83//83 65//65 141//141 +f 144//144 142//142 75//75 +f 65//65 56//56 139//139 +f 144//144 75//75 79//79 +f 139//139 56//56 137//137 +f 56//56 68//68 137//137 +f 137//137 68//68 135//135 +f 68//68 78//78 135//135 +f 135//135 78//78 133//133 +f 78//78 82//82 133//133 +f 133//133 82//82 131//131 +f 131//131 82//82 86//86 +f 131//131 86//86 132//132 +f 132//132 86//86 59//59 +f 130//130 132//132 59//59 +f 130//130 59//59 61//61 +f 134//134 130//130 61//61 +f 134//134 61//61 64//64 +f 136//136 134//134 64//64 +f 94//94 98//98 231//231 +f 98//98 229//229 231//231 +f 224//224 151//151 112//112 +f 98//98 102//102 229//229 +f 102//102 227//227 229//229 +f 226//226 224//224 114//114 +f 114//114 224//224 112//112 +f 227//227 104//104 225//225 +f 102//102 104//104 227//227 +f 104//104 106//106 225//225 +f 228//228 226//226 96//96 +f 96//96 226//226 114//114 +f 225//225 106//106 54//54 +f 87//87 228//228 96//96 +f 106//106 53//53 54//54 +f 230//230 228//228 87//87 +f 99//99 230//230 87//87 +f 232//232 230//230 99//99 +f 109//109 232//232 99//99 +f 234//234 232//232 109//109 +f 111//111 234//234 109//109 +f 111//111 236//236 234//234 +f 117//117 236//236 111//111 +f 117//117 237//237 236//236 +f 117//117 91//91 237//237 +f 91//91 235//235 237//237 +f 91//91 90//90 235//235 +f 90//90 233//233 235//235 +f 90//90 94//94 233//233 +f 94//94 231//231 233//233 +f 112//112 144//144 79//79 +f 144//144 112//112 151//151 +f 240//240 241//241 242//242 +f 242//242 241//241 243//243 +f 244//244 245//245 246//246 +f 240//240 247//247 241//241 +f 248//248 247//247 240//240 +f 249//249 244//244 246//246 +f 248//248 250//250 247//247 +f 251//251 250//250 248//248 +f 251//251 252//252 250//250 +f 253//253 246//246 254//254 +f 253//253 255//255 249//249 +f 253//253 249//249 246//246 +f 256//256 257//257 251//251 +f 251//251 257//257 252//252 +f 258//258 254//254 259//259 +f 258//258 253//253 254//254 +f 260//260 261//261 256//256 +f 256//256 261//261 262//262 +f 263//263 259//259 264//264 +f 263//263 258//258 259//259 +f 256//256 262//262 257//257 +f 262//262 265//265 257//257 +f 266//266 264//264 267//267 +f 266//266 263//263 264//264 +f 268//268 269//269 267//267 +f 269//269 266//266 267//267 +f 270//270 271//271 268//268 +f 271//271 269//269 268//268 +f 272//272 273//273 270//270 +f 273//273 271//271 270//270 +f 274//274 275//275 272//272 +f 275//275 273//273 272//272 +f 242//242 243//243 274//274 +f 243//243 275//275 274//274 +f 276//276 277//277 278//278 +f 279//279 277//277 276//276 +f 260//260 280//280 261//261 +f 276//276 278//278 281//281 +f 280//280 262//262 261//261 +f 281//281 278//278 282//282 +f 281//281 282//282 283//283 +f 283//283 282//282 284//284 +f 262//262 285//285 265//265 +f 283//283 284//284 286//286 +f 280//280 287//287 285//285 +f 280//280 285//285 262//262 +f 288//288 284//284 289//289 +f 286//286 284//284 288//288 +f 287//287 290//290 291//291 +f 287//287 291//291 285//285 +f 244//244 289//289 245//245 +f 249//249 289//289 244//244 +f 290//290 292//292 293//293 +f 290//290 293//293 291//291 +f 288//288 289//289 249//249 +f 288//288 249//249 255//255 +f 292//292 294//294 295//295 +f 292//292 295//295 293//293 +f 294//294 296//296 297//297 +f 294//294 297//297 295//295 +f 296//296 298//298 299//299 +f 296//296 299//299 297//297 +f 298//298 300//300 301//301 +f 299//299 298//298 301//301 +f 300//300 302//302 303//303 +f 301//301 300//300 303//303 +f 302//302 277//277 279//279 +f 303//303 302//302 279//279 +f 1//1 3//3 253//253 +f 253//253 3//3 255//255 +f 6//6 1//1 258//258 +f 1//1 253//253 258//258 +f 8//8 6//6 263//263 +f 6//6 258//258 263//263 +f 8//8 263//263 266//266 +f 10//10 8//8 269//269 +f 8//8 266//266 269//269 +f 12//12 10//10 271//271 +f 271//271 10//10 269//269 +f 14//14 12//12 273//273 +f 273//273 12//12 271//271 +f 16//16 14//14 275//275 +f 14//14 273//273 275//275 +f 18//18 16//16 243//243 +f 16//16 275//275 243//243 +f 241//241 18//18 243//243 +f 247//247 18//18 241//241 +f 247//247 19//19 18//18 +f 250//250 19//19 247//247 +f 250//250 21//21 19//19 +f 252//252 23//23 21//21 +f 252//252 21//21 250//250 +f 257//257 23//23 252//252 +f 257//257 25//25 23//23 +f 265//265 27//27 25//25 +f 257//257 265//265 25//25 +f 285//285 29//29 27//27 +f 265//265 285//285 27//27 +f 285//285 291//291 29//29 +f 293//293 31//31 29//29 +f 291//291 293//293 29//29 +f 295//295 33//33 31//31 +f 293//293 295//295 31//31 +f 33//33 297//297 35//35 +f 295//295 297//297 33//33 +f 35//35 299//299 37//37 +f 297//297 299//299 35//35 +f 37//37 301//301 39//39 +f 37//37 299//299 301//301 +f 39//39 303//303 41//41 +f 39//39 301//301 303//303 +f 41//41 279//279 43//43 +f 303//303 279//279 41//41 +f 279//279 276//276 43//43 +f 43//43 281//281 45//45 +f 43//43 276//276 281//281 +f 45//45 283//283 47//47 +f 45//45 281//281 283//283 +f 47//47 286//286 49//49 +f 47//47 283//283 286//286 +f 49//49 288//288 3//3 +f 49//49 286//286 288//288 +f 3//3 288//288 255//255 +f 51//51 107//107 304//304 +f 108//108 101//101 305//305 +f 101//101 100//100 306//306 +f 107//107 105//105 307//307 +f 100//100 89//89 308//308 +f 103//103 309//309 105//105 +f 89//89 88//88 308//308 +f 97//97 310//310 103//103 +f 88//88 115//115 311//311 +f 97//97 93//93 312//312 +f 95//95 313//313 93//93 +f 113//113 314//314 115//115 +f 92//92 315//315 95//95 +f 92//92 116//116 315//315 +f 116//116 110//110 316//316 +f 110//110 108//108 317//317 +f 77//77 318//318 81//81 +f 80//80 76//76 319//319 +f 70//70 320//320 77//77 +f 321//321 70//70 69//69 +f 76//76 74//74 322//322 +f 58//58 321//321 69//69 +f 58//58 57//57 323//323 +f 72//72 324//324 74//74 +f 57//57 84//84 325//325 +f 67//67 326//326 72//72 +f 63//63 327//327 67//67 +f 52//52 328//328 84//84 +f 63//63 62//62 329//329 +f 62//62 60//60 330//330 +f 85//85 330//330 60//60 +f 81//81 331//331 85//85 +f 113//113 332//332 333//333 +f 80//80 332//332 113//113 +f 80//80 319//319 332//332 +f 199//199 334//334 158//158 +f 335//335 334//334 199//199 +f 336//336 335//335 198//198 +f 198//198 335//335 199//199 +f 337//337 336//336 197//197 +f 197//197 336//336 198//198 +f 196//196 338//338 337//337 +f 196//196 337//337 197//197 +f 195//195 338//338 196//196 +f 195//195 339//339 338//338 +f 188//188 339//339 195//195 +f 188//188 340//340 339//339 +f 189//189 340//340 188//188 +f 189//189 341//341 340//340 +f 190//190 341//341 189//189 +f 190//190 342//342 341//341 +f 191//191 342//342 190//190 +f 191//191 343//343 342//342 +f 191//191 192//192 343//343 +f 343//343 192//192 344//344 +f 192//192 193//193 344//344 +f 344//344 193//193 345//345 +f 193//193 194//194 345//345 +f 345//345 194//194 346//346 +f 194//194 152//152 346//346 +f 346//346 152//152 347//347 +f 152//152 145//145 347//347 +f 347//347 145//145 348//348 +f 145//145 146//146 348//348 +f 348//348 146//146 349//349 +f 146//146 147//147 349//349 +f 147//147 350//350 349//349 +f 148//148 350//350 147//147 +f 148//148 351//351 350//350 +f 149//149 351//351 148//148 +f 149//149 352//352 351//351 +f 150//150 352//352 149//149 +f 150//150 353//353 352//352 +f 153//153 353//353 150//150 +f 153//153 354//354 353//353 +f 154//154 354//354 153//153 +f 154//154 355//355 354//354 +f 155//155 355//355 154//154 +f 356//356 355//355 155//155 +f 156//156 356//356 155//155 +f 357//357 356//356 156//156 +f 157//157 357//357 156//156 +f 334//334 357//357 157//157 +f 158//158 334//334 157//157 +f 223//223 358//358 187//187 +f 359//359 358//358 223//223 +f 360//360 359//359 222//222 +f 222//222 359//359 223//223 +f 361//361 360//360 221//221 +f 221//221 360//360 222//222 +f 220//220 362//362 361//361 +f 220//220 361//361 221//221 +f 219//219 362//362 220//220 +f 219//219 363//363 362//362 +f 218//218 363//363 219//219 +f 218//218 364//364 363//363 +f 201//201 364//364 218//218 +f 201//201 365//365 364//364 +f 203//203 365//365 201//201 +f 203//203 366//366 365//365 +f 214//214 366//366 203//203 +f 214//214 367//367 366//366 +f 214//214 215//215 367//367 +f 367//367 215//215 368//368 +f 215//215 216//216 368//368 +f 368//368 216//216 369//369 +f 216//216 217//217 369//369 +f 369//369 217//217 370//370 +f 217//217 180//180 370//370 +f 370//370 180//180 371//371 +f 180//180 179//179 371//371 +f 371//371 179//179 372//372 +f 179//179 178//178 372//372 +f 372//372 178//178 373//373 +f 178//178 177//177 373//373 +f 177//177 374//374 373//373 +f 167//167 374//374 177//177 +f 167//167 375//375 374//374 +f 162//162 375//375 167//167 +f 162//162 376//376 375//375 +f 181//181 376//376 162//162 +f 181//181 377//377 376//376 +f 182//182 377//377 181//181 +f 182//182 378//378 377//377 +f 183//183 378//378 182//182 +f 183//183 379//379 378//378 +f 184//184 379//379 183//183 +f 380//380 379//379 184//184 +f 185//185 380//380 184//184 +f 381//381 380//380 185//185 +f 186//186 381//381 185//185 +f 358//358 381//381 186//186 +f 187//187 358//358 186//186 +f 113//113 246//246 80//80 +f 246//246 113//113 254//254 +f 254//254 113//113 112//112 +f 80//80 246//246 245//245 +f 254//254 112//112 259//259 +f 259//259 112//112 264//264 +f 264//264 112//112 267//267 +f 267//267 112//112 268//268 +f 268//268 112//112 270//270 +f 270//270 112//112 272//272 +f 272//272 112//112 274//274 +f 274//274 112//112 242//242 +f 242//242 112//112 240//240 +f 240//240 112//112 248//248 +f 248//248 112//112 251//251 +f 251//251 112//112 256//256 +f 256//256 112//112 260//260 +f 80//80 289//289 79//79 +f 289//289 80//80 245//245 +f 79//79 289//289 284//284 +f 79//79 284//284 282//282 +f 79//79 282//282 278//278 +f 79//79 278//278 277//277 +f 79//79 277//277 302//302 +f 79//79 302//302 300//300 +f 79//79 300//300 298//298 +f 79//79 298//298 296//296 +f 79//79 296//296 294//294 +f 79//79 294//294 292//292 +f 79//79 292//292 290//290 +f 79//79 290//290 287//287 +f 79//79 287//287 280//280 +f 79//79 280//280 260//260 +f 79//79 260//260 112//112 +f 318//318 330//330 331//331 +f 330//330 318//318 382//382 +f 330//330 382//382 329//329 +f 329//329 382//382 320//320 +f 329//329 320//320 327//327 +f 327//327 320//320 321//321 +f 327//327 321//321 326//326 +f 326//326 321//321 323//323 +f 326//326 323//323 324//324 +f 324//324 323//323 325//325 +f 324//324 325//325 322//322 +f 322//322 325//325 328//328 +f 322//322 328//328 319//319 +f 319//319 328//328 383//383 +f 319//319 383//383 304//304 +f 319//319 304//304 333//333 +f 333//333 304//304 307//307 +f 333//333 307//307 314//314 +f 314//314 307//307 311//311 +f 311//311 307//307 309//309 +f 311//311 309//309 308//308 +f 308//308 309//309 310//310 +f 308//308 310//310 312//312 +f 308//308 312//312 306//306 +f 306//306 312//312 313//313 +f 306//306 313//313 305//305 +f 305//305 313//313 317//317 +f 317//317 313//313 315//315 +f 317//317 315//315 384//384 +f 384//384 315//315 316//316 +f 306//306 100//100 308//308 +f 308//308 88//88 311//311 +f 311//311 115//115 314//314 +f 305//305 101//101 306//306 +f 108//108 305//305 317//317 +f 113//113 333//333 314//314 +f 110//110 317//317 384//384 +f 316//316 110//110 384//384 +f 116//116 316//316 315//315 +f 315//315 313//313 95//95 +f 313//313 312//312 93//93 +f 312//312 310//310 97//97 +f 310//310 309//309 103//103 +f 309//309 307//307 105//105 +f 307//307 304//304 107//107 +f 319//319 76//76 322//322 +f 322//322 74//74 324//324 +f 324//324 72//72 326//326 +f 326//326 67//67 327//327 +f 327//327 63//63 329//329 +f 329//329 62//62 330//330 +f 330//330 85//85 331//331 +f 318//318 331//331 81//81 +f 382//382 318//318 77//77 +f 52//52 383//383 328//328 +f 328//328 325//325 84//84 +f 320//320 382//382 77//77 +f 325//325 323//323 57//57 +f 321//321 320//320 70//70 +f 323//323 321//321 58//58 +f 332//332 319//319 333//333 +f 52//52 304//304 383//383 +f 304//304 52//52 51//51 +f 385//385 386//386 387//387 +f 386//386 385//385 388//388 +f 386//386 388//388 389//389 +f 389//389 388//388 390//390 +f 389//389 390//390 391//391 +f 391//391 390//390 392//392 +f 391//391 392//392 393//393 +f 393//393 392//392 394//394 +f 393//393 394//394 395//395 +f 395//395 394//394 396//396 +f 395//395 396//396 397//397 +f 397//397 396//396 398//398 +f 397//397 398//398 399//399 +f 399//399 398//398 400//400 +f 399//399 400//400 401//401 +f 401//401 400//400 402//402 +f 401//401 402//402 403//403 +f 403//403 402//402 404//404 +f 403//403 404//404 405//405 +f 405//405 404//404 406//406 +f 405//405 406//406 407//407 +f 407//407 406//406 408//408 +f 396//396 11//11 13//13 +f 11//11 396//396 394//394 +f 398//398 13//13 15//15 +f 13//13 398//398 396//396 +f 400//400 15//15 17//17 +f 15//15 400//400 398//398 +f 402//402 17//17 20//20 +f 17//17 402//402 400//400 +f 402//402 22//22 404//404 +f 22//22 402//402 20//20 +f 404//404 24//24 406//406 +f 24//24 404//404 22//22 +f 406//406 26//26 408//408 +f 26//26 406//406 24//24 +f 408//408 28//28 407//407 +f 28//28 408//408 26//26 +f 407//407 30//30 405//405 +f 30//30 407//407 28//28 +f 405//405 32//32 403//403 +f 32//32 405//405 30//30 +f 401//401 32//32 34//34 +f 32//32 401//401 403//403 +f 399//399 34//34 36//36 +f 34//34 399//399 401//401 +f 397//397 36//36 38//38 +f 36//36 397//397 399//399 +f 395//395 38//38 40//40 +f 38//38 395//395 397//397 +f 393//393 40//40 42//42 +f 40//40 393//393 395//395 +f 391//391 42//42 44//44 +f 42//42 391//391 393//393 +f 46//46 391//391 44//44 +f 391//391 46//46 389//389 +f 48//48 389//389 46//46 +f 389//389 48//48 386//386 +f 2//2 386//386 48//48 +f 386//386 2//2 387//387 +f 4//4 387//387 2//2 +f 387//387 4//4 385//385 +f 5//5 385//385 4//4 +f 385//385 5//5 388//388 +f 7//7 388//388 5//5 +f 388//388 7//7 390//390 +f 392//392 7//7 9//9 +f 7//7 392//392 390//390 +f 394//394 9//9 11//11 +f 9//9 394//394 392//392 +f 371//371 369//369 370//370 +f 369//369 371//371 372//372 +f 369//369 372//372 368//368 +f 368//368 372//372 373//373 +f 368//368 373//373 367//367 +f 367//367 373//373 374//374 +f 367//367 374//374 366//366 +f 366//366 374//374 375//375 +f 366//366 375//375 365//365 +f 365//365 375//375 376//376 +f 365//365 376//376 364//364 +f 364//364 376//376 377//377 +f 364//364 377//377 363//363 +f 363//363 377//377 378//378 +f 363//363 378//378 362//362 +f 362//362 378//378 379//379 +f 362//362 379//379 361//361 +f 361//361 379//379 380//380 +f 361//361 380//380 360//360 +f 360//360 380//380 381//381 +f 360//360 381//381 359//359 +f 359//359 381//381 358//358 +f 409//409 410//410 411//411 +f 410//410 409//409 412//412 +f 412//412 409//409 413//413 +f 412//412 413//413 414//414 +f 414//414 413//413 415//415 +f 415//415 413//413 416//416 +f 415//415 416//416 417//417 +f 417//417 416//416 418//418 +f 417//417 418//418 419//419 +f 419//419 418//418 420//420 +f 419//419 420//420 421//421 +f 421//421 420//420 422//422 +f 421//421 422//422 423//423 +f 423//423 422//422 424//424 +f 423//423 424//424 425//425 +f 425//425 424//424 426//426 +f 425//425 426//426 427//427 +f 428//428 429//429 430//430 +f 429//429 428//428 431//431 +f 429//429 431//431 432//432 +f 432//432 431//431 433//433 +f 432//432 433//433 434//434 +f 434//434 433//433 435//435 +f 435//435 433//433 436//436 +f 435//435 436//436 437//437 +f 437//437 436//436 438//438 +f 437//437 438//438 439//439 +f 439//439 438//438 440//440 +f 439//439 440//440 441//441 +f 441//441 440//440 442//442 +f 442//442 440//440 443//443 +f 442//442 443//443 444//444 +f 444//444 443//443 445//445 +f 430//430 446//446 447//447 +f 446//446 430//430 429//429 +f 447//447 446//446 448//448 +f 447//447 448//448 449//449 +f 449//449 448//448 450//450 +f 449//449 450//450 451//451 +f 449//449 451//451 452//452 +f 452//452 451//451 453//453 +f 452//452 453//453 454//454 +f 452//452 454//454 455//455 +f 455//455 454//454 456//456 +f 455//455 456//456 457//457 +f 457//457 456//456 458//458 +f 457//457 458//458 459//459 +f 459//459 458//458 460//460 +f 459//459 460//460 445//445 +f 459//459 445//445 443//443 +f 459//459 443//443 461//461 +f 459//459 461//461 462//462 +f 462//462 461//461 463//463 +f 462//462 463//463 464//464 +f 464//464 463//463 465//465 +f 465//465 463//463 466//466 +f 212//212 462//462 464//464 +f 462//462 212//212 211//211 +f 210//210 462//462 211//211 +f 462//462 210//210 459//459 +f 457//457 210//210 209//209 +f 210//210 457//457 459//459 +f 455//455 209//209 208//208 +f 209//209 455//455 457//457 +f 452//452 208//208 207//207 +f 208//208 452//452 455//455 +f 449//449 207//207 206//206 +f 207//207 449//449 452//452 +f 447//447 206//206 205//205 +f 206//206 447//447 449//449 +f 430//430 205//205 204//204 +f 205//205 430//430 447//447 +f 430//430 164//164 428//428 +f 164//164 430//430 204//204 +f 431//431 164//164 168//168 +f 164//164 431//431 428//428 +f 433//433 168//168 169//169 +f 168//168 433//433 431//431 +f 436//436 169//169 170//170 +f 169//169 436//436 433//433 +f 438//438 170//170 171//171 +f 170//170 438//438 436//436 +f 440//440 171//171 172//172 +f 171//171 440//440 438//438 +f 443//443 172//172 173//173 +f 172//172 443//443 440//440 +f 174//174 443//443 173//173 +f 443//443 174//174 461//461 +f 175//175 461//461 174//174 +f 461//461 175//175 463//463 +f 176//176 463//463 175//175 +f 463//463 176//176 466//466 +f 213//213 466//466 176//176 +f 466//466 213//213 465//465 +f 213//213 464//464 465//465 +f 464//464 213//213 212//212 +f 456//456 419//419 421//421 +f 419//419 456//456 454//454 +f 456//456 423//423 458//458 +f 423//423 456//456 421//421 +f 458//458 425//425 460//460 +f 425//425 458//458 423//423 +f 460//460 427//427 445//445 +f 427//427 460//460 425//425 +f 445//445 426//426 444//444 +f 426//426 445//445 427//427 +f 444//444 424//424 442//442 +f 424//424 444//444 426//426 +f 442//442 422//422 441//441 +f 422//422 442//442 424//424 +f 439//439 422//422 420//420 +f 422//422 439//439 441//441 +f 437//437 420//420 418//418 +f 420//420 437//437 439//439 +f 435//435 418//418 416//416 +f 418//418 435//435 437//437 +f 434//434 416//416 413//413 +f 416//416 434//434 435//435 +f 432//432 413//413 409//409 +f 413//413 432//432 434//434 +f 429//429 409//409 411//411 +f 409//409 429//429 432//432 +f 410//410 429//429 411//411 +f 429//429 410//410 446//446 +f 448//448 410//410 412//412 +f 410//410 448//448 446//446 +f 450//450 412//412 414//414 +f 412//412 450//450 448//448 +f 451//451 414//414 415//415 +f 414//414 451//451 450//450 +f 453//453 415//415 417//417 +f 415//415 453//453 451//451 +f 454//454 417//417 419//419 +f 417//417 454//454 453//453 +f 467//467 468//468 469//469 +f 468//468 467//467 470//470 +f 468//468 470//470 471//471 +f 472//472 473//473 474//474 +f 473//473 472//472 475//475 +f 469//469 476//476 477//477 +f 476//476 469//469 468//468 +f 476//476 468//468 474//474 +f 476//476 474//474 473//473 +f 477//477 476//476 478//478 +f 129//129 472//472 128//128 +f 472//472 129//129 475//475 +f 473//473 129//129 239//239 +f 129//129 473//473 475//475 +f 238//238 473//473 239//239 +f 473//473 238//238 476//476 +f 478//478 238//238 165//165 +f 238//238 478//478 476//476 +f 166//166 478//478 165//165 +f 478//478 166//166 477//477 +f 469//469 166//166 202//202 +f 166//166 469//469 477//477 +f 469//469 161//161 467//467 +f 161//161 469//469 202//202 +f 470//470 161//161 163//163 +f 161//161 470//470 467//467 +f 125//125 470//470 163//163 +f 470//470 125//125 471//471 +f 468//468 125//125 127//127 +f 125//125 468//468 471//471 +f 126//126 468//468 127//127 +f 468//468 126//126 474//474 +f 472//472 126//126 128//128 +f 126//126 472//472 474//474 +f 479//479 480//480 481//481 +f 480//480 479//479 482//482 +f 482//482 479//479 483//483 +f 482//482 484//484 480//480 +f 484//484 482//482 485//485 +f 484//484 486//486 487//487 +f 486//486 484//484 488//488 +f 488//488 484//484 485//485 +f 484//484 119//119 118//118 +f 119//119 484//484 487//487 +f 484//484 120//120 480//480 +f 120//120 484//484 118//118 +f 480//480 159//159 481//481 +f 159//159 480//480 120//120 +f 479//479 159//159 160//160 +f 159//159 479//479 481//481 +f 121//121 479//479 160//160 +f 479//479 121//121 483//483 +f 482//482 121//121 123//123 +f 121//121 482//482 483//483 +f 122//122 482//482 123//123 +f 482//482 122//122 485//485 +f 122//122 488//488 485//485 +f 488//488 122//122 124//124 +f 486//486 124//124 200//200 +f 124//124 486//486 488//488 +f 486//486 119//119 487//487 +f 119//119 486//486 200//200 +f 347//347 345//345 346//346 +f 345//345 347//347 348//348 +f 345//345 348//348 344//344 +f 344//344 348//348 349//349 +f 344//344 349//349 343//343 +f 343//343 349//349 350//350 +f 343//343 350//350 342//342 +f 342//342 350//350 351//351 +f 342//342 351//351 341//341 +f 341//341 351//351 352//352 +f 341//341 352//352 340//340 +f 340//340 352//352 353//353 +f 340//340 353//353 339//339 +f 339//339 353//353 354//354 +f 339//339 354//354 338//338 +f 338//338 354//354 355//355 +f 338//338 355//355 337//337 +f 337//337 355//355 356//356 +f 337//337 356//356 336//336 +f 336//336 356//356 357//357 +f 336//336 357//357 335//335 +f 335//335 357//357 334//334 +# 972 faces, 0 coords texture + +# End of File diff --git a/resources/robots/g1_description/meshes/zed_mount.stl b/resources/robots/g1_description/meshes/zed_mount.stl new file mode 100644 index 0000000..fb1e928 Binary files /dev/null and b/resources/robots/g1_description/meshes/zed_mount.stl differ diff --git a/setup.py b/setup.py index cd12ef3..42c2e8c 100644 --- a/setup.py +++ b/setup.py @@ -8,4 +8,4 @@ setup(name='unitree_rl_gym', packages=find_packages(), author_email='support@unitree.com', description='Template RL environments for Unitree Robots', - install_requires=['isaacgym', 'rsl-rl', 'matplotlib', 'numpy==1.20', 'tensorboard', 'mujoco==3.2.3', 'pyyaml']) + install_requires=['rsl-rl', 'matplotlib','tensorboard', 'pyyaml'])