isaacgym environment added
This commit is contained in:
Rooholla-KhorramBakht
parent
b535e6595a
commit
c14cffd08c
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
|
|
@ -0,0 +1,682 @@
|
||||||
|
<?xml version="1.0" ?>
|
||||||
|
<!-- =================================================================================== -->
|
||||||
|
<!-- | This document was autogenerated by xacro from go2_description.urdf | -->
|
||||||
|
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
|
||||||
|
<!-- =================================================================================== -->
|
||||||
|
<!-- This URDF was automatically created by SolidWorks to URDF Exporter! Originally created by
|
||||||
|
Stephen Brawner (brawner@gmail.com)
|
||||||
|
Commit Version: 1.6.0-4-g7f85cfe Build Version: 1.6.7995.38578
|
||||||
|
For more information, please see http://wiki.ros.org/sw_urdf_exporter -->
|
||||||
|
<robot name="go2_description">
|
||||||
|
<link name="base">
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="0.021112 0 -0.005366"/>
|
||||||
|
<mass value="6.921"/>
|
||||||
|
<inertia ixx="0.02448" ixy="0.00012166" ixz="0.0014849" iyy="0.098077" iyz="-3.12E-05" izz="0.107"/>
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="../dae/base.dae"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="">
|
||||||
|
<color rgba="1 1 1 1"/>
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<box size="0.3762 0.0935 0.154"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
<link name="Head_upper">
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<mass value="0.001"/>
|
||||||
|
<inertia ixx="9.6e-06" ixy="0" ixz="0" iyy="9.6e-06" iyz="0" izz="9.6e-06"/>
|
||||||
|
</inertial>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length="0.09" radius="0.05"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
<joint dont_collapse="true" name="Head_upper_joint" type="fixed">
|
||||||
|
<origin rpy="0 0 0" xyz="0.285 0 0.01"/>
|
||||||
|
<parent link="base"/>
|
||||||
|
<child link="Head_upper"/>
|
||||||
|
<axis xyz="0 0 0"/>
|
||||||
|
</joint>
|
||||||
|
<link name="Head_lower">
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<mass value="0.001"/>
|
||||||
|
<inertia ixx="9.6e-06" ixy="0" ixz="0" iyy="9.6e-06" iyz="0" izz="9.6e-06"/>
|
||||||
|
</inertial>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<sphere radius="0.047"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
<joint dont_collapse="true" name="Head_lower_joint" type="fixed">
|
||||||
|
<origin rpy="0 0 0" xyz="0.008 0 -0.07"/>
|
||||||
|
<parent link="Head_upper"/>
|
||||||
|
<child link="Head_lower"/>
|
||||||
|
<axis xyz="0 0 0"/>
|
||||||
|
</joint>
|
||||||
|
<link name="FL_hip">
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="-0.0054 0.00194 -0.000105"/>
|
||||||
|
<mass value="0.678"/>
|
||||||
|
<inertia ixx="0.00048" ixy="-3.01E-06" ixz="1.11E-06" iyy="0.000884" iyz="-1.42E-06" izz="0.000596"/>
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="../dae/hip.dae"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="">
|
||||||
|
<color rgba="1 1 1 1"/>
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<!-- <collision>
|
||||||
|
<origin rpy="1.5707963267948966 0 0" xyz="0 0.08 0"/>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length="0.04" radius="0.046"/>
|
||||||
|
</geometry>
|
||||||
|
</collision> -->
|
||||||
|
</link>
|
||||||
|
<joint name="FL_hip_joint" type="revolute">
|
||||||
|
<origin rpy="0 0 0" xyz="0.1934 0.0465 0"/>
|
||||||
|
<parent link="base"/>
|
||||||
|
<child link="FL_hip"/>
|
||||||
|
<axis xyz="1 0 0"/>
|
||||||
|
<dynamics damping="0" friction="0"/>
|
||||||
|
<limit effort="23.7" lower="-1.0472" upper="1.0472" velocity="30.1"/>
|
||||||
|
</joint>
|
||||||
|
<link name="FL_thigh">
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="-0.00374 -0.0223 -0.0327"/>
|
||||||
|
<mass value="1.152"/>
|
||||||
|
<inertia ixx="0.00584" ixy="8.72E-05" ixz="-0.000289" iyy="0.0058" iyz="0.000808" izz="0.00103"/>
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="../dae/thigh.dae"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="">
|
||||||
|
<color rgba="1 1 1 1"/>
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 1.5707963267948966 0" xyz="0 0 -0.1065"/>
|
||||||
|
<geometry>
|
||||||
|
<box size="0.213 0.0245 0.034"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
<joint name="FL_thigh_joint" type="revolute">
|
||||||
|
<origin rpy="0 0 0" xyz="0 0.0955 0"/>
|
||||||
|
<parent link="FL_hip"/>
|
||||||
|
<child link="FL_thigh"/>
|
||||||
|
<axis xyz="0 1 0"/>
|
||||||
|
<dynamics damping="0" friction="0"/>
|
||||||
|
<limit effort="23.7" lower="-1.5708" upper="3.4907" velocity="30.1"/>
|
||||||
|
</joint>
|
||||||
|
<link name="FL_calf">
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="0.00548 -0.000975 -0.115"/>
|
||||||
|
<mass value="0.154"/>
|
||||||
|
<inertia ixx="0.00108" ixy="3.4E-07" ixz="1.72E-05" iyy="0.0011" iyz="8.28E-06" izz="3.29E-05"/>
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="../dae/calf.dae"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="">
|
||||||
|
<color rgba="1 1 1 1"/>
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 -0.21 0" xyz="0.008 0 -0.06"/>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length="0.12" radius="0.012"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
<joint name="FL_calf_joint" type="revolute">
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 -0.213"/>
|
||||||
|
<parent link="FL_thigh"/>
|
||||||
|
<child link="FL_calf"/>
|
||||||
|
<axis xyz="0 1 0"/>
|
||||||
|
<dynamics damping="0" friction="0"/>
|
||||||
|
<limit effort="45.43" lower="-2.7227" upper="-0.83776" velocity="15.70"/>
|
||||||
|
</joint>
|
||||||
|
<link name="FL_calflower">
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length="0.065" radius="0.011"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
<joint name="FL_calflower_joint" type="fixed">
|
||||||
|
<origin rpy="0 0.05 0" xyz="0.020 0 -0.148"/>
|
||||||
|
<parent link="FL_calf"/>
|
||||||
|
<child link="FL_calflower"/>
|
||||||
|
<axis xyz="0 0 0"/>
|
||||||
|
</joint>
|
||||||
|
<link name="FL_calflower1">
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length="0.03" radius="0.0155"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
<joint name="FL_calflower1_joint" type="fixed">
|
||||||
|
<origin rpy="0 0.48 0" xyz="-0.01 0 -0.04"/>
|
||||||
|
<parent link="FL_calflower"/>
|
||||||
|
<child link="FL_calflower1"/>
|
||||||
|
<axis xyz="0 0 0"/>
|
||||||
|
</joint>
|
||||||
|
<link name="FL_foot">
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<mass value="0.04"/>
|
||||||
|
<inertia ixx="9.6e-06" ixy="0" ixz="0" iyy="9.6e-06" iyz="0" izz="9.6e-06"/>
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="../dae/foot.dae"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="">
|
||||||
|
<color rgba="1 1 1 1"/>
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 0 0" xyz="-0.002 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<sphere radius="0.022"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
<joint dont_collapse="true" name="FL_foot_joint" type="fixed">
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 -0.213"/>
|
||||||
|
<parent link="FL_calf"/>
|
||||||
|
<child link="FL_foot"/>
|
||||||
|
<axis xyz="0 0 0"/>
|
||||||
|
</joint>
|
||||||
|
<link name="FR_hip">
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="-0.0054 -0.00194 -0.000105"/>
|
||||||
|
<mass value="0.678"/>
|
||||||
|
<inertia ixx="0.00048" ixy="3.01E-06" ixz="1.11E-06" iyy="0.000884" iyz="1.42E-06" izz="0.000596"/>
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin rpy="3.1415 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="../dae/hip.dae"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="">
|
||||||
|
<color rgba="1 1 1 1"/>
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<!-- <collision>
|
||||||
|
<origin rpy="1.5707963267948966 0 0" xyz="0 -0.08 0"/>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length="0.04" radius="0.046"/>
|
||||||
|
</geometry>
|
||||||
|
</collision> -->
|
||||||
|
</link>
|
||||||
|
<joint name="FR_hip_joint" type="revolute">
|
||||||
|
<origin rpy="0 0 0" xyz="0.1934 -0.0465 0"/>
|
||||||
|
<parent link="base"/>
|
||||||
|
<child link="FR_hip"/>
|
||||||
|
<axis xyz="1 0 0"/>
|
||||||
|
<dynamics damping="0" friction="0"/>
|
||||||
|
<limit effort="23.7" lower="-1.0472" upper="1.0472" velocity="30.1"/>
|
||||||
|
</joint>
|
||||||
|
<link name="FR_thigh">
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="-0.00374 0.0223 -0.0327"/>
|
||||||
|
<mass value="1.152"/>
|
||||||
|
<inertia ixx="0.00584" ixy="-8.72E-05" ixz="-0.000289" iyy="0.0058" iyz="-0.000808" izz="0.00103"/>
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="../dae/thigh_mirror.dae"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="">
|
||||||
|
<color rgba="1 1 1 1"/>
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 1.5707963267948966 0" xyz="0 0 -0.1065"/>
|
||||||
|
<geometry>
|
||||||
|
<box size="0.213 0.0245 0.034"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
<joint name="FR_thigh_joint" type="revolute">
|
||||||
|
<origin rpy="0 0 0" xyz="0 -0.0955 0"/>
|
||||||
|
<parent link="FR_hip"/>
|
||||||
|
<child link="FR_thigh"/>
|
||||||
|
<axis xyz="0 1 0"/>
|
||||||
|
<dynamics damping="0" friction="0"/>
|
||||||
|
<limit effort="23.7" lower="-1.5708" upper="3.4907" velocity="30.1"/>
|
||||||
|
</joint>
|
||||||
|
<link name="FR_calf">
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="0.00548 0.000975 -0.115"/>
|
||||||
|
<mass value="0.154"/>
|
||||||
|
<inertia ixx="0.00108" ixy="-3.4E-07" ixz="1.72E-05" iyy="0.0011" iyz="-8.28E-06" izz="3.29E-05"/>
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="../dae/calf_mirror.dae"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="">
|
||||||
|
<color rgba="1 1 1 1"/>
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 -0.2 0" xyz="0.01 0 -0.06"/>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length="0.12" radius="0.013"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
<joint name="FR_calf_joint" type="revolute">
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 -0.213"/>
|
||||||
|
<parent link="FR_thigh"/>
|
||||||
|
<child link="FR_calf"/>
|
||||||
|
<axis xyz="0 1 0"/>
|
||||||
|
<dynamics damping="0" friction="0"/>
|
||||||
|
<limit effort="45.43" lower="-2.7227" upper="-0.83776" velocity="15.70"/>
|
||||||
|
</joint>
|
||||||
|
<link name="FR_calflower">
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length="0.065" radius="0.011"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
<joint name="FR_calflower_joint" type="fixed">
|
||||||
|
<origin rpy="0 0.05 0" xyz="0.020 0 -0.148"/>
|
||||||
|
<parent link="FR_calf"/>
|
||||||
|
<child link="FR_calflower"/>
|
||||||
|
<axis xyz="0 0 0"/>
|
||||||
|
</joint>
|
||||||
|
<link name="FR_calflower1">
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length="0.03" radius="0.0155"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
<joint name="FR_calflower1_joint" type="fixed">
|
||||||
|
<origin rpy="0 0.48 0" xyz="-0.01 0 -0.04"/>
|
||||||
|
<parent link="FR_calflower"/>
|
||||||
|
<child link="FR_calflower1"/>
|
||||||
|
<axis xyz="0 0 0"/>
|
||||||
|
</joint>
|
||||||
|
<link name="FR_foot">
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<mass value="0.04"/>
|
||||||
|
<inertia ixx="9.6e-06" ixy="0" ixz="0" iyy="9.6e-06" iyz="0" izz="9.6e-06"/>
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="../dae/foot.dae"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="">
|
||||||
|
<color rgba="1 1 1 1"/>
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 0 0" xyz="-0.002 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<sphere radius="0.022"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
<joint dont_collapse="true" name="FR_foot_joint" type="fixed">
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 -0.213"/>
|
||||||
|
<parent link="FR_calf"/>
|
||||||
|
<child link="FR_foot"/>
|
||||||
|
<axis xyz="0 0 0"/>
|
||||||
|
</joint>
|
||||||
|
<link name="RL_hip">
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="0.0054 0.00194 -0.000105"/>
|
||||||
|
<mass value="0.678"/>
|
||||||
|
<inertia ixx="0.00048" ixy="3.01E-06" ixz="-1.11E-06" iyy="0.000884" iyz="-1.42E-06" izz="0.000596"/>
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 3.1415 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="../dae/hip.dae"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="">
|
||||||
|
<color rgba="1 1 1 1"/>
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<!-- <collision>
|
||||||
|
<origin rpy="1.5707963267948966 0 0" xyz="0 0.08 0"/>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length="0.04" radius="0.046"/>
|
||||||
|
</geometry>
|
||||||
|
</collision> -->
|
||||||
|
</link>
|
||||||
|
<joint name="RL_hip_joint" type="revolute">
|
||||||
|
<origin rpy="0 0 0" xyz="-0.1934 0.0465 0"/>
|
||||||
|
<parent link="base"/>
|
||||||
|
<child link="RL_hip"/>
|
||||||
|
<axis xyz="1 0 0"/>
|
||||||
|
<dynamics damping="0" friction="0"/>
|
||||||
|
<limit effort="23.7" lower="-1.0472" upper="1.0472" velocity="30.1"/>
|
||||||
|
</joint>
|
||||||
|
<link name="RL_thigh">
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="-0.00374 -0.0223 -0.0327"/>
|
||||||
|
<mass value="1.152"/>
|
||||||
|
<inertia ixx="0.00584" ixy="8.72E-05" ixz="-0.000289" iyy="0.0058" iyz="0.000808" izz="0.00103"/>
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="../dae/thigh.dae"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="">
|
||||||
|
<color rgba="1 1 1 1"/>
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 1.5707963267948966 0" xyz="0 0 -0.1065"/>
|
||||||
|
<geometry>
|
||||||
|
<box size="0.213 0.0245 0.034"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
<joint name="RL_thigh_joint" type="revolute">
|
||||||
|
<origin rpy="0 0 0" xyz="0 0.0955 0"/>
|
||||||
|
<parent link="RL_hip"/>
|
||||||
|
<child link="RL_thigh"/>
|
||||||
|
<axis xyz="0 1 0"/>
|
||||||
|
<dynamics damping="0" friction="0"/>
|
||||||
|
<limit effort="23.7" lower="-0.5236" upper="4.5379" velocity="30.1"/>
|
||||||
|
</joint>
|
||||||
|
<link name="RL_calf">
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="0.00548 -0.000975 -0.115"/>
|
||||||
|
<mass value="0.154"/>
|
||||||
|
<inertia ixx="0.00108" ixy="3.4E-07" ixz="1.72E-05" iyy="0.0011" iyz="8.28E-06" izz="3.29E-05"/>
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="../dae/calf.dae"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="">
|
||||||
|
<color rgba="1 1 1 1"/>
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 -0.2 0" xyz="0.01 0 -0.06"/>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length="0.12" radius="0.013"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
<joint name="RL_calf_joint" type="revolute">
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 -0.213"/>
|
||||||
|
<parent link="RL_thigh"/>
|
||||||
|
<child link="RL_calf"/>
|
||||||
|
<axis xyz="0 1 0"/>
|
||||||
|
<dynamics damping="0" friction="0"/>
|
||||||
|
<limit effort="45.43" lower="-2.7227" upper="-0.83776" velocity="15.70"/>
|
||||||
|
</joint>
|
||||||
|
<link name="RL_calflower">
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length="0.065" radius="0.011"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
<joint name="RL_calflower_joint" type="fixed">
|
||||||
|
<origin rpy="0 0.05 0" xyz="0.020 0 -0.148"/>
|
||||||
|
<parent link="RL_calf"/>
|
||||||
|
<child link="RL_calflower"/>
|
||||||
|
<axis xyz="0 0 0"/>
|
||||||
|
</joint>
|
||||||
|
<link name="RL_calflower1">
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length="0.03" radius="0.0155"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
<joint name="RL_calflower1_joint" type="fixed">
|
||||||
|
<origin rpy="0 0.48 0" xyz="-0.01 0 -0.04"/>
|
||||||
|
<parent link="RL_calflower"/>
|
||||||
|
<child link="RL_calflower1"/>
|
||||||
|
<axis xyz="0 0 0"/>
|
||||||
|
</joint>
|
||||||
|
<link name="RL_foot">
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<mass value="0.04"/>
|
||||||
|
<inertia ixx="9.6e-06" ixy="0" ixz="0" iyy="9.6e-06" iyz="0" izz="9.6e-06"/>
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="../dae/foot.dae"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="">
|
||||||
|
<color rgba="1 1 1 1"/>
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 0 0" xyz="-0.002 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<sphere radius="0.022"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
<joint dont_collapse="true" name="RL_foot_joint" type="fixed">
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 -0.213"/>
|
||||||
|
<parent link="RL_calf"/>
|
||||||
|
<child link="RL_foot"/>
|
||||||
|
<axis xyz="0 0 0"/>
|
||||||
|
</joint>
|
||||||
|
<link name="RR_hip">
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="0.0054 -0.00194 -0.000105"/>
|
||||||
|
<mass value="0.678"/>
|
||||||
|
<inertia ixx="0.00048" ixy="-3.01E-06" ixz="-1.11E-06" iyy="0.000884" iyz="1.42E-06" izz="0.000596"/>
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin rpy="3.1415 3.1415 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="../dae/hip.dae"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="">
|
||||||
|
<color rgba="1 1 1 1"/>
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<!-- <collision>
|
||||||
|
<origin rpy="1.5707963267948966 0 0" xyz="0 -0.08 0"/>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length="0.04" radius="0.046"/>
|
||||||
|
</geometry>
|
||||||
|
</collision> -->
|
||||||
|
</link>
|
||||||
|
<joint name="RR_hip_joint" type="revolute">
|
||||||
|
<origin rpy="0 0 0" xyz="-0.1934 -0.0465 0"/>
|
||||||
|
<parent link="base"/>
|
||||||
|
<child link="RR_hip"/>
|
||||||
|
<axis xyz="1 0 0"/>
|
||||||
|
<dynamics damping="0" friction="0"/>
|
||||||
|
<limit effort="23.7" lower="-1.0472" upper="1.0472" velocity="30.1"/>
|
||||||
|
</joint>
|
||||||
|
<link name="RR_thigh">
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="-0.00374 0.0223 -0.0327"/>
|
||||||
|
<mass value="1.152"/>
|
||||||
|
<inertia ixx="0.00584" ixy="-8.72E-05" ixz="-0.000289" iyy="0.0058" iyz="-0.000808" izz="0.00103"/>
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="../dae/thigh_mirror.dae"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="">
|
||||||
|
<color rgba="1 1 1 1"/>
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 1.5707963267948966 0" xyz="0 0 -0.1065"/>
|
||||||
|
<geometry>
|
||||||
|
<box size="0.213 0.0245 0.034"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
<joint name="RR_thigh_joint" type="revolute">
|
||||||
|
<origin rpy="0 0 0" xyz="0 -0.0955 0"/>
|
||||||
|
<parent link="RR_hip"/>
|
||||||
|
<child link="RR_thigh"/>
|
||||||
|
<axis xyz="0 1 0"/>
|
||||||
|
<dynamics damping="0" friction="0"/>
|
||||||
|
<limit effort="23.7" lower="-0.5236" upper="4.5379" velocity="30.1"/>
|
||||||
|
</joint>
|
||||||
|
<link name="RR_calf">
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="0.00548 0.000975 -0.115"/>
|
||||||
|
<mass value="0.154"/>
|
||||||
|
<inertia ixx="0.00108" ixy="-3.4E-07" ixz="1.72E-05" iyy="0.0011" iyz="-8.28E-06" izz="3.29E-05"/>
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="../dae/calf_mirror.dae"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="">
|
||||||
|
<color rgba="1 1 1 1"/>
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 -0.2 0" xyz="0.01 0 -0.06"/>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length="0.12" radius="0.013"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
<joint name="RR_calf_joint" type="revolute">
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 -0.213"/>
|
||||||
|
<parent link="RR_thigh"/>
|
||||||
|
<child link="RR_calf"/>
|
||||||
|
<axis xyz="0 1 0"/>
|
||||||
|
<dynamics damping="0" friction="0"/>
|
||||||
|
<limit effort="45.43" lower="-2.7227" upper="-0.83776" velocity="15.70"/>
|
||||||
|
</joint>
|
||||||
|
<link name="RR_calflower">
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length="0.065" radius="0.011"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
<joint name="RR_calflower_joint" type="fixed">
|
||||||
|
<origin rpy="0 0.05 0" xyz="0.020 0 -0.148"/>
|
||||||
|
<parent link="RR_calf"/>
|
||||||
|
<child link="RR_calflower"/>
|
||||||
|
<axis xyz="0 0 0"/>
|
||||||
|
</joint>
|
||||||
|
<link name="RR_calflower1">
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length="0.03" radius="0.0155"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
<joint name="RR_calflower1_joint" type="fixed">
|
||||||
|
<origin rpy="0 0.48 0" xyz="-0.01 0 -0.04"/>
|
||||||
|
<parent link="RR_calflower"/>
|
||||||
|
<child link="RR_calflower1"/>
|
||||||
|
<axis xyz="0 0 0"/>
|
||||||
|
</joint>
|
||||||
|
<link name="RR_foot">
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<mass value="0.04"/>
|
||||||
|
<inertia ixx="9.6e-06" ixy="0" ixz="0" iyy="9.6e-06" iyz="0" izz="9.6e-06"/>
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="../dae/foot.dae"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="">
|
||||||
|
<color rgba="1 1 1 1"/>
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 0 0" xyz="-0.002 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<sphere radius="0.022"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
<joint dont_collapse="true" name="RR_foot_joint" type="fixed">
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 -0.213"/>
|
||||||
|
<parent link="RR_calf"/>
|
||||||
|
<child link="RR_foot"/>
|
||||||
|
<axis xyz="0 0 0"/>
|
||||||
|
</joint>
|
||||||
|
<link name="imu">
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<mass value="0"/>
|
||||||
|
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="imu_joint" type="fixed">
|
||||||
|
<origin rpy="0 0 0" xyz="-0.02557 0 0.04232"/>
|
||||||
|
<parent link="base"/>
|
||||||
|
<child link="imu"/>
|
||||||
|
<axis xyz="0 0 0"/>
|
||||||
|
</joint>
|
||||||
|
<link name="radar">
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<mass value="0"/>
|
||||||
|
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="radar_joint" type="fixed">
|
||||||
|
<origin rpy="0 2.8782 0" xyz="0.28945 0 -0.046825"/>
|
||||||
|
<parent link="base"/>
|
||||||
|
<child link="radar"/>
|
||||||
|
<axis xyz="0 0 0"/>
|
||||||
|
</joint>
|
||||||
|
</robot>
|
||||||
|
|
||||||
Binary file not shown.
|
|
@ -0,0 +1,7 @@
|
||||||
|
|
||||||
|
|
||||||
|
import os
|
||||||
|
|
||||||
|
MINI_GYM_ROOT_DIR = os.path.join(os.path.dirname(os.path.dirname(os.path.realpath(__file__))), '..')
|
||||||
|
print(MINI_GYM_ROOT_DIR)
|
||||||
|
MINI_GYM_ENVS_DIR = os.path.join(MINI_GYM_ROOT_DIR, 'go_gym', 'envs')
|
||||||
|
|
@ -0,0 +1,137 @@
|
||||||
|
# License: see [LICENSE, LICENSES/legged_gym/LICENSE]
|
||||||
|
|
||||||
|
import sys
|
||||||
|
|
||||||
|
import gym
|
||||||
|
import torch
|
||||||
|
from isaacgym import gymapi, gymutil
|
||||||
|
|
||||||
|
from gym import spaces
|
||||||
|
import numpy as np
|
||||||
|
|
||||||
|
|
||||||
|
# Base class for RL tasks
|
||||||
|
class BaseTask(gym.Env):
|
||||||
|
|
||||||
|
def __init__(self, cfg, sim_params, physics_engine, sim_device, headless, eval_cfg=None):
|
||||||
|
self.gym = gymapi.acquire_gym()
|
||||||
|
|
||||||
|
if isinstance(physics_engine, str) and physics_engine == "SIM_PHYSX":
|
||||||
|
physics_engine = gymapi.SIM_PHYSX
|
||||||
|
|
||||||
|
self.sim_params = sim_params
|
||||||
|
self.physics_engine = physics_engine
|
||||||
|
self.sim_device = sim_device
|
||||||
|
sim_device_type, self.sim_device_id = gymutil.parse_device_str(self.sim_device)
|
||||||
|
self.headless = headless
|
||||||
|
|
||||||
|
# env device is GPU only if sim is on GPU and use_gpu_pipeline=True, otherwise returned tensors are copied to CPU by physX.
|
||||||
|
if sim_device_type == 'cuda' and sim_params.use_gpu_pipeline:
|
||||||
|
self.device = self.sim_device
|
||||||
|
else:
|
||||||
|
self.device = 'cpu'
|
||||||
|
|
||||||
|
# graphics device for rendering, -1 for no rendering
|
||||||
|
self.graphics_device_id = self.sim_device_id
|
||||||
|
if self.headless == True:
|
||||||
|
self.graphics_device_id = self.sim_device_id
|
||||||
|
|
||||||
|
self.num_obs = cfg.env.num_observations
|
||||||
|
self.num_privileged_obs = cfg.env.num_privileged_obs
|
||||||
|
self.num_actions = cfg.env.num_actions
|
||||||
|
|
||||||
|
if eval_cfg is not None:
|
||||||
|
self.num_eval_envs = eval_cfg.env.num_envs
|
||||||
|
self.num_train_envs = cfg.env.num_envs
|
||||||
|
self.num_envs = self.num_eval_envs + self.num_train_envs
|
||||||
|
else:
|
||||||
|
self.num_eval_envs = 0
|
||||||
|
self.num_train_envs = cfg.env.num_envs
|
||||||
|
self.num_envs = cfg.env.num_envs
|
||||||
|
|
||||||
|
# optimization flags for pytorch JIT
|
||||||
|
torch._C._jit_set_profiling_mode(False)
|
||||||
|
torch._C._jit_set_profiling_executor(False)
|
||||||
|
|
||||||
|
# allocate buffers
|
||||||
|
self.obs_buf = torch.zeros(self.num_envs, self.num_obs, device=self.device, dtype=torch.float)
|
||||||
|
self.rew_buf = torch.zeros(self.num_envs, device=self.device, dtype=torch.float)
|
||||||
|
self.rew_buf_pos = torch.zeros(self.num_envs, device=self.device, dtype=torch.float)
|
||||||
|
self.rew_buf_neg = torch.zeros(self.num_envs, device=self.device, dtype=torch.float)
|
||||||
|
self.reset_buf = torch.ones(self.num_envs, device=self.device, dtype=torch.long)
|
||||||
|
self.episode_length_buf = torch.zeros(self.num_envs, device=self.device, dtype=torch.long)
|
||||||
|
self.time_out_buf = torch.zeros(self.num_envs, device=self.device, dtype=torch.bool)
|
||||||
|
self.privileged_obs_buf = torch.zeros(self.num_envs, self.num_privileged_obs, device=self.device,
|
||||||
|
dtype=torch.float)
|
||||||
|
# self.num_privileged_obs = self.num_obs
|
||||||
|
|
||||||
|
self.extras = {}
|
||||||
|
|
||||||
|
# create envs, sim and viewer
|
||||||
|
self.create_sim()
|
||||||
|
self.gym.prepare_sim(self.sim)
|
||||||
|
|
||||||
|
# todo: read from config
|
||||||
|
self.enable_viewer_sync = True
|
||||||
|
self.viewer = None
|
||||||
|
|
||||||
|
# if running with a viewer, set up keyboard shortcuts and camera
|
||||||
|
if self.headless == False:
|
||||||
|
# subscribe to keyboard shortcuts
|
||||||
|
self.viewer = self.gym.create_viewer(
|
||||||
|
self.sim, gymapi.CameraProperties())
|
||||||
|
self.gym.subscribe_viewer_keyboard_event(
|
||||||
|
self.viewer, gymapi.KEY_ESCAPE, "QUIT")
|
||||||
|
self.gym.subscribe_viewer_keyboard_event(
|
||||||
|
self.viewer, gymapi.KEY_V, "toggle_viewer_sync")
|
||||||
|
|
||||||
|
def get_observations(self):
|
||||||
|
return self.obs_buf
|
||||||
|
|
||||||
|
def get_privileged_observations(self):
|
||||||
|
return self.privileged_obs_buf
|
||||||
|
|
||||||
|
def reset_idx(self, env_ids):
|
||||||
|
"""Reset selected robots"""
|
||||||
|
raise NotImplementedError
|
||||||
|
|
||||||
|
def reset(self):
|
||||||
|
""" Reset all robots"""
|
||||||
|
self.reset_idx(torch.arange(self.num_envs, device=self.device))
|
||||||
|
obs, privileged_obs, _, _, _ = self.step(
|
||||||
|
torch.zeros(self.num_envs, self.num_actions, device=self.device, requires_grad=False))
|
||||||
|
return obs, privileged_obs
|
||||||
|
|
||||||
|
def step(self, actions):
|
||||||
|
raise NotImplementedError
|
||||||
|
|
||||||
|
def render_gui(self, sync_frame_time=True):
|
||||||
|
if self.viewer:
|
||||||
|
# check for window closed
|
||||||
|
if self.gym.query_viewer_has_closed(self.viewer):
|
||||||
|
sys.exit()
|
||||||
|
|
||||||
|
# check for keyboard events
|
||||||
|
for evt in self.gym.query_viewer_action_events(self.viewer):
|
||||||
|
if evt.action == "QUIT" and evt.value > 0:
|
||||||
|
sys.exit()
|
||||||
|
elif evt.action == "toggle_viewer_sync" and evt.value > 0:
|
||||||
|
self.enable_viewer_sync = not self.enable_viewer_sync
|
||||||
|
|
||||||
|
# fetch results
|
||||||
|
if self.device != 'cpu':
|
||||||
|
self.gym.fetch_results(self.sim, True)
|
||||||
|
|
||||||
|
# step graphics
|
||||||
|
if self.enable_viewer_sync:
|
||||||
|
self.gym.step_graphics(self.sim)
|
||||||
|
self.gym.draw_viewer(self.viewer, self.sim, True)
|
||||||
|
if sync_frame_time:
|
||||||
|
self.gym.sync_frame_time(self.sim)
|
||||||
|
else:
|
||||||
|
self.gym.poll_viewer_events(self.viewer)
|
||||||
|
|
||||||
|
def close(self):
|
||||||
|
if self.headless == False:
|
||||||
|
self.gym.destroy_viewer(self.viewer)
|
||||||
|
self.gym.destroy_sim(self.sim)
|
||||||
|
|
@ -0,0 +1,181 @@
|
||||||
|
import numpy as np
|
||||||
|
import torch
|
||||||
|
from matplotlib import pyplot as plt
|
||||||
|
|
||||||
|
|
||||||
|
def is_met(scale, l2_err, threshold):
|
||||||
|
return (l2_err / scale) < threshold
|
||||||
|
|
||||||
|
|
||||||
|
def key_is_met(metric_cache, config, ep_len, target_key, env_id, threshold):
|
||||||
|
# metric_cache[target_key][env_id] / ep_len
|
||||||
|
scale = 1
|
||||||
|
l2_err = 0
|
||||||
|
return is_met(scale, l2_err, threshold)
|
||||||
|
|
||||||
|
|
||||||
|
class Curriculum:
|
||||||
|
def set_to(self, low, high, value=1.0):
|
||||||
|
inds = np.logical_and(
|
||||||
|
self.grid >= low[:, None],
|
||||||
|
self.grid <= high[:, None]
|
||||||
|
).all(axis=0)
|
||||||
|
|
||||||
|
assert len(inds) != 0, "You are intializing your distribution with an empty domain!"
|
||||||
|
|
||||||
|
self.weights[inds] = value
|
||||||
|
|
||||||
|
def __init__(self, seed, **key_ranges):
|
||||||
|
self.rng = np.random.RandomState(seed)
|
||||||
|
|
||||||
|
self.cfg = cfg = {}
|
||||||
|
self.indices = indices = {}
|
||||||
|
for key, v_range in key_ranges.items():
|
||||||
|
bin_size = (v_range[1] - v_range[0]) / v_range[2]
|
||||||
|
cfg[key] = np.linspace(v_range[0] + bin_size / 2, v_range[1] - bin_size / 2, v_range[2])
|
||||||
|
indices[key] = np.linspace(0, v_range[2]-1, v_range[2])
|
||||||
|
|
||||||
|
self.lows = np.array([range[0] for range in key_ranges.values()])
|
||||||
|
self.highs = np.array([range[1] for range in key_ranges.values()])
|
||||||
|
|
||||||
|
# self.bin_sizes = {key: arr[1] - arr[0] for key, arr in cfg.items()}
|
||||||
|
self.bin_sizes = {key: (v_range[1] - v_range[0]) / v_range[2] for key, v_range in key_ranges.items()}
|
||||||
|
|
||||||
|
self._raw_grid = np.stack(np.meshgrid(*cfg.values(), indexing='ij'))
|
||||||
|
self._idx_grid = np.stack(np.meshgrid(*indices.values(), indexing='ij'))
|
||||||
|
self.keys = [*key_ranges.keys()]
|
||||||
|
self.grid = self._raw_grid.reshape([len(self.keys), -1])
|
||||||
|
self.idx_grid = self._idx_grid.reshape([len(self.keys), -1])
|
||||||
|
# self.grid = np.stack([params.flatten() for params in raw_grid])
|
||||||
|
|
||||||
|
self._l = l = len(self.grid[0])
|
||||||
|
self.ls = {key: len(self.cfg[key]) for key in self.cfg.keys()}
|
||||||
|
|
||||||
|
self.weights = np.zeros(l)
|
||||||
|
self.indices = np.arange(l)
|
||||||
|
|
||||||
|
def __len__(self):
|
||||||
|
return self._l
|
||||||
|
|
||||||
|
def __getitem__(self, *keys):
|
||||||
|
pass
|
||||||
|
|
||||||
|
def update(self, **kwargs):
|
||||||
|
# bump the envelop if
|
||||||
|
pass
|
||||||
|
|
||||||
|
def sample_bins(self, batch_size, low=None, high=None):
|
||||||
|
"""default to uniform"""
|
||||||
|
if low is not None and high is not None: # if bounds given
|
||||||
|
valid_inds = np.logical_and(
|
||||||
|
self.grid >= low[:, None],
|
||||||
|
self.grid <= high[:, None]
|
||||||
|
).all(axis=0)
|
||||||
|
temp_weights = np.zeros_like(self.weights)
|
||||||
|
temp_weights[valid_inds] = self.weights[valid_inds]
|
||||||
|
inds = self.rng.choice(self.indices, batch_size, p=temp_weights / temp_weights.sum())
|
||||||
|
else: # if no bounds given
|
||||||
|
inds = self.rng.choice(self.indices, batch_size, p=self.weights / self.weights.sum())
|
||||||
|
|
||||||
|
return self.grid.T[inds], inds
|
||||||
|
|
||||||
|
def sample_uniform_from_cell(self, centroids):
|
||||||
|
bin_sizes = np.array([*self.bin_sizes.values()])
|
||||||
|
low, high = centroids + bin_sizes / 2, centroids - bin_sizes / 2
|
||||||
|
return self.rng.uniform(low, high)#.clip(self.lows, self.highs)
|
||||||
|
|
||||||
|
def sample(self, batch_size, low=None, high=None):
|
||||||
|
cgf_centroid, inds = self.sample_bins(batch_size, low=low, high=high)
|
||||||
|
return np.stack([self.sample_uniform_from_cell(v_range) for v_range in cgf_centroid]), inds
|
||||||
|
|
||||||
|
|
||||||
|
class SumCurriculum(Curriculum):
|
||||||
|
def __init__(self, seed, **kwargs):
|
||||||
|
super().__init__(seed, **kwargs)
|
||||||
|
|
||||||
|
self.success = np.zeros(len(self))
|
||||||
|
self.trials = np.zeros(len(self))
|
||||||
|
|
||||||
|
def update(self, bin_inds, l1_error, threshold):
|
||||||
|
is_success = l1_error < threshold
|
||||||
|
self.success[bin_inds[is_success]] += 1
|
||||||
|
self.trials[bin_inds] += 1
|
||||||
|
|
||||||
|
def success_rates(self, *keys):
|
||||||
|
s_rate = self.success / (self.trials + 1e-6)
|
||||||
|
s_rate = s_rate.reshape(list(self.ls.values()))
|
||||||
|
marginals = tuple(i for i, key in enumerate(self.keys) if key not in keys)
|
||||||
|
if marginals:
|
||||||
|
return s_rate.mean(axis=marginals)
|
||||||
|
return s_rate
|
||||||
|
|
||||||
|
|
||||||
|
class RewardThresholdCurriculum(Curriculum):
|
||||||
|
def __init__(self, seed, **kwargs):
|
||||||
|
super().__init__(seed, **kwargs)
|
||||||
|
|
||||||
|
self.episode_reward_lin = np.zeros(len(self))
|
||||||
|
self.episode_reward_ang = np.zeros(len(self))
|
||||||
|
self.episode_lin_vel_raw = np.zeros(len(self))
|
||||||
|
self.episode_ang_vel_raw = np.zeros(len(self))
|
||||||
|
self.episode_duration = np.zeros(len(self))
|
||||||
|
|
||||||
|
def get_local_bins(self, bin_inds, ranges=0.1):
|
||||||
|
if isinstance(ranges, float):
|
||||||
|
ranges = np.ones(self.grid.shape[0]) * ranges
|
||||||
|
bin_inds = bin_inds.reshape(-1)
|
||||||
|
|
||||||
|
adjacent_inds = np.logical_and(
|
||||||
|
self.grid[:, None, :].repeat(bin_inds.shape[0], axis=1) >= self.grid[:, bin_inds, None] - ranges.reshape(-1, 1, 1),
|
||||||
|
self.grid[:, None, :].repeat(bin_inds.shape[0], axis=1) <= self.grid[:, bin_inds, None] + ranges.reshape(-1, 1, 1)
|
||||||
|
).all(axis=0)
|
||||||
|
|
||||||
|
return adjacent_inds
|
||||||
|
|
||||||
|
def update(self, bin_inds, task_rewards, success_thresholds, local_range=0.5):
|
||||||
|
|
||||||
|
is_success = 1.
|
||||||
|
for task_reward, success_threshold in zip(task_rewards, success_thresholds):
|
||||||
|
is_success = is_success * (task_reward > success_threshold).cpu()
|
||||||
|
if len(success_thresholds) == 0:
|
||||||
|
is_success = np.array([False] * len(bin_inds))
|
||||||
|
else:
|
||||||
|
is_success = np.array(is_success.bool())
|
||||||
|
|
||||||
|
# if len(is_success) > 0 and is_success.any():
|
||||||
|
# print("successes")
|
||||||
|
|
||||||
|
self.weights[bin_inds[is_success]] = np.clip(self.weights[bin_inds[is_success]] + 0.2, 0, 1)
|
||||||
|
adjacents = self.get_local_bins(bin_inds[is_success], ranges=local_range)
|
||||||
|
for adjacent in adjacents:
|
||||||
|
#print(adjacent)
|
||||||
|
#print(self.grid[:, adjacent])
|
||||||
|
adjacent_inds = np.array(adjacent.nonzero()[0])
|
||||||
|
self.weights[adjacent_inds] = np.clip(self.weights[adjacent_inds] + 0.2, 0, 1)
|
||||||
|
|
||||||
|
def log(self, bin_inds, lin_vel_raw=None, ang_vel_raw=None, episode_duration=None):
|
||||||
|
self.episode_lin_vel_raw[bin_inds] = lin_vel_raw.cpu().numpy()
|
||||||
|
self.episode_ang_vel_raw[bin_inds] = ang_vel_raw.cpu().numpy()
|
||||||
|
self.episode_duration[bin_inds] = episode_duration.cpu().numpy()
|
||||||
|
|
||||||
|
if __name__ == '__main__':
|
||||||
|
r = RewardThresholdCurriculum(100, x=(-1, 1, 5), y=(-1, 1, 2), z=(-1, 1, 11))
|
||||||
|
|
||||||
|
assert r._raw_grid.shape == (3, 5, 2, 11), "grid shape is wrong: {}".format(r.grid.shape)
|
||||||
|
|
||||||
|
low, high = np.array([-1.0, -0.6, -1.0]), np.array([1.0, 0.6, 1.0])
|
||||||
|
|
||||||
|
# r.set_to(low, high, value=1.0)
|
||||||
|
|
||||||
|
adjacents = r.get_local_bins(np.array([10, ]), range=0.5)
|
||||||
|
for adjacent in adjacents:
|
||||||
|
adjacent_inds = np.array(adjacent.nonzero()[0])
|
||||||
|
print(adjacent_inds)
|
||||||
|
r.update(bin_inds=adjacent_inds, lin_vel_rewards=np.ones_like(adjacent_inds),
|
||||||
|
ang_vel_rewards=np.ones_like(adjacent_inds), lin_vel_threshold=0.0, ang_vel_threshold=0.0,
|
||||||
|
local_range=0.5)
|
||||||
|
|
||||||
|
samples, bins = r.sample(10_000)
|
||||||
|
|
||||||
|
plt.scatter(*samples.T[:2])
|
||||||
|
plt.show()
|
||||||
File diff suppressed because it is too large
Load Diff
|
|
@ -0,0 +1,421 @@
|
||||||
|
# License: see [LICENSE, LICENSES/legged_gym/LICENSE]
|
||||||
|
|
||||||
|
from params_proto import PrefixProto, ParamsProto
|
||||||
|
|
||||||
|
|
||||||
|
class Cfg(PrefixProto, cli=False):
|
||||||
|
class env(PrefixProto, cli=False):
|
||||||
|
num_envs = 4096
|
||||||
|
num_observations = 235
|
||||||
|
num_scalar_observations = 42
|
||||||
|
# if not None a privilige_obs_buf will be returned by step() (critic obs for assymetric training). None is returned otherwise
|
||||||
|
num_privileged_obs = 18
|
||||||
|
privileged_future_horizon = 1
|
||||||
|
num_actions = 12
|
||||||
|
num_observation_history = 15
|
||||||
|
env_spacing = 3. # not used with heightfields/trimeshes
|
||||||
|
send_timeouts = True # send time out information to the algorithm
|
||||||
|
episode_length_s = 20 # episode length in seconds
|
||||||
|
observe_vel = True
|
||||||
|
observe_only_ang_vel = False
|
||||||
|
observe_only_lin_vel = False
|
||||||
|
observe_yaw = False
|
||||||
|
observe_contact_states = False
|
||||||
|
observe_command = True
|
||||||
|
observe_height_command = False
|
||||||
|
observe_gait_commands = False
|
||||||
|
observe_timing_parameter = False
|
||||||
|
observe_clock_inputs = False
|
||||||
|
observe_two_prev_actions = False
|
||||||
|
observe_imu = False
|
||||||
|
record_video = True
|
||||||
|
recording_width_px = 360
|
||||||
|
recording_height_px = 240
|
||||||
|
recording_mode = "COLOR"
|
||||||
|
num_recording_envs = 1
|
||||||
|
debug_viz = False
|
||||||
|
all_agents_share = False
|
||||||
|
|
||||||
|
priv_observe_friction = True
|
||||||
|
priv_observe_friction_indep = True
|
||||||
|
priv_observe_ground_friction = False
|
||||||
|
priv_observe_ground_friction_per_foot = False
|
||||||
|
priv_observe_restitution = True
|
||||||
|
priv_observe_base_mass = True
|
||||||
|
priv_observe_com_displacement = True
|
||||||
|
priv_observe_motor_strength = False
|
||||||
|
priv_observe_motor_offset = False
|
||||||
|
priv_observe_joint_friction = True
|
||||||
|
priv_observe_Kp_factor = True
|
||||||
|
priv_observe_Kd_factor = True
|
||||||
|
priv_observe_contact_forces = False
|
||||||
|
priv_observe_contact_states = False
|
||||||
|
priv_observe_body_velocity = False
|
||||||
|
priv_observe_foot_height = False
|
||||||
|
priv_observe_body_height = False
|
||||||
|
priv_observe_gravity = False
|
||||||
|
priv_observe_terrain_type = False
|
||||||
|
priv_observe_clock_inputs = False
|
||||||
|
priv_observe_doubletime_clock_inputs = False
|
||||||
|
priv_observe_halftime_clock_inputs = False
|
||||||
|
priv_observe_desired_contact_states = False
|
||||||
|
priv_observe_dummy_variable = False
|
||||||
|
|
||||||
|
class terrain(PrefixProto, cli=False):
|
||||||
|
mesh_type = 'trimesh' # "heightfield" # none, plane, heightfield or trimesh
|
||||||
|
horizontal_scale = 0.1 # [m]
|
||||||
|
vertical_scale = 0.005 # [m]
|
||||||
|
border_size = 0 # 25 # [m]
|
||||||
|
curriculum = True
|
||||||
|
static_friction = 1.0
|
||||||
|
dynamic_friction = 1.0
|
||||||
|
restitution = 0.0
|
||||||
|
terrain_noise_magnitude = 0.1
|
||||||
|
# rough terrain only:
|
||||||
|
terrain_smoothness = 0.005
|
||||||
|
measure_heights = True
|
||||||
|
# 1mx1.6m rectangle (without center line)
|
||||||
|
measured_points_x = [-0.8, -0.7, -0.6, -0.5, -0.4, -0.3, -0.2, -0.1, 0., 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8]
|
||||||
|
measured_points_y = [-0.5, -0.4, -0.3, -0.2, -0.1, 0., 0.1, 0.2, 0.3, 0.4, 0.5]
|
||||||
|
selected = False # select a unique terrain type and pass all arguments
|
||||||
|
terrain_kwargs = None # Dict of arguments for selected terrain
|
||||||
|
min_init_terrain_level = 0
|
||||||
|
max_init_terrain_level = 5 # starting curriculum state
|
||||||
|
terrain_length = 8.
|
||||||
|
terrain_width = 8.
|
||||||
|
num_rows = 10 # number of terrain rows (levels)
|
||||||
|
num_cols = 20 # number of terrain cols (types)
|
||||||
|
# terrain types: [smooth slope, rough slope, stairs up, stairs down, discrete]
|
||||||
|
terrain_proportions = [0.1, 0.1, 0.35, 0.25, 0.2]
|
||||||
|
# trimesh only:
|
||||||
|
slope_treshold = 0.75 # slopes above this threshold will be corrected to vertical surfaces
|
||||||
|
difficulty_scale = 1.
|
||||||
|
x_init_range = 1.
|
||||||
|
y_init_range = 1.
|
||||||
|
yaw_init_range = 0.
|
||||||
|
x_init_offset = 0.
|
||||||
|
y_init_offset = 0.
|
||||||
|
teleport_robots = True
|
||||||
|
teleport_thresh = 2.0
|
||||||
|
max_platform_height = 0.2
|
||||||
|
center_robots = False
|
||||||
|
center_span = 5
|
||||||
|
|
||||||
|
class commands(PrefixProto, cli=False):
|
||||||
|
command_curriculum = False
|
||||||
|
max_reverse_curriculum = 1.
|
||||||
|
max_forward_curriculum = 1.
|
||||||
|
yaw_command_curriculum = False
|
||||||
|
max_yaw_curriculum = 1.
|
||||||
|
exclusive_command_sampling = False
|
||||||
|
num_commands = 3
|
||||||
|
resampling_time = 10. # time before command are changed[s]
|
||||||
|
subsample_gait = False
|
||||||
|
gait_interval_s = 10. # time between resampling gait params
|
||||||
|
vel_interval_s = 10.
|
||||||
|
jump_interval_s = 20. # time between jumps
|
||||||
|
jump_duration_s = 0.1 # duration of jump
|
||||||
|
jump_height = 0.3
|
||||||
|
heading_command = True # if true: compute ang vel command from heading error
|
||||||
|
global_reference = False
|
||||||
|
observe_accel = False
|
||||||
|
distributional_commands = False
|
||||||
|
curriculum_type = "RewardThresholdCurriculum"
|
||||||
|
lipschitz_threshold = 0.9
|
||||||
|
|
||||||
|
num_lin_vel_bins = 20
|
||||||
|
lin_vel_step = 0.3
|
||||||
|
num_ang_vel_bins = 20
|
||||||
|
ang_vel_step = 0.3
|
||||||
|
distribution_update_extension_distance = 1
|
||||||
|
curriculum_seed = 100
|
||||||
|
|
||||||
|
lin_vel_x = [-1.0, 1.0] # min max [m/s]
|
||||||
|
lin_vel_y = [-1.0, 1.0] # min max [m/s]
|
||||||
|
ang_vel_yaw = [-1, 1] # min max [rad/s]
|
||||||
|
body_height_cmd = [-0.05, 0.05]
|
||||||
|
impulse_height_commands = False
|
||||||
|
|
||||||
|
limit_vel_x = [-10.0, 10.0]
|
||||||
|
limit_vel_y = [-0.6, 0.6]
|
||||||
|
limit_vel_yaw = [-10.0, 10.0]
|
||||||
|
limit_body_height = [-0.05, 0.05]
|
||||||
|
limit_gait_phase = [0, 0.01]
|
||||||
|
limit_gait_offset = [0, 0.01]
|
||||||
|
limit_gait_bound = [0, 0.01]
|
||||||
|
limit_gait_frequency = [2.0, 2.01]
|
||||||
|
limit_gait_duration = [0.49, 0.5]
|
||||||
|
limit_footswing_height = [0.06, 0.061]
|
||||||
|
limit_body_pitch = [0.0, 0.01]
|
||||||
|
limit_body_roll = [0.0, 0.01]
|
||||||
|
limit_aux_reward_coef = [0.0, 0.01]
|
||||||
|
limit_compliance = [0.0, 0.01]
|
||||||
|
limit_stance_width = [0.0, 0.01]
|
||||||
|
limit_stance_length = [0.0, 0.01]
|
||||||
|
|
||||||
|
num_bins_vel_x = 25
|
||||||
|
num_bins_vel_y = 3
|
||||||
|
num_bins_vel_yaw = 25
|
||||||
|
num_bins_body_height = 1
|
||||||
|
num_bins_gait_frequency = 11
|
||||||
|
num_bins_gait_phase = 11
|
||||||
|
num_bins_gait_offset = 2
|
||||||
|
num_bins_gait_bound = 2
|
||||||
|
num_bins_gait_duration = 3
|
||||||
|
num_bins_footswing_height = 1
|
||||||
|
num_bins_body_pitch = 1
|
||||||
|
num_bins_body_roll = 1
|
||||||
|
num_bins_aux_reward_coef = 1
|
||||||
|
num_bins_compliance = 1
|
||||||
|
num_bins_compliance = 1
|
||||||
|
num_bins_stance_width = 1
|
||||||
|
num_bins_stance_length = 1
|
||||||
|
|
||||||
|
heading = [-3.14, 3.14]
|
||||||
|
|
||||||
|
gait_phase_cmd_range = [0.0, 0.01]
|
||||||
|
gait_offset_cmd_range = [0.0, 0.01]
|
||||||
|
gait_bound_cmd_range = [0.0, 0.01]
|
||||||
|
gait_frequency_cmd_range = [2.0, 2.01]
|
||||||
|
gait_duration_cmd_range = [0.49, 0.5]
|
||||||
|
footswing_height_range = [0.06, 0.061]
|
||||||
|
body_pitch_range = [0.0, 0.01]
|
||||||
|
body_roll_range = [0.0, 0.01]
|
||||||
|
aux_reward_coef_range = [0.0, 0.01]
|
||||||
|
compliance_range = [0.0, 0.01]
|
||||||
|
stance_width_range = [0.0, 0.01]
|
||||||
|
stance_length_range = [0.0, 0.01]
|
||||||
|
|
||||||
|
exclusive_phase_offset = True
|
||||||
|
binary_phases = False
|
||||||
|
pacing_offset = False
|
||||||
|
balance_gait_distribution = True
|
||||||
|
gaitwise_curricula = True
|
||||||
|
|
||||||
|
class curriculum_thresholds(PrefixProto, cli=False):
|
||||||
|
tracking_lin_vel = 0.8 # closer to 1 is tighter
|
||||||
|
tracking_ang_vel = 0.5
|
||||||
|
tracking_contacts_shaped_force = 0.8 # closer to 1 is tighter
|
||||||
|
tracking_contacts_shaped_vel = 0.8
|
||||||
|
|
||||||
|
class init_state(PrefixProto, cli=False):
|
||||||
|
pos = [0.0, 0.0, 1.] # x,y,z [m]
|
||||||
|
rot = [0.0, 0.0, 0.0, 1.0] # x,y,z,w [quat]
|
||||||
|
lin_vel = [0.0, 0.0, 0.0] # x,y,z [m/s]
|
||||||
|
ang_vel = [0.0, 0.0, 0.0] # x,y,z [rad/s]
|
||||||
|
# target angles when action = 0.0
|
||||||
|
default_joint_angles = {"joint_a": 0., "joint_b": 0.}
|
||||||
|
|
||||||
|
class control(PrefixProto, cli=False):
|
||||||
|
control_type = 'actuator_net' #'P' # P: position, V: velocity, T: torques
|
||||||
|
# PD Drive parameters:
|
||||||
|
stiffness = {'joint_a': 10.0, 'joint_b': 15.} # [N*m/rad]
|
||||||
|
damping = {'joint_a': 1.0, 'joint_b': 1.5} # [N*m*s/rad]
|
||||||
|
# action scale: target angle = actionScale * action + defaultAngle
|
||||||
|
action_scale = 0.5
|
||||||
|
hip_scale_reduction = 1.0
|
||||||
|
# decimation: Number of control action updates @ sim DT per policy DT
|
||||||
|
decimation = 4
|
||||||
|
|
||||||
|
class asset(PrefixProto, cli=False):
|
||||||
|
file = ""
|
||||||
|
foot_name = "None" # name of the feet bodies, used to index body state and contact force tensors
|
||||||
|
penalize_contacts_on = []
|
||||||
|
terminate_after_contacts_on = []
|
||||||
|
disable_gravity = False
|
||||||
|
# merge bodies connected by fixed joints. Specific fixed joints can be kept by adding " <... dont_collapse="true">
|
||||||
|
collapse_fixed_joints = True
|
||||||
|
fix_base_link = False # fixe the base of the robot
|
||||||
|
default_dof_drive_mode = 3 # see GymDofDriveModeFlags (0 is none, 1 is pos tgt, 2 is vel tgt, 3 effort)
|
||||||
|
self_collisions = 0 # 1 to disable, 0 to enable...bitwise filter
|
||||||
|
# replace collision cylinders with capsules, leads to faster/more stable simulation
|
||||||
|
replace_cylinder_with_capsule = True
|
||||||
|
flip_visual_attachments = False # Some .obj meshes must be flipped from y-up to z-up
|
||||||
|
|
||||||
|
density = 0.001
|
||||||
|
angular_damping = 0.
|
||||||
|
linear_damping = 0.
|
||||||
|
max_angular_velocity = 1000.
|
||||||
|
max_linear_velocity = 1000.
|
||||||
|
armature = 0.
|
||||||
|
thickness = 0.01
|
||||||
|
|
||||||
|
class domain_rand(PrefixProto, cli=False):
|
||||||
|
rand_interval_s = 10
|
||||||
|
randomize_rigids_after_start = True
|
||||||
|
randomize_friction = True
|
||||||
|
friction_range = [0.5, 1.25] # increase range
|
||||||
|
randomize_restitution = False
|
||||||
|
restitution_range = [0, 1.0]
|
||||||
|
randomize_base_mass = False
|
||||||
|
# add link masses, increase range, randomize inertia, randomize joint properties
|
||||||
|
added_mass_range = [-1., 1.]
|
||||||
|
randomize_com_displacement = False
|
||||||
|
# add link masses, increase range, randomize inertia, randomize joint properties
|
||||||
|
com_displacement_range = [-0.15, 0.15]
|
||||||
|
randomize_motor_strength = False
|
||||||
|
motor_strength_range = [0.9, 1.1]
|
||||||
|
randomize_Kp_factor = False
|
||||||
|
Kp_factor_range = [0.8, 1.3]
|
||||||
|
randomize_Kd_factor = False
|
||||||
|
Kd_factor_range = [0.5, 1.5]
|
||||||
|
gravity_rand_interval_s = 7
|
||||||
|
gravity_impulse_duration = 1.0
|
||||||
|
randomize_gravity = False
|
||||||
|
gravity_range = [-1.0, 1.0]
|
||||||
|
push_robots = True
|
||||||
|
push_interval_s = 15
|
||||||
|
max_push_vel_xy = 1.
|
||||||
|
randomize_lag_timesteps = True
|
||||||
|
lag_timesteps = 6
|
||||||
|
|
||||||
|
class rewards(PrefixProto, cli=False):
|
||||||
|
only_positive_rewards = True # if true negative total rewards are clipped at zero (avoids early termination problems)
|
||||||
|
only_positive_rewards_ji22_style = False
|
||||||
|
sigma_rew_neg = 5
|
||||||
|
reward_container_name = "CoRLRewards"
|
||||||
|
tracking_sigma = 0.25 # tracking reward = exp(-error^2/sigma)
|
||||||
|
tracking_sigma_lat = 0.25 # tracking reward = exp(-error^2/sigma)
|
||||||
|
tracking_sigma_long = 0.25 # tracking reward = exp(-error^2/sigma)
|
||||||
|
tracking_sigma_yaw = 0.25 # tracking reward = exp(-error^2/sigma)
|
||||||
|
soft_dof_pos_limit = 1. # percentage of urdf limits, values above this limit are penalized
|
||||||
|
soft_dof_vel_limit = 1.
|
||||||
|
soft_torque_limit = 1.
|
||||||
|
base_height_target = 1.
|
||||||
|
max_contact_force = 100. # forces above this value are penalized
|
||||||
|
use_terminal_body_height = False
|
||||||
|
terminal_body_height = 0.20
|
||||||
|
use_terminal_foot_height = False
|
||||||
|
terminal_foot_height = -0.005
|
||||||
|
use_terminal_roll_pitch = False
|
||||||
|
terminal_body_ori = 0.5
|
||||||
|
kappa_gait_probs = 0.07
|
||||||
|
gait_force_sigma = 50.
|
||||||
|
gait_vel_sigma = 0.5
|
||||||
|
footswing_height = 0.09
|
||||||
|
|
||||||
|
class reward_scales(ParamsProto, cli=False):
|
||||||
|
termination = -0.0
|
||||||
|
tracking_lin_vel = 1.0
|
||||||
|
tracking_ang_vel = 0.5
|
||||||
|
lin_vel_z = -2.0
|
||||||
|
ang_vel_xy = -0.05
|
||||||
|
orientation = -0.
|
||||||
|
torques = -0.00001
|
||||||
|
dof_vel = -0.
|
||||||
|
dof_acc = -2.5e-7
|
||||||
|
base_height = -0.
|
||||||
|
feet_air_time = 1.0
|
||||||
|
collision = -1.
|
||||||
|
feet_stumble = -0.0
|
||||||
|
action_rate = -0.01
|
||||||
|
stand_still = -0.
|
||||||
|
tracking_lin_vel_lat = 0.
|
||||||
|
tracking_lin_vel_long = 0.
|
||||||
|
tracking_contacts = 0.
|
||||||
|
tracking_contacts_shaped = 0.
|
||||||
|
tracking_contacts_shaped_force = 0.
|
||||||
|
tracking_contacts_shaped_vel = 0.
|
||||||
|
jump = 0.0
|
||||||
|
energy = 0.0
|
||||||
|
energy_expenditure = 0.0
|
||||||
|
survival = 0.0
|
||||||
|
dof_pos_limits = 0.0
|
||||||
|
feet_contact_forces = 0.
|
||||||
|
feet_slip = 0.
|
||||||
|
feet_clearance_cmd_linear = 0.
|
||||||
|
dof_pos = 0.
|
||||||
|
action_smoothness_1 = 0.
|
||||||
|
action_smoothness_2 = 0.
|
||||||
|
base_motion = 0.
|
||||||
|
feet_impact_vel = 0.0
|
||||||
|
raibert_heuristic = 0.0
|
||||||
|
|
||||||
|
class normalization(PrefixProto, cli=False):
|
||||||
|
clip_observations = 100.
|
||||||
|
clip_actions = 100.
|
||||||
|
|
||||||
|
friction_range = [0.05, 4.5]
|
||||||
|
ground_friction_range = [0.05, 4.5]
|
||||||
|
restitution_range = [0, 1.0]
|
||||||
|
added_mass_range = [-1., 3.]
|
||||||
|
com_displacement_range = [-0.1, 0.1]
|
||||||
|
motor_strength_range = [0.9, 1.1]
|
||||||
|
motor_offset_range = [-0.05, 0.05]
|
||||||
|
Kp_factor_range = [0.8, 1.3]
|
||||||
|
Kd_factor_range = [0.5, 1.5]
|
||||||
|
joint_friction_range = [0.0, 0.7]
|
||||||
|
contact_force_range = [0.0, 50.0]
|
||||||
|
contact_state_range = [0.0, 1.0]
|
||||||
|
body_velocity_range = [-6.0, 6.0]
|
||||||
|
foot_height_range = [0.0, 0.15]
|
||||||
|
body_height_range = [0.0, 0.60]
|
||||||
|
gravity_range = [-1.0, 1.0]
|
||||||
|
motion = [-0.01, 0.01]
|
||||||
|
|
||||||
|
class obs_scales(PrefixProto, cli=False):
|
||||||
|
lin_vel = 2.0
|
||||||
|
ang_vel = 0.25
|
||||||
|
dof_pos = 1.0
|
||||||
|
dof_vel = 0.05
|
||||||
|
imu = 0.1
|
||||||
|
height_measurements = 5.0
|
||||||
|
friction_measurements = 1.0
|
||||||
|
body_height_cmd = 2.0
|
||||||
|
gait_phase_cmd = 1.0
|
||||||
|
gait_freq_cmd = 1.0
|
||||||
|
footswing_height_cmd = 0.15
|
||||||
|
body_pitch_cmd = 0.3
|
||||||
|
body_roll_cmd = 0.3
|
||||||
|
aux_reward_cmd = 1.0
|
||||||
|
compliance_cmd = 1.0
|
||||||
|
stance_width_cmd = 1.0
|
||||||
|
stance_length_cmd = 1.0
|
||||||
|
segmentation_image = 1.0
|
||||||
|
rgb_image = 1.0
|
||||||
|
depth_image = 1.0
|
||||||
|
|
||||||
|
class noise(PrefixProto, cli=False):
|
||||||
|
add_noise = True
|
||||||
|
noise_level = 1.0 # scales other values
|
||||||
|
|
||||||
|
class noise_scales(PrefixProto, cli=False):
|
||||||
|
dof_pos = 0.01
|
||||||
|
dof_vel = 1.5
|
||||||
|
lin_vel = 0.1
|
||||||
|
ang_vel = 0.2
|
||||||
|
imu = 0.1
|
||||||
|
gravity = 0.05
|
||||||
|
contact_states = 0.05
|
||||||
|
height_measurements = 0.1
|
||||||
|
friction_measurements = 0.0
|
||||||
|
segmentation_image = 0.0
|
||||||
|
rgb_image = 0.0
|
||||||
|
depth_image = 0.0
|
||||||
|
|
||||||
|
# viewer camera:
|
||||||
|
class viewer(PrefixProto, cli=False):
|
||||||
|
ref_env = 0
|
||||||
|
pos = [10, 0, 6] # [m]
|
||||||
|
lookat = [11., 5, 3.] # [m]
|
||||||
|
|
||||||
|
class sim(PrefixProto, cli=False):
|
||||||
|
dt = 0.005
|
||||||
|
substeps = 1
|
||||||
|
gravity = [0., 0., -9.81] # [m/s^2]
|
||||||
|
up_axis = 1 # 0 is y, 1 is z
|
||||||
|
|
||||||
|
use_gpu_pipeline = True
|
||||||
|
|
||||||
|
class physx(PrefixProto, cli=False):
|
||||||
|
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 [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
|
||||||
|
contact_collection = 2 # 0: never, 1: last sub-step, 2: all sub-steps (default=2)
|
||||||
|
|
@ -0,0 +1,106 @@
|
||||||
|
from typing import Union
|
||||||
|
|
||||||
|
from params_proto import Meta
|
||||||
|
|
||||||
|
from Go2Py.sim.gym.envs.base.legged_robot_config import Cfg
|
||||||
|
|
||||||
|
|
||||||
|
def config_go2(Cnfg: Union[Cfg, Meta]):
|
||||||
|
_ = Cnfg.init_state
|
||||||
|
|
||||||
|
_.pos = [0.0, 0.0, 0.34] # x,y,z [m]
|
||||||
|
_.default_joint_angles = { # = target angles [rad] when action = 0.0
|
||||||
|
'FL_hip_joint': 0.1, # [rad]
|
||||||
|
'RL_hip_joint': 0.1, # [rad]
|
||||||
|
'FR_hip_joint': -0.1, # [rad]
|
||||||
|
'RR_hip_joint': -0.1, # [rad]
|
||||||
|
|
||||||
|
'FL_thigh_joint': 0.8, # [rad]
|
||||||
|
'RL_thigh_joint': 1., # [rad]
|
||||||
|
'FR_thigh_joint': 0.8, # [rad]
|
||||||
|
'RR_thigh_joint': 1., # [rad]
|
||||||
|
|
||||||
|
'FL_calf_joint': -1.5, # [rad]
|
||||||
|
'RL_calf_joint': -1.5, # [rad]
|
||||||
|
'FR_calf_joint': -1.5, # [rad]
|
||||||
|
'RR_calf_joint': -1.5 # [rad]
|
||||||
|
}
|
||||||
|
|
||||||
|
_ = Cnfg.control
|
||||||
|
_.control_type = 'P'
|
||||||
|
_.stiffness = {'joint': 20.} # [N*m/rad]
|
||||||
|
_.damping = {'joint': 0.5} # [N*m*s/rad]
|
||||||
|
# action scale: target angle = actionScale * action + defaultAngle
|
||||||
|
_.action_scale = 0.25
|
||||||
|
_.hip_scale_reduction = 0.5
|
||||||
|
# decimation: Number of control action updates @ sim DT per policy DT
|
||||||
|
_.decimation = 4
|
||||||
|
|
||||||
|
_ = Cnfg.asset
|
||||||
|
_.file = '{MINI_GYM_ROOT_DIR}/assets/urdf/go2.urdf'
|
||||||
|
_.foot_name = "foot"
|
||||||
|
_.penalize_contacts_on = ["thigh", "calf"]
|
||||||
|
_.terminate_after_contacts_on = ["base", "Head_lower", "Head_upper"]
|
||||||
|
_.self_collisions = 0 # 1 to disable, 0 to enable...bitwise filter
|
||||||
|
_.flip_visual_attachments = True
|
||||||
|
_.fix_base_link = False
|
||||||
|
|
||||||
|
_ = Cnfg.rewards
|
||||||
|
_.soft_dof_pos_limit = 0.9
|
||||||
|
_.base_height_target = 0.34
|
||||||
|
|
||||||
|
_ = Cnfg.reward_scales
|
||||||
|
_.torques = -0.0001
|
||||||
|
_.action_rate = -0.01
|
||||||
|
_.dof_pos_limits = -10.0
|
||||||
|
_.orientation = -5.
|
||||||
|
_.base_height = -30.
|
||||||
|
|
||||||
|
_ = Cnfg.terrain
|
||||||
|
_.mesh_type = 'trimesh'
|
||||||
|
_.measure_heights = False
|
||||||
|
_.terrain_noise_magnitude = 0.0
|
||||||
|
_.teleport_robots = True
|
||||||
|
_.border_size = 50
|
||||||
|
|
||||||
|
_.terrain_proportions = [0, 0, 0, 0, 0, 0, 0, 0, 1.0]
|
||||||
|
_.curriculum = False
|
||||||
|
|
||||||
|
_ = Cnfg.env
|
||||||
|
_.num_observations = 42
|
||||||
|
_.observe_vel = False
|
||||||
|
_.num_envs = 4000
|
||||||
|
|
||||||
|
_ = Cnfg.commands
|
||||||
|
_.lin_vel_x = [-1.0, 1.0]
|
||||||
|
_.lin_vel_y = [-1.0, 1.0]
|
||||||
|
|
||||||
|
_ = Cnfg.commands
|
||||||
|
_.heading_command = False
|
||||||
|
_.resampling_time = 10.0
|
||||||
|
_.command_curriculum = True
|
||||||
|
_.num_lin_vel_bins = 30
|
||||||
|
_.num_ang_vel_bins = 30
|
||||||
|
_.lin_vel_x = [-0.6, 0.6]
|
||||||
|
_.lin_vel_y = [-0.6, 0.6]
|
||||||
|
_.ang_vel_yaw = [-1, 1]
|
||||||
|
|
||||||
|
_ = Cnfg.domain_rand
|
||||||
|
_.randomize_base_mass = True
|
||||||
|
_.added_mass_range = [-1, 3]
|
||||||
|
_.push_robots = False
|
||||||
|
_.max_push_vel_xy = 0.5
|
||||||
|
_.randomize_friction = True
|
||||||
|
_.friction_range = [0.05, 4.5]
|
||||||
|
_.randomize_restitution = True
|
||||||
|
_.restitution_range = [0.0, 1.0]
|
||||||
|
_.restitution = 0.5 # default terrain restitution
|
||||||
|
_.randomize_com_displacement = True
|
||||||
|
_.com_displacement_range = [-0.1, 0.1]
|
||||||
|
_.randomize_motor_strength = True
|
||||||
|
_.motor_strength_range = [0.9, 1.1]
|
||||||
|
_.randomize_Kp_factor = False
|
||||||
|
_.Kp_factor_range = [0.8, 1.3]
|
||||||
|
_.randomize_Kd_factor = False
|
||||||
|
_.Kd_factor_range = [0.5, 1.5]
|
||||||
|
_.rand_interval_s = 6
|
||||||
|
|
@ -0,0 +1,50 @@
|
||||||
|
from isaacgym import gymutil, gymapi
|
||||||
|
import torch
|
||||||
|
from params_proto import Meta
|
||||||
|
from typing import Union
|
||||||
|
|
||||||
|
from Go2Py.sim.gym.envs.base.legged_robot import LeggedRobot
|
||||||
|
from Go2Py.sim.gym.envs.base.legged_robot_config import Cfg
|
||||||
|
|
||||||
|
|
||||||
|
class VelocityTrackingEasyEnv(LeggedRobot):
|
||||||
|
def __init__(self, sim_device, headless, num_envs=None, prone=False, deploy=False,
|
||||||
|
cfg: Cfg = None, eval_cfg: Cfg = None, initial_dynamics_dict=None, physics_engine="SIM_PHYSX"):
|
||||||
|
|
||||||
|
if num_envs is not None:
|
||||||
|
cfg.env.num_envs = num_envs
|
||||||
|
|
||||||
|
sim_params = gymapi.SimParams()
|
||||||
|
gymutil.parse_sim_config(vars(cfg.sim), sim_params)
|
||||||
|
super().__init__(cfg, sim_params, physics_engine, sim_device, headless, eval_cfg, initial_dynamics_dict)
|
||||||
|
|
||||||
|
|
||||||
|
def step(self, actions):
|
||||||
|
self.obs_buf, self.privileged_obs_buf, self.rew_buf, self.reset_buf, self.extras = super().step(actions)
|
||||||
|
|
||||||
|
self.foot_positions = self.rigid_body_state.view(self.num_envs, self.num_bodies, 13)[:, self.feet_indices,
|
||||||
|
0:3]
|
||||||
|
|
||||||
|
self.extras.update({
|
||||||
|
"privileged_obs": self.privileged_obs_buf,
|
||||||
|
"joint_pos": self.dof_pos.cpu().numpy(),
|
||||||
|
"joint_vel": self.dof_vel.cpu().numpy(),
|
||||||
|
"joint_pos_target": self.joint_pos_target.cpu().detach().numpy(),
|
||||||
|
"joint_vel_target": torch.zeros(12),
|
||||||
|
"body_linear_vel": self.base_lin_vel.cpu().detach().numpy(),
|
||||||
|
"body_angular_vel": self.base_ang_vel.cpu().detach().numpy(),
|
||||||
|
"body_linear_vel_cmd": self.commands.cpu().numpy()[:, 0:2],
|
||||||
|
"body_angular_vel_cmd": self.commands.cpu().numpy()[:, 2:],
|
||||||
|
"contact_states": (self.contact_forces[:, self.feet_indices, 2] > 1.).detach().cpu().numpy().copy(),
|
||||||
|
"foot_positions": (self.foot_positions).detach().cpu().numpy().copy(),
|
||||||
|
"body_pos": self.root_states[:, 0:3].detach().cpu().numpy(),
|
||||||
|
"torques": self.torques.detach().cpu().numpy()
|
||||||
|
})
|
||||||
|
|
||||||
|
return self.obs_buf, self.rew_buf, self.reset_buf, self.extras
|
||||||
|
|
||||||
|
def reset(self):
|
||||||
|
self.reset_idx(torch.arange(self.num_envs, device=self.device))
|
||||||
|
obs, _, _, _ = self.step(torch.zeros(self.num_envs, self.num_actions, device=self.device, requires_grad=False))
|
||||||
|
return obs
|
||||||
|
|
||||||
|
|
@ -0,0 +1,202 @@
|
||||||
|
import torch
|
||||||
|
import numpy as np
|
||||||
|
from Go2Py.sim.gym.utils.math_utils import quat_apply_yaw, wrap_to_pi, get_scale_shift
|
||||||
|
from isaacgym.torch_utils import *
|
||||||
|
from isaacgym import gymapi
|
||||||
|
|
||||||
|
class CoRLRewards:
|
||||||
|
def __init__(self, env):
|
||||||
|
self.env = env
|
||||||
|
|
||||||
|
def load_env(self, env):
|
||||||
|
self.env = env
|
||||||
|
|
||||||
|
# ------------ reward functions----------------
|
||||||
|
def _reward_tracking_lin_vel(self):
|
||||||
|
# Tracking of linear velocity commands (xy axes)
|
||||||
|
lin_vel_error = torch.sum(torch.square(self.env.commands[:, :2] - self.env.base_lin_vel[:, :2]), dim=1)
|
||||||
|
return torch.exp(-lin_vel_error / self.env.cfg.rewards.tracking_sigma)
|
||||||
|
|
||||||
|
def _reward_tracking_ang_vel(self):
|
||||||
|
# Tracking of angular velocity commands (yaw)
|
||||||
|
ang_vel_error = torch.square(self.env.commands[:, 2] - self.env.base_ang_vel[:, 2])
|
||||||
|
return torch.exp(-ang_vel_error / self.env.cfg.rewards.tracking_sigma_yaw)
|
||||||
|
|
||||||
|
def _reward_lin_vel_z(self):
|
||||||
|
# Penalize z axis base linear velocity
|
||||||
|
return torch.square(self.env.base_lin_vel[:, 2])
|
||||||
|
|
||||||
|
def _reward_ang_vel_xy(self):
|
||||||
|
# Penalize xy axes base angular velocity
|
||||||
|
return torch.sum(torch.square(self.env.base_ang_vel[:, :2]), dim=1)
|
||||||
|
|
||||||
|
def _reward_orientation(self):
|
||||||
|
# Penalize non flat base orientation
|
||||||
|
return torch.sum(torch.square(self.env.projected_gravity[:, :2]), dim=1)
|
||||||
|
|
||||||
|
def _reward_torques(self):
|
||||||
|
# Penalize torques
|
||||||
|
return torch.sum(torch.square(self.env.torques), dim=1)
|
||||||
|
|
||||||
|
def _reward_dof_acc(self):
|
||||||
|
# Penalize dof accelerations
|
||||||
|
return torch.sum(torch.square((self.env.last_dof_vel - self.env.dof_vel) / self.env.dt), dim=1)
|
||||||
|
|
||||||
|
def _reward_action_rate(self):
|
||||||
|
# Penalize changes in actions
|
||||||
|
return torch.sum(torch.square(self.env.last_actions - self.env.actions), dim=1)
|
||||||
|
|
||||||
|
def _reward_collision(self):
|
||||||
|
# Penalize collisions on selected bodies
|
||||||
|
return torch.sum(1. * (torch.norm(self.env.contact_forces[:, self.env.penalised_contact_indices, :], dim=-1) > 0.1),
|
||||||
|
dim=1)
|
||||||
|
|
||||||
|
def _reward_dof_pos_limits(self):
|
||||||
|
# Penalize dof positions too close to the limit
|
||||||
|
out_of_limits = -(self.env.dof_pos - self.env.dof_pos_limits[:, 0]).clip(max=0.) # lower limit
|
||||||
|
out_of_limits += (self.env.dof_pos - self.env.dof_pos_limits[:, 1]).clip(min=0.)
|
||||||
|
return torch.sum(out_of_limits, dim=1)
|
||||||
|
|
||||||
|
def _reward_jump(self):
|
||||||
|
reference_heights = 0
|
||||||
|
body_height = self.env.base_pos[:, 2] - reference_heights
|
||||||
|
jump_height_target = self.env.commands[:, 3] + self.env.cfg.rewards.base_height_target
|
||||||
|
reward = - torch.square(body_height - jump_height_target)
|
||||||
|
return reward
|
||||||
|
|
||||||
|
def _reward_tracking_contacts_shaped_force(self):
|
||||||
|
foot_forces = torch.norm(self.env.contact_forces[:, self.env.feet_indices, :], dim=-1)
|
||||||
|
desired_contact = self.env.desired_contact_states
|
||||||
|
|
||||||
|
reward = 0
|
||||||
|
for i in range(4):
|
||||||
|
reward += - (1 - desired_contact[:, i]) * (
|
||||||
|
1 - torch.exp(-1 * foot_forces[:, i] ** 2 / self.env.cfg.rewards.gait_force_sigma))
|
||||||
|
return reward / 4
|
||||||
|
|
||||||
|
def _reward_tracking_contacts_shaped_vel(self):
|
||||||
|
foot_velocities = torch.norm(self.env.foot_velocities, dim=2).view(self.env.num_envs, -1)
|
||||||
|
desired_contact = self.env.desired_contact_states
|
||||||
|
reward = 0
|
||||||
|
for i in range(4):
|
||||||
|
reward += - (desired_contact[:, i] * (
|
||||||
|
1 - torch.exp(-1 * foot_velocities[:, i] ** 2 / self.env.cfg.rewards.gait_vel_sigma)))
|
||||||
|
return reward / 4
|
||||||
|
|
||||||
|
def _reward_dof_pos(self):
|
||||||
|
# Penalize dof positions
|
||||||
|
return torch.sum(torch.square(self.env.dof_pos - self.env.default_dof_pos), dim=1)
|
||||||
|
|
||||||
|
def _reward_dof_vel(self):
|
||||||
|
# Penalize dof velocities
|
||||||
|
return torch.sum(torch.square(self.env.dof_vel), dim=1)
|
||||||
|
|
||||||
|
def _reward_action_smoothness_1(self):
|
||||||
|
# Penalize changes in actions
|
||||||
|
diff = torch.square(self.env.joint_pos_target[:, :self.env.num_actuated_dof] - self.env.last_joint_pos_target[:, :self.env.num_actuated_dof])
|
||||||
|
diff = diff * (self.env.last_actions[:, :self.env.num_dof] != 0) # ignore first step
|
||||||
|
return torch.sum(diff, dim=1)
|
||||||
|
|
||||||
|
def _reward_action_smoothness_2(self):
|
||||||
|
# Penalize changes in actions
|
||||||
|
diff = torch.square(self.env.joint_pos_target[:, :self.env.num_actuated_dof] - 2 * self.env.last_joint_pos_target[:, :self.env.num_actuated_dof] + self.env.last_last_joint_pos_target[:, :self.env.num_actuated_dof])
|
||||||
|
diff = diff * (self.env.last_actions[:, :self.env.num_dof] != 0) # ignore first step
|
||||||
|
diff = diff * (self.env.last_last_actions[:, :self.env.num_dof] != 0) # ignore second step
|
||||||
|
return torch.sum(diff, dim=1)
|
||||||
|
|
||||||
|
def _reward_feet_slip(self):
|
||||||
|
contact = self.env.contact_forces[:, self.env.feet_indices, 2] > 1.
|
||||||
|
contact_filt = torch.logical_or(contact, self.env.last_contacts)
|
||||||
|
self.env.last_contacts = contact
|
||||||
|
foot_velocities = torch.square(torch.norm(self.env.foot_velocities[:, :, 0:2], dim=2).view(self.env.num_envs, -1))
|
||||||
|
rew_slip = torch.sum(contact_filt * foot_velocities, dim=1)
|
||||||
|
return rew_slip
|
||||||
|
|
||||||
|
def _reward_feet_contact_vel(self):
|
||||||
|
reference_heights = 0
|
||||||
|
near_ground = self.env.foot_positions[:, :, 2] - reference_heights < 0.03
|
||||||
|
foot_velocities = torch.square(torch.norm(self.env.foot_velocities[:, :, 0:3], dim=2).view(self.env.num_envs, -1))
|
||||||
|
rew_contact_vel = torch.sum(near_ground * foot_velocities, dim=1)
|
||||||
|
return rew_contact_vel
|
||||||
|
|
||||||
|
def _reward_feet_contact_forces(self):
|
||||||
|
# penalize high contact forces
|
||||||
|
return torch.sum((torch.norm(self.env.contact_forces[:, self.env.feet_indices, :],
|
||||||
|
dim=-1) - self.env.cfg.rewards.max_contact_force).clip(min=0.), dim=1)
|
||||||
|
|
||||||
|
def _reward_feet_clearance_cmd_linear(self):
|
||||||
|
phases = 1 - torch.abs(1.0 - torch.clip((self.env.foot_indices * 2.0) - 1.0, 0.0, 1.0) * 2.0)
|
||||||
|
foot_height = (self.env.foot_positions[:, :, 2]).view(self.env.num_envs, -1)# - reference_heights
|
||||||
|
target_height = self.env.commands[:, 9].unsqueeze(1) * phases + 0.02 # offset for foot radius 2cm
|
||||||
|
rew_foot_clearance = torch.square(target_height - foot_height) * (1 - self.env.desired_contact_states)
|
||||||
|
return torch.sum(rew_foot_clearance, dim=1)
|
||||||
|
|
||||||
|
def _reward_feet_impact_vel(self):
|
||||||
|
prev_foot_velocities = self.env.prev_foot_velocities[:, :, 2].view(self.env.num_envs, -1)
|
||||||
|
contact_states = torch.norm(self.env.contact_forces[:, self.env.feet_indices, :], dim=-1) > 1.0
|
||||||
|
|
||||||
|
rew_foot_impact_vel = contact_states * torch.square(torch.clip(prev_foot_velocities, -100, 0))
|
||||||
|
|
||||||
|
return torch.sum(rew_foot_impact_vel, dim=1)
|
||||||
|
|
||||||
|
|
||||||
|
def _reward_collision(self):
|
||||||
|
# Penalize collisions on selected bodies
|
||||||
|
return torch.sum(1. * (torch.norm(self.env.contact_forces[:, self.env.penalised_contact_indices, :], dim=-1) > 0.1),
|
||||||
|
dim=1)
|
||||||
|
|
||||||
|
def _reward_orientation_control(self):
|
||||||
|
# Penalize non flat base orientation
|
||||||
|
roll_pitch_commands = self.env.commands[:, 10:12]
|
||||||
|
quat_roll = quat_from_angle_axis(-roll_pitch_commands[:, 1],
|
||||||
|
torch.tensor([1, 0, 0], device=self.env.device, dtype=torch.float))
|
||||||
|
quat_pitch = quat_from_angle_axis(-roll_pitch_commands[:, 0],
|
||||||
|
torch.tensor([0, 1, 0], device=self.env.device, dtype=torch.float))
|
||||||
|
|
||||||
|
desired_base_quat = quat_mul(quat_roll, quat_pitch)
|
||||||
|
desired_projected_gravity = quat_rotate_inverse(desired_base_quat, self.env.gravity_vec)
|
||||||
|
|
||||||
|
return torch.sum(torch.square(self.env.projected_gravity[:, :2] - desired_projected_gravity[:, :2]), dim=1)
|
||||||
|
|
||||||
|
def _reward_raibert_heuristic(self):
|
||||||
|
cur_footsteps_translated = self.env.foot_positions - self.env.base_pos.unsqueeze(1)
|
||||||
|
footsteps_in_body_frame = torch.zeros(self.env.num_envs, 4, 3, device=self.env.device)
|
||||||
|
for i in range(4):
|
||||||
|
footsteps_in_body_frame[:, i, :] = quat_apply_yaw(quat_conjugate(self.env.base_quat),
|
||||||
|
cur_footsteps_translated[:, i, :])
|
||||||
|
|
||||||
|
# nominal positions: [FR, FL, RR, RL]
|
||||||
|
if self.env.cfg.commands.num_commands >= 13:
|
||||||
|
desired_stance_width = self.env.commands[:, 12:13]
|
||||||
|
desired_ys_nom = torch.cat([desired_stance_width / 2, -desired_stance_width / 2, desired_stance_width / 2, -desired_stance_width / 2], dim=1)
|
||||||
|
else:
|
||||||
|
desired_stance_width = 0.3
|
||||||
|
desired_ys_nom = torch.tensor([desired_stance_width / 2, -desired_stance_width / 2, desired_stance_width / 2, -desired_stance_width / 2], device=self.env.device).unsqueeze(0)
|
||||||
|
|
||||||
|
if self.env.cfg.commands.num_commands >= 14:
|
||||||
|
desired_stance_length = self.env.commands[:, 13:14]
|
||||||
|
desired_xs_nom = torch.cat([desired_stance_length / 2, desired_stance_length / 2, -desired_stance_length / 2, -desired_stance_length / 2], dim=1)
|
||||||
|
else:
|
||||||
|
desired_stance_length = 0.45
|
||||||
|
desired_xs_nom = torch.tensor([desired_stance_length / 2, desired_stance_length / 2, -desired_stance_length / 2, -desired_stance_length / 2], device=self.env.device).unsqueeze(0)
|
||||||
|
|
||||||
|
# raibert offsets
|
||||||
|
phases = torch.abs(1.0 - (self.env.foot_indices * 2.0)) * 1.0 - 0.5
|
||||||
|
frequencies = self.env.commands[:, 4]
|
||||||
|
x_vel_des = self.env.commands[:, 0:1]
|
||||||
|
yaw_vel_des = self.env.commands[:, 2:3]
|
||||||
|
y_vel_des = yaw_vel_des * desired_stance_length / 2
|
||||||
|
desired_ys_offset = phases * y_vel_des * (0.5 / frequencies.unsqueeze(1))
|
||||||
|
desired_ys_offset[:, 2:4] *= -1
|
||||||
|
desired_xs_offset = phases * x_vel_des * (0.5 / frequencies.unsqueeze(1))
|
||||||
|
|
||||||
|
desired_ys_nom = desired_ys_nom + desired_ys_offset
|
||||||
|
desired_xs_nom = desired_xs_nom + desired_xs_offset
|
||||||
|
|
||||||
|
desired_footsteps_body_frame = torch.cat((desired_xs_nom.unsqueeze(2), desired_ys_nom.unsqueeze(2)), dim=2)
|
||||||
|
|
||||||
|
err_raibert_heuristic = torch.abs(desired_footsteps_body_frame - footsteps_in_body_frame[:, :, 0:2])
|
||||||
|
|
||||||
|
reward = torch.sum(torch.square(err_raibert_heuristic), dim=(1, 2))
|
||||||
|
|
||||||
|
return reward
|
||||||
|
|
@ -0,0 +1,72 @@
|
||||||
|
import isaacgym
|
||||||
|
assert isaacgym
|
||||||
|
import torch
|
||||||
|
import gym
|
||||||
|
|
||||||
|
class HistoryWrapper(gym.Wrapper):
|
||||||
|
def __init__(self, env):
|
||||||
|
super().__init__(env)
|
||||||
|
self.env = env
|
||||||
|
|
||||||
|
self.obs_history_length = self.env.cfg.env.num_observation_history
|
||||||
|
|
||||||
|
self.num_obs_history = self.obs_history_length * self.num_obs
|
||||||
|
self.obs_history = torch.zeros(self.env.num_envs, self.num_obs_history, dtype=torch.float,
|
||||||
|
device=self.env.device, requires_grad=False)
|
||||||
|
self.num_privileged_obs = self.num_privileged_obs
|
||||||
|
|
||||||
|
def step(self, action):
|
||||||
|
# privileged information and observation history are stored in info
|
||||||
|
obs, rew, done, info = self.env.step(action)
|
||||||
|
privileged_obs = info["privileged_obs"]
|
||||||
|
|
||||||
|
self.obs_history = torch.cat((self.obs_history[:, self.env.num_obs:], obs), dim=-1)
|
||||||
|
return {'obs': obs, 'privileged_obs': privileged_obs, 'obs_history': self.obs_history}, rew, done, info
|
||||||
|
|
||||||
|
def get_observations(self):
|
||||||
|
obs = self.env.get_observations()
|
||||||
|
privileged_obs = self.env.get_privileged_observations()
|
||||||
|
self.obs_history = torch.cat((self.obs_history[:, self.env.num_obs:], obs), dim=-1)
|
||||||
|
return {'obs': obs, 'privileged_obs': privileged_obs, 'obs_history': self.obs_history}
|
||||||
|
|
||||||
|
def reset_idx(self, env_ids): # it might be a problem that this isn't getting called!!
|
||||||
|
ret = super().reset_idx(env_ids)
|
||||||
|
self.obs_history[env_ids, :] = 0
|
||||||
|
return ret
|
||||||
|
|
||||||
|
def reset(self):
|
||||||
|
ret = super().reset()
|
||||||
|
privileged_obs = self.env.get_privileged_observations()
|
||||||
|
self.obs_history[:, :] = 0
|
||||||
|
return {"obs": ret, "privileged_obs": privileged_obs, "obs_history": self.obs_history}
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
from tqdm import trange
|
||||||
|
import matplotlib.pyplot as plt
|
||||||
|
|
||||||
|
import ml_logger as logger
|
||||||
|
|
||||||
|
from gym_learn.ppo import Runner
|
||||||
|
from Go2Py.sim.gym.envs.wrappers.history_wrapper import HistoryWrapper
|
||||||
|
from gym_learn.ppo.actor_critic import AC_Args
|
||||||
|
|
||||||
|
from Go2Py.sim.gym.envs.base.legged_robot_config import Cfg
|
||||||
|
from Go2Py.sim.gym.envs.mini_cheetah.mini_cheetah_config import config_mini_cheetah
|
||||||
|
config_mini_cheetah(Cfg)
|
||||||
|
|
||||||
|
test_env = gym.make("VelocityTrackingEasyEnv-v0", cfg=Cfg)
|
||||||
|
env = HistoryWrapper(test_env)
|
||||||
|
|
||||||
|
env.reset()
|
||||||
|
action = torch.zeros(test_env.num_envs, 12)
|
||||||
|
for i in trange(3):
|
||||||
|
obs, rew, done, info = env.step(action)
|
||||||
|
print(obs.keys())
|
||||||
|
print(f"obs: {obs['obs']}")
|
||||||
|
print(f"privileged obs: {obs['privileged_obs']}")
|
||||||
|
print(f"obs_history: {obs['obs_history']}")
|
||||||
|
|
||||||
|
img = env.render('rgb_array')
|
||||||
|
plt.imshow(img)
|
||||||
|
plt.show()
|
||||||
|
|
@ -0,0 +1,2 @@
|
||||||
|
from .math_utils import *
|
||||||
|
from .terrain import Terrain
|
||||||
|
|
@ -0,0 +1,38 @@
|
||||||
|
# License: see [LICENSE, LICENSES/legged_gym/LICENSE]
|
||||||
|
|
||||||
|
from typing import Tuple
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
import torch
|
||||||
|
from isaacgym.torch_utils import quat_apply, normalize
|
||||||
|
from torch import Tensor
|
||||||
|
|
||||||
|
|
||||||
|
# @ torch.jit.script
|
||||||
|
def quat_apply_yaw(quat, vec):
|
||||||
|
quat_yaw = quat.clone().view(-1, 4)
|
||||||
|
quat_yaw[:, :2] = 0.
|
||||||
|
quat_yaw = normalize(quat_yaw)
|
||||||
|
return quat_apply(quat_yaw, vec)
|
||||||
|
|
||||||
|
|
||||||
|
# @ torch.jit.script
|
||||||
|
def wrap_to_pi(angles):
|
||||||
|
angles %= 2 * np.pi
|
||||||
|
angles -= 2 * np.pi * (angles > np.pi)
|
||||||
|
return angles
|
||||||
|
|
||||||
|
|
||||||
|
# @ torch.jit.script
|
||||||
|
def torch_rand_sqrt_float(lower, upper, shape, device):
|
||||||
|
# type: (float, float, Tuple[int, int], str) -> Tensor
|
||||||
|
r = 2 * torch.rand(*shape, device=device) - 1
|
||||||
|
r = torch.where(r < 0., -torch.sqrt(-r), torch.sqrt(r))
|
||||||
|
r = (r + 1.) / 2.
|
||||||
|
return (upper - lower) * r + lower
|
||||||
|
|
||||||
|
|
||||||
|
def get_scale_shift(range):
|
||||||
|
scale = 2. / (range[1] - range[0])
|
||||||
|
shift = (range[1] + range[0]) / 2.
|
||||||
|
return scale, shift
|
||||||
|
|
@ -0,0 +1,180 @@
|
||||||
|
# License: see [LICENSE, LICENSES/legged_gym/LICENSE]
|
||||||
|
|
||||||
|
import math
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
from isaacgym import terrain_utils
|
||||||
|
from numpy.random import choice
|
||||||
|
|
||||||
|
from Go2Py.sim.gym.envs.base.legged_robot_config import Cfg
|
||||||
|
|
||||||
|
|
||||||
|
class Terrain:
|
||||||
|
def __init__(self, cfg: Cfg.terrain, num_robots, eval_cfg=None, num_eval_robots=0) -> None:
|
||||||
|
|
||||||
|
self.cfg = cfg
|
||||||
|
self.eval_cfg = eval_cfg
|
||||||
|
self.num_robots = num_robots
|
||||||
|
self.type = cfg.mesh_type
|
||||||
|
if self.type in ["none", 'plane']:
|
||||||
|
return
|
||||||
|
self.train_rows, self.train_cols, self.eval_rows, self.eval_cols = self.load_cfgs()
|
||||||
|
self.tot_rows = len(self.train_rows) + len(self.eval_rows)
|
||||||
|
self.tot_cols = max(len(self.train_cols), len(self.eval_cols))
|
||||||
|
self.cfg.env_length = cfg.terrain_length
|
||||||
|
self.cfg.env_width = cfg.terrain_width
|
||||||
|
|
||||||
|
self.height_field_raw = np.zeros((self.tot_rows, self.tot_cols), dtype=np.int16)
|
||||||
|
|
||||||
|
self.initialize_terrains()
|
||||||
|
|
||||||
|
self.heightsamples = self.height_field_raw
|
||||||
|
if self.type == "trimesh":
|
||||||
|
self.vertices, self.triangles = terrain_utils.convert_heightfield_to_trimesh(self.height_field_raw,
|
||||||
|
self.cfg.horizontal_scale,
|
||||||
|
self.cfg.vertical_scale,
|
||||||
|
self.cfg.slope_treshold)
|
||||||
|
|
||||||
|
def load_cfgs(self):
|
||||||
|
self._load_cfg(self.cfg)
|
||||||
|
self.cfg.row_indices = np.arange(0, self.cfg.tot_rows)
|
||||||
|
self.cfg.col_indices = np.arange(0, self.cfg.tot_cols)
|
||||||
|
self.cfg.x_offset = 0
|
||||||
|
self.cfg.rows_offset = 0
|
||||||
|
if self.eval_cfg is None:
|
||||||
|
return self.cfg.row_indices, self.cfg.col_indices, [], []
|
||||||
|
else:
|
||||||
|
self._load_cfg(self.eval_cfg)
|
||||||
|
self.eval_cfg.row_indices = np.arange(self.cfg.tot_rows, self.cfg.tot_rows + self.eval_cfg.tot_rows)
|
||||||
|
self.eval_cfg.col_indices = np.arange(0, self.eval_cfg.tot_cols)
|
||||||
|
self.eval_cfg.x_offset = self.cfg.tot_rows
|
||||||
|
self.eval_cfg.rows_offset = self.cfg.num_rows
|
||||||
|
return self.cfg.row_indices, self.cfg.col_indices, self.eval_cfg.row_indices, self.eval_cfg.col_indices
|
||||||
|
|
||||||
|
def _load_cfg(self, cfg):
|
||||||
|
cfg.proportions = [np.sum(cfg.terrain_proportions[:i + 1]) for i in range(len(cfg.terrain_proportions))]
|
||||||
|
|
||||||
|
cfg.num_sub_terrains = cfg.num_rows * cfg.num_cols
|
||||||
|
cfg.env_origins = np.zeros((cfg.num_rows, cfg.num_cols, 3))
|
||||||
|
|
||||||
|
cfg.width_per_env_pixels = int(cfg.terrain_length / cfg.horizontal_scale)
|
||||||
|
cfg.length_per_env_pixels = int(cfg.terrain_width / cfg.horizontal_scale)
|
||||||
|
|
||||||
|
cfg.border = int(cfg.border_size / cfg.horizontal_scale)
|
||||||
|
cfg.tot_cols = int(cfg.num_cols * cfg.width_per_env_pixels) + 2 * cfg.border
|
||||||
|
cfg.tot_rows = int(cfg.num_rows * cfg.length_per_env_pixels) + 2 * cfg.border
|
||||||
|
|
||||||
|
def initialize_terrains(self):
|
||||||
|
self._initialize_terrain(self.cfg)
|
||||||
|
if self.eval_cfg is not None:
|
||||||
|
self._initialize_terrain(self.eval_cfg)
|
||||||
|
|
||||||
|
def _initialize_terrain(self, cfg):
|
||||||
|
if cfg.curriculum:
|
||||||
|
self.curriculum(cfg)
|
||||||
|
elif cfg.selected:
|
||||||
|
self.selected_terrain(cfg)
|
||||||
|
else:
|
||||||
|
self.randomized_terrain(cfg)
|
||||||
|
|
||||||
|
def randomized_terrain(self, cfg):
|
||||||
|
for k in range(cfg.num_sub_terrains):
|
||||||
|
# Env coordinates in the world
|
||||||
|
(i, j) = np.unravel_index(k, (cfg.num_rows, cfg.num_cols))
|
||||||
|
|
||||||
|
choice = np.random.uniform(0, 1)
|
||||||
|
difficulty = np.random.choice([0.5, 0.75, 0.9])
|
||||||
|
terrain = self.make_terrain(cfg, choice, difficulty, cfg.proportions)
|
||||||
|
self.add_terrain_to_map(cfg, terrain, i, j)
|
||||||
|
|
||||||
|
def curriculum(self, cfg):
|
||||||
|
for j in range(cfg.num_cols):
|
||||||
|
for i in range(cfg.num_rows):
|
||||||
|
difficulty = i / cfg.num_rows * cfg.difficulty_scale
|
||||||
|
choice = j / cfg.num_cols + 0.001
|
||||||
|
|
||||||
|
terrain = self.make_terrain(cfg, choice, difficulty, cfg.proportions)
|
||||||
|
self.add_terrain_to_map(cfg, terrain, i, j)
|
||||||
|
|
||||||
|
def selected_terrain(self, cfg):
|
||||||
|
terrain_type = cfg.terrain_kwargs.pop('type')
|
||||||
|
for k in range(cfg.num_sub_terrains):
|
||||||
|
# Env coordinates in the world
|
||||||
|
(i, j) = np.unravel_index(k, (cfg.num_rows, cfg.num_cols))
|
||||||
|
|
||||||
|
terrain = terrain_utils.SubTerrain("terrain",
|
||||||
|
width=cfg.width_per_env_pixels,
|
||||||
|
length=cfg.width_per_env_pixels,
|
||||||
|
vertical_scale=cfg.vertical_scale,
|
||||||
|
horizontal_scale=cfg.horizontal_scale)
|
||||||
|
|
||||||
|
eval(terrain_type)(terrain, **cfg.terrain_kwargs.terrain_kwargs)
|
||||||
|
self.add_terrain_to_map(cfg, terrain, i, j)
|
||||||
|
|
||||||
|
def make_terrain(self, cfg, choice, difficulty, proportions):
|
||||||
|
terrain = terrain_utils.SubTerrain("terrain",
|
||||||
|
width=cfg.width_per_env_pixels,
|
||||||
|
length=cfg.width_per_env_pixels,
|
||||||
|
vertical_scale=cfg.vertical_scale,
|
||||||
|
horizontal_scale=cfg.horizontal_scale)
|
||||||
|
slope = difficulty * 0.4
|
||||||
|
step_height = 0.05 + 0.18 * difficulty
|
||||||
|
discrete_obstacles_height = 0.05 + difficulty * (cfg.max_platform_height - 0.05)
|
||||||
|
stepping_stones_size = 1.5 * (1.05 - difficulty)
|
||||||
|
stone_distance = 0.05 if difficulty == 0 else 0.1
|
||||||
|
if choice < proportions[0]:
|
||||||
|
if choice < proportions[0] / 2:
|
||||||
|
slope *= -1
|
||||||
|
terrain_utils.pyramid_sloped_terrain(terrain, slope=slope, platform_size=3.)
|
||||||
|
elif choice < proportions[1]:
|
||||||
|
terrain_utils.pyramid_sloped_terrain(terrain, slope=slope, platform_size=3.)
|
||||||
|
terrain_utils.random_uniform_terrain(terrain, min_height=-0.05, max_height=0.05,
|
||||||
|
step=self.cfg.terrain_smoothness, downsampled_scale=0.2)
|
||||||
|
elif choice < proportions[3]:
|
||||||
|
if choice < proportions[2]:
|
||||||
|
step_height *= -1
|
||||||
|
terrain_utils.pyramid_stairs_terrain(terrain, step_width=0.31, step_height=step_height, platform_size=3.)
|
||||||
|
elif choice < proportions[4]:
|
||||||
|
num_rectangles = 20
|
||||||
|
rectangle_min_size = 1.
|
||||||
|
rectangle_max_size = 2.
|
||||||
|
terrain_utils.discrete_obstacles_terrain(terrain, discrete_obstacles_height, rectangle_min_size,
|
||||||
|
rectangle_max_size, num_rectangles, platform_size=3.)
|
||||||
|
elif choice < proportions[5]:
|
||||||
|
terrain_utils.stepping_stones_terrain(terrain, stone_size=stepping_stones_size,
|
||||||
|
stone_distance=stone_distance, max_height=0., platform_size=4.)
|
||||||
|
elif choice < proportions[6]:
|
||||||
|
pass
|
||||||
|
elif choice < proportions[7]:
|
||||||
|
pass
|
||||||
|
elif choice < proportions[8]:
|
||||||
|
terrain_utils.random_uniform_terrain(terrain, min_height=-cfg.terrain_noise_magnitude,
|
||||||
|
max_height=cfg.terrain_noise_magnitude, step=0.005,
|
||||||
|
downsampled_scale=0.2)
|
||||||
|
elif choice < proportions[9]:
|
||||||
|
terrain_utils.random_uniform_terrain(terrain, min_height=-0.05, max_height=0.05,
|
||||||
|
step=self.cfg.terrain_smoothness, downsampled_scale=0.2)
|
||||||
|
terrain.height_field_raw[0:terrain.length // 2, :] = 0
|
||||||
|
|
||||||
|
return terrain
|
||||||
|
|
||||||
|
def add_terrain_to_map(self, cfg, terrain, row, col):
|
||||||
|
i = row
|
||||||
|
j = col
|
||||||
|
# map coordinate system
|
||||||
|
start_x = cfg.border + i * cfg.length_per_env_pixels + cfg.x_offset
|
||||||
|
end_x = cfg.border + (i + 1) * cfg.length_per_env_pixels + cfg.x_offset
|
||||||
|
start_y = cfg.border + j * cfg.width_per_env_pixels
|
||||||
|
end_y = cfg.border + (j + 1) * cfg.width_per_env_pixels
|
||||||
|
self.height_field_raw[start_x: end_x, start_y:end_y] = terrain.height_field_raw
|
||||||
|
|
||||||
|
env_origin_x = (i + 0.5) * cfg.terrain_length + cfg.x_offset * terrain.horizontal_scale
|
||||||
|
env_origin_y = (j + 0.5) * cfg.terrain_width
|
||||||
|
x1 = int((cfg.terrain_length / 2. - 1) / terrain.horizontal_scale) + cfg.x_offset
|
||||||
|
x2 = int((cfg.terrain_length / 2. + 1) / terrain.horizontal_scale) + cfg.x_offset
|
||||||
|
y1 = int((cfg.terrain_width / 2. - 1) / terrain.horizontal_scale)
|
||||||
|
y2 = int((cfg.terrain_width / 2. + 1) / terrain.horizontal_scale)
|
||||||
|
env_origin_z = np.max(self.height_field_raw[start_x: end_x, start_y:end_y]) * terrain.vertical_scale
|
||||||
|
|
||||||
|
cfg.env_origins[i, j] = [env_origin_x, env_origin_y, env_origin_z]
|
||||||
|
|
||||||
|
|
@ -0,0 +1,206 @@
|
||||||
|
import isaacgym
|
||||||
|
|
||||||
|
assert isaacgym
|
||||||
|
import matplotlib.pyplot as plt
|
||||||
|
import torch
|
||||||
|
from tqdm import trange
|
||||||
|
|
||||||
|
from Go2Py.sim.gym.envs import *
|
||||||
|
from Go2Py.sim.gym.envs.base.legged_robot_config import Cfg
|
||||||
|
from Go2Py.sim.gym.envs.go2.go2_config import config_go2
|
||||||
|
from Go2Py.sim.gym.envs.go2.velocity_tracking import VelocityTrackingEasyEnv
|
||||||
|
|
||||||
|
|
||||||
|
def run_env(render=False, headless=False):
|
||||||
|
# prepare environment
|
||||||
|
config_go2(Cfg)
|
||||||
|
|
||||||
|
Cfg.commands.num_lin_vel_bins = 30
|
||||||
|
Cfg.commands.num_ang_vel_bins = 30
|
||||||
|
Cfg.curriculum_thresholds.tracking_ang_vel = 0.7
|
||||||
|
Cfg.curriculum_thresholds.tracking_lin_vel = 0.8
|
||||||
|
Cfg.curriculum_thresholds.tracking_contacts_shaped_vel = 0.9
|
||||||
|
Cfg.curriculum_thresholds.tracking_contacts_shaped_force = 0.9
|
||||||
|
|
||||||
|
Cfg.commands.distributional_commands = True
|
||||||
|
|
||||||
|
Cfg.env.priv_observe_motion = False
|
||||||
|
Cfg.env.priv_observe_gravity_transformed_motion = True
|
||||||
|
Cfg.domain_rand.randomize_friction_indep = False
|
||||||
|
Cfg.env.priv_observe_friction_indep = False
|
||||||
|
Cfg.domain_rand.randomize_friction = True
|
||||||
|
Cfg.env.priv_observe_friction = False
|
||||||
|
Cfg.domain_rand.friction_range = [0.0, 0.0]
|
||||||
|
Cfg.domain_rand.randomize_restitution = True
|
||||||
|
Cfg.env.priv_observe_restitution = False
|
||||||
|
Cfg.domain_rand.restitution_range = [0.0, 1.0]
|
||||||
|
Cfg.domain_rand.randomize_base_mass = True
|
||||||
|
Cfg.env.priv_observe_base_mass = False
|
||||||
|
Cfg.domain_rand.added_mass_range = [-1.0, 3.0]
|
||||||
|
Cfg.domain_rand.randomize_gravity = True
|
||||||
|
Cfg.domain_rand.gravity_range = [-1.0, 1.0]
|
||||||
|
Cfg.domain_rand.gravity_rand_interval_s = 2.0
|
||||||
|
Cfg.domain_rand.gravity_impulse_duration = 0.5
|
||||||
|
Cfg.env.priv_observe_gravity = True
|
||||||
|
Cfg.domain_rand.randomize_com_displacement = False
|
||||||
|
Cfg.domain_rand.com_displacement_range = [-0.15, 0.15]
|
||||||
|
Cfg.env.priv_observe_com_displacement = False
|
||||||
|
Cfg.domain_rand.randomize_ground_friction = True
|
||||||
|
Cfg.env.priv_observe_ground_friction = False
|
||||||
|
Cfg.env.priv_observe_ground_friction_per_foot = False
|
||||||
|
Cfg.domain_rand.ground_friction_range = [0.3, 2.0]
|
||||||
|
Cfg.domain_rand.randomize_motor_strength = True
|
||||||
|
Cfg.domain_rand.motor_strength_range = [0.9, 1.1]
|
||||||
|
Cfg.env.priv_observe_motor_strength = False
|
||||||
|
Cfg.domain_rand.randomize_motor_offset = True
|
||||||
|
Cfg.domain_rand.motor_offset_range = [-0.02, 0.02]
|
||||||
|
Cfg.env.priv_observe_motor_offset = False
|
||||||
|
Cfg.domain_rand.push_robots = False
|
||||||
|
Cfg.domain_rand.randomize_Kp_factor = False
|
||||||
|
Cfg.env.priv_observe_Kp_factor = False
|
||||||
|
Cfg.domain_rand.randomize_Kd_factor = False
|
||||||
|
Cfg.env.priv_observe_Kd_factor = False
|
||||||
|
Cfg.env.priv_observe_body_velocity = False
|
||||||
|
Cfg.env.priv_observe_body_height = False
|
||||||
|
Cfg.env.priv_observe_desired_contact_states = False
|
||||||
|
Cfg.env.priv_observe_contact_forces = False
|
||||||
|
Cfg.env.priv_observe_foot_displacement = False
|
||||||
|
|
||||||
|
Cfg.env.num_privileged_obs = 3
|
||||||
|
Cfg.env.num_observation_history = 30
|
||||||
|
Cfg.reward_scales.feet_contact_forces = 0.0
|
||||||
|
|
||||||
|
Cfg.domain_rand.rand_interval_s = 4
|
||||||
|
Cfg.commands.num_commands = 15
|
||||||
|
Cfg.env.observe_two_prev_actions = True
|
||||||
|
Cfg.env.observe_yaw = True
|
||||||
|
Cfg.env.num_observations = 71
|
||||||
|
Cfg.env.num_scalar_observations = 71
|
||||||
|
Cfg.env.observe_gait_commands = True
|
||||||
|
Cfg.env.observe_timing_parameter = False
|
||||||
|
Cfg.env.observe_clock_inputs = True
|
||||||
|
|
||||||
|
Cfg.domain_rand.tile_height_range = [-0.0, 0.0]
|
||||||
|
Cfg.domain_rand.tile_height_curriculum = False
|
||||||
|
Cfg.domain_rand.tile_height_update_interval = 1000, 3000
|
||||||
|
Cfg.domain_rand.tile_height_curriculum_step = 0.01
|
||||||
|
Cfg.terrain.border_size = 0.0
|
||||||
|
|
||||||
|
Cfg.commands.resampling_time = 10
|
||||||
|
|
||||||
|
Cfg.reward_scales.feet_slip = -0.04
|
||||||
|
Cfg.reward_scales.action_smoothness_1 = -0.1
|
||||||
|
Cfg.reward_scales.action_smoothness_2 = -0.1
|
||||||
|
Cfg.reward_scales.dof_vel = -1e-4
|
||||||
|
Cfg.reward_scales.dof_pos = -0.05
|
||||||
|
Cfg.reward_scales.jump = 10.0
|
||||||
|
Cfg.reward_scales.base_height = 0.0
|
||||||
|
Cfg.rewards.base_height_target = 0.30
|
||||||
|
Cfg.reward_scales.estimation_bonus = 0.0
|
||||||
|
|
||||||
|
Cfg.reward_scales.feet_impact_vel = -0.0
|
||||||
|
|
||||||
|
# rewards.footswing_height = 0.09
|
||||||
|
Cfg.reward_scales.feet_clearance = -0.0
|
||||||
|
Cfg.reward_scales.feet_clearance_cmd = -15.
|
||||||
|
|
||||||
|
# reward_scales.feet_contact_forces = -0.01
|
||||||
|
|
||||||
|
Cfg.rewards.reward_container_name = "CoRLRewards"
|
||||||
|
Cfg.rewards.only_positive_rewards = False
|
||||||
|
Cfg.rewards.only_positive_rewards_ji22_style = True
|
||||||
|
Cfg.rewards.sigma_rew_neg = 0.02
|
||||||
|
|
||||||
|
Cfg.reward_scales.hop_symmetry = 0.0
|
||||||
|
Cfg.rewards.kappa_gait_probs = 0.07
|
||||||
|
Cfg.rewards.gait_force_sigma = 100.
|
||||||
|
Cfg.rewards.gait_vel_sigma = 10.
|
||||||
|
|
||||||
|
Cfg.reward_scales.tracking_contacts_shaped_force = 4.0
|
||||||
|
Cfg.reward_scales.tracking_contacts_shaped_vel = 4.0
|
||||||
|
|
||||||
|
Cfg.reward_scales.collision = -5.0
|
||||||
|
|
||||||
|
Cfg.commands.lin_vel_x = [-1.0, 1.0]
|
||||||
|
Cfg.commands.lin_vel_y = [-0.6, 0.6]
|
||||||
|
Cfg.commands.ang_vel_yaw = [-1.0, 1.0]
|
||||||
|
Cfg.commands.body_height_cmd = [-0.25, 0.15]
|
||||||
|
Cfg.commands.gait_frequency_cmd_range = [1.5, 4.0]
|
||||||
|
Cfg.commands.gait_phase_cmd_range = [0.0, 1.0]
|
||||||
|
Cfg.commands.gait_offset_cmd_range = [0.0, 1.0]
|
||||||
|
Cfg.commands.gait_bound_cmd_range = [0.0, 1.0]
|
||||||
|
Cfg.commands.gait_duration_cmd_range = [0.5, 0.5]
|
||||||
|
Cfg.commands.footswing_height_range = [0.03, 0.25]
|
||||||
|
|
||||||
|
Cfg.reward_scales.lin_vel_z = -0.02
|
||||||
|
Cfg.reward_scales.ang_vel_xy = -0.001
|
||||||
|
Cfg.reward_scales.base_height = 0.0
|
||||||
|
Cfg.reward_scales.feet_air_time = 0.0
|
||||||
|
|
||||||
|
Cfg.commands.limit_vel_x = [-5.0, 5.0]
|
||||||
|
Cfg.commands.limit_vel_y = [-0.6, 0.6]
|
||||||
|
Cfg.commands.limit_vel_yaw = [-5.0, 5.0]
|
||||||
|
Cfg.commands.limit_body_height = [-0.25, 0.15]
|
||||||
|
Cfg.commands.limit_gait_frequency = [1.5, 4.0]
|
||||||
|
Cfg.commands.limit_gait_phase = [0.0, 1.0]
|
||||||
|
Cfg.commands.limit_gait_offset = [0.0, 1.0]
|
||||||
|
Cfg.commands.limit_gait_bound = [0.0, 1.0]
|
||||||
|
Cfg.commands.limit_gait_duration = [0.5, 0.5]
|
||||||
|
Cfg.commands.limit_footswing_height = [0.03, 0.25]
|
||||||
|
|
||||||
|
Cfg.commands.num_bins_vel_x = 21
|
||||||
|
Cfg.commands.num_bins_vel_y = 1
|
||||||
|
Cfg.commands.num_bins_vel_yaw = 21
|
||||||
|
Cfg.commands.num_bins_body_height = 1
|
||||||
|
Cfg.commands.num_bins_gait_frequency = 1
|
||||||
|
Cfg.commands.num_bins_gait_phase = 1
|
||||||
|
Cfg.commands.num_bins_gait_offset = 1
|
||||||
|
Cfg.commands.num_bins_gait_bound = 1
|
||||||
|
Cfg.commands.num_bins_gait_duration = 1
|
||||||
|
Cfg.commands.num_bins_footswing_height = 1
|
||||||
|
|
||||||
|
Cfg.normalization.friction_range = [0, 1]
|
||||||
|
Cfg.normalization.ground_friction_range = [0, 1]
|
||||||
|
|
||||||
|
Cfg.commands.exclusive_phase_offset = False
|
||||||
|
Cfg.commands.pacing_offset = False
|
||||||
|
Cfg.commands.binary_phases = True
|
||||||
|
Cfg.commands.gaitwise_curricula = True
|
||||||
|
|
||||||
|
# 5 times per second
|
||||||
|
|
||||||
|
Cfg.env.num_envs = 3
|
||||||
|
Cfg.domain_rand.push_interval_s = 1
|
||||||
|
Cfg.terrain.num_rows = 3
|
||||||
|
Cfg.terrain.num_cols = 5
|
||||||
|
Cfg.terrain.border_size = 0
|
||||||
|
Cfg.domain_rand.randomize_friction = True
|
||||||
|
Cfg.domain_rand.friction_range = [1.0, 1.01]
|
||||||
|
Cfg.domain_rand.randomize_base_mass = True
|
||||||
|
Cfg.domain_rand.added_mass_range = [0., 6.]
|
||||||
|
Cfg.terrain.terrain_noise_magnitude = 0.0
|
||||||
|
# Cfg.asset.fix_base_link = True
|
||||||
|
|
||||||
|
Cfg.domain_rand.lag_timesteps = 6
|
||||||
|
Cfg.domain_rand.randomize_lag_timesteps = True
|
||||||
|
Cfg.control.control_type = "P"
|
||||||
|
|
||||||
|
env = VelocityTrackingEasyEnv(sim_device='cuda:0', headless=False, cfg=Cfg)
|
||||||
|
env.reset()
|
||||||
|
|
||||||
|
if render and headless:
|
||||||
|
img = env.render(mode="rgb_array")
|
||||||
|
plt.imshow(img)
|
||||||
|
plt.show()
|
||||||
|
print("Show the first frame and exit.")
|
||||||
|
exit()
|
||||||
|
|
||||||
|
for i in trange(1000, desc="Running"):
|
||||||
|
actions = 0. * torch.ones(env.num_envs, env.num_actions, device=env.device)
|
||||||
|
obs, rew, done, info = env.step(actions)
|
||||||
|
# breakpoint()
|
||||||
|
print("Done")
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == '__main__':
|
||||||
|
run_env(render=True, headless=False)
|
||||||
|
|
@ -0,0 +1,185 @@
|
||||||
|
name: walk_these_ways
|
||||||
|
channels:
|
||||||
|
- pytorch
|
||||||
|
- nvidia
|
||||||
|
- defaults
|
||||||
|
dependencies:
|
||||||
|
- _libgcc_mutex=0.1=main
|
||||||
|
- _openmp_mutex=5.1=1_gnu
|
||||||
|
- blas=1.0=mkl
|
||||||
|
- brotli-python=1.0.9=py38h6a678d5_7
|
||||||
|
- bzip2=1.0.8=h7b6447c_0
|
||||||
|
- ca-certificates=2023.12.12=h06a4308_0
|
||||||
|
- certifi=2024.2.2=py38h06a4308_0
|
||||||
|
- charset-normalizer=2.0.4=pyhd3eb1b0_0
|
||||||
|
- cuda-cudart=11.8.89=0
|
||||||
|
- cuda-cupti=11.8.87=0
|
||||||
|
- cuda-libraries=11.8.0=0
|
||||||
|
- cuda-nvrtc=11.8.89=0
|
||||||
|
- cuda-nvtx=11.8.86=0
|
||||||
|
- cuda-runtime=11.8.0=0
|
||||||
|
- ffmpeg=4.3=hf484d3e_0
|
||||||
|
- filelock=3.13.1=py38h06a4308_0
|
||||||
|
- freetype=2.12.1=h4a9f257_0
|
||||||
|
- gmp=6.2.1=h295c915_3
|
||||||
|
- gmpy2=2.1.2=py38heeb90bb_0
|
||||||
|
- gnutls=3.6.15=he1e5248_0
|
||||||
|
- idna=3.4=py38h06a4308_0
|
||||||
|
- intel-openmp=2023.1.0=hdb19cb5_46306
|
||||||
|
- jinja2=3.1.3=py38h06a4308_0
|
||||||
|
- jpeg=9e=h5eee18b_1
|
||||||
|
- lame=3.100=h7b6447c_0
|
||||||
|
- lcms2=2.12=h3be6417_0
|
||||||
|
- ld_impl_linux-64=2.38=h1181459_1
|
||||||
|
- lerc=3.0=h295c915_0
|
||||||
|
- libcublas=11.11.3.6=0
|
||||||
|
- libcufft=10.9.0.58=0
|
||||||
|
- libcufile=1.8.1.2=0
|
||||||
|
- libcurand=10.3.4.107=0
|
||||||
|
- libcusolver=11.4.1.48=0
|
||||||
|
- libcusparse=11.7.5.86=0
|
||||||
|
- libdeflate=1.17=h5eee18b_1
|
||||||
|
- libffi=3.4.4=h6a678d5_0
|
||||||
|
- libgcc-ng=11.2.0=h1234567_1
|
||||||
|
- libgomp=11.2.0=h1234567_1
|
||||||
|
- libiconv=1.16=h7f8727e_2
|
||||||
|
- libidn2=2.3.4=h5eee18b_0
|
||||||
|
- libjpeg-turbo=2.0.0=h9bf148f_0
|
||||||
|
- libnpp=11.8.0.86=0
|
||||||
|
- libnvjpeg=11.9.0.86=0
|
||||||
|
- libpng=1.6.39=h5eee18b_0
|
||||||
|
- libstdcxx-ng=11.2.0=h1234567_1
|
||||||
|
- libtasn1=4.19.0=h5eee18b_0
|
||||||
|
- libtiff=4.5.1=h6a678d5_0
|
||||||
|
- libunistring=0.9.10=h27cfd23_0
|
||||||
|
- libwebp-base=1.3.2=h5eee18b_0
|
||||||
|
- llvm-openmp=14.0.6=h9e868ea_0
|
||||||
|
- lz4-c=1.9.4=h6a678d5_0
|
||||||
|
- markupsafe=2.1.3=py38h5eee18b_0
|
||||||
|
- mkl=2023.1.0=h213fc3f_46344
|
||||||
|
- mkl-service=2.4.0=py38h5eee18b_1
|
||||||
|
- mkl_fft=1.3.8=py38h5eee18b_0
|
||||||
|
- mkl_random=1.2.4=py38hdb19cb5_0
|
||||||
|
- mpc=1.1.0=h10f8cd9_1
|
||||||
|
- mpfr=4.0.2=hb69a4c5_1
|
||||||
|
- mpmath=1.3.0=py38h06a4308_0
|
||||||
|
- ncurses=6.4=h6a678d5_0
|
||||||
|
- nettle=3.7.3=hbbd107a_1
|
||||||
|
- networkx=3.1=py38h06a4308_0
|
||||||
|
- numpy-base=1.24.3=py38h060ed82_1
|
||||||
|
- openh264=2.1.1=h4ff587b_0
|
||||||
|
- openjpeg=2.4.0=h3ad879b_0
|
||||||
|
- openssl=3.0.13=h7f8727e_0
|
||||||
|
- pillow=10.2.0=py38h5eee18b_0
|
||||||
|
- pip=23.3.1=py38h06a4308_0
|
||||||
|
- pysocks=1.7.1=py38h06a4308_0
|
||||||
|
- python=3.8.18=h955ad1f_0
|
||||||
|
- pytorch=2.2.0=py3.8_cuda11.8_cudnn8.7.0_0
|
||||||
|
- pytorch-cuda=11.8=h7e8668a_5
|
||||||
|
- pytorch-mutex=1.0=cuda
|
||||||
|
- pyyaml=6.0.1=py38h5eee18b_0
|
||||||
|
- readline=8.2=h5eee18b_0
|
||||||
|
- requests=2.31.0=py38h06a4308_1
|
||||||
|
- setuptools=68.2.2=py38h06a4308_0
|
||||||
|
- sqlite=3.41.2=h5eee18b_0
|
||||||
|
- sympy=1.12=py38h06a4308_0
|
||||||
|
- tbb=2021.8.0=hdb19cb5_0
|
||||||
|
- tk=8.6.12=h1ccaba5_0
|
||||||
|
- torchaudio=2.2.0=py38_cu118
|
||||||
|
- torchtriton=2.2.0=py38
|
||||||
|
- torchvision=0.17.0=py38_cu118
|
||||||
|
- typing_extensions=4.9.0=py38h06a4308_1
|
||||||
|
- wheel=0.41.2=py38h06a4308_0
|
||||||
|
- xz=5.4.5=h5eee18b_0
|
||||||
|
- yaml=0.2.5=h7b6447c_0
|
||||||
|
- zlib=1.2.13=h5eee18b_0
|
||||||
|
- zstd=1.5.5=hc292b87_0
|
||||||
|
- pip:
|
||||||
|
- aiofile==3.8.8
|
||||||
|
- aiofiles==23.2.1
|
||||||
|
- aniso8601==9.0.1
|
||||||
|
- argcomplete==3.2.2
|
||||||
|
- argparse==1.4.0
|
||||||
|
- boto3==1.34.42
|
||||||
|
- botocore==1.34.42
|
||||||
|
- bracex==2.4
|
||||||
|
- cachetools==5.3.2
|
||||||
|
- caio==0.9.13
|
||||||
|
- cloudpickle==1.3.0
|
||||||
|
- contourpy==1.1.1
|
||||||
|
- cycler==0.12.1
|
||||||
|
- dill==0.3.8
|
||||||
|
- expandvars==0.12.0
|
||||||
|
- fonttools==4.48.1
|
||||||
|
- functional-notations==0.5.2
|
||||||
|
- google-api-core==2.17.1
|
||||||
|
- google-api-python-client==2.118.0
|
||||||
|
- google-auth==2.27.0
|
||||||
|
- google-auth-httplib2==0.2.0
|
||||||
|
- google-cloud-core==2.4.1
|
||||||
|
- google-cloud-storage==2.14.0
|
||||||
|
- google-crc32c==1.5.0
|
||||||
|
- google-resumable-media==2.7.0
|
||||||
|
- googleapis-common-protos==1.62.0
|
||||||
|
- graphene==3.3
|
||||||
|
- graphql-core==3.2.3
|
||||||
|
- graphql-relay==3.2.0
|
||||||
|
- graphql-server-core==1.1.1
|
||||||
|
- gym==0.26.2
|
||||||
|
- gym-notices==0.0.8
|
||||||
|
- h11==0.9.0
|
||||||
|
- httpcore==0.11.1
|
||||||
|
- httplib2==0.22.0
|
||||||
|
- httptools==0.6.1
|
||||||
|
- httpx==0.15.4
|
||||||
|
- imageio==2.34.0
|
||||||
|
- imageio-ffmpeg==0.4.9
|
||||||
|
- importlib-resources==6.1.1
|
||||||
|
- jaynes==0.9.8
|
||||||
|
- jmespath==1.0.1
|
||||||
|
- kiwisolver==1.4.5
|
||||||
|
- lazy-loader==0.3
|
||||||
|
- matplotlib==3.7.4
|
||||||
|
- ml-dash==0.3.20
|
||||||
|
- ml-logger==0.8.117
|
||||||
|
- more-itertools==10.2.0
|
||||||
|
- multidict==4.6.1
|
||||||
|
- ninja==1.11.1.1
|
||||||
|
- numpy==1.23.5
|
||||||
|
- pandas==2.0.3
|
||||||
|
- params-proto==2.10.5
|
||||||
|
- promise==2.3
|
||||||
|
- protobuf==4.25.2
|
||||||
|
- pyasn1==0.5.1
|
||||||
|
- pyasn1-modules==0.3.0
|
||||||
|
- pycurl==7.45.2
|
||||||
|
- pyparsing==3.1.1
|
||||||
|
- pytest-runner==6.0.1
|
||||||
|
- pytz==2024.1
|
||||||
|
- pywavelets==1.4.1
|
||||||
|
- requests-futures==1.0.1
|
||||||
|
- requests-toolbelt==1.0.0
|
||||||
|
- rfc3986==1.5.0
|
||||||
|
- rsa==4.9
|
||||||
|
- s3transfer==0.10.0
|
||||||
|
- sanic==20.9.0
|
||||||
|
- sanic-cors==0.10.0.post3
|
||||||
|
- sanic-graphql==1.1.0
|
||||||
|
- sanic-plugins-framework==0.9.5
|
||||||
|
- scikit-image==0.21.0
|
||||||
|
- scikit-video==1.1.11
|
||||||
|
- six==1.16.0
|
||||||
|
- sniffio==1.3.0
|
||||||
|
- termcolor==2.4.0
|
||||||
|
- tifffile==2023.7.10
|
||||||
|
- tqdm==4.66.2
|
||||||
|
- typing==3.7.4.3
|
||||||
|
- tzdata==2024.1
|
||||||
|
- ujson==5.9.0
|
||||||
|
- uritemplate==4.1.1
|
||||||
|
- urllib3==1.26.18
|
||||||
|
- uvloop==0.19.0
|
||||||
|
- waterbear==2.6.8
|
||||||
|
- wcmatch==8.5
|
||||||
|
- websockets==8.1
|
||||||
|
prefix: /home/rstaion/miniconda3/envs/walk_these_ways
|
||||||
Loading…
Reference in New Issue