Skip to content

Commit

Permalink
updating damping etc
Browse files Browse the repository at this point in the history
  • Loading branch information
@wuallenwu
wuallenwu committed Jul 22, 2024
1 parent 142662c commit 840ca4e
Show file tree
Hide file tree
Showing 5 changed files with 90 additions and 80 deletions.
67 changes: 27 additions & 40 deletions sim/humanoid_gym/envs/only_legs_config.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -37,6 +37,7 @@ class safety:
torque_limit = 0.85

class asset(LeggedRobotCfg.asset):

file = str(stompy_urdf_path(legs_only=True))

name = "stompy"
Expand All @@ -51,7 +52,7 @@ class asset(LeggedRobotCfg.asset):
self_collisions = 1 # 1 to disable, 0 to enable...bitwise filter

collapse_fixed_joints = True

default_dof_drive_mode = 3 # see GymDofDriveModeFlags (0 is none, 1 is pos tgt, 2 is vel tgt, 3 effort)
flip_visual_attachments = False
replace_cylinder_with_capsule = False
fix_base_link = False
Expand All @@ -62,8 +63,8 @@ class terrain(LeggedRobotCfg.terrain):
curriculum = False
# rough terrain only:
measure_heights = False
static_friction = 0.6
dynamic_friction = 0.6
static_friction = 2.0
dynamic_friction = 2.0
terrain_length = 8.0
terrain_width = 8.0
num_rows = 10 # number of terrain rows (levels)
Expand All @@ -75,7 +76,7 @@ class terrain(LeggedRobotCfg.terrain):

class noise:
add_noise = True
noise_level = 0.6 # scales other values
noise_level = 1.0 # scales other values

class noise_scales:
dof_pos = 0.05
Expand All @@ -95,32 +96,18 @@ class init_state(LeggedRobotCfg.init_state):
default_joint_angles[joint] = default_positions[joint]

class control(LeggedRobotCfg.control):
stiffness = {
"hip yaw": 90,
"hip roll": 90,
"hip pitch": 90,
"knee pitch": 90,
"ankle pitch": 24,
"ankle roll": 24,
}
damping = {
"hip yaw": 2.25,
"hip roll": 2.25,
"hip pitch": 2.25,
"knee pitch": 2.25,
"ankle pitch": 1.5,
"ankle roll": 1.5,
}
stiffness = Stompy.stiffness()
damping = Stompy.damping()
# pfb30 - todo
# effort = {

# }
# velocity = {

# }
action_scale = 0.25
action_scale = 1

decimation = 4 # 100hz
decimation = 10 # 100hz

class sim(LeggedRobotCfg.sim):
dt = 0.001 # 1000 Hz
Expand All @@ -130,7 +117,7 @@ class sim(LeggedRobotCfg.sim):
class physx(LeggedRobotCfg.sim.physx):
num_threads = 10
# pfb30
solver_type = 1 # 0: pgs, 1: tgs
solver_type = 0 # 0: pgs, 1: tgs
num_position_iterations = 4
num_velocity_iterations = 1
contact_offset = 0.01 # [m]
Expand Down Expand Up @@ -186,23 +173,23 @@ class rewards:
max_contact_force = 100 # forces above this value are penalized

class scales:
# reference motion tracking
joint_pos = 1.6
feet_clearance = 1.0
feet_contact_number = 1.2
# # gait
feet_air_time = 1.0
foot_slip = -0.05
feet_distance = 0.2
knee_distance = 0.2
# contact
feet_contact_forces = -0.01
# # vel tracking
tracking_lin_vel = 1.2
tracking_ang_vel = 1.1
vel_mismatch_exp = 0.5 # lin_z; ang x,y
low_speed = 0.2
track_vel_hard = 0.5
# # reference motion tracking
# joint_pos = 1.6
# feet_clearance = 1.0
# feet_contact_number = 1.2
# # # gait
# feet_air_time = 1.0
# foot_slip = -0.05
# feet_distance = 0.2
# knee_distance = 0.2
# # contact
# feet_contact_forces = -0.01
# # # vel tracking
# tracking_lin_vel = 1.2
# tracking_ang_vel = 1.1
# vel_mismatch_exp = 0.5 # lin_z; ang x,y
# low_speed = 0.2
# track_vel_hard = 0.5

# above this was removed for standing policy
# base pos
Expand Down
32 changes: 18 additions & 14 deletions sim/humanoid_gym/sims2sim.py
100644 → 100755
View file
Original file line number Diff line number Diff line change
@@ -1,5 +1,5 @@
# 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:
#
Expand Down Expand Up @@ -30,7 +30,7 @@
"""
Difference setup
python sim/humanoid_gym/play.py --task only_legs_ppo --sim_device cpu
python sim/humanoid_gym/sims2sim.py --load_model policy_1.pt
python sim/humanoid_gym/sims2sim.py --load_model policy_1.pt
"""
import math
import numpy as np
Expand All @@ -42,10 +42,11 @@

