Skip to content

yzh / agielf2_amp

转到一个项目
Commit acced63a by liufq
Options

1.add elf12

parent e6651b9a
正在显示 包含 2833 行增加 和 0 行删除
humanoid/envs/elf12/elf12_dh_stand_config.py 0 → 100644
# SPDX-FileCopyrightText: Copyright (c) 2021 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
# SPDX-FileCopyrightText: Copyright (c) 2021 ETH Zurich, Nikita Rudin
# SPDX-FileCopyrightText: Copyright (c) 2024 Beijing RobotEra TECHNOLOGY CO.,LTD. All rights reserved.
# SPDX-License-Identifier: BSD-3-Clause
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are met:
#
# 1. Redistributions of source code must retain the above copyright notice, this
# list of conditions and the following disclaimer.
#
# 2. Redistributions in binary form must reproduce the above copyright notice,
# this list of conditions and the following disclaimer in the documentation
# and/or other materials provided with the distribution.
#
# 3. Neither the name of the copyright holder nor the names of its
# contributors may be used to endorse or promote products derived from
# this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
# FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
# DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
# SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
# OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
# Copyright (c) 2024, AgiBot Inc. All rights reserved.
from humanoid.envs.base.legged_robot_config import LeggedRobotCfg, LeggedRobotCfgPPO
class X1DHStandCfg(LeggedRobotCfg):
"""
Configuration class for the XBotL humanoid robot.
"""
class env(LeggedRobotCfg.env):
# change the observation dim
frame_stack = 66 #all histroy obs num
short_frame_stack = 5 #short history step
c_frame_stack = 3 #all histroy privileged obs num
num_single_obs = 47
num_observations = int(frame_stack * num_single_obs)
single_num_privileged_obs = 73
single_linvel_index = 53
num_privileged_obs = int(c_frame_stack * single_num_privileged_obs)
num_actions = 12
num_envs = 4096
episode_length_s = 24 #episode length in seconds
use_ref_actions = False
num_commands = 5 # sin_pos cos_pos vx vy vz
class safety:
# safety factors
pos_limit = 1.0
vel_limit = 1.0
torque_limit = 0.85
class asset(LeggedRobotCfg.asset):
file = '{LEGGED_GYM_ROOT_DIR}/resources/robots/x1/urdf/x1.urdf'
xml_file = '{LEGGED_GYM_ROOT_DIR}/resources/robots/x1/mjcf/xyber_x1_flat.xml'
name = "x1"
foot_name = "ankle_roll"
knee_name = "knee_pitch"
terminate_after_contacts_on = ['base_link']
penalize_contacts_on = ["base_link"]
self_collisions = 0 # 1 to disable, 0 to enable...bitwise filter
flip_visual_attachments = False
replace_cylinder_with_capsule = False
fix_base_link = False
class terrain(LeggedRobotCfg.terrain):
# mesh_type = 'plane'
mesh_type = 'trimesh'
curriculum = False
# rough terrain only:
measure_heights = False
static_friction = 0.6
dynamic_friction = 0.6
terrain_length = 8.
terrain_width = 8.
num_rows = 20 # number of terrain rows (levels)
num_cols = 20 # number of terrain cols (types)
max_init_terrain_level = 5 # starting curriculum state
platform = 3.
terrain_dict = {"flat": 0.3,
"rough flat": 0.2,
"slope up": 0.2,
"slope down": 0.2,
"rough slope up": 0.0,
"rough slope down": 0.0,
"stairs up": 0.,
"stairs down": 0.,
"discrete": 0.1,
"wave": 0.0,}
terrain_proportions = list(terrain_dict.values())
rough_flat_range = [0.005, 0.01] # meter
slope_range = [0, 0.1] # rad
rough_slope_range = [0.005, 0.02]
stair_width_range = [0.25, 0.25]
stair_height_range = [0.01, 0.1]
discrete_height_range = [0.0, 0.01]
restitution = 0.
class noise(LeggedRobotCfg.noise):
add_noise = True
noise_level = 1.5 # scales other values
class noise_scales(LeggedRobotCfg.noise.noise_scales):
dof_pos = 0.02
dof_vel = 1.5
ang_vel = 0.2
lin_vel = 0.1
quat = 0.1
gravity = 0.05
height_measurements = 0.1
class init_state(LeggedRobotCfg.init_state):
pos = [0.0, 0.0, 0.7]
default_joint_angles = { # = target angles [rad] when action = 0.0
'left_hip_pitch_joint': 0.4,
'left_hip_roll_joint': 0.05,
'left_hip_yaw_joint': -0.31,
'left_knee_pitch_joint': 0.49,
'left_ankle_pitch_joint': -0.21,
'left_ankle_roll_joint': 0.0,
'right_hip_pitch_joint': -0.4,
'right_hip_roll_joint': -0.05,
'right_hip_yaw_joint': 0.31,
'right_knee_pitch_joint': 0.49,
'right_ankle_pitch_joint': -0.21,
'right_ankle_roll_joint': 0.0,
}
class control(LeggedRobotCfg.control):
# PD Drive parameters:
control_type = 'P'
stiffness = {'hip_pitch_joint': 30, 'hip_roll_joint': 40,'hip_yaw_joint': 35,
'knee_pitch_joint': 100, 'ankle_pitch_joint': 35, 'ankle_roll_joint': 35}
damping = {'hip_pitch_joint': 3, 'hip_roll_joint': 3.0,'hip_yaw_joint': 4,
'knee_pitch_joint': 10, 'ankle_pitch_joint': 0.5, 'ankle_roll_joint': 0.5}
# action scale: target angle = actionScale * action + defaultAngle
action_scale = 0.5
# decimation: Number of control action updates @ sim DT per policy DT
decimation = 10 # 50hz 100hz
class sim(LeggedRobotCfg.sim):
dt = 0.001 # 200 Hz 1000 Hz
substeps = 1 # 2
up_axis = 1 # 0 is y, 1 is z
class physx(LeggedRobotCfg.sim.physx):
num_threads = 10
solver_type = 1 # 0: pgs, 1: tgs
num_position_iterations = 4
num_velocity_iterations = 0
contact_offset = 0.01 # [m]
rest_offset = 0.0 # [m]
bounce_threshold_velocity = 0.5 # 0.5 #0.5 [m/s]
max_depenetration_velocity = 1.0
max_gpu_contact_pairs = 2**23 # 2**24 -> needed for 8000 envs and more
default_buffer_size_multiplier = 5
# 0: never, 1: last sub-step, 2: all sub-steps (default=2)
contact_collection = 2
class domain_rand(LeggedRobotCfg.domain_rand):
randomize_friction = True
friction_range = [0.2, 1.3]
restitution_range = [0.0, 0.4]
# push
push_robots = True
push_interval_s = 4 # every this second, push robot
update_step = 2000 * 24 # after this count, increase push_duration index
push_duration = [0, 0.05, 0.1, 0.15, 0.2, 0.25] # increase push duration during training
max_push_vel_xy = 0.2
max_push_ang_vel = 0.2
randomize_base_mass = True
added_mass_range = [-3, 3] # base mass rand range, base mass is all fix link sum mass
randomize_com = True
com_displacement_range = [[-0.05, 0.05],
[-0.05, 0.05],
[-0.05, 0.05]]
randomize_gains = True
stiffness_multiplier_range = [0.8, 1.2] # Factor
damping_multiplier_range = [0.8, 1.2] # Factor
randomize_torque = True
torque_multiplier_range = [0.8, 1.2]
randomize_link_mass = True
added_link_mass_range = [0.9, 1.1]
randomize_motor_offset = True
motor_offset_range = [-0.035, 0.035] # Offset to add to the motor angles
randomize_joint_friction = True
randomize_joint_friction_each_joint = False
joint_friction_range = [0.01, 1.15]
joint_1_friction_range = [0.01, 1.15]
joint_2_friction_range = [0.01, 1.15]
joint_3_friction_range = [0.01, 1.15]
joint_4_friction_range = [0.5, 1.3]
joint_5_friction_range = [0.5, 1.3]
joint_6_friction_range = [0.01, 1.15]
joint_7_friction_range = [0.01, 1.15]
joint_8_friction_range = [0.01, 1.15]
joint_9_friction_range = [0.5, 1.3]
joint_10_friction_range = [0.5, 1.3]
randomize_joint_damping = True
randomize_joint_damping_each_joint = False
joint_damping_range = [0.3, 1.5]
joint_1_damping_range = [0.3, 1.5]
joint_2_damping_range = [0.3, 1.5]
joint_3_damping_range = [0.3, 1.5]
joint_4_damping_range = [0.9, 1.5]
joint_5_damping_range = [0.9, 1.5]
joint_6_damping_range = [0.3, 1.5]
joint_7_damping_range = [0.3, 1.5]
joint_8_damping_range = [0.3, 1.5]
joint_9_damping_range = [0.9, 1.5]
joint_10_damping_range = [0.9, 1.5]
randomize_joint_armature = True
randomize_joint_armature_each_joint = False
joint_armature_range = [0.0001, 0.05] # Factor
joint_1_armature_range = [0.0001, 0.05]
joint_2_armature_range = [0.0001, 0.05]
joint_3_armature_range = [0.0001, 0.05]
joint_4_armature_range = [0.0001, 0.05]
joint_5_armature_range = [0.0001, 0.05]
joint_6_armature_range = [0.0001, 0.05]
joint_7_armature_range = [0.0001, 0.05]
joint_8_armature_range = [0.0001, 0.05]
joint_9_armature_range = [0.0001, 0.05]
joint_10_armature_range = [0.0001, 0.05]
add_lag = True
randomize_lag_timesteps = True
randomize_lag_timesteps_perstep = False
lag_timesteps_range = [5, 40]
add_dof_lag = True
randomize_dof_lag_timesteps = True
randomize_dof_lag_timesteps_perstep = False
dof_lag_timesteps_range = [0, 40]
add_dof_pos_vel_lag = False
randomize_dof_pos_lag_timesteps = False
randomize_dof_pos_lag_timesteps_perstep = False
dof_pos_lag_timesteps_range = [7, 25]
randomize_dof_vel_lag_timesteps = False
randomize_dof_vel_lag_timesteps_perstep = False
dof_vel_lag_timesteps_range = [7, 25]
add_imu_lag = False
randomize_imu_lag_timesteps = True
randomize_imu_lag_timesteps_perstep = False
imu_lag_timesteps_range = [1, 10]
randomize_coulomb_friction = True
joint_coulomb_range = [0.1, 0.9]
joint_viscous_range = [0.05, 0.1]
class commands(LeggedRobotCfg.commands):
curriculum = True
max_curriculum = 1.5
# Vers: lin_vel_x, lin_vel_y, ang_vel_yaw, heading (in heading mode ang_vel_yaw is recomputed from heading error)
num_commands = 4
resampling_time = 25. # time before command are changed[s]
gait = ["walk_omnidirectional","stand","walk_omnidirectional"] # gait type during training
# proportion during whole life time
gait_time_range = {"walk_sagittal": [2,6],
"walk_lateral": [2,6],
"rotate": [2,3],
"stand": [2,3],
"walk_omnidirectional": [4,6]}
heading_command = False # if true: compute ang vel command from heading error
stand_com_threshold = 0.05 # if (lin_vel_x, lin_vel_y, ang_vel_yaw).norm < this, robot should stand
sw_switch = True # use stand_com_threshold or not
class ranges:
lin_vel_x = [-0.4, 1.2] # min max [m/s]
lin_vel_y = [-0.4, 0.4] # min max [m/s]
ang_vel_yaw = [-0.6, 0.6] # min max [rad/s]
heading = [-3.14, 3.14]
class rewards:
soft_dof_pos_limit = 0.98
soft_dof_vel_limit = 0.9
soft_torque_limit = 0.9
base_height_target = 0.61
foot_min_dist = 0.2
foot_max_dist = 1.0
# final_swing_joint_pos = final_swing_joint_delta_pos + default_pos
final_swing_joint_delta_pos = [0.25, 0.05, -0.11, 0.35, -0.16, 0.0, -0.25, -0.05, 0.11, 0.35, -0.16, 0.0]
target_feet_height = 0.03
target_feet_height_max = 0.06
feet_to_ankle_distance = 0.041
cycle_time = 0.7
# if true negative total rewards are clipped at zero (avoids early termination problems)
only_positive_rewards = True
# tracking reward = exp(-error*sigma)
tracking_sigma = 5
max_contact_force = 700 # forces above this value are penalized
class scales:
ref_joint_pos = 2.2
feet_clearance = 1.
feet_contact_number = 2.0
# gait
feet_air_time = 1.2
foot_slip = -0.1
feet_distance = 0.2
knee_distance = 0.2
# contact
feet_contact_forces = -0.01
# vel tracking
tracking_lin_vel = 1.8
tracking_ang_vel = 1.1
vel_mismatch_exp = 0.5 # lin_z; ang x,y
low_speed = 0.2
track_vel_hard = 0.5
# base pos
default_joint_pos = 1.0
orientation = 1.
feet_rotation = 0.3
base_height = 0.2
base_acc = 0.2
# energy
action_smoothness = -0.002
torques = -8e-9
dof_vel = -2e-8
dof_acc = -1e-7
collision = -1.
stand_still = 2.5
# limits
dof_vel_limits = -1
dof_pos_limits = -10.
dof_torque_limits = -0.1
class normalization:
class obs_scales:
lin_vel = 2.
ang_vel = 1.
dof_pos = 1.
dof_vel = 0.05
quat = 1.
height_measurements = 5.0
clip_observations = 100.
clip_actions = 100.
class X1DHStandCfgPPO(LeggedRobotCfgPPO):
seed = 5
runner_class_name = 'DHOnPolicyRunner' # DWLOnPolicyRunner
class policy:
init_noise_std = 1.0
actor_hidden_dims = [512, 256, 128]
critic_hidden_dims = [768, 256, 128]
state_estimator_hidden_dims=[256, 128, 64]
#for long_history cnn only
kernel_size=[6, 4]
filter_size=[32, 16]
stride_size=[3, 2]
lh_output_dim= 64 #long history output dim
in_channels = X1DHStandCfg.env.frame_stack
class algorithm(LeggedRobotCfgPPO.algorithm):
entropy_coef = 0.001
learning_rate = 1e-5
num_learning_epochs = 2
gamma = 0.994
lam = 0.9
num_mini_batches = 4
if X1DHStandCfg.terrain.measure_heights:
lin_vel_idx = (X1DHStandCfg.env.single_num_privileged_obs + X1DHStandCfg.terrain.num_height) * (X1DHStandCfg.env.c_frame_stack - 1) + X1DHStandCfg.env.single_linvel_index
else:
lin_vel_idx = X1DHStandCfg.env.single_num_privileged_obs * (X1DHStandCfg.env.c_frame_stack - 1) + X1DHStandCfg.env.single_linvel_index
class runner:
policy_class_name = 'ActorCriticDH'
algorithm_class_name = 'DHPPO'
num_steps_per_env = 24 # per iteration
max_iterations = 20000 # number of policy updates
# logging
save_interval = 100 # check for potential saves every this many iterations
experiment_name = 'x1_dh_stand'
run_name = ''
# load and resume
resume = False
load_run = -1 # -1 = last run
checkpoint = -1 # -1 = last saved model
resume_path = None # updated from load_run and chkpt
humanoid/envs/elf12/elf12_dh_stand_env.py 0 → 100644
# SPDX-FileCopyrightText: Copyright (c) 2021 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
# SPDX-FileCopyrightText: Copyright (c) 2021 ETH Zurich, Nikita Rudin
# SPDX-FileCopyrightText: Copyright (c) 2024 Beijing RobotEra TECHNOLOGY CO.,LTD. All rights reserved.
# SPDX-License-Identifier: BSD-3-Clause
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are met:
#
# 1. Redistributions of source code must retain the above copyright notice, this
# list of conditions and the following disclaimer.
#
# 2. Redistributions in binary form must reproduce the above copyright notice,
# this list of conditions and the following disclaimer in the documentation
# and/or other materials provided with the distribution.
#
# 3. Neither the name of the copyright holder nor the names of its
# contributors may be used to endorse or promote products derived from
# this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
# FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
# DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
# SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
# OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
# Copyright (c) 2024, AgiBot Inc. All rights reserved.
from humanoid.envs.base.legged_robot_config import LeggedRobotCfg
from isaacgym.torch_utils import *
from isaacgym import gymtorch, gymapi
from humanoid.utils.math import wrap_to_pi
import torch
from humanoid.envs import LeggedRobot
from humanoid.utils.terrain import Terrain
def copysign_new(a, b):
a = torch.tensor(a, device=b.device, dtype=torch.float)
a = a.expand_as(b)
return torch.abs(a) * torch.sign(b)
def get_euler_rpy(q):
qx, qy, qz, qw = 0, 1, 2, 3
# roll (x-axis rotation)
sinr_cosp = 2.0 * (q[..., qw] * q[..., qx] + q[..., qy] * q[..., qz])
cosr_cosp = q[..., qw] * q[..., qw] - q[..., qx] * \
q[..., qx] - q[..., qy] * q[..., qy] + q[..., qz] * q[..., qz]
roll = torch.atan2(sinr_cosp, cosr_cosp)
# pitch (y-axis rotation)
sinp = 2.0 * (q[..., qw] * q[..., qy] - q[..., qz] * q[..., qx])
pitch = torch.where(torch.abs(sinp) >= 1, copysign_new(
np.pi / 2.0, sinp), torch.asin(sinp))
# yaw (z-axis rotation)
siny_cosp = 2.0 * (q[..., qw] * q[..., qz] + q[..., qx] * q[..., qy])
cosy_cosp = q[..., qw] * q[..., qw] + q[..., qx] * \
q[..., qx] - q[..., qy] * q[..., qy] - q[..., qz] * q[..., qz]
yaw = torch.atan2(siny_cosp, cosy_cosp)
return roll % (2*np.pi), pitch % (2*np.pi), yaw % (2*np.pi)
def get_euler_xyz_tensor(quat):
r, p, w = get_euler_rpy(quat)
# stack r, p, w in dim1
euler_xyz = torch.stack((r, p, w), dim=-1)
euler_xyz[euler_xyz > np.pi] -= 2 * np.pi
return euler_xyz
class X1DHStandEnv(LeggedRobot):
'''
X1DHStandEnv is a class that represents a custom environment for a legged robot.
Args:
cfg (LeggedRobotCfg): Configuration object for the legged robot.
sim_params: Parameters for the simulation.
physics_engine: Physics engine used in the simulation.
sim_device: Device used for the simulation.
headless: Flag indicating whether the simulation should be run in headless mode.
Attributes:
last_feet_z (float): The z-coordinate of the last feet position.
feet_height (torch.Tensor): Tensor representing the height of the feet.
sim (gymtorch.GymSim): The simulation object.
terrain (Terrain): The terrain object.
up_axis_idx (int): The index representing the up axis.
command_input (torch.Tensor): Tensor representing the command input.
privileged_obs_buf (torch.Tensor): Tensor representing the privileged observations buffer.
obs_buf (torch.Tensor): Tensor representing the observations buffer.
obs_history (collections.deque): Deque containing the history of observations.
critic_history (collections.deque): Deque containing the history of critic observations.
Methods:
_push_robots(): Randomly pushes the robots by setting a randomized base velocity.
_get_phase(): Calculates the phase of the gait cycle.
_get_stance_mask(): Calculates the gait phase.
compute_ref_state(): Computes the reference state.
create_sim(): Creates the simulation, terrain, and environments.
_get_noise_scale_vec(cfg): Sets a vector used to scale the noise added to the observations.
step(actions): Performs a simulation step with the given actions.
compute_observations(): Computes the observations.
reset_idx(env_ids): Resets the environment for the specified environment IDs.
'''
def __init__(self, cfg: LeggedRobotCfg, sim_params, physics_engine, sim_device, headless):
super().__init__(cfg, sim_params, physics_engine, sim_device, headless)
self.last_feet_z = self.cfg.rewards.feet_to_ankle_distance
self.feet_height = torch.zeros((self.num_envs, 2), device=self.device)
self.ref_dof_pos = torch.zeros((self.num_envs, self.num_actions), device=self.device)
def _push_robots(self):
""" Random pushes the robots. Emulates an impulse by setting a randomized base velocity.
"""
max_vel = self.cfg.domain_rand.max_push_vel_xy
max_push_angular = self.cfg.domain_rand.max_push_ang_vel
self.rand_push_force[:, :2] = torch_rand_float(
-max_vel, max_vel, (self.num_envs, 2), device=self.device) # lin vel x/y
self.root_states[:, 7:9] = self.rand_push_force[:, :2]
self.rand_push_torque = torch_rand_float(
-max_push_angular, max_push_angular, (self.num_envs, 3), device=self.device) #angular vel xyz
self.root_states[:, 10:13] = self.rand_push_torque
self.gym.set_actor_root_state_tensor(
self.sim, gymtorch.unwrap_tensor(self.root_states))
def _get_phase(self):
cycle_time = self.cfg.rewards.cycle_time
if self.cfg.commands.sw_switch:
stand_command = (torch.norm(self.commands[:, :3], dim=1) <= self.cfg.commands.stand_com_threshold)
self.phase_length_buf[stand_command] = 0 # set this as 0 for which env is standing
# self.gait_start is rand 0 or 0.5
phase = (self.phase_length_buf * self.dt / cycle_time + self.gait_start) * (~stand_command)
else:
phase = self.episode_length_buf * self.dt / cycle_time + self.gait_start
# phase continue increase，if want robot stand, set 0
return phase
def _get_stance_mask(self):
# return float mask 1 is stance, 0 is swing
phase = self._get_phase()
sin_pos = torch.sin(2 * torch.pi * phase)
stance_mask = torch.zeros((self.num_envs, 2), device=self.device)
# left foot stance
stance_mask[:, 0] = sin_pos >= 0
# right foot stance
stance_mask[:, 1] = sin_pos < 0
# Add double support phase
stance_mask[torch.abs(sin_pos) < 0.1] = 1
# stand mask == 1 means stand leg
return stance_mask
def generate_gait_time(self,envs):
if len(envs) == 0:
return
# rand sample
random_tensor_list = []
for i in range(len(self.cfg.commands.gait)):
name = self.cfg.commands.gait[i]
gait_time_range = self.cfg.commands.gait_time_range[name]
random_tensor_single = torch_rand_float(gait_time_range[0],
gait_time_range[1],
(len(envs), 1),device=self.device)
random_tensor_list.append(random_tensor_single)
random_tensor = torch.cat([random_tensor_list[i] for i in range(len(self.cfg.commands.gait))], dim=1)
current_sum = torch.sum(random_tensor,dim=1,keepdim=True)
# scaled_tensor store proportion for each gait type
scaled_tensor = random_tensor * (self.max_episode_length / current_sum)
scaled_tensor[:,1:] = scaled_tensor[:,:-1].clone()
scaled_tensor[:,0] *= 0.0
# self.gait_time accumulate gait_duration_tick
# self.gait_time = |__gait1__|__gait2__|__gait3__|
# self.gait_time triger resample gait command
self.gait_time[envs] = torch.cumsum(scaled_tensor,dim=1).int()
def _resample_commands(self):
""" Randommly select commands of some environments
Args:
env_ids (List[int]): Environments ids for which new commands are needed
"""
for i in range(len(self.cfg.commands.gait)):
# if env finish current gait type, resample command for next gait
env_ids = (self.episode_length_buf == self.gait_time[:,i]).nonzero(as_tuple=False).flatten()
if len(env_ids) > 0:
# according to gait type create a name
name = '_resample_' + self.cfg.commands.gait[i] + '_command'
# get function from self based on name
resample_command = getattr(self, name)
# resample_command stands for _resample_stand_command/_resample_walk_sagittal_command/...
resample_command(env_ids)
def _resample_stand_command(self, env_ids):
self.commands[env_ids, 0] = torch.zeros(len(env_ids), device=self.device)
self.commands[env_ids, 1] = torch.zeros(len(env_ids), device=self.device)
if self.cfg.commands.heading_command:
self.commands[env_ids, 3] = torch.zeros(len(env_ids), device=self.device)
else:
self.commands[env_ids, 2] = torch.zeros(len(env_ids), device=self.device)
def _resample_walk_sagittal_command(self, env_ids):
self.commands[env_ids, 0] = torch_rand_float(self.command_ranges["lin_vel_x"][0], self.command_ranges["lin_vel_x"][1], (len(env_ids), 1), device=self.device).squeeze(1)
self.commands[env_ids, 1] = torch.zeros(len(env_ids), device=self.device)
if self.cfg.commands.heading_command:
self.commands[env_ids, 3] = torch.zeros(len(env_ids), device=self.device)
else:
self.commands[env_ids, 2] = torch.zeros(len(env_ids), device=self.device)
def _resample_walk_lateral_command(self, env_ids):
self.commands[env_ids, 0] = torch.zeros(len(env_ids), device=self.device)
self.commands[env_ids, 1] = torch_rand_float(self.command_ranges["lin_vel_y"][0], self.command_ranges["lin_vel_y"][1], (len(env_ids), 1), device=self.device).squeeze(1)
if self.cfg.commands.heading_command:
self.commands[env_ids, 3] = torch.zeros(len(env_ids), device=self.device)
else:
self.commands[env_ids, 2] = torch.zeros(len(env_ids), device=self.device)
def _resample_rotate_command(self, env_ids):
self.commands[env_ids, 0] = torch.zeros(len(env_ids), device=self.device)
self.commands[env_ids, 1] = torch.zeros(len(env_ids), device=self.device)
if self.cfg.commands.heading_command:
self.commands[env_ids, 3] = torch_rand_float(self.command_ranges["heading"][0], self.command_ranges["heading"][1], (len(env_ids), 1), device=self.device).squeeze(1)
else:
self.commands[env_ids, 2] = torch_rand_float(self.command_ranges["ang_vel_yaw"][0], self.command_ranges["ang_vel_yaw"][1], (len(env_ids), 1), device=self.device).squeeze(1)
def _resample_walk_omnidirectional_command(self,env_ids):
self.commands[env_ids, 0] = torch_rand_float(self.command_ranges["lin_vel_x"][0], self.command_ranges["lin_vel_x"][1], (len(env_ids), 1), device=self.device).squeeze(1)
self.commands[env_ids, 1] = torch_rand_float(self.command_ranges["lin_vel_y"][0], self.command_ranges["lin_vel_y"][1], (len(env_ids), 1), device=self.device).squeeze(1)
if self.cfg.commands.heading_command:
self.commands[env_ids, 3] = torch_rand_float(self.command_ranges["heading"][0], self.command_ranges["heading"][1], (len(env_ids), 1), device=self.device).squeeze(1)
else:
self.commands[env_ids, 2] = torch_rand_float(self.command_ranges["ang_vel_yaw"][0], self.command_ranges["ang_vel_yaw"][1], (len(env_ids), 1), device=self.device).squeeze(1)
# self.commands[env_ids, :2] *= (torch.norm(self.commands[env_ids, :2], dim=1) > 0.05).unsqueeze(1)
def _post_physics_step_callback(self):
""" Callback called before computing terminations, rewards, and observations
Default behaviour: Compute ang vel command based on target and heading, compute measured terrain heights and randomly push robots
"""
self.phase_length_buf += 1
self._resample_commands()
if self.cfg.commands.heading_command:
forward = quat_apply(self.base_quat, self.forward_vec)
heading = torch.atan2(forward[:, 1], forward[:, 0])
self.commands[:, 2] = torch.clip(0.5*wrap_to_pi(self.commands[:, 3] - heading), -1., 1.)
if self.cfg.terrain.measure_heights:
# get all robot surrounding height
self.measured_heights = self._get_heights()
if self.cfg.domain_rand.push_robots:
i = int(self.common_step_counter/self.cfg.domain_rand.update_step)
if i >= len(self.cfg.domain_rand.push_duration):
i = len(self.cfg.domain_rand.push_duration) - 1
duration = self.cfg.domain_rand.push_duration[i]/self.dt
if self.common_step_counter % self.cfg.domain_rand.push_interval <= duration:
self._push_robots()
else:
self.rand_push_force.zero_()
self.rand_push_torque.zero_()
def compute_ref_state(self):
phase = self._get_phase()
sin_pos = torch.sin(2 * torch.pi * phase)
sin_pos_l = sin_pos.clone()
sin_pos_r = sin_pos.clone()
self.ref_dof_pos = torch.zeros_like(self.dof_pos)
# left swing
sin_pos_l[sin_pos_l > 0] = 0
self.ref_dof_pos[:, 0] = -sin_pos_l * self.cfg.rewards.final_swing_joint_delta_pos[0]
self.ref_dof_pos[:, 1] = -sin_pos_l * self.cfg.rewards.final_swing_joint_delta_pos[1]
self.ref_dof_pos[:, 2] = -sin_pos_l * self.cfg.rewards.final_swing_joint_delta_pos[2]
self.ref_dof_pos[:, 3] = -sin_pos_l * self.cfg.rewards.final_swing_joint_delta_pos[3]
self.ref_dof_pos[:, 4] = -sin_pos_l * self.cfg.rewards.final_swing_joint_delta_pos[4]
self.ref_dof_pos[:, 5] = -sin_pos_l * self.cfg.rewards.final_swing_joint_delta_pos[5]
# right
sin_pos_r[sin_pos_r < 0] = 0
self.ref_dof_pos[:, 6] = sin_pos_r * self.cfg.rewards.final_swing_joint_delta_pos[6]
self.ref_dof_pos[:, 7] = sin_pos_r * self.cfg.rewards.final_swing_joint_delta_pos[7]
self.ref_dof_pos[:, 8] = sin_pos_r * self.cfg.rewards.final_swing_joint_delta_pos[8]
self.ref_dof_pos[:, 9] = sin_pos_r * self.cfg.rewards.final_swing_joint_delta_pos[9]
self.ref_dof_pos[:, 10] = sin_pos_r * self.cfg.rewards.final_swing_joint_delta_pos[10]
self.ref_dof_pos[:, 11] = sin_pos_r * self.cfg.rewards.final_swing_joint_delta_pos[11]
self.ref_dof_pos[torch.abs(sin_pos) < 0.1] = 0.
# if use_ref_actions=True, action += ref_action
self.ref_action = 2 * self.ref_dof_pos
# self.ref_dof_pos set ref dof pos for swing leg, ref_dof_pos=0 for stance leg
self.ref_dof_pos += self.default_dof_pos
def create_sim(self):
""" Creates simulation, terrain and evironments
"""
self.up_axis_idx = 2 # 2 for z, 1 for y -> adapt gravity accordingly
self.sim = self.gym.create_sim(
self.sim_device_id, self.graphics_device_id, self.physics_engine, self.sim_params)
mesh_type = self.cfg.terrain.mesh_type
if mesh_type in ['heightfield', 'trimesh']:
self.terrain = Terrain(self.cfg.terrain, self.num_envs)
if mesh_type == 'plane':
self._create_ground_plane()
elif mesh_type == 'heightfield':
self._create_heightfield()
elif mesh_type == 'trimesh':
self._create_trimesh()
elif mesh_type is not None:
raise ValueError(
"Terrain mesh type not recognised. Allowed types are [None, plane, heightfield, trimesh]")
self._create_envs()
def _get_noise_scale_vec(self, cfg):
""" Sets a vector used to scale the noise added to the observations.
[NOTE]: Must be adapted when changing the observations structure
Args:
cfg (Dict): Environment config file
Returns:
[torch.Tensor]: Vector of scales used to multiply a uniform distribution in [-1, 1]
"""
noise_vec = torch.zeros(
self.cfg.env.num_single_obs, device=self.device)
self.add_noise = self.cfg.noise.add_noise
noise_scales = self.cfg.noise.noise_scales
noise_vec[0: self.cfg.env.num_commands] = 0. # commands
noise_vec[self.cfg.env.num_commands: self.cfg.env.num_commands+self.num_actions] = noise_scales.dof_pos * self.obs_scales.dof_pos
noise_vec[self.cfg.env.num_commands+self.num_actions: self.cfg.env.num_commands+2*self.num_actions] = noise_scales.dof_vel * self.obs_scales.dof_vel
noise_vec[self.cfg.env.num_commands+2*self.num_actions: self.cfg.env.num_commands+3*self.num_actions] = 0. # previous actions
noise_vec[self.cfg.env.num_commands+3*self.num_actions: self.cfg.env.num_commands+3*self.num_actions + 3] = noise_scales.ang_vel * self.obs_scales.ang_vel # ang vel
noise_vec[self.cfg.env.num_commands+3*self.num_actions + 3: self.cfg.env.num_commands+3*self.num_actions + 6] = noise_scales.quat * self.obs_scales.quat # euler x,y
return noise_vec
def step(self, actions):
if self.cfg.env.use_ref_actions:
actions += self.ref_action
return super().step(actions)
def compute_observations(self):
phase = self._get_phase()
self.compute_ref_state()
sin_pos = torch.sin(2 * torch.pi * phase).unsqueeze(1)
cos_pos = torch.cos(2 * torch.pi * phase).unsqueeze(1)
stance_mask = self._get_stance_mask()
contact_mask = self.contact_forces[:, self.feet_indices, 2] > 5.
self.command_input = torch.cat(
(sin_pos, cos_pos, self.commands[:, :3] * self.commands_scale), dim=1)
# critic no lag
diff = self.dof_pos - self.ref_dof_pos
# 73
privileged_obs_buf = torch.cat((
self.command_input, # 2 + 3
(self.dof_pos - self.default_joint_pd_target) * self.obs_scales.dof_pos, # 12
self.dof_vel * self.obs_scales.dof_vel, # 12
self.actions, # 12
diff, # 12
self.base_lin_vel * self.obs_scales.lin_vel, # 3
self.base_ang_vel * self.obs_scales.ang_vel, # 3
self.base_euler_xyz * self.obs_scales.quat, # 3
self.rand_push_force[:, :2], # 2
self.rand_push_torque, # 3
self.env_frictions, # 1
self.body_mass / 10., # 1 # sum of all fix link mass
stance_mask, # 2
contact_mask, # 2
), dim=-1)
# random add dof_pos and dof_vel same lag
if self.cfg.domain_rand.add_dof_lag:
if self.cfg.domain_rand.randomize_dof_lag_timesteps_perstep:
self.dof_lag_timestep = torch.randint(self.cfg.domain_rand.dof_lag_timesteps_range[0],
self.cfg.domain_rand.dof_lag_timesteps_range[1]+1,(self.num_envs,),device=self.device)
cond = self.dof_lag_timestep > self.last_dof_lag_timestep + 1
self.dof_lag_timestep[cond] = self.last_dof_lag_timestep[cond] + 1
self.last_dof_lag_timestep = self.dof_lag_timestep.clone()
self.lagged_dof_pos = self.dof_lag_buffer[torch.arange(self.num_envs), :self.num_actions, self.dof_lag_timestep.long()]
self.lagged_dof_vel = self.dof_lag_buffer[torch.arange(self.num_envs), -self.num_actions:, self.dof_lag_timestep.long()]
# random add dof_pos and dof_vel different lag
elif self.cfg.domain_rand.add_dof_pos_vel_lag:
if self.cfg.domain_rand.randomize_dof_pos_lag_timesteps_perstep:
self.dof_pos_lag_timestep = torch.randint(self.cfg.domain_rand.dof_pos_lag_timesteps_range[0],
self.cfg.domain_rand.dof_pos_lag_timesteps_range[1]+1,(self.num_envs,),device=self.device)
cond = self.dof_pos_lag_timestep > self.last_dof_pos_lag_timestep + 1
self.dof_pos_lag_timestep[cond] = self.last_dof_pos_lag_timestep[cond] + 1
self.last_dof_pos_lag_timestep = self.dof_pos_lag_timestep.clone()
self.lagged_dof_pos = self.dof_pos_lag_buffer[torch.arange(self.num_envs), :, self.dof_pos_lag_timestep.long()]
if self.cfg.domain_rand.randomize_dof_vel_lag_timesteps_perstep:
self.dof_vel_lag_timestep = torch.randint(self.cfg.domain_rand.dof_vel_lag_timesteps_range[0],
self.cfg.domain_rand.dof_vel_lag_timesteps_range[1]+1,(self.num_envs,),device=self.device)
cond = self.dof_vel_lag_timestep > self.last_dof_vel_lag_timestep + 1
self.dof_vel_lag_timestep[cond] = self.last_dof_vel_lag_timestep[cond] + 1
self.last_dof_vel_lag_timestep = self.dof_vel_lag_timestep.clone()
self.lagged_dof_vel = self.dof_vel_lag_buffer[torch.arange(self.num_envs), :, self.dof_vel_lag_timestep.long()]
# dof_pos and dof_vel has no lag
else:
self.lagged_dof_pos = self.dof_pos
self.lagged_dof_vel = self.dof_vel
# imu lag, including rpy and omega
if self.cfg.domain_rand.add_imu_lag:
if self.cfg.domain_rand.randomize_imu_lag_timesteps_perstep:
self.imu_lag_timestep = torch.randint(self.cfg.domain_rand.imu_lag_timesteps_range[0],
self.cfg.domain_rand.imu_lag_timesteps_range[1]+1,(self.num_envs,),device=self.device)
cond = self.imu_lag_timestep > self.last_imu_lag_timestep + 1
self.imu_lag_timestep[cond] = self.last_imu_lag_timestep[cond] + 1
self.last_imu_lag_timestep = self.imu_lag_timestep.clone()
self.lagged_imu = self.imu_lag_buffer[torch.arange(self.num_envs), :, self.imu_lag_timestep.int()]
self.lagged_base_ang_vel = self.lagged_imu[:,:3].clone()
self.lagged_base_euler_xyz = self.lagged_imu[:,-3:].clone()
# no imu lag
else:
self.lagged_base_ang_vel = self.base_ang_vel[:,:3]
self.lagged_base_euler_xyz = self.base_euler_xyz[:,-3:]
# obs q and dq
q = (self.lagged_dof_pos - self.default_dof_pos) * self.obs_scales.dof_pos
dq = self.lagged_dof_vel * self.obs_scales.dof_vel
# 47
obs_buf = torch.cat((
self.command_input, # 5 = 2D(sin cos) + 3D(vel_x, vel_y, aug_vel_yaw)
q, # 12
dq, # 12
self.actions, # 12
self.lagged_base_ang_vel * self.obs_scales.ang_vel, # 3
self.lagged_base_euler_xyz * self.obs_scales.quat, # 3
), dim=-1)
if self.cfg.env.num_single_obs == 48:
stand_command = (torch.norm(self.commands[:, :3], dim=1, keepdim=True) <= self.cfg.commands.stand_com_threshold)
obs_buf = torch.cat((obs_buf, stand_command),dim=1)
if self.cfg.terrain.measure_heights:
heights = torch.clip(self.root_states[:, 2].unsqueeze(1) - 0.5 - self.measured_heights, -1, 1.) * self.obs_scales.height_measurements
privileged_obs_buf = torch.cat((privileged_obs_buf.clone(), heights), dim=-1)
if self.add_noise:
# add obs noise
obs_now = obs_buf.clone() + (2 * torch.rand_like(obs_buf) -1) * self.noise_scale_vec * self.cfg.noise.noise_level
else:
obs_now = obs_buf.clone()
self.obs_history.append(obs_now)
self.critic_history.append(privileged_obs_buf)
obs_buf_all = torch.stack([self.obs_history[i]
for i in range(self.obs_history.maxlen)], dim=1) # N,T,K
self.obs_buf = obs_buf_all.reshape(self.num_envs, -1) # N, T*K
self.privileged_obs_buf = torch.cat([self.critic_history[i] for i in range(self.cfg.env.c_frame_stack)], dim=1)
def reset_idx(self, env_ids):
""" Reset some environments.
Calls self._reset_dofs(env_ids), self._reset_root_states(env_ids), and self._resample_commands(env_ids)
[Optional] calls self._update_terrain_curriculum(env_ids), self.update_command_curriculum(env_ids) and
Logs episode info
Resets some buffers
Args:
env_ids (list[int]): List of environment ids which must be reset
"""
if len(env_ids) == 0:
return
# update curriculum
if self.cfg.terrain.curriculum:
self._update_terrain_curriculum(env_ids)
# avoid updating command curriculum at each step since the maximum command is common to all envs
if self.cfg.commands.curriculum and (self.common_step_counter % self.max_episode_length==0):
self.update_command_curriculum(env_ids)
# reset rand dof_pos and dof_vel=0
self._reset_dofs(env_ids)
# reset base position
self._reset_root_states(env_ids)
# Randomize joint parameters, like torque gain friction ...
self.randomize_dof_props(env_ids)
self._refresh_actor_dof_props(env_ids)
self.randomize_lag_props(env_ids)
# reset buffers
self.last_last_actions[env_ids] = 0.
self.actions[env_ids] = 0.
self.last_actions[env_ids] = 0.
self.last_rigid_state[env_ids] = 0.
self.last_dof_vel[env_ids] = 0.
self.last_root_vel[env_ids] = 0.
self.feet_air_time[env_ids] = 0.
self.episode_length_buf[env_ids] = 0
self.phase_length_buf[env_ids] = 0
self.reset_buf[env_ids] = 1
# rand 0 or 0.5
self.gait_start[env_ids] = torch.randint(0, 2, (len(env_ids),)).to(self.device)*0.5
#resample command
self.generate_gait_time(env_ids)
self._resample_commands()
# fill extras
self.extras["episode"] = {}
for key in self.episode_sums.keys():
self.extras["episode"]['rew_' + key] = torch.mean(self.episode_sums[key][env_ids]) / self.max_episode_length_s
self.episode_sums[key][env_ids] = 0.
# log additional curriculum info
if self.cfg.terrain.mesh_type == "trimesh":
self.extras["episode"]["terrain_level"] = torch.mean(self.terrain_levels.float())
if self.cfg.commands.curriculum:
self.extras["episode"]["max_command_x"] = self.command_ranges["lin_vel_x"][1]
# send timeout info to the algorithm
if self.cfg.env.send_timeouts:
self.extras["time_outs"] = self.time_out_buf
# fix reset gravity bug
self.gym.refresh_actor_root_state_tensor(self.sim)
self.gym.refresh_net_contact_force_tensor(self.sim)
self.gym.refresh_rigid_body_state_tensor(self.sim)
self.base_quat[env_ids] = self.root_states[env_ids, 3:7]
self.base_euler_xyz = get_euler_xyz_tensor(self.base_quat)
self.projected_gravity[env_ids] = quat_rotate_inverse(self.base_quat[env_ids], self.gravity_vec[env_ids])
self.base_lin_vel[env_ids] = quat_rotate_inverse(self.base_quat[env_ids], self.root_states[env_ids, 7:10])
self.base_ang_vel[env_ids] = quat_rotate_inverse(self.base_quat[env_ids], self.root_states[env_ids, 10:13])
self.feet_quat = self.rigid_state[:, self.feet_indices, 3:7]
self.feet_euler_xyz = get_euler_xyz_tensor(self.feet_quat)
# clear obs history buffer and privileged obs buffer
for i in range(self.obs_history.maxlen):
self.obs_history[i][env_ids] *= 0
for i in range(self.critic_history.maxlen):
self.critic_history[i][env_ids] *= 0
def _init_buffers(self):
""" Initialize torch tensors which will contain simulation states and processed quantities
"""
super()._init_buffers()
self.gait_time = torch.zeros(self.num_envs, len(self.cfg.commands.gait) ,dtype=torch.int, device=self.device, requires_grad=False)
self.phase_length_buf = torch.zeros(
self.num_envs, device=self.device, dtype=torch.long)
self.gait_start = torch.randint(0, 2, (self.num_envs,)).to(self.device)*0.5
# ================================================ Rewards ================================================== #
def _reward_ref_joint_pos(self):
"""
Calculates the reward based on the difference between the current joint positions and the target joint positions.
"""
joint_pos = self.dof_pos.clone()
pos_target = self.ref_dof_pos.clone()
stand_command = (torch.norm(self.commands[:, :3], dim=1) <= self.cfg.commands.stand_com_threshold)
pos_target[stand_command] = self.default_dof_pos.clone()
diff = joint_pos - pos_target
r = torch.exp(-2 * torch.norm(diff, dim=1)) - 0.2 * torch.norm(diff, dim=1).clamp(0, 0.5)
r[stand_command] = 1.0
return r
def _reward_feet_distance(self):
"""
Calculates the reward based on the distance between the feet. Penilize feet get close to each other or too far away.
"""
foot_pos = self.rigid_state[:, self.feet_indices, :2]
foot_dist = torch.norm(foot_pos[:, 0, :] - foot_pos[:, 1, :], dim=1)
fd = self.cfg.rewards.foot_min_dist
max_df = self.cfg.rewards.foot_max_dist
d_min = torch.clamp(foot_dist - fd, -0.5, 0.)
d_max = torch.clamp(foot_dist - max_df, 0, 0.5)
return (torch.exp(-torch.abs(d_min) * 100) + torch.exp(-torch.abs(d_max) * 100)) / 2
def _reward_knee_distance(self):
"""
Calculates the reward based on the distance between the knee of the humanoid.
"""
foot_pos = self.rigid_state[:, self.knee_indices, :2]
foot_dist = torch.norm(foot_pos[:, 0, :] - foot_pos[:, 1, :], dim=1)
fd = self.cfg.rewards.foot_min_dist
max_df = self.cfg.rewards.foot_max_dist / 2
d_min = torch.clamp(foot_dist - fd, -0.5, 0.)
d_max = torch.clamp(foot_dist - max_df, 0, 0.5)
return (torch.exp(-torch.abs(d_min) * 100) + torch.exp(-torch.abs(d_max) * 100)) / 2
def _reward_foot_slip(self):
"""
Calculates the reward for minimizing foot slip. The reward is based on the contact forces
and the speed of the feet. A contact threshold is used to determine if the foot is in contact
with the ground. The speed of the foot is calculated and scaled by the contact conditions.
"""
contact = self.contact_forces[:, self.feet_indices, 2] > 5.
foot_speed_norm = torch.norm(self.rigid_state[:, self.feet_indices, 10:12], dim=2)
rew = torch.sqrt(foot_speed_norm)
rew *= contact
return torch.sum(rew, dim=1)
def _reward_feet_air_time(self):
"""
Calculates the reward for feet air time, promoting longer steps. This is achieved by
checking the first contact with the ground after being in the air. The air time is
limited to a maximum value for reward calculation.
"""
contact = self.contact_forces[:, self.feet_indices, 2] > 5.
stance_mask = self._get_stance_mask().clone()
stance_mask[torch.norm(self.commands[:, :3], dim=1) < 0.05] = 1
self.contact_filt = torch.logical_or(torch.logical_or(contact, stance_mask), self.last_contacts)
self.last_contacts = contact
first_contact = (self.feet_air_time > 0.) * self.contact_filt
self.feet_air_time += self.dt
air_time = self.feet_air_time.clamp(0, 0.5) * first_contact
self.feet_air_time *= ~self.contact_filt
return air_time.sum(dim=1)
def _reward_feet_contact_number(self):
"""
Calculates a reward based on the number of feet contacts aligning with the gait phase.
Rewards or penalizes depending on whether the foot contact matches the expected gait phase.
"""
contact = self.contact_forces[:, self.feet_indices, 2] > 5.
stance_mask = self._get_stance_mask().clone()
stance_mask[torch.norm(self.commands[:, :3], dim=1) <= self.cfg.commands.stand_com_threshold] = 1
reward = torch.where(contact == stance_mask, 1, -0.3)
return torch.mean(reward, dim=1)
def _reward_orientation(self):
"""
Calculates the reward for maintaining a flat base orientation. It penalizes deviation
from the desired base orientation using the base euler angles and the projected gravity vector.
"""
quat_mismatch = torch.exp(-torch.sum(torch.abs(self.base_euler_xyz[:, :2]), dim=1) * 10)
orientation = torch.exp(-torch.norm(self.projected_gravity[:, :2], dim=1) * 20)
return (quat_mismatch + orientation) / 2.
def _reward_feet_contact_forces(self):
"""
Calculates the reward for keeping contact forces within a specified range. Penalizes
high contact forces on the feet.
"""
return torch.sum((torch.norm(self.contact_forces[:, self.feet_indices, :], dim=-1) - self.cfg.rewards.max_contact_force).clip(0, 400), dim=1)
def _reward_default_joint_pos(self):
"""
Calculates the reward for keeping joint positions close to default positions, with a focus
on penalizing deviation in yaw and roll directions. Excludes yaw and roll from the main penalty.
"""
joint_diff = self.dof_pos - self.default_joint_pd_target
left_yaw_roll = joint_diff[:, [1,2,5]]
right_yaw_roll = joint_diff[:, [7,8,11]]
yaw_roll = torch.norm(left_yaw_roll, dim=1) + torch.norm(right_yaw_roll, dim=1)
yaw_roll = torch.clamp(yaw_roll - 0.1, 0, 50)
return torch.exp(-yaw_roll * 100) - 0.01 * torch.norm(joint_diff, dim=1)
def _reward_base_height(self):
"""
Calculates the reward based on the robot's base height. Penalizes deviation from a target base height.
The reward is computed based on the height difference between the robot's base and the average height
of its feet when they are in contact with the ground.
"""
stance_mask = self._get_stance_mask()
measured_heights = torch.sum(
self.rigid_state[:, self.feet_indices, 2] * stance_mask, dim=1) / torch.sum(stance_mask, dim=1)
base_height = self.root_states[:, 2] - (measured_heights - self.cfg.rewards.feet_to_ankle_distance)
return torch.exp(-torch.abs(base_height - self.cfg.rewards.base_height_target) * 100)
def _reward_base_acc(self):
"""
Computes the reward based on the base's acceleration. Penalizes high accelerations of the robot's base,
encouraging smoother motion.
"""
root_acc = self.last_root_vel - self.root_states[:, 7:13]
rew = torch.exp(-torch.norm(root_acc, dim=1) * 3)
return rew
def _reward_vel_mismatch_exp(self):
"""
Computes a reward based on the mismatch in the robot's linear and angular velocities.
Encourages the robot to maintain a stable velocity by penalizing large deviations.
"""
lin_mismatch = torch.exp(-torch.square(self.base_lin_vel[:, 2]) * 10)
ang_mismatch = torch.exp(-torch.norm(self.base_ang_vel[:, :2], dim=1) * 5.)
c_update = (lin_mismatch + ang_mismatch) / 2.
return c_update
def _reward_track_vel_hard(self):
"""
Calculates a reward for accurately tracking both linear and angular velocity commands.
Penalizes deviations from specified linear and angular velocity targets.
"""
# Tracking of linear velocity commands (xy axes)
lin_vel_error = torch.norm(
self.commands[:, :2] - self.base_lin_vel[:, :2], dim=1)
lin_vel_error_exp = torch.exp(-lin_vel_error * 10)
# Tracking of angular velocity commands (yaw)
ang_vel_error = torch.abs(
self.commands[:, 2] - self.base_ang_vel[:, 2])
ang_vel_error_exp = torch.exp(-ang_vel_error * 10)
linear_error = 0.2 * (lin_vel_error + ang_vel_error)
r = (lin_vel_error_exp + ang_vel_error_exp) / 2. - linear_error
return r
def _reward_tracking_lin_vel(self):
"""
Tracks linear velocity commands along the xy axes.
Calculates a reward based on how closely the robot's linear velocity matches the commanded values.
"""
stand_command = (torch.norm(self.commands[:, :3], dim=1) <= self.cfg.commands.stand_com_threshold)
lin_vel_error_square = torch.sum(torch.square(
self.commands[:, :2] - self.base_lin_vel[:, :2]), dim=1)
lin_vel_error_abs = torch.sum(torch.abs(
self.commands[:, :2] - self.base_lin_vel[:, :2]), dim=1)
r_square = torch.exp(-lin_vel_error_square * self.cfg.rewards.tracking_sigma)
r_abs = torch.exp(-lin_vel_error_abs * self.cfg.rewards.tracking_sigma * 2)
r = torch.where(stand_command, r_abs, r_square)
return r
def _reward_tracking_ang_vel(self):
"""
Tracks angular velocity commands for yaw rotation.
Computes a reward based on how closely the robot's angular velocity matches the commanded yaw values.
"""
stand_command = (torch.norm(self.commands[:, :3], dim=1) <= self.cfg.commands.stand_com_threshold)
ang_vel_error_square = torch.square(
self.commands[:, 2] - self.base_ang_vel[:, 2])
ang_vel_error_abs = torch.abs(
self.commands[:, 2] - self.base_ang_vel[:, 2])
r_square = torch.exp(-ang_vel_error_square * self.cfg.rewards.tracking_sigma)
r_abs = torch.exp(-ang_vel_error_abs * self.cfg.rewards.tracking_sigma * 2)
r = torch.where(stand_command, r_abs, r_square)
return r
def _reward_feet_clearance(self):
"""
Calculates reward based on the clearance of the swing leg from the ground during movement.
Encourages appropriate lift of the feet during the swing phase of the gait.
"""
# Compute feet contact mask
contact = self.contact_forces[:, self.feet_indices, 2] > 5.
# Get the z-position of the feet and compute the change in z-position
feet_z = self.rigid_state[:, self.feet_indices, 2] - self.cfg.rewards.feet_to_ankle_distance
delta_z = feet_z - self.last_feet_z
self.feet_height += delta_z
self.last_feet_z = feet_z
# Compute swing mask
swing_mask = 1 - self._get_stance_mask()
# feet height should larger than target feet height at the peak
rew_pos = (self.feet_height > self.cfg.rewards.target_feet_height) * (self.feet_height < self.cfg.rewards.target_feet_height_max)
rew_pos = torch.sum(rew_pos * swing_mask, dim=1)
self.feet_height *= ~contact
return rew_pos
def _reward_low_speed(self):
"""
Rewards or penalizes the robot based on its speed relative to the commanded speed.
This function checks if the robot is moving too slow, too fast, or at the desired speed,
and if the movement direction matches the command.
"""
# Calculate the absolute value of speed and command for comparison
absolute_speed = torch.abs(self.base_lin_vel[:, 0])
absolute_command = torch.abs(self.commands[:, 0])
# Define speed criteria for desired range
speed_too_low = absolute_speed < 0.5 * absolute_command
speed_too_high = absolute_speed > 1.2 * absolute_command
speed_desired = ~(speed_too_low | speed_too_high)
# Check if the speed and command directions are mismatched
sign_mismatch = torch.sign(
self.base_lin_vel[:, 0]) != torch.sign(self.commands[:, 0])
# Initialize reward tensor
reward = torch.zeros_like(self.base_lin_vel[:, 0])
# Assign rewards based on conditions
# Speed too low
reward[speed_too_low] = -1.0
# Speed too high
reward[speed_too_high] = 0.
# Speed within desired range
reward[speed_desired] = 1.2
# Sign mismatch has the highest priority
reward[sign_mismatch] = -2.0
return reward * (self.commands[:, 0].abs() > 0.05)
def _reward_torques(self):
"""
Penalizes the use of high torques in the robot's joints. Encourages efficient movement by minimizing
the necessary force exerted by the motors.
"""
return torch.sum(torch.square(self.torques), dim=1)
def _reward_ankle_torques(self):
"""
Penalizes the use of high torques in the robot's joints. Encourages efficient movement by minimizing
the necessary force exerted by the motors.
"""
ankle_idx = [4,5,10,11]
return torch.sum(torch.square(self.torques[:,ankle_idx]), dim=1)
def _reward_feet_rotation(self):
feet_euler_xyz = self.feet_euler_xyz
rotation = torch.sum(torch.square(feet_euler_xyz[:,:,:2]),dim=[1,2])
# rotation = torch.sum(torch.square(feet_euler_xyz[:,:,1]),dim=1)
r = torch.exp(-rotation*15)
return r
def _reward_dof_vel(self):
"""
Penalizes high velocities at the degrees of freedom (DOF) of the robot. This encourages smoother and
more controlled movements.
"""
return torch.sum(torch.square(self.dof_vel), dim=1)
def _reward_dof_acc(self):
"""
Penalizes high accelerations at the robot's degrees of freedom (DOF). This is important for ensuring
smooth and stable motion, reducing wear on the robot's mechanical parts.
"""
return torch.sum(torch.square((self.last_dof_vel - self.dof_vel) / self.dt), dim=1)
def _reward_collision(self):
"""
Penalizes collisions of the robot with the environment, specifically focusing on selected body parts.
This encourages the robot to avoid undesired contact with objects or surfaces.
"""
return torch.sum(1.*(torch.norm(self.contact_forces[:, self.penalised_contact_indices, :], dim=-1) > 0.1), dim=1)
def _reward_action_smoothness(self):
"""
Encourages smoothness in the robot's actions by penalizing large differences between consecutive actions.
This is important for achieving fluid motion and reducing mechanical stress.
"""
term_1 = torch.sum(torch.square(
self.last_actions - self.actions), dim=1)
term_2 = torch.sum(torch.square(
self.actions + self.last_last_actions - 2 * self.last_actions), dim=1)
term_3 = 0.05 * torch.sum(torch.abs(self.actions), dim=1)
return term_1 + term_2 + term_3
def _reward_termination(self):
# Terminal reward / penalty
return self.reset_buf * ~self.time_out_buf
def _reward_stand_still(self):
# penalize motion at zero commands
stand_command = (torch.norm(self.commands[:, :3], dim=1) <= self.cfg.commands.stand_com_threshold)
r = torch.exp(-torch.sum(torch.square(self.dof_pos - self.default_dof_pos), dim=1))
r = torch.where(stand_command, r.clone(),
torch.zeros_like(r))
return r
def _reward_feet_stumble(self):
# Penalize feet hitting vertical surfaces
return torch.any(torch.norm(self.contact_forces[:, self.feet_indices, :2], dim=2) >\
5 *torch.abs(self.contact_forces[:, self.feet_indices, 2]), dim=1)
def _reward_dof_pos_limits(self):
# Penalize dof positions too close to the limit
out_of_limits = -(self.dof_pos - self.dof_pos_limits[:, 0]).clip(max=0.) # lower limit
out_of_limits += (self.dof_pos - self.dof_pos_limits[:, 1]).clip(min=0.)
return torch.sum(out_of_limits, dim=1)
def _reward_dof_vel_limits(self):
# Penalize dof velocities too close to the limit
# clip to max error = 1 rad/s per joint to avoid huge penalties
return torch.sum((torch.abs(self.dof_vel) - self.dof_vel_limits*self.cfg.rewards.soft_dof_vel_limit).clip(min=0., max=1.), dim=1)
def _reward_dof_torque_limits(self):
# penalize torques too close to the limit
return torch.sum((torch.abs(self.torques) - self.torque_limits*self.cfg.rewards.soft_torque_limit).clip(min=0.), dim=1)
\ No newline at end of file
resources/robots/elf12/urdf/elf12.urdf 0 → 100755
<?xml version="1.0" encoding="utf-8"?>
<robot
name="elf12">
<link
name="base_link">
<inertial>
<origin
xyz="0.00030047 8.28e-06 0.03546793"
rpy="0 0 0" />
<mass
value="3.24038695" />
<inertia
ixx="0.02280423"
ixy="3.8e-07"
ixz="-1.083e-05"
iyy="0.00335281"
iyz="1.75e-06"
izz="0.02410611" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="../meshes/base_link.STL" />
</geometry>
<material
name="">
<color
rgba="0.75294 0.75294 0.75294 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="../meshes/base_link.STL" />
</geometry>
</collision>
</link>
<link
name="l_hip_z_link">
<inertial>
<origin
xyz="-0.0951743 -0.00029544 0.01157699"
rpy="0 0 0" />
<mass
value="1.26002534" />
<inertia
ixx="0.00250858"
ixy="-1.87e-05"
ixz="-0.00093719"
iyy="0.00299504"
iyz="6.14e-06"
izz="0.00188897" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="../meshes/l_hip_z_link.STL" />
</geometry>
<material
name="">
<color
rgba="0.752941176470588 0.752941176470588 0.752941176470588 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="../meshes/l_hip_z_link.STL" />
</geometry>
</collision>
</link>
<joint
name="l_hip_z_joint"
type="revolute">
<origin
xyz="0 0.1 -0.1008"
rpy="0 0 0" />
<parent
link="base_link" />
<child
link="l_hip_z_link" />
<axis
xyz="0 0 1" />
<limit
lower="-0.785"
upper="0.785"
effort="20"
velocity="20" />
</joint>
<link
name="l_hip_x_link">
<inertial>
<origin
xyz="-0.00349545 -0.05016077 0.00015601"
rpy="0 0 0" />
<mass
value="1.75826055" />
<inertia
ixx="0.00145734"
ixy="0.000276"
ixz="2.571e-05"
iyy="0.00183603"
iyz="-4e-08"
izz="0.00181117" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="../meshes/l_hip_x_link.STL" />
</geometry>
<material
name="">
<color
rgba="0.752941176470588 0.752941176470588 0.752941176470588 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="../meshes/l_hip_x_link.STL" />
</geometry>
</collision>
</link>
<joint
name="l_hip_x_joint"
type="revolute">
<origin
xyz="0 0 0"
rpy="0 0 0" />
<parent
link="l_hip_z_link" />
<child
link="l_hip_x_link" />
<axis
xyz="1 0 0" />
<limit
lower="-0.523"
upper="0.785"
effort="40"
velocity="20" />
</joint>
<link
name="l_hip_y_link">
<inertial>
<origin
xyz="0.00237237 0.02408075 -0.04874487"
rpy="0 0 0" />
<mass
value="2.66296335" />
<inertia
ixx="0.02678784"
ixy="0.00016594"
ixz="0.00075939"
iyy="0.02619934"
iyz="-0.00371457"
izz="0.0045187" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="../meshes/l_hip_y_link.STL" />
</geometry>
<material
name="">
<color
rgba="0.752941176470588 0.752941176470588 0.752941176470588 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="../meshes/l_hip_y_link.STL" />
</geometry>
</collision>
</link>
<joint
name="l_hip_y_joint"
type="revolute">
<origin
xyz="0 0 0"
rpy="0 0 0" />
<parent
link="l_hip_x_link" />
<child
link="l_hip_y_link" />
<axis
xyz="0 1 0" />
<limit
lower="-2"
upper="1"
effort="100"
velocity="20" />
</joint>
<link
name="l_knee_y_link">
<inertial>
<origin
xyz="0.00711393 -0.00031499 -0.10977806"
rpy="0 0 0" />
<mass
value="1.63072153" />
<inertia
ixx="0.01042782"
ixy="1.62e-06"
ixz="-0.00054999"
iyy="0.01018545"
iyz="-0.00015956"
izz="0.00140744" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="../meshes/l_knee_y_link.STL" />
</geometry>
<material
name="">
<color
rgba="0.752941176470588 0.752941176470588 0.752941176470588 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="../meshes/l_knee_y_link.STL" />
</geometry>
</collision>
</link>
<joint
name="l_knee_y_joint"
type="revolute">
<origin
xyz="0 0 -0.3"
rpy="0 0 0" />
<parent
link="l_hip_y_link" />
<child
link="l_knee_y_link" />
<axis
xyz="0 1 0" />
<limit
lower="0"
upper="2.355"
effort="120"
velocity="20" />
</joint>
<link
name="l_ankle_y_link">
<inertial>
<origin
xyz="0.0 0.0 -0.00046563"
rpy="0 0 0" />
<mass
value="0.01206963" />
<inertia
ixx="2.89e-06"
ixy="-0.0"
ixz="-0.0"
iyy="3.4e-07"
iyz="-0.0"
izz="3.01e-06" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="../meshes/l_ankle_y_link.STL" />
</geometry>
<material
name="">
<color
rgba="0.752941176470588 0.752941176470588 0.752941176470588 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="../meshes/l_ankle_y_link.STL" />
</geometry>
</collision>
</link>
<joint
name="l_ankle_y_joint"
type="revolute">
<origin
xyz="0 0 -0.3"
rpy="0 0 0" />
<parent
link="l_knee_y_link" />
<child
link="l_ankle_y_link" />
<axis
xyz="0 1 0" />
<limit
lower="-2"
upper="2"
effort="14"
velocity="20" />
</joint>
<link
name="l_ankle_x_link">
<inertial>
<origin
xyz="-0.00088171 0.0 -0.02245193"
rpy="0 0 0" />
<mass
value="0.27064154" />
<inertia
ixx="0.00014549"
ixy="-0.0"
ixz="5.53e-06"
iyy="0.00071745"
iyz="-0.0"
izz="0.00081666" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="../meshes/l_ankle_x_link.STL" />
</geometry>
<material
name="">
<color
rgba="0.752941176470588 0.752941176470588 0.752941176470588 1" />
</material>
</visual>
<!-- <collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="../meshes/l_ankle_x_link.STL" />
</geometry>
</collision> -->
<collision name="l_foot_1">
<origin rpy="0 1.5708 0" xyz="0.0 0.03 -0.03"/>
<geometry>
<cylinder length="0.185" radius="0.01"/>
</geometry>
</collision>
<collision name="l_foot_2">
<origin rpy="0 1.5708 0" xyz="0.0 -0.03 -0.03"/>
<geometry>
<cylinder length="0.185" radius="0.01"/>
</geometry>
</collision>
</link>
<joint
name="l_ankle_x_joint"
type="revolute">
<origin
xyz="0 0 0"
rpy="0 0 0" />
<parent
link="l_ankle_y_link" />
<child
link="l_ankle_x_link" />
<axis
xyz="1 0 0" />
<limit
lower="-0.349"
upper="0.349"
effort="7"
velocity="20" />
</joint>
<link
name="r_hip_z_link">
<inertial>
<origin
xyz="-0.0951743 0.00029543 0.01157699"
rpy="0 0 0" />
<mass
value="1.26002506" />
<inertia
ixx="0.00250858"
ixy="1.87e-05"
ixz="-0.00093719"
iyy="0.00299504"
iyz="-6.14e-06"
izz="0.00188897" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="../meshes/r_hip_z_link.STL" />
</geometry>
<material
name="">
<color
rgba="0.752941176470588 0.752941176470588 0.752941176470588 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="../meshes/r_hip_z_link.STL" />
</geometry>
</collision>
</link>
<joint
name="r_hip_z_joint"
type="revolute">
<origin
xyz="0 -0.1 -0.1008"
rpy="0 0 0" />
<parent
link="base_link" />
<child
link="r_hip_z_link" />
<axis
xyz="0 0 1" />
<limit
lower="-0.785"
upper="0.785"
effort="20"
velocity="20" />
</joint>
<link
name="r_hip_x_link">
<inertial>
<origin
xyz="-0.00349545 0.05016077 0.000156"
rpy="0 0 0" />
<mass
value="1.75825984" />
<inertia
ixx="0.00145734"
ixy="-0.000276"
ixz="2.571e-05"
iyy="0.00183603"
iyz="4e-08"
izz="0.00181117" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="../meshes/r_hip_x_link.STL" />
</geometry>
<material
name="">
<color
rgba="0.752941176470588 0.752941176470588 0.752941176470588 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="../meshes/r_hip_x_link.STL" />
</geometry>
</collision>
</link>
<joint
name="r_hip_x_joint"
type="revolute">
<origin
xyz="0 0 0"
rpy="0 0 0" />
<parent
link="r_hip_z_link" />
<child
link="r_hip_x_link" />
<axis
xyz="1 0 0" />
<limit
lower="-0.785"
upper="0.523"
effort="40"
velocity="20" />
</joint>
<link
name="r_hip_y_link">
<inertial>
<origin
xyz="0.00237237 -0.02408076 -0.04874488"
rpy="0 0 0" />
<mass
value="2.66296401" />
<inertia
ixx="0.02678786"
ixy="-0.00016594"
ixz="0.00075939"
iyy="0.02619935"
iyz="0.00371458"
izz="0.0045187" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="../meshes/r_hip_y_link.STL" />
</geometry>
<material
name="">
<color
rgba="0.752941176470588 0.752941176470588 0.752941176470588 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="../meshes/r_hip_y_link.STL" />
</geometry>
</collision>
</link>
<joint
name="r_hip_y_joint"
type="revolute">
<origin
xyz="0 0 0"
rpy="0 0 0" />
<parent
link="r_hip_x_link" />
<child
link="r_hip_y_link" />
<axis
xyz="0 1 0" />
<limit
lower="-2"
upper="1"
effort="100"
velocity="20" />
</joint>
<link
name="r_knee_y_link">
<inertial>
<origin
xyz="0.00711396 0.00031502 -0.109778"
rpy="0 0 0" />
<mass
value="1.63072158" />
<inertia
ixx="0.01042781"
ixy="-1.62e-06"
ixz="-0.00054998"
iyy="0.01018544"
iyz="0.00015955"
izz="0.00140744" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="../meshes/r_knee_y_link.STL" />
</geometry>
<material
name="">
<color
rgba="0.752941176470588 0.752941176470588 0.752941176470588 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="../meshes/r_knee_y_link.STL" />
</geometry>
</collision>
</link>
<joint
name="r_knee_y_joint"
type="revolute">
<origin
xyz="0 0 -0.3"
rpy="0 0 0" />
<parent
link="r_hip_y_link" />
<child
link="r_knee_y_link" />
<axis
xyz="0 1 0" />
<limit
lower="0"
upper="2.355"
effort="120"
velocity="20" />
</joint>
<link
name="r_ankle_y_link">
<inertial>
<origin
xyz="0.0 0.0 -0.00046563"
rpy="0 0 0" />
<mass
value="0.01206963" />
<inertia
ixx="2.89e-06"
ixy="-0.0"
ixz="-0.0"
iyy="3.4e-07"
iyz="-0.0"
izz="3.01e-06" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="../meshes/r_ankle_y_link.STL" />
</geometry>
<material
name="">
<color
rgba="0.752941176470588 0.752941176470588 0.752941176470588 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="../meshes/r_ankle_y_link.STL" />
</geometry>
</collision>
</link>
<joint
name="r_ankle_y_joint"
type="revolute">
<origin
xyz="0 0 -0.3"
rpy="0 0 0" />
<parent
link="r_knee_y_link" />
<child
link="r_ankle_y_link" />
<axis
xyz="0 1 0" />
<limit
lower="-2"
upper="2"
effort="14"
velocity="20" />
</joint>
<link
name="r_ankle_x_link">
<inertial>
<origin
xyz="-0.00088171 0.0 -0.02245193"
rpy="0 0 0" />
<mass
value="0.27064154" />
<inertia
ixx="0.00014549"
ixy="-0.0"
ixz="5.53e-06"
iyy="0.00071745"
iyz="-0.0"
izz="0.00081666" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="../meshes/r_ankle_x_link.STL" />
</geometry>
<material
name="">
<color
rgba="0.752941176470588 0.752941176470588 0.752941176470588 1" />
</material>
</visual>
<!-- <collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="../meshes/r_ankle_x_link.STL" />
</geometry>
</collision> -->
<collision name="r_foot_1">
<origin rpy="0 1.5708 0" xyz="0.0 0.03 -0.03"/>
<geometry>
<cylinder length="0.185" radius="0.01"/>
</geometry>
</collision>
<collision name="r_foot_2">
<origin rpy="0 1.5708 0" xyz="0.0 -0.03 -0.03"/>
<geometry>
<cylinder length="0.185" radius="0.01"/>
</geometry>
</collision>
</link>
<joint
name="r_ankle_x_joint"
type="revolute">
<origin
xyz="0 0 0"
rpy="0 0 0" />
<parent
link="r_ankle_y_link" />
<child
link="r_ankle_x_link" />
<axis
xyz="1 0 0" />
<limit
lower="-0.349"
upper="0.349"
effort="6"
velocity="20" />
</joint>
<link
name="waist_z_link">
<inertial>
<origin
xyz="-0.00448944 0.0 -0.02389195"
rpy="0 0 0" />
<mass
value="0.21792014" />
<inertia
ixx="9.089e-05"
ixy="-0.0"
ixz="3.456e-05"
iyy="0.0001491"
iyz="-0.0"
izz="0.00013001" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="../meshes/waist_z_link.STL" />
</geometry>
<material
name="">
<color
rgba="0.752941176470588 0.752941176470588 0.752941176470588 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="../meshes/waist_z_link.STL" />
</geometry>
</collision>
</link>
<joint
name="waist_z_joint"
type="fixed">
<origin
xyz="0 0 0.1167"
rpy="0 0 0" />
<parent
link="base_link" />
<child
link="waist_z_link" />
<axis
xyz="0 0 1" />
<limit
lower="-1.57"
upper="1.57"
effort="40"
velocity="20" />
</joint>
<link
name="waist_x_link">
<inertial>
<origin
xyz="0.0 0.0 0.0"
rpy="0 0 0" />
<mass
value="0.05316739" />
<inertia
ixx="9.7e-06"
ixy="-0.0"
ixz="-0.0"
iyy="9.7e-06"
iyz="-0.0"
izz="1.819e-05" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="../meshes/waist_x_link.STL" />
</geometry>
<material
name="">
<color
rgba="0.752941176470588 0.752941176470588 0.752941176470588 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="../meshes/waist_x_link.STL" />
</geometry>
</collision>
</link>
<joint
name="waist_x_joint"
type="fixed">
<origin
xyz="0 0 0"
rpy="0 0 0" />
<parent
link="waist_z_link" />
<child
link="waist_x_link" />
<axis
xyz="1 0 0" />
<limit
lower="-0.262"
upper="0.262"
effort="50"
velocity="20" />
</joint>
<link
name="waist_y_link">
<inertial>
<origin
xyz="-0.00308477 0.00223688 0.1873963"
rpy="0 0 0" />
<mass
value="8.1645757" />
<inertia
ixx="0.12637158"
ixy="-0.00018869"
ixz="-0.0005031"
iyy="0.08145202"
iyz="-0.00060193"
izz="0.06836301" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="../meshes/waist_y_link.STL" />
</geometry>
<material
name="">
<color
rgba="0.752941176470588 0.752941176470588 0.752941176470588 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="../meshes/waist_y_link.STL" />
</geometry>
</collision>
</link>
<joint
name="waist_y_joint"
type="fixed">
<origin
xyz="0 0 0"
rpy="0 0 0" />
<parent
link="waist_x_link" />
<child
link="waist_y_link" />
<axis
xyz="0 1 0" />
<limit
lower="-0.523"
upper="0.523"
effort="50"
velocity="20" />
</joint>
<link
name="l_shld_y_link">
<inertial>
<origin
xyz="0.00068297 -0.00449372 2.22e-06"
rpy="0 0 0" />
<mass
value="0.53804971" />
<inertia
ixx="0.00030648"
ixy="4.03e-06"
ixz="2e-08"
iyy="0.00029153"
iyz="1e-07"
izz="0.00032312" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="../meshes/l_shld_y_link.STL" />
</geometry>
<material
name="">
<color
rgba="0.752941176470588 0.752941176470588 0.752941176470588 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="../meshes/l_shld_y_link.STL" />
</geometry>
</collision>
</link>
<joint
name="l_shld_y_joint"
type="fixed">
<origin
xyz="0 0.21705 0.30075"
rpy="0 0 0" />
<parent
link="waist_y_link" />
<child
link="l_shld_y_link" />
<axis
xyz="0 1 0" />
<limit
lower="-2"
upper="2"
effort="27"
velocity="20" />
</joint>
<link
name="l_shld_x_link">
<inertial>
<origin
xyz="-2.047e-05 5.316e-05 -0.06162654"
rpy="0 0 0" />
<mass
value="0.48847489" />
<inertia
ixx="0.00027194"
ixy="4e-08"
ixz="-5.4e-07"
iyy="0.00033709"
iyz="-5.4e-07"
izz="0.0002884" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="../meshes/l_shld_x_link.STL" />
</geometry>
<material
name="">
<color
rgba="0.752941176470588 0.752941176470588 0.752941176470588 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="../meshes/l_shld_x_link.STL" />
</geometry>
</collision>
</link>
<joint
name="l_shld_x_joint"
type="fixed">
<origin
xyz="0 0 0"
rpy="0 0 0" />
<parent
link="l_shld_y_link" />
<child
link="l_shld_x_link" />
<axis
xyz="1 0 0" />
<limit
lower="-0.5"
upper="1.57"
effort="27"
velocity="20" />
</joint>
<link
name="l_shld_z_link">
<inertial>
<origin
xyz="4.164e-05 0.00022669 -0.2212782"
rpy="0 0 0" />
<mass
value="0.59309528" />
<inertia
ixx="0.00136573"
ixy="5.3e-07"
ixz="-1e-08"
iyy="0.00133989"
iyz="1.22e-06"
izz="0.00030342" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="../meshes/l_shld_z_link.STL" />
</geometry>
<material
name="">
<color
rgba="0.752941176470588 0.752941176470588 0.752941176470588 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="../meshes/l_shld_z_link.STL" />
</geometry>
</collision>
</link>
<joint
name="l_shld_z_joint"
type="fixed">
<origin
xyz="0 0 0"
rpy="0 0 0" />
<parent
link="l_shld_x_link" />
<child
link="l_shld_z_link" />
<axis
xyz="0 0 1" />
<limit
lower="-0.785"
upper="0.785"
effort="7"
velocity="20" />
</joint>
<link
name="l_elb_y_link">
<inertial>
<origin
xyz="5.192e-05 1.999e-05 -0.08561189"
rpy="0 0 0" />
<mass
value="0.50005725" />
<inertia
ixx="0.00043713"
ixy="-4e-08"
ixz="-4.9e-07"
iyy="0.00036043"
iyz="6.3e-07"
izz="0.00030266" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="../meshes/l_elb_y_link.STL" />
</geometry>
<material
name="">
<color
rgba="0.752941176470588 0.752941176470588 0.752941176470588 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="../meshes/l_elb_y_link.STL" />
</geometry>
</collision>
</link>
<joint
name="l_elb_y_joint"
type="fixed">
<origin
xyz="0 0 -0.24"
rpy="0 0 0" />
<parent
link="l_shld_z_link" />
<child
link="l_elb_y_link" />
<axis
xyz="0 1 0" />
<limit
lower="-2.355"
upper="0.5"
effort="27"
velocity="20" />
</joint>
<link
name="l_elb_z_link">
<inertial>
<origin
xyz="1.97e-06 -1.756e-05 -0.18282539"
rpy="0 0 0" />
<mass
value="0.13457416" />
<inertia
ixx="0.00040001"
ixy="-0.0"
ixz="-2e-08"
iyy="0.00039996"
iyz="1.5e-07"
izz="5.662e-05" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="../meshes/l_elb_z_link.STL" />
</geometry>
<material
name="">
<color
rgba="0.752941176470588 0.752941176470588 0.752941176470588 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="../meshes/l_elb_z_link.STL" />
</geometry>
</collision>
</link>
<joint
name="l_elb_z_joint"
type="fixed">
<origin
xyz="0 0 0"
rpy="0 0 0" />
<parent
link="l_elb_y_link" />
<child
link="l_elb_z_link" />
<axis
xyz="0 0 1" />
<limit
lower="-0.785"
upper="0.785"
effort="7"
velocity="20" />
</joint>
<link
name="r_shld_y_link">
<inertial>
<origin
xyz="0.00068297 0.00449372 -2.22e-06"
rpy="0 0 0" />
<mass
value="0.53804971" />
<inertia
ixx="0.00030648"
ixy="-4.03e-06"
ixz="-2e-08"
iyy="0.00029153"
iyz="1e-07"
izz="0.00032312" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="../meshes/r_shld_y_link.STL" />
</geometry>
<material
name="">
<color
rgba="0.752941176470588 0.752941176470588 0.752941176470588 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="../meshes/r_shld_y_link.STL" />
</geometry>
</collision>
</link>
<joint
name="r_shld_y_joint"
type="fixed">
<origin
xyz="0 -0.21705 0.30075"
rpy="0 0 0" />
<parent
link="waist_y_link" />
<child
link="r_shld_y_link" />
<axis
xyz="0 1 0" />
<limit
lower="-2"
upper="2"
effort="27"
velocity="20" />
</joint>
<link
name="r_shld_x_link">
<inertial>
<origin
xyz="2.047e-05 -5.316e-05 -0.06162654"
rpy="0 0 0" />
<mass
value="0.48847489" />
<inertia
ixx="0.00027194"
ixy="4e-08"
ixz="5.4e-07"
iyy="0.00033709"
iyz="5.4e-07"
izz="0.0002884" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="../meshes/r_shld_x_link.STL" />
</geometry>
<material
name="">
<color
rgba="0.752941176470588 0.752941176470588 0.752941176470588 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="../meshes/r_shld_x_link.STL" />
</geometry>
</collision>
</link>
<joint
name="r_shld_x_joint"
type="fixed">
<origin
xyz="0 0 0"
rpy="0 0 0" />
<parent
link="r_shld_y_link" />
<child
link="r_shld_x_link" />
<axis
xyz="1 0 0" />
<limit
lower="-1.57"
upper="0.5"
effort="27"
velocity="20" />
</joint>
<link
name="r_shld_z_link">
<inertial>
<origin
xyz="-4.164e-05 -0.00022669 -0.2212782"
rpy="0 0 0" />
<mass
value="0.59309528" />
<inertia
ixx="0.00136573"
ixy="5.3e-07"
ixz="1e-08"
iyy="0.00133989"
iyz="-1.22e-06"
izz="0.00030342" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="../meshes/r_shld_z_link.STL" />
</geometry>
<material
name="">
<color
rgba="0.752941176470588 0.752941176470588 0.752941176470588 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="../meshes/r_shld_z_link.STL" />
</geometry>
</collision>
</link>
<joint
name="r_shld_z_joint"
type="fixed">
<origin
xyz="0 0 0"
rpy="0 0 0" />
<parent
link="r_shld_x_link" />
<child
link="r_shld_z_link" />
<axis
xyz="0 0 1" />
<limit
lower="-0.785"
upper="0.785"
effort="7"
velocity="20" />
</joint>
<link
name="r_elb_y_link">
<inertial>
<origin
xyz="-5.192e-05 -1.999e-05 -0.08561189"
rpy="0 0 0" />
<mass
value="0.50005725" />
<inertia
ixx="0.00043713"
ixy="-4e-08"
ixz="4.9e-07"
iyy="0.00036043"
iyz="-6.3e-07"
izz="0.00030266" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="../meshes/r_elb_y_link.STL" />
</geometry>
<material
name="">
<color
rgba="0.752941176470588 0.752941176470588 0.752941176470588 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="../meshes/r_elb_y_link.STL" />
</geometry>
</collision>
</link>
<joint
name="r_elb_y_joint"
type="fixed">
<origin
xyz="0 0 -0.24"
rpy="0 0 0" />
<parent
link="r_shld_z_link" />
<child
link="r_elb_y_link" />
<axis
xyz="0 1 0" />
<limit
lower="-2.355"
upper="0.5"
effort="27"
velocity="20" />
</joint>
<link
name="r_elb_z_link">
<inertial>
<origin
xyz="-1.97e-06 1.756e-05 -0.18282539"
rpy="0 0 0" />
<mass
value="0.13457416" />
<inertia
ixx="0.00040001"
ixy="-0.0"
ixz="2e-08"
iyy="0.00039996"
iyz="-1.5e-07"
izz="5.662e-05" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="../meshes/r_elb_z_link.STL" />
</geometry>
<material
name="">
<color
rgba="0.752941176470588 0.752941176470588 0.752941176470588 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="../meshes/r_elb_z_link.STL" />
</geometry>
</collision>
</link>
<joint
name="r_elb_z_joint"
type="fixed">
<origin
xyz="0 0 0"
rpy="0 0 0" />
<parent
link="r_elb_y_link" />
<child
link="r_elb_z_link" />
<axis
xyz="0 0 1" />
<limit
lower="-0.785"
upper="0.785"
effort="7"
velocity="20" />
</joint>
</robot>
\ No newline at end of file
Markdown 格式
0% 或
您添加了 0 人 到此讨论。请谨慎行事。
请先完成此评论的编辑！
请 注册 或 登录 后发表评论