EmbodiedAI
/
unitree_rl_gym
mirror of https://github.com/unitreerobotics/unitree_rl_gym.git
3
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge 1d5830fd51 into 757b051580

This commit is contained in:
dev_Nhh 2025-02-21 18:17:13 +08:00 committed by GitHub
parent 757b051580 1d5830fd51
commit 237f0c7a62
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
50 changed files with 11664 additions and 111 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
MUJOCO_LOG.TXT Normal file
View File

@ -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.

52
deploy/deploy_mujoco/configs/g1.yaml
View File

@ -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]

120
deploy/deploy_mujoco/configs/g1_nav.yaml Normal file
View File

@ -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]

101
deploy/deploy_mujoco/deploy_mujoco.py
View File

@ -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()

178
deploy/deploy_mujoco/deploy_mujoco_navigation.py Normal file
View File

@ -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)

0
deploy/deploy_real/__init__.py Normal file
View File

254
deploy/deploy_real/act_to_dof.py Normal file
View File

@ -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()

0
deploy/deploy_real/common/__init__.py Normal file
View File

62
deploy/deploy_real/common/command_helper_ros.py Normal file
View File

@ -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

232
deploy/deploy_real/common/crc.py Normal file
View File

@ -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)

184
deploy/deploy_real/common/np_math.py Normal file
View File

@ -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)

258
deploy/deploy_real/common/step_command.py Normal file
View File

@ -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()

226
deploy/deploy_real/common/utils.py Normal file
View File

@ -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 <https://www.mathworks.com/help/map/ref/wraptopi.html>`_
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)

36
deploy/deploy_real/common/xml_helper.py Normal file
View File

@ -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()

50
deploy/deploy_real/config.py
View File

@ -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,15 +25,50 @@ 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"]

109
deploy/deploy_real/configs/g1.yaml
View File

@ -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]

176
deploy/deploy_real/configs/g1_eetrack.yaml Normal file
View File

@ -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]

153
deploy/deploy_real/configs/g1_nav.yaml Normal file
View File

@ -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]

42
deploy/deploy_real/configs/g1_old.yaml Normal file
View File

@ -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]

121
deploy/deploy_real/configs/ik.yaml Normal file
View File

@ -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]

175
deploy/deploy_real/deploy_real.py
View File

@ -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)

265
deploy/deploy_real/deploy_real_old.py Normal file
View File

@ -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")

322
deploy/deploy_real/deploy_real_ros.py Normal file
View File

@ -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)

943
deploy/deploy_real/deploy_real_ros_eetrack.py Normal file
View File

@ -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)

319
deploy/deploy_real/deploy_real_ros_ikctrl.py Normal file
View File

@ -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)

461
deploy/deploy_real/deploy_real_ros_navigation.py Normal file
View File

@ -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)

556
deploy/deploy_real/deploy_real_step.py Normal file
View File

@ -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")

552
deploy/deploy_real/deploy_real_step_ros.py Normal file
View File

@ -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)

119
deploy/deploy_real/fake_world_tf_pub.py Executable file
View File

@ -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()

210
deploy/deploy_real/ikctrl.py Normal file
View File

@ -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()

0
deploy/deploy_real/localization/__init__.py Normal file
View File

205
deploy/deploy_real/localization/pelvis_pub.py Normal file
View File

@ -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()

27
deploy/deploy_real/localization/state_pub.launch.py Normal file
View File

@ -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]),
])

64
deploy/deploy_real/localization/state_pub.py Normal file
View File

@ -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()

1626
deploy/deploy_real/math_utils.py Normal file
View File

File diff suppressed because it is too large Load Diff

28
deploy/deploy_real/state_pub.launch.py Normal file
View File

@ -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]),
])

64
deploy/deploy_real/state_pub.py Normal file
View File

@ -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()

50
deploy/deploy_real/test.py Normal file
View File

@ -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()

