EmbodiedAI
/
Go2Py_SIM
mirror of https://github.com/machines-in-motion/Go2Py.git
3
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Updated gitignore, locomotion and upstream changes

This commit is contained in:
Nimesh Khandelwal 2024-02-15 19:03:39 -05:00
parent 515ccc722b 4e67ddae04
commit cb19d4ec71
86 changed files with 11322 additions and 133 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
.gitignore vendored
View File

@ -168,10 +168,15 @@ deploy/dock_ws/log
deploy/dock_ws/install deploy/dock_ws/install
deploy/dock_ws/build deploy/dock_ws/build
<<<<<<< HEAD
*build* *build*
*install* *install*
*log* *log*
*lib* *lib*
=======
deploy/nav2_ws/install
deploy/nav2_ws/log
>>>>>>> origin/main
_isaac_sim _isaac_sim
.vscode .vscode

18
Dockerfile.dock
View File

@ -1,12 +1,16 @@
FROM isaac_ros_dev-aarch64 # FROM isaac_ros_dev-aarch64
FROM ros:humble
ENV DEBIAN_FRONTEND=noninteractive ENV DEBIAN_FRONTEND=noninteractive
SHELL ["/bin/bash", "-c"]
# uodate and install dependencies # uodate and install dependencies
RUN apt-get update && apt-get install -y \ RUN apt-get update && apt-get install -y \
ros-humble-rmw-cyclonedds-cpp ros-humble-rosidl-generator-dds-idl \ ros-humble-rmw-cyclonedds-cpp ros-humble-rosidl-generator-dds-idl \
ros-humble-realsense2-camera \ ros-humble-realsense2-camera \
ros-humble-pointcloud-to-laserscan \ ros-humble-pointcloud-to-laserscan \
ros-humble-isaac-ros-visual-slam \ libyaml-cpp-dev \
ros-humble-isaac-ros-occupancy-grid-localizer\ # ros-humble-isaac-ros-visual-slam \
# ros-humble-isaac-ros-occupancy-grid-localizer\
libboost-all-dev\
build-essential \ build-essential \
cmake \ cmake \
git \ git \
@ -17,9 +21,9 @@ RUN cd / && git clone https://github.com/unitreerobotics/unitree_ros2 && cd /uni
git clone https://github.com/ros2/rmw_cyclonedds -b humble && git clone https://github.com/eclipse-cyclonedds/cyclonedds -b releases/0.10.x &&\ git clone https://github.com/ros2/rmw_cyclonedds -b humble && git clone https://github.com/eclipse-cyclonedds/cyclonedds -b releases/0.10.x &&\
cd .. && colcon build --packages-select cyclonedds && source /opt/ros/humble/setup.bash && colcon build cd .. && colcon build --packages-select cyclonedds && source /opt/ros/humble/setup.bash && colcon build
RUN echo "source /opt/ros/humble/setup.bash" >> /usr/local/bin/scripts/workspace-entrypoint.sh # RUN echo "source /opt/ros/humble/setup.bash" >> /usr/local/bin/scripts/workspace-entrypoint.sh
RUN echo "source /unitree_ros2/cyclonedds_ws/install/setup.bash" >> /usr/local/bin/scripts/workspace-entrypoint.sh # RUN echo "source /unitree_ros2/cyclonedds_ws/install/setup.bash" >> /usr/local/bin/scripts/workspace-entrypoint.sh
RUN echo "export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp" >> /usr/local/bin/scripts/workspace-entrypoint.sh # RUN echo "export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp" >> /usr/local/bin/scripts/workspace-entrypoint.sh
# RUN echo "export CYCLONEDDS_URI='<CycloneDDS><Domain><General><Interfaces> <NetworkInterface name="eth0" priority="default" multicast="default" /> </Interfaces></General></Domain></CycloneDDS>'" >> /usr/local/bin/scripts/workspace-entrypoint.sh # RUN echo "export CYCLONEDDS_URI='<CycloneDDS><Domain><General><Interfaces> <NetworkInterface name="eth0" priority="default" multicast="default" /> </Interfaces></General></Domain></CycloneDDS>'" >> /usr/local/bin/scripts/workspace-entrypoint.sh
# copy the go2py ros2 nodes # copy the go2py ros2 nodes
@ -36,4 +40,4 @@ COPY deploy/scripts /root/scripts
COPY deploy/launch /root/launch COPY deploy/launch /root/launch
# set the entrypoint to bash # set the entrypoint to bash
# ENTRYPOINT ["/bin/bash"] # ENTRYPOINT ["/bin/bash"]
ENTRYPOINT ["/bin/bash", "/root/scripts/hw_start.sh"] ENTRYPOINT ["/bin/bash", "/root/scripts/dock_hw_start.sh"]

42
Dockerfile.robot Normal file
View File