from humanoid.envs import XBotLCfg
import torch
from sim.stompy_legs.joints import Stompy

JOINT_NAMES = [
'left hip pitch', 'left hip yaw','left hip roll',
'left knee pitch', 'left ankle pitch', 'left ankle roll', 'right hip pitch', 'right hip yaw',
'left hip pitch', 'left hip yaw','left hip roll',
'left knee pitch', 'left ankle pitch', 'left ankle roll', 'right hip pitch', 'right hip yaw',
'right hip roll' , 'right knee pitch', 'right ankle pitch','right ankle roll'
]

Expand All @@ -64,22 +65,22 @@ class cmd:
def quaternion_to_euler_array(quat):
# Ensure quaternion is in the correct format [x, y, z, w]
x, y, z, w = quat

# Roll (x-axis rotation)
t0 = +2.0 * (w * x + y * z)
t1 = +1.0 - 2.0 * (x * x + y * y)
roll_x = np.arctan2(t0, t1)

# Pitch (y-axis rotation)
t2 = +2.0 * (w * y - z * x)
t2 = np.clip(t2, -1.0, 1.0)
pitch_y = np.arcsin(t2)

# Yaw (z-axis rotation)
t3 = +2.0 * (w * z + x * y)
t4 = +1.0 - 2.0 * (y * y + z * z)
yaw_z = np.arctan2(t3, t4)

# Returns roll, pitch, yaw in a NumPy array in radians
return np.array([roll_x, pitch_y, yaw_z])

Expand Down Expand Up @@ -170,7 +171,7 @@ def run_mujoco(policy, cfg):
for i in range(cfg.env.frame_stack):
policy_input[0, i * cfg.env.num_single_obs : (i + 1) * cfg.env.num_single_obs] = hist_obs[i][0, :]
action[:] = policy(torch.tensor(policy_input))[0].detach().numpy()

# pfb30 - todo test
# action[:] = actions_sim[kk]
kk+=1
Expand All @@ -185,7 +186,7 @@ def run_mujoco(policy, cfg):
target_dq, dq, cfg.robot_config.kds, default) # Calc torques

tau = np.clip(tau, -cfg.robot_config.tau_limit, cfg.robot_config.tau_limit) # Clamp torques

data.ctrl = tau

mujoco.mj_step(model, data)
Expand All @@ -210,12 +211,15 @@ class sim_config:
mujoco_model_path = f'sim/stompy_legs/robot_fixed.xml'
sim_duration = 60.0
dt = 0.001
decimation = 4
decimation = 10
# pfb30 - todo this should be update more often
class robot_config:
kps = np.array([90, 90, 90, 90, 24, 24, 90, 90, 90, 90, 24, 24], dtype=np.double)
kds = np.array([2.25, 2.25, 2.25, 2.25, 1.5, 1.5, 2.25, 2.25, 2.25, 2.25, 1.5, 1.5], dtype=np.double)
tau_limit = np.array([90, 90, 90, 90, 24, 24, 90, 90, 90, 90, 24, 24], dtype=np.double) * 0.85
kp_factor = 10
kd_factor = 1
tau_factor = 0.85
tau_limit = np.array(list(Stompy.stiffness().values()) + list(Stompy.stiffness().values())) * tau_factor
kps = tau_limit * kp_factor
kds = np.array(list(Stompy.damping().values()) + list(Stompy.damping().values())) * kd_factor

policy = torch.jit.load(args.load_model)
run_mujoco(policy, Sim2simCfg())
21 changes: 21 additions & 0 deletions sim/stompy_legs/joints.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -151,7 +151,28 @@ def default_limits(cls) -> Dict[str, Dict[str, float]]:
"lower": 1,
"upper": 2.3,
},
}

@classmethod
def stiffness(cls) -> Dict[str, float]:
return {
"hip pitch": 90,
"hip yaw": 90,
"hip roll": 90,
"knee pitch": 90,
"ankle pitch": 24,
"ankle roll": 24,
}

@classmethod
def damping(cls) -> Dict[str, float]:
return {
"hip pitch": 9,
"hip yaw": 9,
"hip roll": 9,
"knee pitch": 9,
"ankle pitch": 2.4,
"ankle roll": 2.4,
}