12
deploy/deploy_real/test_policy.py Normal file
View File

@ -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())

BIN
deploy/pre_train/g1/policy.pt Normal file
View File

Binary file not shown.

15
legged_gym/envs/base/legged_robot_config.py
View File

@ -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.

71
legged_gym/envs/g1/g1_config.py
View File

@ -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'

47
legged_gym/envs/g1/g1_env.py
View File

@ -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

93
legged_gym/scripts/play.py
View File

@ -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()

29
resources/robots/g1_description/g1_29dof_rev_1_0.urdf
View File

@ -1,3 +1,4 @@
<?xml version="1.0" encoding="utf-8"?>
<robot name="g1_29dof_rev_1_0">
<mujoco>
<compiler meshdir="meshes" discardvisual="false"/>
@ -215,6 +216,12 @@
</material>
</visual>
<collision>
<origin xyz="0.03826199 0.0 -0.02540915" rpy="0 0 0"/>
<geometry>
<box size="0.20820672 0.07558269 0.02"/>
</geometry>
</collision>
<!-- <collision>
<origin xyz="-0.05 0.025 -0.03" rpy="0 0 0"/>
<geometry>
<sphere radius="0.005"/>
@ -237,7 +244,7 @@
<geometry>
<sphere radius="0.005"/>
</geometry>
</collision>
</collision> -->
</link>
<joint name="left_ankle_roll_joint" type="revolute">
<origin xyz="0 0 -0.017558" rpy="0 0 0"/>
@ -407,6 +414,12 @@
</material>
</visual>
<collision>
<origin xyz="0.03826199 0.0 -0.02540915" rpy="0 0 0"/>
<geometry>
<box size="0.20820672 0.07558269 0.02"/>
</geometry>
</collision>
<!-- <collision>
<origin xyz="-0.05 0.025 -0.03" rpy="0 0 0"/>
<geometry>
<sphere radius="0.005"/>
@ -429,7 +442,7 @@
<geometry>
<sphere radius="0.005"/>
</geometry>
</collision>
</collision> -->
</link>
<joint name="right_ankle_roll_joint" type="revolute">
<origin xyz="0 0 -0.017558" rpy="0 0 0"/>
@ -830,6 +843,12 @@
<color rgba="0.7 0.7 0.7 1"/>
</material>
</visual>
<collision>
<origin xyz="0.07 -0.01 0" rpy="0 0 -0.3"/>
<geometry>
<box size="0.12 0.03 0.08"/>
</geometry>
</collision>
</link>
<link name="right_shoulder_pitch_link">
<inertial>
@ -1054,5 +1073,11 @@
<color rgba="0.7 0.7 0.7 1"/>
</material>
</visual>
<collision>
<origin xyz="0.07 0.01 0" rpy="0 0 0.3"/>
<geometry>
<box size="0.12 0.03 0.08"/>
</geometry>
</collision>
</link>
</robot>

5
resources/robots/g1_description/g1_29dof_rev_1_0.xml
View File

@ -1,6 +1,11 @@
<mujoco model="g1_29dof_rev_1_0">
<compiler angle="radian" meshdir="meshes"/>
<statistic meansize="0.144785" extent="1.23314" center="0.025392 2.0634e-05 -0.245975"/>
<default>
<joint damping="0.001" armature="0.01" frictionloss="0.1"/>
</default>
<asset>
<mesh name="pelvis" file="pelvis.STL"/>
<mesh name="pelvis_contour_link" file="pelvis_contour_link.STL"/>

1191
resources/robots/g1_description/g1_29dof_rev_1_0_lidar.urdf Executable file
View File

File diff suppressed because it is too large Load Diff

1964
resources/robots/g1_description/meshes/zed2.obj Normal file
View File

File diff suppressed because it is too large Load Diff

BIN
resources/robots/g1_description/meshes/zed_mount.stl Normal file
View File

Binary file not shown.

2
setup.py
View File

@ -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'])