@ -0,0 +1,42 @@
# FROM isaac_ros_dev-aarch64
FROM ros:humble
ENV DEBIAN_FRONTEND=noninteractive
SHELL ["/bin/bash", "-c"]
# uodate and install dependencies
RUN apt-get update && apt-get install -y \
ros-humble-rmw-cyclonedds-cpp ros-humble-rosidl-generator-dds-idl \
libyaml-cpp-dev \
ros-humble-xacro \
# ros-humble-isaac-ros-visual-slam \
# ros-humble-isaac-ros-occupancy-grid-localizer\
libboost-all-dev\
build-essential \
cmake \
git \
&& rm -rf /var/lib/apt/lists/*
# Cheange the ROS2 RMW to CycloneDDS as instructed by Unitree
RUN cd / && git clone https://github.com/unitreerobotics/unitree_ros2 && cd /unitree_ros2/cyclonedds_ws/src && \
git clone https://github.com/ros2/rmw_cyclonedds -b humble && git clone https://github.com/eclipse-cyclonedds/cyclonedds -b releases/0.10.x &&\
cd .. && colcon build --packages-select cyclonedds && source /opt/ros/humble/setup.bash && colcon build
# RUN echo "source /opt/ros/humble/setup.bash" >> /usr/local/bin/scripts/workspace-entrypoint.sh
# RUN echo "source /unitree_ros2/cyclonedds_ws/install/setup.bash" >> /usr/local/bin/scripts/workspace-entrypoint.sh
# RUN echo "export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp" >> /usr/local/bin/scripts/workspace-entrypoint.sh
# RUN echo "export CYCLONEDDS_URI='<CycloneDDS><Domain><General><Interfaces> <NetworkInterface name="eth0" priority="default" multicast="default" /> </Interfaces></General></Domain></CycloneDDS>'" >> /usr/local/bin/scripts/workspace-entrypoint.sh
# copy the go2py ros2 nodes
COPY deploy/robot_ws/src /robot_ws/src
RUN cd /robot_ws && source /opt/ros/humble/setup.bash && colcon build --symlink-install
# Compile the C++ hypervisor bridge
COPY deploy/dds_bridge /dds_bridge
WORKDIR /dds_bridge
RUN ./install.sh && mkdir build && cd build && cmake .. && make
# Copy the script to start the nodes
COPY deploy/scripts /root/scripts
COPY deploy/launch /root/launch
# set the entrypoint to bash
# ENTRYPOINT ["/bin/bash"]
ENTRYPOINT ["/bin/bash", "/root/scripts/robot_hw_start.sh"]

217
Go2Py/assets/dae/base.dae Normal file
View File

File diff suppressed because one or more lines are too long

130
Go2Py/assets/dae/calf.dae Normal file
View File

File diff suppressed because one or more lines are too long

130
Go2Py/assets/dae/calf_mirror.dae Normal file
View File

File diff suppressed because one or more lines are too long

89
Go2Py/assets/dae/foot.dae Normal file
View File

File diff suppressed because one or more lines are too long

130
Go2Py/assets/dae/hip.dae Normal file
View File

File diff suppressed because one or more lines are too long

130
Go2Py/assets/dae/thigh.dae Normal file
View File

File diff suppressed because one or more lines are too long

130
Go2Py/assets/dae/thigh_mirror.dae Normal file
View File

File diff suppressed because one or more lines are too long

682
Go2Py/assets/urdf/go2.urdf Normal file
View File

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

BIN
Go2Py/assets/urdf/go2/go2.usd Normal file
View File

Binary file not shown.

0
Go2Py/sim/__init__.py Normal file
View File

7
Go2Py/sim/gym/__init__.py Executable file
View File

@ -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
Go2Py/sim/gym/envs/__init__.py Executable file
View File

0
Go2Py/sim/gym/envs/base/__init__.py Normal file
View File

137
Go2Py/sim/gym/envs/base/base_task.py Executable file
View File

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

181
Go2Py/sim/gym/envs/base/curriculum.py Normal file
View File

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

1806
Go2Py/sim/gym/envs/base/legged_robot.py Executable file
View File

File diff suppressed because it is too large Load Diff

421
Go2Py/sim/gym/envs/base/legged_robot_config.py Executable file
View File

@ -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
Go2Py/sim/gym/envs/go2/__init__.py Normal file
View File

106
Go2Py/sim/gym/envs/go2/go2_config.py Executable file
View File

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

50
Go2Py/sim/gym/envs/go2/velocity_tracking/__init__.py Normal file
View File

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

202
Go2Py/sim/gym/envs/rewards/corl_rewards.py Normal file
View File

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

72
Go2Py/sim/gym/envs/wrappers/history_wrapper.py Executable file
View File

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

2
Go2Py/sim/gym/utils/__init__.py Executable file
View File

@ -0,0 +1,2 @@
from .math_utils import *
from .terrain import Terrain

38
Go2Py/sim/gym/utils/math_utils.py Executable file
View File

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

180
Go2Py/sim/gym/utils/terrain.py Executable file
View File

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

33
Makefile
View File

@ -1,12 +1,25 @@
docker: docker_dock:
@docker build --no-cache --tag go2py:latest -f Dockerfile.dock . @docker build --no-cache --tag go2py_dock_hw:latest -f Dockerfile.dock .
docker_install: docker_robot:
@cp deploy/scripts/go2py-hw-nodes.service /etc/systemd/system/ @docker build --no-cache --tag go2py_robot_hw:latest -f Dockerfile.robot .
@systemctl enable go2py-hw-nodes.service
@systemctl start go2py-hw-nodes.service
docker_uninstall: docker_dock_install:
@systemctl disable go2py-hw-nodes.service @cp deploy/scripts/go2py-dock-hw-nodes.service /etc/systemd/system/
@systemctl stop go2py-hw-nodes.service @systemctl enable go2py-dock-hw-nodes.service
@rm /etc/systemd/system/go2py-hw-nodes.service @systemctl start go2py-dock-hw-nodes.service
docker_dock_uninstall:
@systemctl disable go2py-dock-hw-nodes.service
@systemctl stop go2py-dock-hw-nodes.service
@rm /etc/systemd/system/go2py-dock-hw-nodes.service
docker_robot_install:
@cp deploy/scripts/go2py-robot-hw-nodes.service /etc/systemd/system/
@systemctl enable go2py-robot-hw-nodes.service
@systemctl start go2py-robot-hw-nodes.service
docker_robot_uninstall:
@systemctl disable go2py-robot-hw-nodes.service
@systemctl stop go2py-robot-hw-nodes.service
@rm /etc/systemd/system/go2py-robot-hw-nodes.service

3
deploy/dock_ws/src/lidar_node/CMakeLists.txt
View File

@ -146,7 +146,8 @@ add_subdirectory(src/driver/HesaiLidar_SDK_2.0)
find_package(CUDA) find_package(CUDA)
if(CUDA_FOUND) if(OFF)
# if(CUDA_FOUND)
message(=============================================================) message(=============================================================)
message("-- CUDA Found. CUDA Support is turned On.") message("-- CUDA Found. CUDA Support is turned On.")

28
deploy/dock_ws/src/lidar_node/node/hesai_ros_driver_node.cc
View File

@ -100,8 +100,34 @@ int main(int argc, char** argv)
ros::MultiThreadedSpinner spinner(2); ros::MultiThreadedSpinner spinner(2);
spinner.spin(); spinner.spin();
#elif ROS2_FOUND #elif ROS2_FOUND
std::cout << "Start of here...";
using namespace std::chrono;
std::unique_lock<std::mutex> lck(g_mtx); std::unique_lock<std::mutex> lck(g_mtx);
g_cv.wait(lck); while(true)
{
auto timeout = steady_clock::now() + milliseconds(10);
// Wait with timeout
if (g_cv.wait_until(lck, timeout) == std::cv_status::timeout) {
// Handle timeout logic here
// std::cout << "Timeout occurred after 10ms." << std::endl;
// std::cout << (int)(demo_ptr->sources_driver_[0]->publish_pointcloud_flag) << std::endl;
for(int i=0; i<demo_ptr->sources_driver_.size(); i++)
{
if(demo_ptr->sources_driver_[i]->publish_pointcloud_flag)
{
demo_ptr->sources_driver_[i]->publish_pointcloud_flag=false;
demo_ptr->sources_driver_[i]->publishPointcloud();
}
}
} else {
// Continue with normal processing as the condition variable was signaled
// ...
std::cout << "Condition variable was set succesffuly" << std::endl;
break;
}
}
// g_cv.wait(lck);
#endif #endif
return 0; return 0;

3
deploy/dock_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/CMakeLists.txt
View File

@ -86,7 +86,8 @@ target_link_libraries(sample
# ) # )
find_package(CUDA ) find_package(CUDA )
if(${CUDA_FOUND}) if(OFF)
# if(${CUDA_FOUND})
set(CUDA_SOURCE_PROPERTY_FORMAT OBJ) set(CUDA_SOURCE_PROPERTY_FORMAT OBJ)
set(CUDA_SEPARABLE_COMPILATION ON) set(CUDA_SEPARABLE_COMPILATION ON)
include_directories(${CUDA_INCLUDE_DIRS}) include_directories(${CUDA_INCLUDE_DIRS})

2
deploy/dock_ws/src/lidar_node/src/manager/node_manager.h
View File

@ -51,7 +51,7 @@ public:
~NodeManager(); ~NodeManager();
NodeManager() = default; NodeManager() = default;
private: // private:
std::vector<SourceDriver::Ptr> sources_driver_; std::vector<SourceDriver::Ptr> sources_driver_;
}; };

96
deploy/dock_ws/src/lidar_node/src/manager/source_driver_ros2.hpp
View File

@ -39,6 +39,9 @@
#include <chrono> #include <chrono>
#include <string> #include <string>
#include <functional> #include <functional>
#include <stdbool.h>
#include <math.h>
#include "sensor_msgs/msg/laser_scan.hpp"
#ifdef __CUDACC__ #ifdef __CUDACC__
#include "hesai_lidar_sdk_gpu.cuh" #include "hesai_lidar_sdk_gpu.cuh"
#else #else
@ -58,6 +61,12 @@ public:
SourceDriver(SourceType src_type) {}; SourceDriver(SourceType src_type) {};
void SpinRos2(){rclcpp::spin(this->node_ptr_);} void SpinRos2(){rclcpp::spin(this->node_ptr_);}
std::shared_ptr<rclcpp::Node> node_ptr_; std::shared_ptr<rclcpp::Node> node_ptr_;
LidarDecodedFrame<LidarPointXYZIRT> pointcloud_;
void publishPointcloud(void);
bool publish_pointcloud_flag;
void publishLaserScan(
sensor_msgs::msg::PointCloud2 *cloud_msg);
protected: protected:
// Save packets subscribed by 'ros_recv_packet_topic' // Save packets subscribed by 'ros_recv_packet_topic'
void RecievePacket(const hesai_ros_driver::msg::UdpFrame::SharedPtr msg); void RecievePacket(const hesai_ros_driver::msg::UdpFrame::SharedPtr msg);
@ -78,13 +87,16 @@ protected:
rclcpp::Subscription<hesai_ros_driver::msg::UdpFrame>::SharedPtr pkt_sub_; rclcpp::Subscription<hesai_ros_driver::msg::UdpFrame>::SharedPtr pkt_sub_;
rclcpp::Publisher<hesai_ros_driver::msg::UdpFrame>::SharedPtr pkt_pub_; rclcpp::Publisher<hesai_ros_driver::msg::UdpFrame>::SharedPtr pkt_pub_;
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_; rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_;
std::shared_ptr<rclcpp::Publisher<sensor_msgs::msg::LaserScan>> laserscan_pub_;
//spin thread while recieve data from ROS topic //spin thread while recieve data from ROS topic
boost::thread* subscription_spin_thread_; boost::thread* subscription_spin_thread_;
}; };
inline void SourceDriver::Init(const YAML::Node& config) inline void SourceDriver::Init(const YAML::Node& config)
{ {
publish_pointcloud_flag = false;
YAML::Node driver_config = YamlSubNodeAbort(config, "driver"); YAML::Node driver_config = YamlSubNodeAbort(config, "driver");
DriverParam driver_param; DriverParam driver_param;
// input related // input related
@ -124,15 +136,15 @@ inline void SourceDriver::Init(const YAML::Node& config)
if (send_point_cloud_ros) { if (send_point_cloud_ros) {
std::string ros_send_point_topic; std::string ros_send_point_topic;
YamlRead<std::string>(config["ros"], "ros_send_point_cloud_topic", ros_send_point_topic, "hesai_points"); YamlRead<std::string>(config["ros"], "ros_send_point_cloud_topic", ros_send_point_topic, "hesai_points");
pub_ = node_ptr_->create_publisher<sensor_msgs::msg::PointCloud2>(ros_send_point_topic, 100); pub_ = node_ptr_->create_publisher<sensor_msgs::msg::PointCloud2>(ros_send_point_topic, 1);
} }
if (send_packet_ros && driver_param.input_param.source_type != DATA_FROM_ROS_PACKET) { if (send_packet_ros && driver_param.input_param.source_type != DATA_FROM_ROS_PACKET) {
std::string ros_send_packet_topic; std::string ros_send_packet_topic;
YamlRead<std::string>(config["ros"], "ros_send_packet_topic", ros_send_packet_topic, "hesai_packets"); YamlRead<std::string>(config["ros"], "ros_send_packet_topic", ros_send_packet_topic, "hesai_packets");
pkt_pub_ = node_ptr_->create_publisher<hesai_ros_driver::msg::UdpFrame>(ros_send_packet_topic, 10); pkt_pub_ = node_ptr_->create_publisher<hesai_ros_driver::msg::UdpFrame>(ros_send_packet_topic, 1);
} }
laserscan_pub_ = node_ptr_->create_publisher<sensor_msgs::msg::LaserScan>("/go2/scan", rclcpp::SensorDataQoS());
if (driver_param.input_param.source_type == DATA_FROM_ROS_PACKET) { if (driver_param.input_param.source_type == DATA_FROM_ROS_PACKET) {
std::string ros_recv_packet_topic; std::string ros_recv_packet_topic;
YamlRead<std::string>(config["ros"], "ros_recv_packet_topic", ros_recv_packet_topic, "hesai_packets"); YamlRead<std::string>(config["ros"], "ros_recv_packet_topic", ros_recv_packet_topic, "hesai_packets");
@ -181,7 +193,16 @@ inline void SourceDriver::SendPacket(const UdpFrame_t& msg, double timestamp)
inline void SourceDriver::SendPointCloud(const LidarDecodedFrame<LidarPointXYZIRT>& msg) inline void SourceDriver::SendPointCloud(const LidarDecodedFrame<LidarPointXYZIRT>& msg)
{ {
pub_->publish(ToRosMsg(msg, frame_id_)); pointcloud_ = msg;
publish_pointcloud_flag = true;
// pub_->publish(ToRosMsg(msg, frame_id_));
}
inline void SourceDriver::publishPointcloud(void)
{
sensor_msgs::msg::PointCloud2 msg = ToRosMsg(pointcloud_, frame_id_);
pub_->publish(msg);
publishLaserScan(&msg);
} }
inline sensor_msgs::msg::PointCloud2 SourceDriver::ToRosMsg(const LidarDecodedFrame<LidarPointXYZIRT>& frame, const std::string& frame_id) inline sensor_msgs::msg::PointCloud2 SourceDriver::ToRosMsg(const LidarDecodedFrame<LidarPointXYZIRT>& frame, const std::string& frame_id)
@ -217,6 +238,8 @@ inline sensor_msgs::msg::PointCloud2 SourceDriver::ToRosMsg(const LidarDecodedFr
for (size_t i = 0; i < frame.points_num; i++) for (size_t i = 0; i < frame.points_num; i++)
{ {
// if(i%2==0) //downsample the number of points by two
// {
LidarPointXYZIRT point = frame.points[i]; LidarPointXYZIRT point = frame.points[i];
*iter_x_ = point.x; *iter_x_ = point.x;
*iter_y_ = point.y; *iter_y_ = point.y;
@ -230,12 +253,14 @@ inline sensor_msgs::msg::PointCloud2 SourceDriver::ToRosMsg(const LidarDecodedFr
++iter_intensity_; ++iter_intensity_;
++iter_ring_; ++iter_ring_;
++iter_timestamp_; ++iter_timestamp_;
// }
} }
printf("frame:%d points:%u packet:%d start time:%lf end time:%lf\n",frame.frame_index, frame.points_num, frame.packet_num, frame.points[0].timestamp, frame.points[frame.points_num - 1].timestamp) ; printf("frame:%d points:%u packet:%d start time:%lf end time:%lf\n",frame.frame_index, frame.points_num, frame.packet_num, frame.points[0].timestamp, frame.points[frame.points_num - 1].timestamp) ;
ros_msg.header.stamp.sec = (uint32_t)floor(frame.points[0].timestamp); // ros_msg.header.stamp.sec = (uint32_t)floor(frame.points[0].timestamp);
ros_msg.header.stamp.nanosec = (uint32_t)round((frame.points[0].timestamp - ros_msg.header.stamp.sec) * 1e9); // ros_msg.header.stamp.nanosec = (uint32_t)round((frame.points[0].timestamp - ros_msg.header.stamp.sec) * 1e9);
ros_msg.header.frame_id = frame_id_; ros_msg.header.frame_id = frame_id_;
ros_msg.header.stamp = rclcpp::Clock().now();
return ros_msg; return ros_msg;
} }
@ -261,4 +286,63 @@ inline void SourceDriver::RecievePacket(const hesai_ros_driver::msg::UdpFrame::S
} }
} }
inline void SourceDriver::publishLaserScan(
sensor_msgs::msg::PointCloud2 *cloud_msg)
{
// build laserscan output
auto scan_msg = std::make_unique<sensor_msgs::msg::LaserScan>();
scan_msg->header = cloud_msg->header;
scan_msg->angle_min = -3.1415;
scan_msg->angle_max = 3.1415;
scan_msg->angle_increment = 0.0087/2.;
scan_msg->time_increment = 0.0;
scan_msg->scan_time = 0.3333;
scan_msg->range_min = 0.2;
scan_msg->range_max = 100;
float max_height_ = 1.0;
float min_height_ = 0.;
// determine amount of rays to create
uint32_t ranges_size = std::ceil(
(scan_msg->angle_max - scan_msg->angle_min) / scan_msg->angle_increment);
// determine if laserscan rays with no obstacle data will evaluate to infinity or max_range
scan_msg->ranges.assign(ranges_size, std::numeric_limits<double>::infinity());
// scan_msg->intensities.assign(ranges_size, 255);
// Iterate through pointcloud
for (sensor_msgs::PointCloud2ConstIterator<float> iter_x(*cloud_msg, "x"),
iter_y(*cloud_msg, "y"), iter_z(*cloud_msg, "z");
iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z)
{
if (std::isnan(*iter_x) || std::isnan(*iter_y) || std::isnan(*iter_z)) {
continue;
}
if (*iter_z > max_height_ || *iter_z < min_height_) {
continue;
}
double range = hypot(*iter_x, *iter_y);
if (range < scan_msg->range_min) {
continue;
}
if (range > scan_msg->range_max) {
continue;
}
double angle = atan2(*iter_y, *iter_x);
if (angle < scan_msg->angle_min || angle > scan_msg->angle_max) {
continue;
}
// overwrite range at laserscan ray if new range is smaller
int index = (angle - scan_msg->angle_min) / scan_msg->angle_increment;
if (range < scan_msg->ranges[index]) {
scan_msg->ranges[index] = range;
}
}
laserscan_pub_->publish(std::move(scan_msg));
}

36
deploy/launch/dock.launch.py Normal file
View File

@ -0,0 +1,36 @@
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import ThisLaunchFileDir
from launch_ros.actions import Node
def generate_launch_description():
return LaunchDescription([
# launch the pointcloud to laser scan converter
Node(
package='hesai_ros_driver',
executable='hesai_ros_driver_node',
name='go2_hesai_ros_driver_node'
),
Node(
name='go2_cam',
namespace='go2/cam',
package='realsense2_camera',
executable='realsense2_camera_node',
parameters=[{
'enable_infra1': True,
'enable_infra2': True,
'enable_color': False,
'enable_depth': False,
'depth_module.emitter_enabled': 0,
'depth_module.profile': '640x480x60',
'enable_gyro': True,
'enable_accel': True,
'gyro_fps': 400,
'accel_fps': 200,
'unite_imu_method': 2,
# 'tf_publish_rate': 0.0
}]
)
])

84
deploy/launch/hw.launch.py
View File

@ -1,84 +0,0 @@
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import ThisLaunchFileDir
from launch_ros.actions import Node
def generate_launch_description():
return LaunchDescription([
# launch the pointcloud to laser scan converter
Node(
package='pointcloud_to_laserscan', executable='pointcloud_to_laserscan_node',
remappings=[('cloud_in', '/go2/lidar_points'),
('scan', '/go2/lidar_scans')],
parameters=[{
'target_frame': 'go2/hesai_lidar',
'transform_tolerance': 0.01,
'min_height': 0.0,
'max_height': 1.0,
'angle_min': -1.5708, # -M_PI/2
'angle_max': 1.5708, # M_PI/2
'angle_increment': 0.0087, # M_PI/360.0
'scan_time': 0.3333,
'range_min': 0.45,
'range_max': 100.0,
'use_inf': True,
'inf_epsilon': 1.0
}],
name='pointcloud_to_laserscan'
),
Node(
package='hesai_ros_driver',
executable='hesai_ros_driver_node',
name='b1_hesai_ros_driver_node'
),
Node(
name='go2_d455_cam',
namespace='go2/d435i_cam',
package='realsense2_camera',
executable='realsense2_camera_node',
parameters=[{
'enable_infra1': True,
'enable_infra2': True,
'enable_color': False,
'enable_depth': False,
'depth_module.emitter_enabled': 0,
'depth_module.profile': '640x480x60',
'enable_gyro': True,
'enable_accel': True,
'gyro_fps': 400,
'accel_fps': 200,
'unite_imu_method': 2,
# 'tf_publish_rate': 0.0
}]
),
# Launch the front looking D455 camera
# IncludeLaunchDescription(
# PythonLaunchDescriptionSource([ThisLaunchFileDir(), '/d455.launch.py'])
# ),
# Run the B1py node
# Node(
# package='b1py_node',
# executable='highlevel',
# name='b1_highlevel_node'
# ),
# Run the B1py calibration TF broadcaster
# Node(
# package='b1py_calib',
# executable='calib_broadcaster',
# name='b1_calib_broadcaster_node'
# ),
# Launch the LiDAR sensor
# IncludeLaunchDescription(
# PythonLaunchDescriptionSource([ThisLaunchFileDir(), '/rslidar.launch.py'])
# ),
# Launch the LiDAR sensor
# IncludeLaunchDescription(
# PythonLaunchDescriptionSource(['/B1Py/deploy/docker/launch/state_estimation/ekf.launch.py'])
# ),
])

52
deploy/launch/robot.launch.py Normal file
View File

@ -0,0 +1,52 @@
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import ThisLaunchFileDir
from launch_ros.actions import Node
from launch.substitutions import Command, FindExecutable, LaunchConfiguration
from ament_index_python.packages import get_package_share_directory
import os
def generate_launch_description():
go2_xacro_file = os.path.join(
get_package_share_directory("go2_description"), "xacro", "robot_virtual_arm.xacro"
)
robot_description = Command(
[FindExecutable(name="xacro"), " ", go2_xacro_file, " DEBUG:=", 'false']
)
return LaunchDescription([
# launch the pointcloud to laser scan converter
Node(
package='go2py_node',
executable='bridge',
name='go2py_bridge'
),
Node(
package="robot_state_publisher",
executable="robot_state_publisher",
name="robot_state_publisher",
output="screen",
parameters=[{"robot_description": robot_description}],
remappings=[
("/joint_states", "/go2/joint_states"),
("/robot_description", "/go2/robot_description"),
],
),
Node(
package="tf2_ros",
executable="static_transform_publisher",
arguments=[
"0.15",
"0",
"0.15",
"0",
"0",
"0.707107",
"0.707107",
"/trunk",
"/go2/hesai_lidar",
],
name="static_tf_pub_trunk_to_lidar",
),
])

63
deploy/nav2_ws/src/sportmode_nav2/CMakeLists.txt Normal file
View File

@ -0,0 +1,63 @@
cmake_minimum_required(VERSION 3.5)
project(sportmode_nav2)
# Default to C99
if(NOT CMAKE_C_STANDARD)
set(CMAKE_C_STANDARD 99)
endif()
# Default to C++14
if(NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD 14)
endif()
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
include_directories(include include/common include/nlohmann)
link_directories(src)
set (
DEPENDENCY_LIST
rclcpp
std_msgs
rosbag2_cpp
sensor_msgs
geometry_msgs
nav_msgs
tf2
tf2_geometry_msgs
tf2_ros
tf2_sensor_msgs
)
# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED)
find_package(rosbag2_cpp REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(nav_msgs REQUIRED)
find_package(tf2 REQUIRED)
find_package(tf2_geometry_msgs REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(tf2_sensor_msgs REQUIRED)
# Install params config files.
install(DIRECTORY
params
launch
DESTINATION share/${PROJECT_NAME}
USE_SOURCE_PERMISSIONS
)
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
ament_lint_auto_find_test_dependencies()
endif()
ament_package()

27
deploy/nav2_ws/src/sportmode_nav2/launch/mapping.launch.py Normal file
View File

@ -0,0 +1,27 @@
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.actions import Node
def generate_launch_description():
robot_localization = IncludeLaunchDescription(
PythonLaunchDescriptionSource([os.path.join(
get_package_share_directory('sportmode_nav2'), 'launch'),
'/ros_ekf.launch.py'])
)
async_mapping = IncludeLaunchDescription(
PythonLaunchDescriptionSource([os.path.join(
get_package_share_directory('sportmode_nav2'), 'launch'),
'/mapping_async.launch.py'])
)
return LaunchDescription([
robot_localization,
async_mapping,
])

59
deploy/nav2_ws/src/sportmode_nav2/launch/mapping_async.launch.py Normal file
View File

@ -0,0 +1,59 @@
import os
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, LogInfo
from launch.conditions import UnlessCondition
from launch.substitutions import LaunchConfiguration, PythonExpression
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory
from nav2_common.launch import HasNodeParams
def generate_launch_description():
use_sim_time = LaunchConfiguration('use_sim_time', default=False)
params_file = LaunchConfiguration('params_file')
default_params_file = os.path.join(get_package_share_directory("sportmode_nav2"),
'params', 'mapping_async.yaml')
declare_use_sim_time_argument = DeclareLaunchArgument(
'use_sim_time',
default_value='true',
description='Use simulation/Gazebo clock')
declare_params_file_cmd = DeclareLaunchArgument(
'params_file',
default_value=default_params_file,
description='Full path to the ROS2 parameters file to use for the slam_toolbox node')
# If the provided param file doesn't have slam_toolbox params, we must pass the
# default_params_file instead. This could happen due to automatic propagation of
# LaunchArguments. See:
# https://github.com/ros-planning/navigation2/pull/2243#issuecomment-800479866
has_node_params = HasNodeParams(source_file=params_file,
node_name='slam_toolbox')
actual_params_file = PythonExpression(['"', params_file, '" if ', has_node_params,
' else "', default_params_file, '"'])
log_param_change = LogInfo(msg=['provided params_file ', params_file,
' does not contain slam_toolbox parameters. Using default: ',
default_params_file],
condition=UnlessCondition(has_node_params))
start_async_slam_toolbox_node = Node(
parameters=[
actual_params_file,
{'use_sim_time': use_sim_time}
],
package='slam_toolbox',
executable='async_slam_toolbox_node',
name='slam_toolbox',
output='screen')
ld = LaunchDescription()
ld.add_action(declare_use_sim_time_argument)
ld.add_action(declare_params_file_cmd)
ld.add_action(log_param_change)
ld.add_action(start_async_slam_toolbox_node)
return ld

35
deploy/nav2_ws/src/sportmode_nav2/launch/ros_ekf.launch.py Normal file
View File

@ -0,0 +1,35 @@
# Copyright 2018 Open Source Robotics Foundation, Inc.
# Copyright 2019 Samsung Research America
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
from launch import LaunchDescription
from ament_index_python.packages import get_package_share_directory
import launch_ros.actions
import os
import yaml
from launch.substitutions import EnvironmentVariable
import pathlib
import launch.actions
from launch.actions import DeclareLaunchArgument
def generate_launch_description():
return LaunchDescription([
launch_ros.actions.Node(
package='robot_localization',
executable='ekf_node',
name='ekf_filter_node',
output='screen',
parameters=[os.path.join(get_package_share_directory("sportmode_nav2"), 'params', 'ros_ekf.yaml')],
),
])

22
deploy/nav2_ws/src/sportmode_nav2/package.xml Normal file
View File

@ -0,0 +1,22 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>sportmode_nav2</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="zeendi@163.com">zeen</maintainer>
<license>TODO: License declaration</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>rclcpp</depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<test_depend>robot_locoalization</test_depend>
<test_depend>slam_toolbox</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

73
deploy/nav2_ws/src/sportmode_nav2/params/mapping_async.yaml Normal file
View File

@ -0,0 +1,73 @@
slam_toolbox:
ros__parameters:
# Plugin params
solver_plugin: solver_plugins::CeresSolver
ceres_linear_solver: SPARSE_NORMAL_CHOLESKY
ceres_preconditioner: SCHUR_JACOBI
ceres_trust_strategy: LEVENBERG_MARQUARDT
ceres_dogleg_type: TRADITIONAL_DOGLEG
ceres_loss_function: None
# ROS Parameters
odom_frame: odom
map_frame: map
base_frame: base_link # ToDo: add a base_footprint to the URDF
scan_topic: /go2/scan
mode: mapping #localization
# if you'd like to immediately start continuing a map at a given pose
# or at the dock, but they are mutually exclusive, if pose is given
# will use pose
#map_file_name: test_steve
# map_start_pose: [0.0, 0.0, 0.0]
#map_start_at_dock: true
debug_logging: false
throttle_scans: 1
transform_publish_period: 0.02 #if 0 never publishes odometry
map_update_interval: 5.0
resolution: 0.05
max_laser_range: 20.0 #for rastering images
minimum_time_interval: 0.5
transform_timeout: 0.2
tf_buffer_duration: 30.
stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps
enable_interactive_mode: true
# General Parameters
use_scan_matching: true
use_scan_barycenter: true
minimum_travel_distance: 0.5
minimum_travel_heading: 0.5
scan_buffer_size: 10
scan_buffer_maximum_scan_distance: 10.0
link_match_minimum_response_fine: 0.1
link_scan_maximum_distance: 1.5
loop_search_maximum_distance: 3.0
do_loop_closing: true
loop_match_minimum_chain_size: 10
loop_match_maximum_variance_coarse: 3.0
loop_match_minimum_response_coarse: 0.35
loop_match_minimum_response_fine: 0.45
# Correlation Parameters - Correlation Parameters
correlation_search_space_dimension: 0.5
correlation_search_space_resolution: 0.01
correlation_search_space_smear_deviation: 0.1
# Correlation Parameters - Loop Closure Parameters
loop_search_space_dimension: 8.0
loop_search_space_resolution: 0.05
loop_search_space_smear_deviation: 0.03
# Scan Matcher Parameters
distance_variance_penalty: 0.5
angle_variance_penalty: 1.0
fine_search_angle_offset: 0.00349
coarse_search_angle_offset: 0.349
coarse_angle_resolution: 0.0349
minimum_angle_penalty: 0.9
minimum_distance_penalty: 0.5
use_response_expansion: true

219
deploy/nav2_ws/src/sportmode_nav2/params/ros_ekf.yaml Normal file
View File

@ -0,0 +1,219 @@
### ekf config file ###
ekf_filter_node:
ros__parameters:
# The frequency, in Hz, at which the filter will output a position estimate. Note that the filter will not begin
# computation until it receives at least one message from one of the inputs. It will then run continuously at the
# frequency specified here, regardless of whether it receives more measurements. Defaults to 30 if unspecified.
frequency: 100.0
# The period, in seconds, after which we consider a sensor to have timed out. In this event, we carry out a predict
# cycle on the EKF without correcting it. This parameter can be thought of as the minimum frequency with which the
# filter will generate new output. Defaults to 1 / frequency if not specified.
sensor_timeout: 0.1
# ekf_localization_node and ukf_localization_node both use a 3D omnidirectional motion model. If this parameter is
# set to true, no 3D information will be used in your state estimate. Use this if you are operating in a planar
# environment and want to ignore the effect of small variations in the ground plane that might otherwise be detected
# by, for example, an IMU. Defaults to false if unspecified.
two_d_mode: false
# Use this parameter to provide an offset to the transform generated by ekf_localization_node. This can be used for
# future dating the transform, which is required for interaction with some other packages. Defaults to 0.0 if
# unspecified.
transform_time_offset: 0.0
# Use this parameter to provide specify how long the tf listener should wait for a transform to become available.
# Defaults to 0.0 if unspecified.
transform_timeout: 0.0
# If you're having trouble, try setting this to true, and then echo the /diagnostics_agg topic to see if the node is
# unhappy with any settings or data.
print_diagnostics: true
# Debug settings. Not for the faint of heart. Outputs a ludicrous amount of information to the file specified by
# debug_out_file. I hope you like matrices! Please note that setting this to true will have strongly deleterious
# effects on the performance of the node. Defaults to false if unspecified.
debug: false
# Defaults to "robot_localization_debug.txt" if unspecified. Please specify the full path.
debug_out_file: /path/to/debug/file.txt
# Whether we'll allow old measurements to cause a re-publication of the updated state
permit_corrected_publication: false
# Whether to publish the acceleration state. Defaults to false if unspecified.
publish_acceleration: false
# Whether to broadcast the transformation over the /tf topic. Defaults to true if unspecified.
publish_tf: true
# REP-105 (http://www.ros.org/reps/rep-0105.html) specifies four principal coordinate frames: base_link, odom, map, and
# earth. base_link is the coordinate frame that is affixed to the robot. Both odom and map are world-fixed frames.
# The robot's position in the odom frame will drift over time, but is accurate in the short term and should be
# continuous. The odom frame is therefore the best frame for executing local motion plans. The map frame, like the odom
# frame, is a world-fixed coordinate frame, and while it contains the most globally accurate position estimate for your
# robot, it is subject to discrete jumps, e.g., due to the fusion of GPS data or a correction from a map-based
# localization node. The earth frame is used to relate multiple map frames by giving them a common reference frame.
# ekf_localization_node and ukf_localization_node are not concerned with the earth frame.
# Here is how to use the following settings:
# 1. Set the map_frame, odom_frame, and base_link frames to the appropriate frame names for your system.
# 1a. If your system does not have a map_frame, just remove it, and make sure "world_frame" is set to the value of
# odom_frame.
# 2. If you are fusing continuous position data such as wheel encoder odometry, visual odometry, or IMU data, set
# "world_frame" to your odom_frame value. This is the default behavior for robot_localization's state estimation nodes.
# 3. If you are fusing global absolute position data that is subject to discrete jumps (e.g., GPS or position updates
# from landmark observations) then:
# 3a. Set your "world_frame" to your map_frame value
# 3b. MAKE SURE something else is generating the odom->base_link transform. Note that this can even be another state
# estimation node from robot_localization! However, that instance should *not* fuse the global data.
map_frame: map # Defaults to "map" if unspecified
odom_frame: odom # Defaults to "odom" if unspecified
base_link_frame: base_link # Defaults to "base_link" if unspecified
world_frame: odom # Defaults to the value of odom_frame if unspecified
# The filter accepts an arbitrary number of inputs from each input message type (nav_msgs/Odometry,
# geometry_msgs/PoseWithCovarianceStamped, geometry_msgs/TwistWithCovarianceStamped,
# sensor_msgs/Imu). To add an input, simply append the next number in the sequence to its "base" name, e.g., odom0,
# odom1, twist0, twist1, imu0, imu1, imu2, etc. The value should be the topic name. These parameters obviously have no
# default values, and must be specified.
odom0: /utlidar/robot_odom
# Each sensor reading updates some or all of the filter's state. These options give you greater control over which
# values from each measurement are fed to the filter. For example, if you have an odometry message as input, but only
# want to use its Z position value, then set the entire vector to false, except for the third entry. The order of the
# values is x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Note that not some message types
# do not provide some of the state variables estimated by the filter. For example, a TwistWithCovarianceStamped message
# has no pose information, so the first six values would be meaningless in that case. Each vector defaults to all false
# if unspecified, effectively making this parameter required for each sensor.
odom0_config: [true, true, true,
true, true, true,
true, true, true,
true, true, true,
false, false, false]
# If you have high-frequency data or are running with a low frequency parameter value, then you may want to increase
# the size of the subscription queue so that more measurements are fused.
odom0_queue_size: 2
# [ADVANCED] Large messages in ROS can exhibit strange behavior when they arrive at a high frequency. This is a result
# of Nagle's algorithm. This option tells the ROS subscriber to use the tcpNoDelay option, which disables Nagle's
# algorithm.
odom0_nodelay: false
# [ADVANCED] When measuring one pose variable with two sensors, a situation can arise in which both sensors under-
# report their covariances. This can lead to the filter rapidly jumping back and forth between each measurement as they
# arrive. In these cases, it often makes sense to (a) correct the measurement covariances, or (b) if velocity is also
# measured by one of the sensors, let one sensor measure pose, and the other velocity. However, doing (a) or (b) isn't
# always feasible, and so we expose the differential parameter. When differential mode is enabled, all absolute pose
# data is converted to velocity data by differentiating the absolute pose measurements. These velocities are then
# integrated as usual. NOTE: this only applies to sensors that provide pose measurements; setting differential to true
# for twist measurements has no effect.
odom0_differential: false
# [ADVANCED] When the node starts, if this parameter is true, then the first measurement is treated as a "zero point"
# for all future measurements. While you can achieve the same effect with the differential paremeter, the key
# difference is that the relative parameter doesn't cause the measurement to be converted to a velocity before
# integrating it. If you simply want your measurements to start at 0 for a given sensor, set this to true.
odom0_relative: false
# [ADVANCED] If your data is subject to outliers, use these threshold settings, expressed as Mahalanobis distances, to
# control how far away from the current vehicle state a sensor measurement is permitted to be. Each defaults to
# numeric_limits<double>::max() if unspecified. It is strongly recommended that these parameters be removed if not
# required. Data is specified at the level of pose and twist variables, rather than for each variable in isolation.
# For messages that have both pose and twist data, the parameter specifies to which part of the message we are applying
# the thresholds.
odom0_pose_rejection_threshold: 5.0
odom0_twist_rejection_threshold: 1.0
# imu0: /utlidar/imu
# imu0_config: [false, false, false,
# true, true, true,
# false, false, false,
# true, true, true,
# true, true, true]
# imu0_nodelay: false
# imu0_differential: false
# imu0_relative: true
# imu0_queue_size: 5
# imu0_pose_rejection_threshold: 0.8 # Note the difference in parameter names
# imu0_twist_rejection_threshold: 0.8 #
# imu0_linear_acceleration_rejection_threshold: 0.8 #
# [ADVANCED] Some IMUs automatically remove acceleration due to gravity, and others don't. If yours doesn't, please set
# this to true, and *make sure* your data conforms to REP-103, specifically, that the data is in ENU frame.
imu0_remove_gravitational_acceleration: true
# [ADVANCED] The EKF and UKF models follow a standard predict/correct cycle. During prediction, if there is no
# acceleration reference, the velocity at time t+1 is simply predicted to be the same as the velocity at time t. During
# correction, this predicted value is fused with the measured value to produce the new velocity estimate. This can be
# problematic, as the final velocity will effectively be a weighted average of the old velocity and the new one. When
# this velocity is the integrated into a new pose, the result can be sluggish covergence. This effect is especially
# noticeable with LIDAR data during rotations. To get around it, users can try inflating the process_noise_covariance
# for the velocity variable in question, or decrease the variance of the variable in question in the measurement
# itself. In addition, users can also take advantage of the control command being issued to the robot at the time we
# make the prediction. If control is used, it will get converted into an acceleration term, which will be used during
# predicition. Note that if an acceleration measurement for the variable in question is available from one of the
# inputs, the control term will be ignored.
# Whether or not we use the control input during predicition. Defaults to false.
use_control: true
# Whether the input (assumed to be cmd_vel) is a geometry_msgs/Twist or geometry_msgs/TwistStamped message. Defaults to
# false.
stamped_control: false
# The last issued control command will be used in prediction for this period. Defaults to 0.2.
control_timeout: 0.2
# Which velocities are being controlled. Order is vx, vy, vz, vroll, vpitch, vyaw.
control_config: [true, false, false, false, false, true]
# Places limits on how large the acceleration term will be. Should match your robot's kinematics.
acceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 3.4]
# Acceleration and deceleration limits are not always the same for robots.
deceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 4.5]
# If your robot cannot instantaneously reach its acceleration limit, the permitted change can be controlled with these
# gains
acceleration_gains: [0.8, 0.0, 0.0, 0.0, 0.0, 0.9]
# If your robot cannot instantaneously reach its deceleration limit, the permitted change can be controlled with these
# gains
deceleration_gains: [1.0, 0.0, 0.0, 0.0, 0.0, 1.0]
# [ADVANCED] The process noise covariance matrix can be difficult to tune, and can vary for each application, so it is
# exposed as a configuration parameter. This matrix represents the noise we add to the total error after each
# prediction step. The better the omnidirectional motion model matches your system, the smaller these values can be.
# However, if users find that a given variable is slow to converge, one approach is to increase the
# process_noise_covariance diagonal value for the variable in question, which will cause the filter's predicted error
# to be larger, which will cause the filter to trust the incoming measurement more during correction. The values are
# ordered as x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Defaults to the matrix below if
# unspecified.
process_noise_covariance: [0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.04, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.02, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.015]
# [ADVANCED] This represents the initial value for the state estimate error covariance matrix. Setting a diagonal
# value (variance) to a large value will result in rapid convergence for initial measurements of the variable in
# question. Users should take care not to use large values for variables that will not be measured directly. The values
# are ordered as x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Defaults to the matrix below
#if unspecified.
initial_estimate_covariance: [1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9]

11
deploy/robot_ws/src/go2_description/CMakeLists.txt Normal file
View File

@ -0,0 +1,11 @@
cmake_minimum_required(VERSION 3.5)
project(go2_description)
find_package(ament_cmake REQUIRED)
# Install launch files.
install(DIRECTORY
launch dae config meshes urdf xacro
DESTINATION share/${PROJECT_NAME}/
)
ament_package()

54
deploy/robot_ws/src/go2_description/README.md Normal file
View File

@ -0,0 +1,54 @@
## go2_urdf
This repository contains the urdf model of go2.
## Build the library
Create a new catkin workspace:
```
# Create the directories
# Do not forget to change <...> parts
mkdir -p <directory_to_ws>/<catkin_ws_name>/src
cd <directory_to_ws>/<catkin_ws_name>/
# Initialize the catkin workspace
catkin init
```
Clone this library:
```
# Navigate to the directory of src
# Do not forget to change <...> parts
cd <directory_to_ws>/<catkin_ws_name>/src
git clone git@github.com:unitreerobotics/go2_urdf.git
```
Build:
```
# Build it
catkin build
# Source it
source <directory_to_ws>/<catkin_ws_name>/devel/setup.bash
```
## Run the library
```
# Show urdf model of go2 in Rviz
roslaunch go2_description go2_rviz.launch
```
## When used for isaac gym or other similiar engine
Collision parameters in urdf can be amended to better train the robot:
Open "go2_description.urdf" in "./go2_description/urdf",
and amend the ` box size="0.213 0.0245 0.034" ` in links of "FL_thigh", "FR_thigh", "RL_thigh", "RR_thigh".
For example, change previous values to ` box size="0.11 0.0245 0.034" ` means the length of the thigh is shortened from 0.213 to 0.11, which can avoid unnecessary collision between the thigh link and the calf link.
The collision model before and after the above amendment are shown as "Normal_collision_model.png" and "Amended_collision_model.png" respectively.

1
deploy/robot_ws/src/go2_description/config/joint_names_go2_description.yaml Normal file
View File

@ -0,0 +1 @@
controller_joint_names: ['', 'FL_hip_joint', 'Fl_thigh_joint', 'FL_calf_joint', 'FR_hip_joint', 'FR_thigh_joint', 'FR_calf_joint', 'RL_hip_joint', 'RL_thigh_joint', 'RL_calf_joint', 'RR_hip_joint', 'RR_thigh_joint', 'RR_calf_joint', ]

70
deploy/robot_ws/src/go2_description/config/robot_control.yaml Executable file
View File

@ -0,0 +1,70 @@
go2_gazebo:
# Publish all joint states -----------------------------------
joint_state_controller:
type: joint_state_controller/JointStateController
publish_rate: 1000
# FL Controllers ---------------------------------------
FL_hip_controller:
type: unitree_legged_control/UnitreeJointController
joint: FL_hip_joint
pid: {p: 100.0, i: 0.0, d: 5.0}
FL_thigh_controller:
type: unitree_legged_control/UnitreeJointController
joint: FL_thigh_joint
pid: {p: 300.0, i: 0.0, d: 8.0}
FL_calf_controller:
type: unitree_legged_control/UnitreeJointController
joint: FL_calf_joint
pid: {p: 300.0, i: 0.0, d: 8.0}
# FR Controllers ---------------------------------------
FR_hip_controller:
type: unitree_legged_control/UnitreeJointController
joint: FR_hip_joint
pid: {p: 100.0, i: 0.0, d: 5.0}
FR_thigh_controller:
type: unitree_legged_control/UnitreeJointController
joint: FR_thigh_joint
pid: {p: 300.0, i: 0.0, d: 8.0}
FR_calf_controller:
type: unitree_legged_control/UnitreeJointController
joint: FR_calf_joint
pid: {p: 300.0, i: 0.0, d: 8.0}
# RL Controllers ---------------------------------------
RL_hip_controller:
type: unitree_legged_control/UnitreeJointController
joint: RL_hip_joint
pid: {p: 100.0, i: 0.0, d: 5.0}
RL_thigh_controller:
type: unitree_legged_control/UnitreeJointController
joint: RL_thigh_joint
pid: {p: 300.0, i: 0.0, d: 8.0}
RL_calf_controller:
type: unitree_legged_control/UnitreeJointController
joint: RL_calf_joint
pid: {p: 300.0, i: 0.0, d: 8.0}
# RR Controllers ---------------------------------------
RR_hip_controller:
type: unitree_legged_control/UnitreeJointController
joint: RR_hip_joint
pid: {p: 100.0, i: 0.0, d: 5.0}
RR_thigh_controller:
type: unitree_legged_control/UnitreeJointController
joint: RR_thigh_joint
pid: {p: 300.0, i: 0.0, d: 8.0}
RR_calf_controller:
type: unitree_legged_control/UnitreeJointController
joint: RR_calf_joint
pid: {p: 300.0, i: 0.0, d: 8.0}

217
deploy/robot_ws/src/go2_description/dae/base.dae Normal file
View File

File diff suppressed because one or more lines are too long

130
deploy/robot_ws/src/go2_description/dae/calf.dae Normal file
View File

File diff suppressed because one or more lines are too long

130
deploy/robot_ws/src/go2_description/dae/calf_mirror.dae Normal file
View File

File diff suppressed because one or more lines are too long

89
deploy/robot_ws/src/go2_description/dae/foot.dae Normal file
View File

File diff suppressed because one or more lines are too long

130
deploy/robot_ws/src/go2_description/dae/hip.dae Normal file
View File

File diff suppressed because one or more lines are too long

130
deploy/robot_ws/src/go2_description/dae/thigh.dae Normal file
View File

File diff suppressed because one or more lines are too long

130
deploy/robot_ws/src/go2_description/dae/thigh_mirror.dae Normal file
View File

File diff suppressed because one or more lines are too long

67
deploy/robot_ws/src/go2_description/launch/visualize_go2.launch.py Normal file
View File

@ -0,0 +1,67 @@
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import Command, FindExecutable, LaunchConfiguration
from launch_ros.actions import Node
def generate_launch_description():
user_debug_parameter_name = "user_debug"
user_debug = LaunchConfiguration(user_debug_parameter_name)
# prefix = LaunchConfiguration("prefix", default="go2/")
go2_xacro_file = os.path.join(
get_package_share_directory("go2_description"), "xacro", "robot.xacro"
)
robot_description = Command(
[FindExecutable(name="xacro"), " ", go2_xacro_file, " DEBUG:=", user_debug]
)
rviz_file = os.path.join(
get_package_share_directory("go2_description"), "launch", "visualize_go2.rviz"
)
return LaunchDescription(
[
DeclareLaunchArgument(
user_debug_parameter_name,
default_value="false",
description="debug or not",
),
Node(
package="robot_state_publisher",
executable="robot_state_publisher",
name="robot_state_publisher",
output="screen",
parameters=[{"robot_description": robot_description}],
remappings=[
("/joint_states", "/go2/joint_states"),
("/robot_description", "/go2/robot_description"),
],
),
Node(
package="rviz2",
executable="rviz2",
name="rviz2",
arguments=["--display-config", rviz_file],
),
Node(
package="tf2_ros",
executable="static_transform_publisher",
arguments=[
"0.15",
"0",
"0.15",
"0",
"0",
"0.707107",
"0.707107",
"/trunk",
"/go2/hesai_lidar",
],
name="static_tf_pub_trunk_to_lidar",
),
]
)

355
deploy/robot_ws/src/go2_description/launch/visualize_go2.rviz Normal file
View File

@ -0,0 +1,355 @@
Panels:
- Class: rviz_common/Displays
Help Height: 78
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /TF1/Frames1
Splitter Ratio: 0.5
Tree Height: 787
- Class: rviz_common/Selection
Name: Selection
- Class: rviz_common/Tool Properties
Expanded:
- /2D Pose Estimate1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.5886790156364441
- Class: rviz_common/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz_default_plugins/Grid
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.029999999329447746
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 0
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 10
Reference Frame: <Fixed Frame>
Value: true
- Alpha: 0.5
Class: rviz_default_plugins/RobotModel
Collision Enabled: false
Description File: ""
Description Source: Topic
Description Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /go2/robot_description
Enabled: true
Links:
All Links Enabled: true
Expand Joint Details: false
Expand Link Details: false
Expand Tree: false
FL_calf:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
FL_calf_rotor:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
FL_foot:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
FL_hip:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
FL_hip_rotor:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
FL_thigh:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
FL_thigh_rotor:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
FR_calf:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
FR_calf_rotor:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
FR_foot:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
FR_hip:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
FR_hip_rotor:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
FR_thigh:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
FR_thigh_rotor:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Link Tree Style: Links in Alphabetic Order
RL_calf:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
RL_calf_rotor:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
RL_foot:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
RL_hip:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
RL_hip_rotor:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
RL_thigh:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
RL_thigh_rotor:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
RR_calf:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
RR_calf_rotor:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
RR_foot:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
RR_hip:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
RR_hip_rotor:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
RR_thigh:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
RR_thigh_rotor:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
base_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
imu_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
trunk:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Name: RobotModel
TF Prefix: ""
Update Interval: 0
Value: true
Visual Enabled: true
- Class: rviz_default_plugins/Axes
Enabled: false
Length: 1
Name: Axes
Radius: 0.10000000149011612
Reference Frame: <Fixed Frame>
Value: false
- Class: rviz_default_plugins/TF
Enabled: false
Frame Timeout: 15
Frames:
All Enabled: false
Marker Scale: 1
Name: TF
Show Arrows: true
Show Axes: true
Show Names: true
Tree:
{}
Update Interval: 0
Value: false
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz_default_plugins/PointCloud2
Color: 255; 255; 255
Color Transformer: Intensity
Decay Time: 0
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 81
Min Color: 0; 0; 0
Min Intensity: 1
Name: PointCloud2
Position Transformer: XYZ
Selectable: true
Size (Pixels): 3
Size (m): 0.009999999776482582
Style: Flat Squares
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /go2/lidar_points
Use Fixed Frame: true
Use rainbow: true
Value: true
Enabled: true
Global Options:
Background Color: 238; 238; 238
Fixed Frame: base
Frame Rate: 30
Name: root
Tools:
- Class: rviz_default_plugins/Interact
Hide Inactive Objects: true
- Class: rviz_default_plugins/MoveCamera
- Class: rviz_default_plugins/Select
- Class: rviz_default_plugins/FocusCamera
- Class: rviz_default_plugins/Measure
Line color: 128; 128; 0
- Class: rviz_default_plugins/SetInitialPose
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /initialpose
- Class: rviz_default_plugins/SetGoal
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /move_base_simple/goal
- Class: rviz_default_plugins/PublishPoint
Single click: true
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /clicked_point
Transformation:
Current:
Class: rviz_default_plugins/TF
Value: true
Views:
Current:
Class: rviz_default_plugins/Orbit
Distance: 5.272065162658691
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 0.18471483886241913
Y: -0.16858524084091187
Z: -0.1451287716627121
Focal Shape Fixed Size: false
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
Pitch: 0.3297958970069885
Target Frame: <Fixed Frame>
Value: Orbit (rviz_default_plugins)
Yaw: 4.5785908699035645
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 1016
Hide Left Dock: false
Hide Right Dock: true
QMainWindow State: 000000ff00000000fd0000000400000000000001b90000039efc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000039e000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d00650072006101000001be000001900000000000000000000000010000010f000002e2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002e2000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006400000003efc0100000002fb0000000800540069006d00650000000000000006400000000000000000fb0000000800540069006d00650100000000000004500000000000000000000005790000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: false
Width: 1848
X: 72
Y: 27

69
deploy/robot_ws/src/go2_description/launch/visualize_go2_virtual_arm.launch.py Normal file
View File

@ -0,0 +1,69 @@
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import Command, FindExecutable, LaunchConfiguration
from launch_ros.actions import Node
def generate_launch_description():
user_debug_parameter_name = "user_debug"
user_debug = LaunchConfiguration(user_debug_parameter_name)
# prefix = LaunchConfiguration("prefix", default="go2/")
go2_xacro_file = os.path.join(
get_package_share_directory("go2_description"),
"xacro",
"robot_virtual_arm.xacro",
)
robot_description = Command(
[FindExecutable(name="xacro"), " ", go2_xacro_file, " DEBUG:=", user_debug]
)
rviz_file = os.path.join(
get_package_share_directory("go2_description"), "launch", "visualize_go2.rviz"
)
return LaunchDescription(
[
DeclareLaunchArgument(
user_debug_parameter_name,
default_value="false",
description="debug or not",
),
Node(
package="robot_state_publisher",
executable="robot_state_publisher",
name="robot_state_publisher",
output="screen",
parameters=[{"robot_description": robot_description}],
remappings=[
("/joint_states", "/go2/joint_states"),
("/robot_description", "/go2/robot_description"),
],
),
Node(
package="rviz2",
executable="rviz2",
name="rviz2",
arguments=["--display-config", rviz_file],
),
Node(
package="tf2_ros",
executable="static_transform_publisher",
arguments=[
"0.15",
"0",
"0.15",
"0",
"0",
"0.707107",
"0.707107",
"/trunk",
"/go2/hesai_lidar",
],
name="static_tf_pub_trunk_to_lidar",
),
]
)

130
deploy/robot_ws/src/go2_description/meshes/calf.dae Executable file
View File

File diff suppressed because one or more lines are too long

130
deploy/robot_ws/src/go2_description/meshes/calf_mirror.dae Executable file
View File

File diff suppressed because one or more lines are too long

89
deploy/robot_ws/src/go2_description/meshes/foot.dae Executable file
View File

File diff suppressed because one or more lines are too long

130
deploy/robot_ws/src/go2_description/meshes/hip.dae Executable file
View File

File diff suppressed because one or more lines are too long

130
deploy/robot_ws/src/go2_description/meshes/thigh.dae Executable file
View File

File diff suppressed because one or more lines are too long

130
deploy/robot_ws/src/go2_description/meshes/thigh_mirror.dae Executable file
View File

File diff suppressed because one or more lines are too long

217
deploy/robot_ws/src/go2_description/meshes/trunk.dae Executable file
View File

File diff suppressed because one or more lines are too long

101
deploy/robot_ws/src/go2_description/meshes/virtual_arm.dae Normal file
View File

File diff suppressed because one or more lines are too long

24
deploy/robot_ws/src/go2_description/package.xml Normal file
View File

@ -0,0 +1,24 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>go2_description</name>
<version>0.0.1</version>
<description>go2_description contains URDF files and meshes of the Unitree Go2 robot</description>
<maintainer email="bd1555@nyu.edu">Bolun Dai</maintainer>
<license>Apache 2.0</license>
<author>Bolun Dai</author>
<buildtool_depend>ament_cmake</buildtool_depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<test_depend>ament_cmake_pytest</test_depend>
<exec_depend>xacro</exec_depend>
<exec_depend>rviz2</exec_depend>
<exec_depend>joint_state_publisher_gui</exec_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

BIN
deploy/robot_ws/src/go2_description/urdf/Amended_collision_model.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 213 KiB

BIN
deploy/robot_ws/src/go2_description/urdf/Normal_collision_model.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 200 KiB

1217
deploy/robot_ws/src/go2_description/urdf/go2_description.urdf Normal file
View File

File diff suppressed because it is too large Load Diff

121
deploy/robot_ws/src/go2_description/xacro/const.xacro Executable file
View File

@ -0,0 +1,121 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<!-- Constants for robot dimensions -->
<xacro:property name="PI" value="3.1415926535897931"/>
<xacro:property name="stick_mass" value="0.00001"/>
<!-- simplified collision value -->
<xacro:property name="trunk_width" value="0.0935"/>
<xacro:property name="trunk_length" value="0.3762"/>
<xacro:property name="trunk_height" value="0.114"/>
<xacro:property name="hip_radius" value="0.046"/>
<xacro:property name="hip_length" value="0.04"/>
<xacro:property name="thigh_shoulder_radius" value="0.041"/>
<xacro:property name="thigh_shoulder_length" value="0.032"/>
<xacro:property name="thigh_width" value="0.0245"/>
<xacro:property name="thigh_height" value="0.034"/>
<xacro:property name="calf_width" value="0.016"/>
<xacro:property name="calf_height" value="0.016"/>
<xacro:property name="foot_radius" value="0.02"/>
<xacro:property name="stick_radius" value="0.01"/>
<xacro:property name="stick_length" value="0.2"/>
<!-- kinematic value -->
<xacro:property name="thigh_offset" value="0.0955"/>
<xacro:property name="thigh_length" value="0.213"/>
<xacro:property name="calf_length" value="0.213"/>
<!-- leg offset from trunk center value -->
<xacro:property name="leg_offset_x" value="0.1934"/>
<xacro:property name="leg_offset_y" value="0.0465"/>
<!-- <xacro:property name="trunk_offset_z" value="0.01675"/> -->
<xacro:property name="hip_offset" value="0.0955"/>
<!-- offset of link locations (left front) -->
<xacro:property name="hip_offset_x" value="0.1934"/>
<xacro:property name="hip_offset_y" value="0.0465"/>
<xacro:property name="hip_offset_z" value="0.0"/>
<xacro:property name="thigh_offset_x" value="0"/>
<xacro:property name="thigh_offset_y" value="0.0955"/>
<xacro:property name="thigh_offset_z" value="0.0"/>
<xacro:property name="calf_offset_x" value="0.0"/>
<xacro:property name="calf_offset_y" value="0.0"/>
<xacro:property name="calf_offset_z" value="-0.213"/>
<!-- joint limits -->
<xacro:property name="damping" value="0.01"/>
<xacro:property name="friction" value="0.2"/>
<xacro:property name="hip_position_max" value="1.0472"/>
<xacro:property name="hip_position_min" value="-1.0472"/>
<xacro:property name="hip_velocity_max" value="30.1"/>
<xacro:property name="hip_torque_max" value="23.7"/>
<xacro:property name="thigh_position_max" value="3.4907"/>
<xacro:property name="thigh_position_min" value="-1.5708"/>
<xacro:property name="thigh_velocity_max" value="30.1"/>
<xacro:property name="thigh_torque_max" value="23.7"/>
<xacro:property name="calf_position_max" value="-0.83776"/>
<xacro:property name="calf_position_min" value="-2.7227"/>
<xacro:property name="calf_velocity_max" value="20.06"/>
<xacro:property name="calf_torque_max" value="35.55"/>
<!-- dynamics inertial value -->
<!-- trunk -->
<xacro:property name="trunk_mass" value="6.921"/>
<xacro:property name="trunk_com_x" value="0.021112"/>
<xacro:property name="trunk_com_y" value="0.00"/>
<xacro:property name="trunk_com_z" value="-0.005366"/>
<xacro:property name="trunk_ixx" value="0.02448"/>
<xacro:property name="trunk_ixy" value="0.00012166"/>
<xacro:property name="trunk_ixz" value="0.0014849"/>
<xacro:property name="trunk_iyy" value="0.098077"/>
<xacro:property name="trunk_iyz" value="-3.12E-05"/>
<xacro:property name="trunk_izz" value="0.107"/>
<!-- hip (left front) -->
<xacro:property name="hip_mass" value="0.678"/>
<xacro:property name="hip_com_x" value="-0.0054"/>
<xacro:property name="hip_com_y" value="0.00194"/>
<xacro:property name="hip_com_z" value="-0.000105"/>
<xacro:property name="hip_ixx" value="0.00048"/>
<xacro:property name="hip_ixy" value="-3.01E-06"/>
<xacro:property name="hip_ixz" value="1.11E-06"/>
<xacro:property name="hip_iyy" value="0.000884"/>
<xacro:property name="hip_iyz" value="-1.42E-06"/>
<xacro:property name="hip_izz" value="0.000596"/>
<!-- thigh -->
<xacro:property name="thigh_mass" value="1.152"/>
<xacro:property name="thigh_com_x" value="-0.00374"/>
<xacro:property name="thigh_com_y" value="-0.0223"/>
<xacro:property name="thigh_com_z" value="-0.0327"/>
<xacro:property name="thigh_ixx" value="0.00584"/>
<xacro:property name="thigh_ixy" value="8.72E-05"/>
<xacro:property name="thigh_ixz" value="-0.000289"/>
<xacro:property name="thigh_iyy" value="0.0058"/>
<xacro:property name="thigh_iyz" value="0.000808"/>
<xacro:property name="thigh_izz" value="0.00103"/>
<!-- calf -->
<xacro:property name="calf_mass" value="0.154"/>
<xacro:property name="calf_com_x" value="0.00548"/>
<xacro:property name="calf_com_y" value="-0.000975"/>
<xacro:property name="calf_com_z" value="-0.115"/>
<xacro:property name="calf_ixx" value="0.00108"/>
<xacro:property name="calf_ixy" value="3.4E-07"/>
<xacro:property name="calf_ixz" value="1.72E-05"/>
<xacro:property name="calf_iyy" value="0.0011"/>
<xacro:property name="calf_iyz" value="8.28E-06"/>
<xacro:property name="calf_izz" value="3.29E-05"/>
<!-- foot -->
<xacro:property name="foot_mass" value="0.06"/>
</robot>

257
deploy/robot_ws/src/go2_description/xacro/gazebo.xacro Normal file
View File

@ -0,0 +1,257 @@
<?xml version="1.0"?>
<robot>
<!-- ros_control plugin -->
<gazebo>
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
<robotNamespace>/go2_gazebo</robotNamespace>
<robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
</plugin>
</gazebo>
<!-- Show the trajectory of trunk center. -->
<!--gazebo>
<plugin name="3dplot" filename="libLinkPlot3DPlugin.so">
<frequency>10</frequency>
<plot>
<link>base</link>
<pose>0 0 0 0 0 0</pose>
<material>Gazebo/Yellow</material>
</plot>
</plugin>
</gazebo-->
<!-- Show the trajectory of foot. You can add another trajectory about another foot. -->
<gazebo>
<plugin name="3dplot" filename="libLinkPlot3DPlugin.so">
<frequency>100</frequency>
<plot>
<link>FL_foot</link>
<pose>0 0 0 0 0 0</pose>
</plot>
</plugin>
</gazebo>
<gazebo>
<plugin filename="libgazebo_ros_force.so" name="gazebo_ros_force">
<bodyName>trunk</bodyName>
<topicName>/apply_force/trunk</topicName>
</plugin>
</gazebo>
<gazebo reference="imu_link">
<gravity>true</gravity>
<sensor name="imu_sensor" type="imu">
<always_on>true</always_on>
<update_rate>1000</update_rate>
<visualize>true</visualize>
<topic>__default_topic__</topic>
<plugin filename="libgazebo_ros_imu_sensor.so" name="imu_plugin">
<topicName>trunk_imu</topicName>
<bodyName>imu_link</bodyName>
<updateRateHZ>1000.0</updateRateHZ>
<gaussianNoise>0.0</gaussianNoise>
<xyzOffset>0 0 0</xyzOffset>
<rpyOffset>0 0 0</rpyOffset>
<frameName>imu_link</frameName>
</plugin>
<pose>0 0 0 0 0 0</pose>
</sensor>
</gazebo>
<!-- Foot contacts. -->
<gazebo reference="FR_calf">
<sensor name="FR_foot_contact" type="contact">
<update_rate>100</update_rate>
<plugin name="contactPlugin" filename="libunitreeFootContactPlugin.so"/>
<contact>
<collision>FR_calf_fixed_joint_lump__FR_foot_collision_1</collision>
</contact>
</sensor>
</gazebo>
<gazebo reference="FL_calf">
<sensor name="FL_foot_contact" type="contact">
<update_rate>100</update_rate>
<plugin name="contactPlugin" filename="libunitreeFootContactPlugin.so"/>
<contact>
<collision>FL_calf_fixed_joint_lump__FL_foot_collision_1</collision>
</contact>
</sensor>
</gazebo>
<gazebo reference="RR_calf">
<sensor name="RR_foot_contact" type="contact">
<update_rate>100</update_rate>
<plugin name="contactPlugin" filename="libunitreeFootContactPlugin.so"/>
<contact>
<collision>RR_calf_fixed_joint_lump__RR_foot_collision_1</collision>
</contact>
</sensor>
</gazebo>
<gazebo reference="RL_calf">
<sensor name="RL_foot_contact" type="contact">
<update_rate>100</update_rate>
<plugin name="contactPlugin" filename="libunitreeFootContactPlugin.so"/>
<contact>
<collision>RL_calf_fixed_joint_lump__RL_foot_collision_1</collision>
</contact>
</sensor>
</gazebo>
<!-- Visualization of Foot contacts. -->
<gazebo reference="FR_foot">
<visual>
<plugin name="drawForcePlugin" filename="libunitreeDrawForcePlugin.so">
<topicName>FR_foot_contact</topicName>
</plugin>
</visual>
</gazebo>
<gazebo reference="FL_foot">
<visual>
<plugin name="drawForcePlugin" filename="libunitreeDrawForcePlugin.so">
<topicName>FL_foot_contact</topicName>
</plugin>
</visual>
</gazebo>
<gazebo reference="RR_foot">
<visual>
<plugin name="drawForcePlugin" filename="libunitreeDrawForcePlugin.so">
<topicName>RR_foot_contact</topicName>
</plugin>
</visual>
</gazebo>
<gazebo reference="RL_foot">
<visual>
<plugin name="drawForcePlugin" filename="libunitreeDrawForcePlugin.so">
<topicName>RL_foot_contact</topicName>
</plugin>
</visual>
</gazebo>
<gazebo reference="base">
<turnGravityOff>false</turnGravityOff>
</gazebo>
<gazebo reference="trunk">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<kp value="1000000.0"/>
<kd value="1.0"/>
</gazebo>
<gazebo reference="stick_link">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
</gazebo>
<gazebo reference="imu_link">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
</gazebo>
<!-- FL leg -->
<gazebo reference="FL_hip">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
</gazebo>
<gazebo reference="FL_thigh">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<self_collide>1</self_collide>
<kp value="1000000.0"/>
<kd value="1.0"/>
</gazebo>
<gazebo reference="FL_calf">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<self_collide>1</self_collide>
</gazebo>
<gazebo reference="FL_foot">
<mu1>0.6</mu1>
<mu2>0.6</mu2>
<self_collide>1</self_collide>
<kp value="1000000.0"/>
<kd value="1.0"/>
</gazebo>
<!-- FR leg -->
<gazebo reference="FR_hip">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
</gazebo>
<gazebo reference="FR_thigh">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<self_collide>1</self_collide>
<kp value="1000000.0"/>
<kd value="1.0"/>
</gazebo>
<gazebo reference="FR_calf">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<self_collide>1</self_collide>
</gazebo>
<gazebo reference="FR_foot">
<mu1>0.6</mu1>
<mu2>0.6</mu2>
<self_collide>1</self_collide>
<kp value="1000000.0"/>
<kd value="1.0"/>
</gazebo>
<!-- RL leg -->
<gazebo reference="RL_hip">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
</gazebo>
<gazebo reference="RL_thigh">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<self_collide>1</self_collide>
<kp value="1000000.0"/>
<kd value="1.0"/>
</gazebo>
<gazebo reference="RL_calf">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<self_collide>1</self_collide>
</gazebo>
<gazebo reference="RL_foot">
<mu1>0.6</mu1>
<mu2>0.6</mu2>
<self_collide>1</self_collide>
<kp value="1000000.0"/>
<kd value="1.0"/>
</gazebo>
<!-- RR leg -->
<gazebo reference="RR_hip">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
</gazebo>
<gazebo reference="RR_thigh">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<self_collide>1</self_collide>
<kp value="1000000.0"/>
<kd value="1.0"/>
</gazebo>
<gazebo reference="RR_calf">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<self_collide>1</self_collide>
</gazebo>
<gazebo reference="RR_foot">
<mu1>0.6</mu1>
<mu2>0.6</mu2>
<self_collide>1</self_collide>
<kp value="1000000.0"/>
<kd value="1.0"/>
</gazebo>
</robot>

176
deploy/robot_ws/src/go2_description/xacro/leg.xacro Executable file
View File

@ -0,0 +1,176 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:include filename="$(find go2_description)/xacro/transmission.xacro"/>
<xacro:macro name="leg" params="name mirror mirror_dae front_hind front_hind_dae">
<joint name="${name}_hip_joint" type="revolute">
<xacro:if value="${(mirror_dae == False) and (front_hind_dae == True)}">
<origin rpy="0 0 0" xyz="${leg_offset_x} ${-leg_offset_y} 0"/>
</xacro:if>
<xacro:if value="${(mirror_dae == True) and (front_hind_dae == True)}">
<origin rpy="0 0 0" xyz="${leg_offset_x} ${leg_offset_y} 0"/>
</xacro:if>
<xacro:if value="${(mirror_dae == False) and (front_hind_dae == False)}">
<origin rpy="0 0 0" xyz="${-leg_offset_x} ${-leg_offset_y} 0"/>
</xacro:if>
<xacro:if value="${(mirror_dae == True) and (front_hind_dae == False)}">
<origin rpy="0 0 0" xyz="${-leg_offset_x} ${leg_offset_y} 0"/>
</xacro:if>
<parent link="trunk"/>
<child link="${name}_hip"/>
<axis xyz="1 0 0"/>
<dynamics damping="${damping}" friction="${friction}"/>
<xacro:if value="${(mirror_dae == True)}">
<limit effort="${hip_torque_max}" velocity="${hip_velocity_max}" lower="${hip_position_min}" upper="${hip_position_max}"/>
</xacro:if>
<xacro:if value="${(mirror_dae == False)}">
<limit effort="${hip_torque_max}" velocity="${hip_velocity_max}" lower="${-hip_position_max}" upper="${-hip_position_min}"/>
</xacro:if>
</joint>
<link name="${name}_hip">
<visual>
<xacro:if value="${(mirror_dae == True) and (front_hind_dae == True)}">
<origin rpy="0 0 0" xyz="0 0 0"/>
</xacro:if>
<xacro:if value="${(mirror_dae == False) and (front_hind_dae == True)}">
<origin rpy="${PI} 0 0" xyz="0 0 0"/>
</xacro:if>
<xacro:if value="${(mirror_dae == True) and (front_hind_dae == False)}">
<origin rpy="0 ${PI} 0" xyz="0 0 0"/>
</xacro:if>
<xacro:if value="${(mirror_dae == False) and (front_hind_dae == False)}">
<origin rpy="${PI} ${PI} 0" xyz="0 0 0"/>
</xacro:if>
<geometry>
<mesh filename="package://go2_description/meshes/hip.dae" scale="1 1 1"/>
</geometry>
<!-- <material name="orange"/> -->
</visual>
<collision>
<origin rpy="${PI/2.0} 0 0" xyz="0 ${hip_offset*mirror} 0"/>
<geometry>
<cylinder length="${hip_length}" radius="${hip_radius}"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="${hip_com_x*front_hind} ${hip_com_y*mirror} ${hip_com_z}"/>
<mass value="${hip_mass}"/>
<inertia
ixx="${hip_ixx}" ixy="${hip_ixy*mirror*front_hind}" ixz="${hip_ixz*front_hind}"
iyy="${hip_iyy}" iyz="${hip_iyz*mirror}"
izz="${hip_izz}"/>
</inertial>
</link>
<joint name="${name}_thigh_joint" type="revolute">
<origin rpy="0 0 0" xyz="0 ${thigh_offset*mirror} 0"/>
<parent link="${name}_hip"/>
<child link="${name}_thigh"/>
<axis xyz="0 1 0"/>
<dynamics damping="${damping}" friction="${friction}"/>
<limit effort="${thigh_torque_max}" velocity="${thigh_velocity_max}" lower="${thigh_position_min}" upper="${thigh_position_max}"/>
</joint>
<link name="${name}_thigh">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<xacro:if value="${mirror_dae == True}">
<mesh filename="package://go2_description/meshes/thigh.dae" scale="1 1 1"/>
</xacro:if>
<xacro:if value="${mirror_dae == False}">
<mesh filename="package://go2_description/meshes/thigh_mirror.dae" scale="1 1 1"/>
</xacro:if>
</geometry>
<!-- <material name="orange"/> -->
</visual>
<collision>
<origin rpy="0 ${PI/2.0} 0" xyz="0 0 ${-thigh_length/2.0}"/>
<geometry>
<box size="${thigh_length} ${thigh_width} ${thigh_height}"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="${thigh_com_x} ${thigh_com_y*mirror} ${thigh_com_z}"/>
<mass value="${thigh_mass}"/>
<inertia
ixx="${thigh_ixx}" ixy="${thigh_ixy*mirror}" ixz="${thigh_ixz}"
iyy="${thigh_iyy}" iyz="${thigh_iyz*mirror}"
izz="${thigh_izz}"/>
</inertial>
</link>
<joint name="${name}_calf_joint" type="revolute">
<origin rpy="0 0 0" xyz="0 0 ${-thigh_length}"/>
<parent link="${name}_thigh"/>
<child link="${name}_calf"/>
<axis xyz="0 1 0"/>
<dynamics damping="${damping}" friction="${friction}"/>
<limit effort="${calf_torque_max}" velocity="${calf_velocity_max}" lower="${calf_position_min}" upper="${calf_position_max}"/>
</joint>
<link name="${name}_calf">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://go2_description/meshes/calf.dae" scale="1 1 1"/>
</geometry>
<!-- <material name="orange"/> -->
</visual>
<collision>
<origin rpy="0 ${PI/2.0} 0" xyz="0 0 ${-calf_length/2.0}"/>
<geometry>
<box size="${calf_length} ${calf_width} ${calf_height}"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="${calf_com_x} ${calf_com_y} ${calf_com_z}"/>
<mass value="${calf_mass}"/>
<inertia
ixx="${calf_ixx}" ixy="${calf_ixy}" ixz="${calf_ixz}"
iyy="${calf_iyy}" iyz="${calf_iyz}"
izz="${calf_izz}"/>
</inertial>
</link>
<joint name="${name}_foot_fixed" type="fixed">
<origin rpy="0 0 0" xyz="0 0 ${-(calf_length)}"/>
<parent link="${name}_calf"/>
<child link="${name}_foot"/>
</joint>
<link name="${name}_foot">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="${foot_radius-0.01}"/>
</geometry>
<!-- <material name="orange"/> -->
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="${foot_radius}"/>
</geometry>
</collision>
<inertial>
<mass value="${foot_mass}"/>
<inertia
ixx="${(2*foot_mass)/5.0*(foot_radius*foot_radius)}" ixy="0.0" ixz="0.0"
iyy="${(2*foot_mass)/5.0*(foot_radius*foot_radius)}" iyz="0.0"
izz="${(2*foot_mass)/5.0*(foot_radius*foot_radius)}"/>
</inertial>
</link>
<xacro:leg_transmission name="${name}"/>
</xacro:macro>
</robot>

40
deploy/robot_ws/src/go2_description/xacro/materials.xacro Executable file
View File

@ -0,0 +1,40 @@
<?xml version="1.0"?>
<robot>
<material name="black">
<color rgba="0.0 0.0 0.0 1.0"/>
</material>
<material name="blue">
<color rgba="0.0 0.0 0.8 1.0"/>
</material>
<material name="green">
<color rgba="0.0 0.8 0.0 1.0"/>
</material>
<material name="grey">
<color rgba="0.2 0.2 0.2 0.0"/>
</material>
<material name="silver">
<color rgba="${233/255} ${233/255} ${216/255} 1.0"/>
</material>
<material name="orange">
<color rgba="${255/255} ${108/255} ${10/255} 1.0"/>
</material>
<material name="brown">
<color rgba="${222/255} ${207/255} ${195/255} 1.0"/>
</material>
<material name="red">
<color rgba="0.8 0.0 0.0 1.0"/>
</material>
<material name="white">
<color rgba="1.0 1.0 1.0 1.0"/>
</material>
</robot>

469
deploy/robot_ws/src/go2_description/xacro/robot.urdf Normal file
View File

@ -0,0 +1,469 @@
<?xml version="1.0" ?>
<!-- =================================================================================== -->
<!-- | This document was autogenerated by xacro from robot.xacro | -->
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
<!-- =================================================================================== -->
<robot name="go2">
<material name="black">
<color rgba="0.0 0.0 0.0 1.0"/>
</material>
<material name="blue">
<color rgba="0.0 0.0 0.8 1.0"/>
</material>
<material name="green">
<color rgba="0.0 0.8 0.0 1.0"/>
</material>
<material name="grey">
<color rgba="0.2 0.2 0.2 0.0"/>
</material>
<material name="silver">
<color rgba="0.9137254901960784 0.9137254901960784 0.8470588235294118 1.0"/>
</material>
<material name="orange">
<color rgba="1.0 0.4235294117647059 0.0392156862745098 1.0"/>
</material>
<material name="brown">
<color rgba="0.8705882352941177 0.8117647058823529 0.7647058823529411 1.0"/>
</material>
<material name="red">
<color rgba="0.8 0.0 0.0 1.0"/>
</material>
<material name="white">
<color rgba="1.0 1.0 1.0 1.0"/>
</material>
<link name="base">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.001 0.001 0.001"/>
</geometry>
</visual>
</link>
<joint name="floating_base" type="fixed">
<origin rpy="0 0 0" xyz="0 0 0"/>
<parent link="base"/>
<child link="trunk"/>
</joint>
<link name="trunk">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.3762 0.0935 0.114"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0.021112 0.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>
</link>
<joint name="imu_joint" type="fixed">
<parent link="trunk"/>
<child link="imu_link"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
</joint>
<link name="imu_link">
<inertial>
<mass value="0.001"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.001 0.001 0.001"/>
</geometry>
<!-- <material name="red"/> -->
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size=".001 .001 .001"/>
</geometry>
</collision>
</link>
<joint name="FR_hip_joint" type="revolute">
<origin rpy="0 0 0" xyz="0.1934 -0.0465 0"/>
<parent link="trunk"/>
<child link="FR_hip"/>
<axis xyz="1 0 0"/>
<dynamics damping="0.01" friction="0.2"/>
<limit effort="23.7" lower="-1.0472" upper="1.0472" velocity="30.1"/>
</joint>
<link name="FR_hip">
<visual>
<origin rpy="3.141592653589793 0 0" xyz="0 0 0"/>
</visual>
<collision>
<origin rpy="1.5707963267948966 0 0" xyz="0 -0.0955 0"/>
<geometry>
<cylinder length="0.04" radius="0.046"/>
</geometry>
</collision>
<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>
</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.01" friction="0.2"/>
<limit effort="23.7" lower="-1.5708" upper="3.4907" velocity="30.1"/>
</joint>
<link name="FR_thigh">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
</visual>
<collision>
<origin rpy="0 1.5707963267948966 0" xyz="0 0 -0.1065"/>
<geometry>
<box size="0.213 0.0245 0.034"/>
</geometry>
</collision>
<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>
</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.01" friction="0.2"/>
<limit effort="35.55" lower="-2.7227" upper="-0.83776" velocity="20.06"/>
</joint>
<link name="FR_calf">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
</visual>
<collision>
<origin rpy="0 1.5707963267948966 0" xyz="0 0 -0.1065"/>
<geometry>
<box size="0.213 0.016 0.016"/>
</geometry>
</collision>
<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>
</link>
<joint name="FR_foot_fixed" type="fixed">
<origin rpy="0 0 0" xyz="0 0 -0.213"/>
<parent link="FR_calf"/>
<child link="FR_foot"/>
</joint>
<link name="FR_foot">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.01"/>
</geometry>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.02"/>
</geometry>
</collision>
<inertial>
<mass value="0.06"/>
<inertia ixx="9.600000000000001e-06" ixy="0.0" ixz="0.0" iyy="9.600000000000001e-06" iyz="0.0" izz="9.600000000000001e-06"/>
</inertial>
</link>
<joint name="FL_hip_joint" type="revolute">
<origin rpy="0 0 0" xyz="0.1934 0.0465 0"/>
<parent link="trunk"/>
<child link="FL_hip"/>
<axis xyz="1 0 0"/>
<dynamics damping="0.01" friction="0.2"/>
<limit effort="23.7" lower="-1.0472" upper="1.0472" velocity="30.1"/>
</joint>
<link name="FL_hip">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
</visual>
<collision>
<origin rpy="1.5707963267948966 0 0" xyz="0 0.0955 0"/>
<geometry>
<cylinder length="0.04" radius="0.046"/>
</geometry>
</collision>
<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>
</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.01" friction="0.2"/>
<limit effort="23.7" lower="-1.5708" upper="3.4907" velocity="30.1"/>
</joint>
<link name="FL_thigh">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
</visual>
<collision>
<origin rpy="0 1.5707963267948966 0" xyz="0 0 -0.1065"/>
<geometry>
<box size="0.213 0.0245 0.034"/>
</geometry>
</collision>
<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>
</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.01" friction="0.2"/>
<limit effort="35.55" lower="-2.7227" upper="-0.83776" velocity="20.06"/>
</joint>
<link name="FL_calf">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
</visual>
<collision>
<origin rpy="0 1.5707963267948966 0" xyz="0 0 -0.1065"/>
<geometry>
<box size="0.213 0.016 0.016"/>
</geometry>
</collision>
<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>
</link>
<joint name="FL_foot_fixed" type="fixed">
<origin rpy="0 0 0" xyz="0 0 -0.213"/>
<parent link="FL_calf"/>
<child link="FL_foot"/>
</joint>
<link name="FL_foot">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.01"/>
</geometry>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.02"/>
</geometry>
</collision>
<inertial>
<mass value="0.06"/>
<inertia ixx="9.600000000000001e-06" ixy="0.0" ixz="0.0" iyy="9.600000000000001e-06" iyz="0.0" izz="9.600000000000001e-06"/>
</inertial>
</link>
<joint name="RR_hip_joint" type="revolute">
<origin rpy="0 0 0" xyz="-0.1934 -0.0465 0"/>
<parent link="trunk"/>
<child link="RR_hip"/>
<axis xyz="1 0 0"/>
<dynamics damping="0.01" friction="0.2"/>
<limit effort="23.7" lower="-1.0472" upper="1.0472" velocity="30.1"/>
</joint>
<link name="RR_hip">
<visual>
<origin rpy="3.141592653589793 3.141592653589793 0" xyz="0 0 0"/>
</visual>
<collision>
<origin rpy="1.5707963267948966 0 0" xyz="0 -0.0955 0"/>
<geometry>
<cylinder length="0.04" radius="0.046"/>
</geometry>
</collision>
<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>
</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.01" friction="0.2"/>
<limit effort="23.7" lower="-1.5708" upper="3.4907" velocity="30.1"/>
</joint>
<link name="RR_thigh">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
</visual>
<collision>
<origin rpy="0 1.5707963267948966 0" xyz="0 0 -0.1065"/>
<geometry>
<box size="0.213 0.0245 0.034"/>
</geometry>
</collision>
<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>
</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.01" friction="0.2"/>
<limit effort="35.55" lower="-2.7227" upper="-0.83776" velocity="20.06"/>
</joint>
<link name="RR_calf">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
</visual>
<collision>
<origin rpy="0 1.5707963267948966 0" xyz="0 0 -0.1065"/>
<geometry>
<box size="0.213 0.016 0.016"/>
</geometry>
</collision>
<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>
</link>
<joint name="RR_foot_fixed" type="fixed">
<origin rpy="0 0 0" xyz="0 0 -0.213"/>
<parent link="RR_calf"/>
<child link="RR_foot"/>
</joint>
<link name="RR_foot">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.01"/>
</geometry>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.02"/>
</geometry>
</collision>
<inertial>
<mass value="0.06"/>
<inertia ixx="9.600000000000001e-06" ixy="0.0" ixz="0.0" iyy="9.600000000000001e-06" iyz="0.0" izz="9.600000000000001e-06"/>
</inertial>
</link>
<joint name="RL_hip_joint" type="revolute">
<origin rpy="0 0 0" xyz="-0.1934 0.0465 0"/>
<parent link="trunk"/>
<child link="RL_hip"/>
<axis xyz="1 0 0"/>
<dynamics damping="0.01" friction="0.2"/>
<limit effort="23.7" lower="-1.0472" upper="1.0472" velocity="30.1"/>
</joint>
<link name="RL_hip">
<visual>
<origin rpy="0 3.141592653589793 0" xyz="0 0 0"/>
</visual>
<collision>
<origin rpy="1.5707963267948966 0 0" xyz="0 0.0955 0"/>
<geometry>
<cylinder length="0.04" radius="0.046"/>
</geometry>
</collision>
<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>
</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.01" friction="0.2"/>
<limit effort="23.7" lower="-1.5708" upper="3.4907" velocity="30.1"/>
</joint>
<link name="RL_thigh">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
</visual>
<collision>
<origin rpy="0 1.5707963267948966 0" xyz="0 0 -0.1065"/>
<geometry>
<box size="0.213 0.0245 0.034"/>
</geometry>
</collision>
<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>
</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.01" friction="0.2"/>
<limit effort="35.55" lower="-2.7227" upper="-0.83776" velocity="20.06"/>
</joint>
<link name="RL_calf">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
</visual>
<collision>
<origin rpy="0 1.5707963267948966 0" xyz="0 0 -0.1065"/>
<geometry>
<box size="0.213 0.016 0.016"/>
</geometry>
</collision>
<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>
</link>
<joint name="RL_foot_fixed" type="fixed">
<origin rpy="0 0 0" xyz="0 0 -0.213"/>
<parent link="RL_calf"/>
<child link="RL_foot"/>
</joint>
<link name="RL_foot">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.02"/>
</geometry>
</collision>
<inertial>
<mass value="0.06"/>
<inertia ixx="9.600000000000001e-06" ixy="0.0" ixz="0.0" iyy="9.600000000000001e-06" iyz="0.0" izz="9.600000000000001e-06"/>
</inertial>
</link>
</robot>

125
deploy/robot_ws/src/go2_description/xacro/robot.xacro Executable file
View File

@ -0,0 +1,125 @@
<?xml version="1.0"?>
<robot name="go2" xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:arg name="DEBUG" default="false"/>
<xacro:include filename="$(find go2_description)/xacro/const.xacro"/>
<xacro:include filename="$(find go2_description)/xacro/materials.xacro"/>
<xacro:include filename="$(find go2_description)/xacro/leg.xacro"/>
<!-- <xacro:include filename="$(find go2_description)/xacro/stairs.xacro"/> -->
<xacro:include filename="$(find go2_description)/xacro/gazebo.xacro"/>
<!-- <xacro:include filename="$(find go2_gazebo)/launch/stairs.urdf.xacro"/> -->
<!-- <xacro:stairs stairs="15" xpos="0" ypos="0" zpos="0" /> -->
<!-- Rotor related joint and link is only for demonstrate location. -->
<!-- Actually, the rotor will rotate and the joint is not fixed. Reduction ratio should be considered. -->
<!-- Debug mode will hung up the robot, use "true" or "false" to switch it. -->
<xacro:if value="$(arg DEBUG)">
<link name="world"/>
<joint name="base_static_joint" type="fixed">
<origin rpy="0 0 0" xyz="0 0 0"/>
<parent link="world"/>
<child link="base_link"/>
</joint>
</xacro:if>
<link name="base_link">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.001 0.001 0.001"/>
</geometry>
</visual>
</link>
<joint name="floating_base" type="fixed">
<origin rpy="0 0 0" xyz="0 0 0"/>
<parent link="base_link"/>
<child link="trunk"/>
</joint>
<link name="trunk">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://go2_description/meshes/trunk.dae" scale="1 1 1"/>
</geometry>
<!-- <material name="orange"/> -->
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="${trunk_length} ${trunk_width} ${trunk_height}"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="${trunk_com_x} ${trunk_com_y} ${trunk_com_z}"/>
<mass value="${trunk_mass}"/>
<inertia
ixx="${trunk_ixx}" ixy="${trunk_ixy}" ixz="${trunk_ixz}"
iyy="${trunk_iyy}" iyz="${trunk_iyz}"
izz="${trunk_izz}"/>
</inertial>
</link>
<joint name="imu_joint" type="fixed">
<parent link="trunk"/>
<child link="imu_link"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
</joint>
<link name="imu_link">
<inertial>
<mass value="0.001"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.001 0.001 0.001"/>
</geometry>
<!-- <material name="red"/> -->
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size=".001 .001 .001"/>
</geometry>
</collision>
</link>
<!--
<joint name="load_joint" type="fixed">
<parent link="trunk"/>
<child link="load_link"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
</joint>
<link name="load_link">
<inertial>
<mass value="5"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0.2"/>
<geometry>
<box size="0.5 0.3 0.2"/>
</geometry>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size=".001 .001 .001"/>
</geometry>
</collision>
</link>
-->
<xacro:leg name="FR" mirror="-1" mirror_dae= "False" front_hind="1" front_hind_dae="True" />
<xacro:leg name="FL" mirror="1" mirror_dae="True" front_hind="1" front_hind_dae="True" />
<xacro:leg name="RR" mirror="-1" mirror_dae="False" front_hind="-1" front_hind_dae="False" />
<xacro:leg name="RL" mirror="1" mirror_dae="True" front_hind="-1" front_hind_dae="False" />
</robot>

147
deploy/robot_ws/src/go2_description/xacro/robot_virtual_arm.xacro Executable file
View File

@ -0,0 +1,147 @@
<?xml version="1.0"?>
<robot name="go2" xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:arg name="DEBUG" default="false"/>
<xacro:include filename="$(find go2_description)/xacro/const.xacro"/>
<xacro:include filename="$(find go2_description)/xacro/materials.xacro"/>
<xacro:include filename="$(find go2_description)/xacro/leg.xacro"/>
<!-- <xacro:include filename="$(find go2_description)/xacro/stairs.xacro"/> -->
<xacro:include filename="$(find go2_description)/xacro/gazebo.xacro"/>
<!-- <xacro:include filename="$(find go2_gazebo)/launch/stairs.urdf.xacro"/> -->
<!-- <xacro:stairs stairs="15" xpos="0" ypos="0" zpos="0" /> -->
<!-- Rotor related joint and link is only for demonstrate location. -->
<!-- Actually, the rotor will rotate and the joint is not fixed. Reduction ratio should be considered. -->
<!-- Debug mode will hung up the robot, use "true" or "false" to switch it. -->
<xacro:if value="$(arg DEBUG)">
<link name="world"/>
<joint name="base_static_joint" type="fixed">
<origin rpy="0 0 0" xyz="0 0 0"/>
<parent link="world"/>
<child link="base_link"/>
</joint>
</xacro:if>
<link name="base_link">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.001 0.001 0.001"/>
</geometry>
</visual>
</link>
<joint name="floating_base" type="fixed">
<origin rpy="0 0 0" xyz="0 0 0"/>
<parent link="base_link"/>
<child link="trunk"/>
</joint>
<link name="trunk">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://go2_description/meshes/trunk.dae" scale="1 1 1"/>
</geometry>
<!-- <material name="orange"/> -->
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="${trunk_length} ${trunk_width} ${trunk_height}"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="${trunk_com_x} ${trunk_com_y} ${trunk_com_z}"/>
<mass value="${trunk_mass}"/>
<inertia
ixx="${trunk_ixx}" ixy="${trunk_ixy}" ixz="${trunk_ixz}"
iyy="${trunk_iyy}" iyz="${trunk_iyz}"
izz="${trunk_izz}"/>
</inertial>
</link>
<joint name="imu_joint" type="fixed">
<parent link="trunk"/>
<child link="imu_link"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
</joint>
<link name="imu_link">
<inertial>
<mass value="0.001"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.001 0.001 0.001"/>
</geometry>
<!-- <material name="red"/> -->
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size=".001 .001 .001"/>
</geometry>
</collision>
</link>
<link name="virtual_arm">
<visual>
<origin rpy="0 0 0" xyz="1.0 0.0 0.0"/>
<geometry>
<mesh filename="package://go2_description/meshes/virtual_arm.dae"/>
</geometry>
<material name="red">
<color rgba="1. 0. 0. 0.5"/>
</material>
</visual>
</link>
<joint name="virtual_arm_joint" type="revolute">
<origin rpy="0 0 0" xyz="0 0 0"/>
<parent link="trunk"/>
<child link="virtual_arm"/>
<axis xyz="0 0 1"/>
<dynamics damping="0" friction="0"/>
<limit effort="100.0" lower="-3.1416" upper="3.1416" velocity="20.0"/>
</joint>
<!--
<joint name="load_joint" type="fixed">
<parent link="trunk"/>
<child link="load_link"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
</joint>
<link name="load_link">
<inertial>
<mass value="5"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0.2"/>
<geometry>
<box size="0.5 0.3 0.2"/>
</geometry>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size=".001 .001 .001"/>
</geometry>
</collision>
</link>
-->
<xacro:leg name="FR" mirror="-1" mirror_dae= "False" front_hind="1" front_hind_dae="True" />
<xacro:leg name="FL" mirror="1" mirror_dae="True" front_hind="1" front_hind_dae="True" />
<xacro:leg name="RR" mirror="-1" mirror_dae="False" front_hind="-1" front_hind_dae="False" />
<xacro:leg name="RL" mirror="1" mirror_dae="True" front_hind="-1" front_hind_dae="False" />
</robot>

42
deploy/robot_ws/src/go2_description/xacro/transmission.xacro Executable file
View File

@ -0,0 +1,42 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:macro name="leg_transmission" params="name">
<transmission name="${name}_hip_tran">
<type>transmission_interface/SimpleTransmission</type>
<joint name="${name}_hip_joint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="${name}_hip_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="${name}_thigh_tran">
<type>transmission_interface/SimpleTransmission</type>
<joint name="${name}_thigh_joint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="${name}_thigh_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="${name}_calf_tran">
<type>transmission_interface/SimpleTransmission</type>
<joint name="${name}_calf_joint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="${name}_calf_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
</xacro:macro>
</robot>

10
deploy/robot_ws/src/go2py_node/src/bridge.cpp
View File

@ -185,10 +185,10 @@ void Custom::lowstate_callback(unitree_go::msg::LowState::SharedPtr data)
joint_state.name.push_back("RL_thigh_joint"); joint_state.name.push_back("RL_thigh_joint");
joint_state.name.push_back("RL_calf_joint"); joint_state.name.push_back("RL_calf_joint");
// add foot force joint state for contact detection // add foot force joint state for contact detection
joint_state.name.push_back("FR_foot_force"); //joint_state.name.push_back("FR_foot_force");
joint_state.name.push_back("FL_foot_force"); //joint_state.name.push_back("FL_foot_force");
joint_state.name.push_back("RR_foot_force"); //joint_state.name.push_back("RR_foot_force");
joint_state.name.push_back("RL_foot_force"); //joint_state.name.push_back("RL_foot_force");
for(int i=0; i<12; i++) for(int i=0; i<12; i++)
{ {
@ -197,10 +197,12 @@ void Custom::lowstate_callback(unitree_go::msg::LowState::SharedPtr data)
joint_state.effort.push_back(data->motor_state[i].tau_est); joint_state.effort.push_back(data->motor_state[i].tau_est);
} }
// use the last four joint_state.position to store the foot force // use the last four joint_state.position to store the foot force
/*
for(int i=0; i<4; i++) for(int i=0; i<4; i++)
{ {
joint_state.effort.push_back(data->foot_force[i]); joint_state.effort.push_back(data->foot_force[i]);
} }
*/
pub_joint->publish(joint_state); pub_joint->publish(joint_state);
pub_imu->publish(imu); pub_imu->publish(imu);
// Check for emergency stop // Check for emergency stop

