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