Expand Down
Empty file modified sim/stompy_legs/robot_fixed.urdf
100644 → 100755
View file
Empty file.
50 changes: 24 additions & 26 deletions sim/stompy_legs/robot_fixed.xml
100644 → 100755
View file
Original file line number Diff line number Diff line change
@@ -1,20 +1,17 @@
<?xml version="1.0" ?>
<mujoco model="lower_limbs">
<compiler angle="radian" meshdir="meshes" eulerseq="zyx"/>
<option iterations="90" timestep="0.001" solver="PGS" gravity="0 0 -9.81">
<flag sensornoise="enable" frictionloss="enable"/>
</option>

<compiler angle="radian" meshdir="meshes" autolimits="true" eulerseq="zyx"/>
<default>
<joint limited="true" damping="0.01" frictionloss="0.01" armature="0.01"/>
<joint limited="true" damping="0.01" armature="0.01" frictionloss="0.01"/>
<motor ctrllimited="true"/>
<geom condim="4" contype="1" conaffinity="15" friction="0.9 0.2 0.2" solref="0.001 2"/>
<equality solref="0.001 2"/>
<default class="visualgeom">
<geom material="visualgeom" condim="1" contype="0" conaffinity="0"/>
</default>
</default>

<option iterations="10" timestep="0.001" solver="PGS" gravity="0 0 -9.81">
</option>
<asset>
<mesh name="leg_assembly_left_1_leg_part_1_2_simple" file="leg_assembly_left_1_leg_part_1_2_simple.stl"/>
<mesh name="leg_assembly_right_1_leg_part_1_2_simple" file="leg_assembly_right_1_leg_part_1_2_simple.stl"/>
Expand Down Expand Up @@ -70,13 +67,14 @@
<geom name="ground" type="plane" pos="0.001 0 0" quat="1 0 0 0" material="matplane" size="0 0 1" condim="1" conaffinity='15'/>
<light directional="true" diffuse="0.6 0.6 0.6" specular="0.2 0.2 0.2" pos="0 0 4" dir="0 0 -1"/>
<light directional="true" diffuse="0.4 0.4 0.4" specular="0.1 0.1 0.1" pos="0 0 5.0" dir="0 0 -1" castshadow="false"/>
<body name="root" pos="0 0 .72" quat="1 0 0 0">
<joint name="floating_base_joint" type="free" />

<body name="root" pos="0 0 0.71" quat="1 0 0 0">
<joint name="root_x" type="slide" axis="1 0 0" limited="false"/>
<joint name="root_y" type="slide" axis="0 1 0" limited="false"/>
<joint name="root_z" type="slide" axis="0 0 1" limited="false"/>
<joint name="root_ball" type="ball" limited="false"/>
<camera name="front" pos="0 -3 1" xyaxes="1 0 0 0 1 2" mode="trackcom"/>
<camera name="side" pos="-2.893 -1.330 0.757" xyaxes="0.405 -0.914 0.000 0.419 0.186 0.889" mode="trackcom"/>
<site name="imu" size="0.01" pos="0 0 0"/>