4
deploy/scripts/dock_hw_start.sh Normal file
View File

@ -0,0 +1,4 @@
source /opt/ros/humble/setup.bash
source /unitree_ros2/cyclonedds_ws/install/setup.bash
export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
source /dock_ws/install/setup.bash && ros2 launch /root/launch/dock.launch.py

6
deploy/scripts/go2py-hw-nodes.service → deploy/scripts/go2py-dock-hw-nodes.service
View File

@ -1,12 +1,12 @@
[Unit] [Unit]
Description=ROS2 device driver container Description=ROS2 device driver container
Requires=docker.service Requires=multi-user.target
After=docker.service After=multi-user.target
[Service] [Service]
Restart=always Restart=always
ExecStartPre=/usr/bin/docker rm -f go2py_docker || true ExecStartPre=/usr/bin/docker rm -f go2py_docker || true
ExecStart=/bin/bash -c '/usr/bin/docker run --rm --name go2py_docker --privileged --network host -v /home/unitree/locomotion:/home/locomotion -v /dev/*:/dev/* -v /etc/localtime:/etc/localtime:ro --runtime nvidia go2py:latest' ExecStart=/bin/bash -c '/usr/bin/docker run --rm --name go2py_docker --privileged --network host -v /home/unitree/locomotion:/home/locomotion -v /dev/*:/dev/* -v /etc/localtime:/etc/localtime:ro --runtime nvidia go2py_dock_hw:latest'
ExecStop=/usr/bin/docker stop -t 2 go2py_docker ExecStop=/usr/bin/docker stop -t 2 go2py_docker
[Install] [Install]

