add navigation yaml, py file
This commit is contained in:
HYUNHONOH98
parent
3d4707dfd6
commit
8b2e6d135a
|
|
@ -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: 132
|
||||
|
||||
# max_cmd: [0.8, 0.5, 1.57]
|
||||
max_cmd: [1.0, 1.0, 1.0]
|
||||
|
|
@ -0,0 +1,509 @@
|
|||
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)
|
||||
|
||||
# Create a mapping tensor
|
||||
# mapping_tensor = torch.zeros((len(sim_b_joints), len(sim_a_joints)), device=env.device)
|
||||
|
||||
|
||||
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()
|
||||
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))
|
||||
|
||||
|
||||
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.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.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)
|
||||
# FIXME(ycho): give `root_state_w`
|
||||
|
||||
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 run_policy(self):
|
||||
if self.remote_controller.button[KeyMap.select] == 1:
|
||||
self._mode_change = True
|
||||
self.mode = Mode.null
|
||||
return
|
||||
self.counter += 1
|
||||
|
||||
# self.cmd[0] = x
|
||||
# self.cmd[1] = y
|
||||
# self.cmd[2] = z
|
||||
# self.cmd[3] = heading
|
||||
|
||||
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()
|
||||
|
||||
self.action = self.action @ self.mapping_tensor.detach().cpu().numpy()
|
||||
target_dof_pos = self.config.default_angles + self.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
|
||||
|
||||
# reduce KP for non-arm joints
|
||||
# for i in self.mot_from_nonarm:
|
||||
# self.low_cmd.motor_cmd[i].kp = 0.05 * float(self.config.kps[i])
|
||||
# self.low_cmd.motor_cmd[i].kd = 0.05 * 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
|
||||
|
||||
|
||||
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)
|
||||
Loading…
Reference in New Issue