<geom type="mesh" rgba="0.615686 0.811765 0.929412 1" mesh="leg_assembly_left_1_leg_part_1_2_simple" class="visualgeom"/>
<geom type="mesh" rgba="0.615686 0.811765 0.929412 1" mesh="leg_assembly_left_1_leg_part_1_2_simple" contype="0" conaffinity="0" group="1" density="0"/>
<geom pos="-0.0209987 -0.0690001 0.0170241" quat="0.5 -0.5 -0.5 0.5" type="mesh" rgba="0.615686 0.811765 0.929412 1" mesh="leg_assembly_right_1_leg_part_1_2_simple" class="visualgeom"/>
Expand Down Expand Up @@ -139,7 +137,7 @@
<geom type="mesh" rgba="0.615686 0.811765 0.929412 1" mesh="left_foot_1_ankle_half_1_left_2_simple" pos="0 0.0145 0.04735" quat="0.707107 0 0.707107 0" contype="0" conaffinity="0" group="1" density="0"/>
<body name="link_left_foot_1_rmd_x4_24_mock_1_inner_rmd_x4_24_1" pos="0.0135 -0.07155 0.01055" quat="-0.5 0.5 0.5 -0.5">
<inertial pos="0.00584595 -0.0275033 -0.0114478" quat="0.994413 -0.0147465 -0.00155004 0.104511" mass="0.422846" diaginertia="0.00136272 0.00135059 0.000429099"/>
<joint name="left ankle roll" pos="0 0 0" axis="0 0 -1" range="1 2.3" />
<joint name="left ankle roll" pos="0 0 0" axis="0 0 -1" range="1 2.23" />
<geom type="mesh" rgba="0.768627 0.886275 0.952941 1" mesh="left_foot_1_rmd_x4_24_mock_1_inner_rmd_x4_24_1_simple" class="visualgeom"/>
<geom type="mesh" rgba="0.768627 0.886275 0.952941 1" mesh="left_foot_1_rmd_x4_24_mock_1_inner_rmd_x4_24_1_simple" contype="0" conaffinity="0" group="1" density="0"/>
<geom pos="0.00275746 -0.0127042 -0.025" quat="0.994522 0 0 0.104528" type="mesh" rgba="0.615686 0.811765 0.929412 1" mesh="left_foot_1_foot_1_simple" class="visualgeom"/>
Expand Down Expand Up @@ -222,18 +220,18 @@
</body>
</worldbody>
<actuator>
<motor name="left hip pitch" joint="left hip pitch" ctrllimited="true" ctrlrange="-90 90" gear="1"/>
<motor name="left hip roll" joint="left hip roll" ctrllimited="true" ctrlrange="-90 90" gear="1"/>
<motor name="left hip yaw" joint="left hip yaw" ctrllimited="true" ctrlrange="-90 90" gear="1"/>
<motor name="left knee pitch" joint="left knee pitch" ctrllimited="true" ctrlrange="-90 90" gear="1"/>
<motor name="left ankle pitch" joint="left ankle pitch" ctrllimited="true" ctrlrange="-24 24" gear="1"/>
<motor name="left ankle roll" joint="left ankle roll" ctrllimited="true" ctrlrange="-24 24" gear="1"/>
<motor name="right hip pitch" joint="right hip pitch" ctrllimited="true" ctrlrange="-90 90" gear="1"/>
<motor name="right hip roll" joint="right hip roll" ctrllimited="true" ctrlrange="-90 90" gear="1"/>
<motor name="right hip yaw" joint="right hip yaw" ctrllimited="true" ctrlrange="-90 90" gear="1"/>
<motor name="right knee pitch" joint="right knee pitch" ctrllimited="true" ctrlrange="-90 90" gear="1"/>
<motor name="right ankle pitch" joint="right ankle pitch" ctrllimited="true" ctrlrange="-24 24" gear="1"/>
<motor name="right ankle roll" joint="right ankle roll" ctrllimited="true" ctrlrange="-24 24" gear="1"/>
<motor name="left hip pitch" joint="left hip pitch" ctrllimited="true" ctrlrange="-200 200" gear="1"/>
<motor name="left hip roll" joint="left hip roll" ctrllimited="true" ctrlrange="-200 200" gear="1"/>
<motor name="left hip yaw" joint="left hip yaw" ctrllimited="true" ctrlrange="-200 200" gear="1"/>
<motor name="left knee pitch" joint="left knee pitch" ctrllimited="true" ctrlrange="-200 200" gear="1"/>
<motor name="left ankle pitch" joint="left ankle pitch" ctrllimited="true" ctrlrange="-200 200" gear="1"/>
<motor name="left ankle roll" joint="left ankle roll" ctrllimited="true" ctrlrange="-200 200" gear="1"/>
<motor name="right hip pitch" joint="right hip pitch" ctrllimited="true" ctrlrange="-200 200" gear="1"/>
<motor name="right hip roll" joint="right hip roll" ctrllimited="true" ctrlrange="-200 200" gear="1"/>
<motor name="right hip yaw" joint="right hip yaw" ctrllimited="true" ctrlrange="-200 200" gear="1"/>
<motor name="right knee pitch" joint="right knee pitch" ctrllimited="true" ctrlrange="-200 200" gear="1"/>
<motor name="right ankle pitch" joint="right ankle pitch" ctrllimited="true" ctrlrange="-200 200" gear="1"/>
<motor name="right ankle roll" joint="right ankle roll" ctrllimited="true" ctrlrange="-200 200" gear="1"/>
</actuator>
<sensor>
<actuatorpos name="left hip pitch_p" actuator="left hip pitch" user="13"/>
Expand Down Expand Up @@ -276,6 +274,6 @@
<gyro name="angular-velocity" site="imu" noise="0.005" cutoff="34.9"/>
</sensor>
<keyframe>
<key name="default" qpos='0 0 0.72 1 0 0 0 1.60595 1.0037 0 2.05 0.236 1.72 -0.01 -2.152 -1.60465 2.1605 0.413 1.75'/>
<key name="default" qpos='0 0 0 1 0 0 0 1.60595 1.0037 0 2.05 0.3292 1.715 -0.01 -2.152 -1.60465 2.1605 0.5008 1.7305'/>
</keyframe>
</mujoco>
</mujoco>

0 comments on commit 840ca4e

Please sign in to comment.