13
deploy/scripts/go2py-robot-hw-nodes.service Normal file
View File

@ -0,0 +1,13 @@
[Unit]
Description=ROS2 device driver container
Requires=multi-user.target
After=multi-user.target
[Service]
Restart=always
ExecStartPre=/usr/bin/docker rm -f go2py_robot_docker || true
ExecStart=/bin/bash -c '/usr/bin/docker run --rm --name go2py_robot_docker --privileged --network host -v /home/unitree/locomotion:/home/locomotion -v /dev/*:/dev/* -v /etc/localtime:/etc/localtime:ro --runtime nvidia go2py_robot_hw:latest'
ExecStop=/usr/bin/docker stop -t 2 go2py_robot_docker
[Install]
WantedBy=default.target

1
deploy/scripts/hw_start.sh
View File

@ -1 +0,0 @@
source /dock_ws/install/setup.bash && ros2 launch /root/launch/hw.launch.py

4
deploy/scripts/robot_hw_start.sh Normal file
View File

@ -0,0 +1,4 @@
source /opt/ros/humble/setup.bash
source /unitree_ros2/cyclonedds_ws/install/setup.bash
export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
source /robot_ws/install/setup.bash && ros2 launch /root/launch/robot.launch.py

206
examples/isaacgym_test.py Normal file
View File

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

185
rl_environment.yml Normal file
View File

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