Merge branch 'main' of github.com:Rooholla-KhorramBakht/Go2Py
This commit is contained in:
jogima-cyber
commit
3e8d59e174
|
|
@ -1,56 +0,0 @@
|
|||
FROM ros:humble
|
||||
ENV DEBIAN_FRONTEND noninteractive
|
||||
SHELL ["/bin/bash", "-c"]
|
||||
RUN apt-get update && apt-get install -y -qq --no-install-recommends \
|
||||
libglvnd-dev \
|
||||
libgl1-mesa-dev \
|
||||
libegl1-mesa-dev \
|
||||
libgles2-mesa-dev \
|
||||
libxext6 \
|
||||
libx11-6 \
|
||||
freeglut3-dev \
|
||||
git \
|
||||
python3-pip \
|
||||
python3-tk \
|
||||
curl \
|
||||
vim \
|
||||
libcgal-dev \
|
||||
libcgal-demo \
|
||||
ros-humble-rmw-cyclonedds-cpp ros-humble-rosidl-generator-dds-idl \
|
||||
libyaml-cpp-dev \
|
||||
ros-humble-xacro \
|
||||
libboost-all-dev\
|
||||
build-essential \
|
||||
cmake \
|
||||
git \
|
||||
&& rm -rf /var/lib/apt/lists/*
|
||||
|
||||
RUN curl -s https://packagecloud.io/install/repositories/github/git-lfs/script.deb.sh | bash
|
||||
RUN apt-get install git-lfs
|
||||
|
||||
# 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
|
||||
|
||||
# Install Python dependencies
|
||||
RUN pip3 install scipy ipykernel warp-lang
|
||||
RUN pip3 install torch torchvision torchaudio --index-url https://download.pytorch.org/whl/cu118
|
||||
RUN pip3 install matplotlib opencv-python proxsuite
|
||||
RUN pip3 install isort black
|
||||
RUN pip3 install warp-lang scikit-learn casadi mujoco pin
|
||||
|
||||
RUN pip install jupyter ipykernel
|
||||
RUN pip install cyclonedds pygame
|
||||
RUN pip install pynput pygame
|
||||
RUN pip install onnx onnxruntime
|
||||
|
||||
# Set environmental variables required for using ROS
|
||||
RUN echo 'source /opt/ros/humble/setup.bash' >> ~/.bashrc
|
||||
RUN echo 'source /unitree_ros2/cyclonedds_ws/install/setup.bash' >> ~/.bashrc
|
||||
RUN echo 'export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp' >> ~/.bashrc
|
||||
RUN echo 'export CYCLONEDDS_URI=file:///home/Go2py/Go2Py/assets/cyclonedds.xml' >> ~/.bashrc
|
||||
|
||||
# Env vars for the nvidia-container-runtime.
|
||||
ENV NVIDIA_VISIBLE_DEVICES all
|
||||
ENV NVIDIA_DRIVER_CAPABILITIES graphics,utility,compute
|
||||
|
|
@ -0,0 +1 @@
|
|||
Dockerfile.x86
|
||||
|
|
@ -0,0 +1,42 @@
|
|||
FROM nvcr.io/nvidia/isaac/ros:aarch64-ros2_humble_42f50fd45227c63eb74af1d69ddc2970
|
||||
ENV DEBIAN_FRONTEND noninteractive
|
||||
ARG CONDA_VER=latest
|
||||
ARG OS_TYPE=aarch64
|
||||
|
||||
RUN apt-get update && apt-get install -y -qq --no-install-recommends cuda \
|
||||
wget \
|
||||
ros-humble-rmw-cyclonedds-cpp ros-humble-rosidl-generator-dds-idl \
|
||||
ros-humble-pcl-ros \
|
||||
&& rm -rf /var/lib/apt/lists/*
|
||||
|
||||
# MID360 Lidar Drivers
|
||||
RUN cd /root && git clone https://github.com/Livox-SDK/Livox-SDK2.git && cd ./Livox-SDK2/ && mkdir build && cd build && cmake .. && make -j4 && make install
|
||||
RUN cd /root && git clone https://github.com/Livox-SDK/livox_ros_driver2.git ws_livox/src/livox_ros_driver2 && cd ws_livox/src/livox_ros_driver2 && source /opt/ros/humble/setup.bash && cd /root/ws_livox/src/livox_ros_driver2 && /bin/bash build.sh humble
|
||||
# FAST-LIO Install
|
||||
RUN cd /root && mkdir -p fastlio-ws/src && cd fastlio-ws/src && git clone https://github.com/Ericsii/FAST_LIO.git --recursive && cd FAST_LIO && git checkout ros2 && cd /root/fastlio-ws && source /root/ws_livox/install/setup.bash && colcon build --symlink-install
|
||||
|
||||
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 "export CYCLONEDDS_HOME=/unitree_ros2/cyclonedds_ws/install/cyclonedds/" >> ~/.bashrc
|
||||
ENV CYCLONEDDS_HOME "/unitree_ros2/cyclonedds_ws/install/cyclonedds"
|
||||
RUN cd /root && git clone https://github.com/eclipse-cyclonedds/cyclonedds-python -b releases/0.10.x && cd cyclonedds-python && pip install .
|
||||
|
||||
# CycloneDDS-cxx
|
||||
# RUN cd /root && git clone https://github.com/eclipse-cyclonedds/cyclonedds-cxx -b releases/0.10.x &&\
|
||||
# cd cyclonedds-cxx && mkdir build install && cd build && cmake .. && \
|
||||
# cmake --build . --target install
|
||||
|
||||
# Install Python dependencies
|
||||
RUN pip install matplotlib opencv-python scipy isort black onnx onnxruntime
|
||||
RUN pip install https://github.com/NVIDIA/warp/releases/download/v1.5.1/warp_lang-1.5.1+cu11-py3-none-manylinux2014_aarch64.whl
|
||||
RUN pip install pygame pynput jupyter ipykernel meshcat
|
||||
|
||||
RUN echo "export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp" >> ~/.bashrc
|
||||
|
||||
# RUN echo "export CYCLONEDDS_URI='<CycloneDDS><Domain><General><Interfaces><NetworkInterface name="eth0" priority="default" multicast="default" /></Interfaces></General></Domain></CycloneDDS>'" >> ~/.bashrc
|
||||
# RUN echo 'source /unitree_ros2/cyclonedds_ws/install/setup.bash' >> ~/.bashrc
|
||||
# RUN echo 'export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp' >> ~/.bashrc
|
||||
# RUN echo 'export CYCLONEDDS_URI=file:///home/Go2py/Go2Py/assets/cyclonedds.xml' >> ~/.bashrc
|
||||
|
|
@ -0,0 +1,79 @@
|
|||
FROM ros:humble
|
||||
ENV DEBIAN_FRONTEND noninteractive
|
||||
ARG CONDA_VER=latest
|
||||
ARG OS_TYPE=x86_64
|
||||
SHELL ["/bin/bash", "-c"]
|
||||
|
||||
RUN apt-get update && apt-get install -y -qq --no-install-recommends \
|
||||
libglvnd-dev \
|
||||
libgl1-mesa-dev \
|
||||
libegl1-mesa-dev \
|
||||
libgles2-mesa-dev \
|
||||
libxext6 \
|
||||
libx11-6 \
|
||||
freeglut3-dev \
|
||||
git \
|
||||
python3-pip \
|
||||
python3-tk \
|
||||
curl \
|
||||
vim \
|
||||
libcgal-dev \
|
||||
libcgal-demo \
|
||||
ros-humble-rmw-cyclonedds-cpp ros-humble-rosidl-generator-dds-idl \
|
||||
libyaml-cpp-dev \
|
||||
ros-humble-xacro \
|
||||
libboost-all-dev\
|
||||
build-essential \
|
||||
cmake \
|
||||
git \
|
||||
wget \
|
||||
&& rm -rf /var/lib/apt/lists/*
|
||||
|
||||
RUN curl -s https://packagecloud.io/install/repositories/github/git-lfs/script.deb.sh | bash
|
||||
RUN apt-get install git-lfs
|
||||
|
||||
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
|
||||
|
||||
|
||||
# Install Miniconda
|
||||
RUN wget https://github.com/conda-forge/miniforge/releases/${CONDA_VER}/download/Miniforge3-Linux-${OS_TYPE}.sh -O ~/miniconda.sh \
|
||||
&& /bin/bash ~/miniconda.sh -b -p /opt/conda \
|
||||
&& rm ~/miniconda.sh \
|
||||
&& ln -s /opt/conda/etc/profile.d/conda.sh /etc/profile.d/conda.sh \
|
||||
&& echo ". /opt/conda/etc/profile.d/conda.sh" >> ~/.bashrc \
|
||||
&& echo "conda activate base" >> ~/.bashrc
|
||||
|
||||
ENV PATH /opt/conda/bin:$PATH
|
||||
|
||||
RUN conda create -n go2py python==3.8.10
|
||||
SHELL ["conda", "run", "-n", "go2py", "/bin/bash", "-c"]
|
||||
|
||||
ENV CONDA_PREFIX /opt/conda
|
||||
|
||||
RUN echo "export CYCLONEDDS_HOME=/unitree_ros2/cyclonedds_ws/install/cyclonedds/" >> ~/.bashrc
|
||||
ENV CYCLONEDDS_HOME "/unitree_ros2/cyclonedds_ws/install/cyclonedds"
|
||||
RUN cd /root && git clone https://github.com/eclipse-cyclonedds/cyclonedds-python -b releases/0.10.x && cd cyclonedds-python && pip install .
|
||||
|
||||
# Install Python dependencies
|
||||
RUN pip install torch torchvision torchaudio --index-url https://download.pytorch.org/whl/cu118
|
||||
RUN pip install matplotlib opencv-python proxsuite scipy isort black
|
||||
RUN pip install warp-lang scikit-learn casadi
|
||||
RUN pip install onnx onnxruntime
|
||||
RUN pip install pygame pynput jupyter ipykernel
|
||||
RUN pip install meshcat mujoco
|
||||
RUN conda install -y -c conda-forge \
|
||||
pinocchio \
|
||||
crocoddyl \
|
||||
mim-solvers
|
||||
|
||||
# Set environmental variables required for using ROS
|
||||
RUN echo 'source /opt/ros/humble/setup.bash' >> ~/.bashrc
|
||||
RUN echo 'source /unitree_ros2/cyclonedds_ws/install/setup.bash' >> ~/.bashrc
|
||||
RUN echo 'export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp' >> ~/.bashrc
|
||||
RUN echo 'export CYCLONEDDS_URI=file:///home/Go2py/Go2Py/assets/cyclonedds.xml' >> ~/.bashrc
|
||||
|
||||
# Env vars for the nvidia-container-runtime.
|
||||
ENV NVIDIA_VISIBLE_DEVICES all
|
||||
ENV NVIDIA_DRIVER_CAPABILITIES graphics,utility,compute
|
||||
|
|
@ -10,9 +10,12 @@ services:
|
|||
- /tmp/.X11-unix:/tmp/.X11-unix
|
||||
- ../:/home/Go2py
|
||||
- /dev/input:/dev/input
|
||||
- /dev/shm:/dev/shm
|
||||
- /dev:/dev
|
||||
environment:
|
||||
- DISPLAY=${DISPLAY}
|
||||
- QT_X11_NO_MITSHM=1
|
||||
runtime: nvidia
|
||||
stdin_open: true
|
||||
tty: true
|
||||
ipc: host
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
|
|
@ -0,0 +1,59 @@
|
|||
<mujoco model="d1_description">
|
||||
<compiler angle="radian" meshdir="../" autolimits="true"/>
|
||||
|
||||
<asset>
|
||||
<mesh name="base_link" file="d1_assets/stl/base_link.STL"/>
|
||||
<mesh name="Link1" file="d1_assets/stl/Link1.STL"/>
|
||||
<mesh name="Link2" file="d1_assets/stl/Link2.STL"/>
|
||||
<mesh name="Link3" file="d1_assets/stl/Link3.STL"/>
|
||||
<mesh name="Link4" file="d1_assets/stl/Link4.STL"/>
|
||||
<mesh name="Link5" file="d1_assets/stl/Link5.STL"/>
|
||||
<mesh name="Link6" file="d1_assets/stl/Link6.STL"/>
|
||||
<mesh name="Link7_1" file="d1_assets/stl/Link7_1.STL"/>
|
||||
<mesh name="Link7_2" file="d1_assets/stl/Link7_2.STL"/>
|
||||
</asset>
|
||||
|
||||
<worldbody>
|
||||
<geom type="mesh" rgba="1 1 1 1" mesh="base_link"/>
|
||||
<body name="Link1" pos="0 0 0.0533" quat="-3.67321e-06 0 0 -1">
|
||||
<inertial pos="0.0024649 0.00010517 0.032696" quat="0.999717 -0.0236435 -0.00169588 -0.00221682" mass="0.13174" diaginertia="6.72365e-05 5.41766e-05 4.66199e-05"/>
|
||||
<joint name="Joint1" pos="0 0 0" axis="0 0 1" range="-2.35 2.35"/>
|
||||
<geom type="mesh" rgba="1 1 1 1" mesh="Link1"/>
|
||||
<body name="Link2" pos="0 0.028 0.0563" quat="-2.59734e-06 -2.59735e-06 -0.707108 -0.707105">
|
||||
<inertial pos="0.0002018 0.19201 -0.027007" quat="0.706347 0.707865 -0.00093681 0.00051949" mass="0.20213" diaginertia="0.00025682 0.000229681 6.33062e-05"/>
|
||||
<joint name="Joint2" pos="0 0 0" axis="0 0 -1" range="-1.57 1.57"/>
|
||||
<geom type="mesh" rgba="1 1 1 1" mesh="Link2"/>
|
||||
<body name="Link3" pos="0 0.2693 0.0009">
|
||||
<inertial pos="0.015164 0.044482 -0.027461" quat="0.690744 0.690771 -0.151176 -0.151179" mass="0.0629" diaginertia="1.84718e-05 1.4964e-05 1.13662e-05"/>
|
||||
<joint name="Joint3" pos="0 0 0" axis="0 0 -1" range="-1.57 1.57"/>
|
||||
<geom type="mesh" rgba="1 1 1 1" mesh="Link3"/>
|
||||
<body name="Link4" pos="0.0577 0.042 -0.0275" quat="0.499998 -0.5 0.500002 -0.5">
|
||||
<inertial pos="-0.00029556 -0.00016104 0.091339" quat="0.999998 -3.46195e-05 0.00170171 0.00102754" mass="0.083332" diaginertia="3.93083e-05 3.4378e-05 1.15967e-05"/>
|
||||
<joint name="Joint4" pos="0 0 0" axis="0 0 1" range="-2.35 2.35"/>
|
||||
<geom type="mesh" rgba="1 1 1 1" mesh="Link4"/>
|
||||
<body name="Link5" pos="-0.0001 -0.0237 0.14018" quat="0.499998 0.5 -0.5 0.500002">
|
||||
<inertial pos="0.040573 0.0062891 -0.023838" quat="0.706153 0.706691 -0.0310982 -0.0311281" mass="0.053817" diaginertia="1.31069e-05 1.1049e-05 8.59523e-06"/>
|
||||
<joint name="Joint5" pos="0 0 0" axis="0 0 -1" range="-1.57 1.57"/>
|
||||
<geom type="mesh" rgba="1 1 1 1" mesh="Link5"/>
|
||||
<body name="Link6" pos="0.0825 -0.0010782 -0.023822" quat="0.499998 -0.5 0.500002 -0.5">
|
||||
<inertial pos="-0.0068528 -3.9973e-06 0.039705" quat="0.494949 0.495288 -0.504676 0.504992" mass="0.077892" diaginertia="4.8843e-05 3.8232e-05 1.7707e-05"/>
|
||||
<joint name="Joint6" pos="0 0 0" axis="0 0 -1" range="-2.35 2.35"/>
|
||||
<geom type="mesh" rgba="1 1 1 1" mesh="Link6"/>
|
||||
<body name="Link7_1" pos="-0.0056012 -0.029636 0.0706" quat="0.499848 -0.50015 -0.49985 -0.500152">
|
||||
<inertial pos="0.018927 0.006 0.012082" quat="0 0.677073 0 0.735916" mass="0.015046" diaginertia="2.62715e-06 2.0229e-06 1.25975e-06"/>
|
||||
<joint name="Joint7_1" pos="0 0 0" axis="0 0 -1" type="slide" range="0 0.03"/>
|
||||
<geom type="mesh" rgba="1 1 1 1" mesh="Link7_1"/>
|
||||
</body>
|
||||
<body name="Link7_2" pos="-0.0056388 0.02964 0.0706" quat="0.500148 0.49985 -0.50015 0.499852">
|
||||
<inertial pos="0.018927 -0.006 0.012082" quat="0 0.677073 0 0.735916" mass="0.015046" diaginertia="2.62715e-06 2.0229e-06 1.25975e-06"/>
|
||||
<joint name="Joint7_2" pos="0 0 0" axis="0 0 1" type="slide" range="-0.03 0"/>
|
||||
<geom type="mesh" rgba="1 1 1 1" mesh="Link7_2"/>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</worldbody>
|
||||
</mujoco>
|
||||
|
|
@ -1,5 +1,5 @@
|
|||
<mujoco model="go2">
|
||||
<compiler angle="radian" meshdir="assets" autolimits="true"/>
|
||||
<compiler angle="radian" meshdir="../" autolimits="true"/>
|
||||
|
||||
<option cone="elliptic" impratio="100"/>
|
||||
|
||||
|
|
@ -46,22 +46,22 @@
|
|||
<texture type="2d" name="groundplane" builtin="checker" mark="edge" rgb1="0.2 0.3 0.4" rgb2="0.1 0.2 0.3" markrgb="0.8 0.8 0.8" width="300" height="300"/>
|
||||
<material name="groundplane" texture="groundplane" texuniform="true" texrepeat="5 5" reflectance="0.2"/>
|
||||
|
||||
<mesh file="base_0.obj"/>
|
||||
<mesh file="base_1.obj"/>
|
||||
<mesh file="base_2.obj"/>
|
||||
<mesh file="base_3.obj"/>
|
||||
<mesh file="base_4.obj"/>
|
||||
<mesh file="hip_0.obj"/>
|
||||
<mesh file="hip_1.obj"/>
|
||||
<mesh file="thigh_0.obj"/>
|
||||
<mesh file="thigh_1.obj"/>
|
||||
<mesh file="thigh_mirror_0.obj"/>
|
||||
<mesh file="thigh_mirror_1.obj"/>
|
||||
<mesh file="calf_0.obj"/>
|
||||
<mesh file="calf_1.obj"/>
|
||||
<mesh file="calf_mirror_0.obj"/>
|
||||
<mesh file="calf_mirror_1.obj"/>
|
||||
<mesh file="foot.obj"/>
|
||||
<mesh file="go2_assets/obj/base_0.obj"/>
|
||||
<mesh file="go2_assets/obj/base_1.obj"/>
|
||||
<mesh file="go2_assets/obj/base_2.obj"/>
|
||||
<mesh file="go2_assets/obj/base_3.obj"/>
|
||||
<mesh file="go2_assets/obj/base_4.obj"/>
|
||||
<mesh file="go2_assets/obj/hip_0.obj"/>
|
||||
<mesh file="go2_assets/obj/hip_1.obj"/>
|
||||
<mesh file="go2_assets/obj/thigh_0.obj"/>
|
||||
<mesh file="go2_assets/obj/thigh_1.obj"/>
|
||||
<mesh file="go2_assets/obj/thigh_mirror_0.obj"/>
|
||||
<mesh file="go2_assets/obj/thigh_mirror_1.obj"/>
|
||||
<mesh file="go2_assets/obj/calf_0.obj"/>
|
||||
<mesh file="go2_assets/obj/calf_1.obj"/>
|
||||
<mesh file="go2_assets/obj/calf_mirror_0.obj"/>
|
||||
<mesh file="go2_assets/obj/calf_mirror_1.obj"/>
|
||||
<mesh file="go2_assets/obj/foot.obj"/>
|
||||
</asset>
|
||||
|
||||
<worldbody>
|
||||
|
|
|
|||
|
|
@ -0,0 +1,306 @@
|
|||
<mujoco model="go2">
|
||||
<compiler angle="radian" meshdir="../" autolimits="true"/>
|
||||
|
||||
<option cone="elliptic" impratio="100"/>
|
||||
|
||||
<default>
|
||||
<default class="go2">
|
||||
<geom friction="0.6" margin="0.001" condim="1"/>
|
||||
<joint axis="0 1 0" damping="0" armature="0.01" frictionloss="0.2"/>
|
||||
<motor ctrlrange="-23.7 23.7"/>
|
||||
<default class="abduction">
|
||||
<joint axis="1 0 0" range="-1.0472 1.0472"/>
|
||||
</default>
|
||||
<default class="hip">
|
||||
<default class="front_hip">
|
||||
<joint range="-1.5708 3.4907"/>
|
||||
</default>
|
||||
<default class="back_hip">
|
||||
<joint range="-0.5236 4.5379"/>
|
||||
</default>
|
||||
</default>
|
||||
<default class="knee">
|
||||
<joint range="-2.7227 -0.83776"/>
|
||||
<motor ctrlrange="-45.43 45.43"/>
|
||||
</default>
|
||||
<default class="arm">
|
||||
<joint range="-2.7227 -0.83776"/>
|
||||
<motor ctrlrange="-45.43 45.43"/>
|
||||
</default>
|
||||
<default class="visual">
|
||||
<geom type="mesh" contype="0" conaffinity="0" group="2"/>
|
||||
</default>
|
||||
<default class="collision">
|
||||
<geom group="3"/>
|
||||
<default class="foot">
|
||||
<geom size="0.022" pos="-0.002 0 -0.213" priority="1" solimp="0.015 1 0.031" condim="6"
|
||||
friction="0.8 0.02 0.01"/>
|
||||
</default>
|
||||
</default>
|
||||
</default>
|
||||
|
||||
</default>
|
||||
|
||||
<asset>
|
||||
<hfield name="terrain" nrow="300" ncol="300" size="5 5 1. 0.1"/>
|
||||
<material name="metal" rgba=".9 .95 .95 1"/>
|
||||
<material name="black" rgba="0 0 0 1"/>
|
||||
<material name="white" rgba="1 1 1 1"/>
|
||||
<material name="gray" rgba="0.671705 0.692426 0.774270 1"/>
|
||||
<texture type="skybox" builtin="gradient" rgb1="0.3 0.5 0.7" rgb2="0 0 0" width="512" height="3072"/>
|
||||
<texture type="2d" name="groundplane" builtin="checker" mark="edge" rgb1="0.2 0.3 0.4" rgb2="0.1 0.2 0.3" markrgb="0.8 0.8 0.8" width="300" height="300"/>
|
||||
<material name="groundplane" texture="groundplane" texuniform="true" texrepeat="5 5" reflectance="0.2"/>
|
||||
|
||||
<mesh file="go2_assets/obj/base_0.obj"/>
|
||||
<mesh file="go2_assets/obj/base_1.obj"/>
|
||||
<mesh file="go2_assets/obj/base_2.obj"/>
|
||||
<mesh file="go2_assets/obj/base_3.obj"/>
|
||||
<mesh file="go2_assets/obj/base_4.obj"/>
|
||||
<mesh file="go2_assets/obj/hip_0.obj"/>
|
||||
<mesh file="go2_assets/obj/hip_1.obj"/>
|
||||
<mesh file="go2_assets/obj/thigh_0.obj"/>
|
||||
<mesh file="go2_assets/obj/thigh_1.obj"/>
|
||||
<mesh file="go2_assets/obj/thigh_mirror_0.obj"/>
|
||||
<mesh file="go2_assets/obj/thigh_mirror_1.obj"/>
|
||||
<mesh file="go2_assets/obj/calf_0.obj"/>
|
||||
<mesh file="go2_assets/obj/calf_1.obj"/>
|
||||
<mesh file="go2_assets/obj/calf_mirror_0.obj"/>
|
||||
<mesh file="go2_assets/obj/calf_mirror_1.obj"/>
|
||||
<mesh file="go2_assets/obj/foot.obj"/>
|
||||
<mesh name="base_link" file="d1_assets/stl/base_link.STL"/>
|
||||
<mesh name="Link1" file="d1_assets/stl/Link1.STL"/>
|
||||
<mesh name="Link2" file="d1_assets/stl/Link2.STL"/>
|
||||
<mesh name="Link3" file="d1_assets/stl/Link3.STL"/>
|
||||
<mesh name="Link4" file="d1_assets/stl/Link4.STL"/>
|
||||
<mesh name="Link5" file="d1_assets/stl/Link5.STL"/>
|
||||
<mesh name="Link6" file="d1_assets/stl/Link6.STL"/>
|
||||
<mesh name="Link7_1" file="d1_assets/stl/Link7_1.STL"/>
|
||||
<mesh name="Link7_2" file="d1_assets/stl/Link7_2.STL"/>
|
||||
</asset>
|
||||
|
||||
<worldbody>
|
||||
<geom name="heightfield" pos="0.0 0.0 -0.1" type="hfield" hfield="terrain"/>
|
||||
<geom name="floor" size="0 0 0.05" type="plane" material="groundplane"/>
|
||||
<body name="base" pos="0 0 0.445" childclass="go2">
|
||||
<light pos="0.0 0.0 2.5" />
|
||||
<camera name="front_camera" mode="fixed" pos="0.321618 0.033305 0.081622" euler="1.57079632679 0. 0." resolution="640 480" fovy="45."/>
|
||||
<inertial pos="0.021112 0 -0.005366" quat="-0.000543471 0.713435 -0.00173769 0.700719" mass="6.921"
|
||||
diaginertia="0.107027 0.0980771 0.0244531"/>
|
||||
<freejoint/>
|
||||
<geom mesh="base_0" material="black" class="visual"/>
|
||||
<geom mesh="base_1" material="black" class="visual"/>
|
||||
<geom mesh="base_2" material="black" class="visual"/>
|
||||
<geom mesh="base_3" material="white" class="visual"/>
|
||||
<geom mesh="base_4" material="gray" class="visual"/>
|
||||
<geom size="0.1881 0.04675 0.057" type="box" class="collision"/>
|
||||
<geom size="0.05 0.045" pos="0.285 0 0.01" type="cylinder" class="collision"/>
|
||||
<geom size="0.047" pos="0.293 0 -0.06" class="collision"/>
|
||||
<site name="imu" pos="-0.02557 0 0.04232"/>
|
||||
|
||||
<body name="FR_hip" pos="0.1934 -0.0465 0">
|
||||
<inertial pos="-0.0054 -0.00194 -0.000105" quat="0.498237 0.505462 0.499245 0.497014" mass="0.678"
|
||||
diaginertia="0.00088403 0.000596003 0.000479967"/>
|
||||
<joint name="FR_hip_joint" class="abduction"/>
|
||||
<geom mesh="hip_0" material="metal" class="visual" quat="4.63268e-05 1 0 0"/>
|
||||
<geom mesh="hip_1" material="gray" class="visual" quat="4.63268e-05 1 0 0"/>
|
||||
<geom size="0.046 0.02" pos="0 -0.08 0" quat="0.707107 0.707107 0 0" type="cylinder" class="collision"/>
|
||||
<body name="FR_thigh" pos="0 -0.0955 0">
|
||||
<inertial pos="-0.00374 0.0223 -0.0327" quat="0.551623 -0.0200632 0.0847635 0.829533" mass="1.152"
|
||||
diaginertia="0.00594973 0.00584149 0.000878787"/>
|
||||
<joint name="FR_thigh_joint" class="front_hip"/>
|
||||
<geom mesh="thigh_mirror_0" material="metal" class="visual"/>
|
||||
<geom mesh="thigh_mirror_1" material="gray" class="visual"/>
|
||||
<geom size="0.1065 0.01225 0.017" pos="0 0 -0.1065" quat="0.707107 0 0.707107 0" type="box" class="collision"/>
|
||||
<body name="FR_calf" pos="0 0 -0.213">
|
||||
<inertial pos="0.00629595 0.000622121 -0.141417" quat="0.703508 -0.00450087 0.00154099 0.710672"
|
||||
mass="0.241352" diaginertia="0.0014901 0.00146356 5.31397e-05"/>
|
||||
<joint name="FR_calf_joint" class="knee"/>
|
||||
<geom mesh="calf_mirror_0" material="gray" class="visual"/>
|
||||
<geom mesh="calf_mirror_1" material="black" class="visual"/>
|
||||
<geom size="0.013 0.06" pos="0.01 0 -0.06" quat="0.995004 0 -0.0998334 0" type="cylinder" class="collision"/>
|
||||
<geom size="0.011 0.0325" pos="0.02 0 -0.148" quat="0.999688 0 0.0249974 0" type="cylinder" class="collision"/>
|
||||
<geom pos="0 0 -0.213" mesh="foot" class="visual" material="black"/>
|
||||
<geom name="FR" class="foot"/>
|
||||
<site name="FR_foot" pos="0 0 -0.213" size="0.02"/>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
|
||||
<body name="FL_hip" pos="0.1934 0.0465 0">
|
||||
<inertial pos="-0.0054 0.00194 -0.000105" quat="0.497014 0.499245 0.505462 0.498237" mass="0.678"
|
||||
diaginertia="0.00088403 0.000596003 0.000479967"/>
|
||||
<joint name="FL_hip_joint" class="abduction"/>
|
||||
<geom mesh="hip_0" material="metal" class="visual"/>
|
||||
<geom mesh="hip_1" material="gray" class="visual"/>
|
||||
<geom size="0.046 0.02" pos="0 0.08 0" quat="1 1 0 0" type="cylinder" class="collision"/>
|
||||
<body name="FL_thigh" pos="0 0.0955 0">
|
||||
<inertial pos="-0.00374 -0.0223 -0.0327" quat="0.829533 0.0847635 -0.0200632 0.551623" mass="1.152"
|
||||
diaginertia="0.00594973 0.00584149 0.000878787"/>
|
||||
<joint name="FL_thigh_joint" class="front_hip"/>
|
||||
<geom mesh="thigh_0" material="metal" class="visual"/>
|
||||
<geom mesh="thigh_1" material="gray" class="visual"/>
|
||||
<geom size="0.1065 0.01225 0.017" pos="0 0 -0.1065" quat="0.707107 0 0.707107 0" type="box" class="collision"/>
|
||||
<body name="FL_calf" pos="0 0 -0.213">
|
||||
<inertial pos="0.00629595 -0.000622121 -0.141417" quat="0.710672 0.00154099 -0.00450087 0.703508"
|
||||
mass="0.241352" diaginertia="0.0014901 0.00146356 5.31397e-05"/>
|
||||
<joint name="FL_calf_joint" class="knee"/>
|
||||
<geom mesh="calf_0" material="gray" class="visual"/>
|
||||
<geom mesh="calf_1" material="black" class="visual"/>
|
||||
<geom size="0.012 0.06" pos="0.008 0 -0.06" quat="0.994493 0 -0.104807 0" type="cylinder" class="collision"/>
|
||||
<geom size="0.011 0.0325" pos="0.02 0 -0.148" quat="0.999688 0 0.0249974 0" type="cylinder" class="collision"/>
|
||||
<geom pos="0 0 -0.213" mesh="foot" class="visual" material="black"/>
|
||||
<geom name="FL" class="foot"/>
|
||||
<site name="FL_foot" pos="0 0 -0.213" size="0.02"/>
|
||||
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
|
||||
<body name="RR_hip" pos="-0.1934 -0.0465 0">
|
||||
<inertial pos="0.0054 -0.00194 -0.000105" quat="0.499245 0.497014 0.498237 0.505462" mass="0.678"
|
||||
diaginertia="0.00088403 0.000596003 0.000479967"/>
|
||||
<joint name="RR_hip_joint" class="abduction"/>
|
||||
<geom mesh="hip_0" material="metal" class="visual" quat="2.14617e-09 4.63268e-05 4.63268e-05 -1"/>
|
||||
<geom mesh="hip_1" material="gray" class="visual" quat="2.14617e-09 4.63268e-05 4.63268e-05 -1"/>
|
||||
<geom size="0.046 0.02" pos="0 -0.08 0" quat="0.707107 0.707107 0 0" type="cylinder" class="collision"/>
|
||||
<body name="RR_thigh" pos="0 -0.0955 0">
|
||||
<inertial pos="-0.00374 0.0223 -0.0327" quat="0.551623 -0.0200632 0.0847635 0.829533" mass="1.152"
|
||||
diaginertia="0.00594973 0.00584149 0.000878787"/>
|
||||
<joint name="RR_thigh_joint" class="back_hip"/>
|
||||
<geom mesh="thigh_mirror_0" material="metal" class="visual"/>
|
||||
<geom mesh="thigh_mirror_1" material="gray" class="visual"/>
|
||||
<geom size="0.1065 0.01225 0.017" pos="0 0 -0.1065" quat="0.707107 0 0.707107 0" type="box" class="collision"/>
|
||||
<body name="RR_calf" pos="0 0 -0.213">
|
||||
<inertial pos="0.00629595 0.000622121 -0.141417" quat="0.703508 -0.00450087 0.00154099 0.710672"
|
||||
mass="0.241352" diaginertia="0.0014901 0.00146356 5.31397e-05"/>
|
||||
<joint name="RR_calf_joint" class="knee"/>
|
||||
<geom mesh="calf_mirror_0" material="gray" class="visual"/>
|
||||
<geom mesh="calf_mirror_1" material="black" class="visual"/>
|
||||
<geom size="0.013 0.06" pos="0.01 0 -0.06" quat="0.995004 0 -0.0998334 0" type="cylinder" class="collision"/>
|
||||
<geom size="0.011 0.0325" pos="0.02 0 -0.148" quat="0.999688 0 0.0249974 0" type="cylinder" class="collision"/>
|
||||
<geom pos="0 0 -0.213" mesh="foot" class="visual" material="black"/>
|
||||
<geom name="RR" class="foot"/>
|
||||
<site name="RR_foot" pos="0 0 -0.213" size="0.02"/>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
|
||||
<body name="RL_hip" pos="-0.1934 0.0465 0">
|
||||
<inertial pos="0.0054 0.00194 -0.000105" quat="0.505462 0.498237 0.497014 0.499245" mass="0.678"
|
||||
diaginertia="0.00088403 0.000596003 0.000479967"/>
|
||||
<joint name="RL_hip_joint" class="abduction"/>
|
||||
<geom mesh="hip_0" material="metal" class="visual" quat="4.63268e-05 0 1 0"/>
|
||||
<geom mesh="hip_1" material="gray" class="visual" quat="4.63268e-05 0 1 0"/>
|
||||
<geom size="0.046 0.02" pos="0 0.08 0" quat="0.707107 0.707107 0 0" type="cylinder" class="collision"/>
|
||||
<body name="RL_thigh" pos="0 0.0955 0">
|
||||
<inertial pos="-0.00374 -0.0223 -0.0327" quat="0.829533 0.0847635 -0.0200632 0.551623" mass="1.152"
|
||||
diaginertia="0.00594973 0.00584149 0.000878787"/>
|
||||
<joint name="RL_thigh_joint" class="back_hip"/>
|
||||
<geom mesh="thigh_0" material="metal" class="visual"/>
|
||||
<geom mesh="thigh_1" material="gray" class="visual"/>
|
||||
<geom size="0.1065 0.01225 0.017" pos="0 0 -0.1065" quat="0.707107 0 0.707107 0" type="box" class="collision"/>
|
||||
<body name="RL_calf" pos="0 0 -0.213">
|
||||
<inertial pos="0.00629595 -0.000622121 -0.141417" quat="0.710672 0.00154099 -0.00450087 0.703508"
|
||||
mass="0.241352" diaginertia="0.0014901 0.00146356 5.31397e-05"/>
|
||||
<joint name="RL_calf_joint" class="knee"/>
|
||||
<geom mesh="calf_0" material="gray" class="visual"/>
|
||||
<geom mesh="calf_1" material="black" class="visual"/>
|
||||
<geom size="0.013 0.06" pos="0.01 0 -0.06" quat="0.995004 0 -0.0998334 0" type="cylinder" class="collision"/>
|
||||
<geom size="0.011 0.0325" pos="0.02 0 -0.148" quat="0.999688 0 0.0249974 0" type="cylinder" class="collision"/>
|
||||
<geom pos="0 0 -0.213" mesh="foot" class="visual" material="black"/>
|
||||
<geom name="RL" class="foot"/>
|
||||
<site name="RL_foot" pos="0 0 -0.213" size="0.02"/>
|
||||
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
|
||||
<body name="d1_base" pos="0 0 0.0533" quat="1. 0. 0. 0.">
|
||||
<geom type="mesh" rgba="1 1 1 1" mesh="base_link"/>
|
||||
<body name="Link1" pos="0 0 0.0533" quat="-3.67321e-06 0 0 -1">
|
||||
<inertial pos="0.0024649 0.00010517 0.032696" quat="0.999717 -0.0236435 -0.00169588 -0.00221682" mass="0.13174" diaginertia="6.72365e-05 5.41766e-05 4.66199e-05"/>
|
||||
<joint name="Joint1" pos="0 0 0" axis="0 0 1" range="-2.35 2.35"/>
|
||||
<geom type="mesh" rgba="1 1 1 1" mesh="Link1"/>
|
||||
<body name="Link2" pos="0 0.028 0.0563" quat="-2.59734e-06 -2.59735e-06 -0.707108 -0.707105">
|
||||
<inertial pos="0.0002018 0.19201 -0.027007" quat="0.706347 0.707865 -0.00093681 0.00051949" mass="0.20213" diaginertia="0.00025682 0.000229681 6.33062e-05"/>
|
||||
<joint name="Joint2" pos="0 0 0" axis="0 0 -1" range="-1.57 1.57"/>
|
||||
<geom type="mesh" rgba="1 1 1 1" mesh="Link2"/>
|
||||
<body name="Link3" pos="0 0.2693 0.0009">
|
||||
<inertial pos="0.015164 0.044482 -0.027461" quat="0.690744 0.690771 -0.151176 -0.151179" mass="0.0629" diaginertia="1.84718e-05 1.4964e-05 1.13662e-05"/>
|
||||
<joint name="Joint3" pos="0 0 0" axis="0 0 -1" range="-1.57 1.57"/>
|
||||
<geom type="mesh" rgba="1 1 1 1" mesh="Link3"/>
|
||||
<body name="Link4" pos="0.0577 0.042 -0.0275" quat="0.499998 -0.5 0.500002 -0.5">
|
||||
<inertial pos="-0.00029556 -0.00016104 0.091339" quat="0.999998 -3.46195e-05 0.00170171 0.00102754" mass="0.083332" diaginertia="3.93083e-05 3.4378e-05 1.15967e-05"/>
|
||||
<joint name="Joint4" pos="0 0 0" axis="0 0 1" range="-2.35 2.35"/>
|
||||
<geom type="mesh" rgba="1 1 1 1" mesh="Link4"/>
|
||||
<body name="Link5" pos="-0.0001 -0.0237 0.14018" quat="0.499998 0.5 -0.5 0.500002">
|
||||
<inertial pos="0.040573 0.0062891 -0.023838" quat="0.706153 0.706691 -0.0310982 -0.0311281" mass="0.053817" diaginertia="1.31069e-05 1.1049e-05 8.59523e-06"/>
|
||||
<joint name="Joint5" pos="0 0 0" axis="0 0 -1" range="-1.57 1.57"/>
|
||||
<geom type="mesh" rgba="1 1 1 1" mesh="Link5"/>
|
||||
<body name="Link6" pos="0.0825 -0.0010782 -0.023822" quat="0.499998 -0.5 0.500002 -0.5">
|
||||
<inertial pos="-0.0068528 -3.9973e-06 0.039705" quat="0.494949 0.495288 -0.504676 0.504992" mass="0.077892" diaginertia="4.8843e-05 3.8232e-05 1.7707e-05"/>
|
||||
<joint name="Joint6" pos="0 0 0" axis="0 0 -1" range="-2.35 2.35"/>
|
||||
<geom type="mesh" rgba="1 1 1 1" mesh="Link6"/>
|
||||
<body name="Link7_1" pos="-0.0056012 -0.029636 0.0706" quat="0.499848 -0.50015 -0.49985 -0.500152">
|
||||
<inertial pos="0.018927 0.006 0.012082" quat="0 0.677073 0 0.735916" mass="0.015046" diaginertia="2.62715e-06 2.0229e-06 1.25975e-06"/>
|
||||
<joint name="Joint7_1" pos="0 0 0" axis="0 0 -1" type="slide" range="0 0.03"/>
|
||||
<geom type="mesh" rgba="1 1 1 1" mesh="Link7_1"/>
|
||||
</body>
|
||||
<body name="Link7_2" pos="-0.0056388 0.02964 0.0706" quat="0.500148 0.49985 -0.50015 0.499852">
|
||||
<inertial pos="0.018927 -0.006 0.012082" quat="0 0.677073 0 0.735916" mass="0.015046" diaginertia="2.62715e-06 2.0229e-06 1.25975e-06"/>
|
||||
<joint name="Joint7_2" pos="0 0 0" axis="0 0 1" type="slide" range="-0.03 0"/>
|
||||
<geom type="mesh" rgba="1 1 1 1" mesh="Link7_2"/>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
|
||||
</body>
|
||||
</worldbody>
|
||||
|
||||
<actuator>
|
||||
<motor class="abduction" name="FR_hip" joint="FR_hip_joint"/>
|
||||
<motor class="hip" name="FR_thigh" joint="FR_thigh_joint"/>
|
||||
<motor class="knee" name="FR_calf" joint="FR_calf_joint"/>
|
||||
<motor class="abduction" name="FL_hip" joint="FL_hip_joint"/>
|
||||
<motor class="hip" name="FL_thigh" joint="FL_thigh_joint"/>
|
||||
<motor class="knee" name="FL_calf" joint="FL_calf_joint"/>
|
||||
<motor class="abduction" name="RR_hip" joint="RR_hip_joint"/>
|
||||
<motor class="hip" name="RR_thigh" joint="RR_thigh_joint"/>
|
||||
<motor class="knee" name="RR_calf" joint="RR_calf_joint"/>
|
||||
<motor class="abduction" name="RL_hip" joint="RL_hip_joint"/>
|
||||
<motor class="hip" name="RL_thigh" joint="RL_thigh_joint"/>
|
||||
<motor class="knee" name="RL_calf" joint="RL_calf_joint"/>
|
||||
|
||||
<motor class="arm" name="Link1" joint="Joint1"/>
|
||||
<motor class="arm" name="Link2" joint="Joint2"/>
|
||||
<motor class="arm" name="Link3" joint="Joint3"/>
|
||||
<motor class="arm" name="Link4" joint="Joint4"/>
|
||||
<motor class="arm" name="Link5" joint="Joint5"/>
|
||||
<motor class="arm" name="Link6" joint="Joint6"/>
|
||||
<motor class="arm" name="Link7_1" joint="Joint7_1"/>
|
||||
<motor class="arm" name="Link7_2" joint="Joint7_2"/>
|
||||
|
||||
|
||||
</actuator>
|
||||
|
||||
<sensor>
|
||||
<!-- IMU -->
|
||||
<accelerometer name="imu_accel" site="imu"/>
|
||||
<gyro name="imu_gyro" site="imu"/>
|
||||
<!-- Contact sensors for the feet -->
|
||||
<touch name="FR_foot_contact" site="FR_foot"/>
|
||||
<touch name="FL_foot_contact" site="RL_foot"/>
|
||||
<touch name="RR_foot_contact" site="RR_foot"/>
|
||||
<touch name="RL_foot_contact" site="RL_foot"/>
|
||||
</sensor>
|
||||
|
||||
<keyframe>
|
||||
<key name="home" qpos="0 0 0.27 1 0 0 0 0 0.9 -1.8 0 0.9 -1.8 0 0.9 -1.8 0 0.9 -1.8 0 0 0 0 0 0 0 0"
|
||||
ctrl="0 0.9 -1.8 0 0.9 -1.8 0 0.9 -1.8 0 0.9 -1.8 0 0 0 0 0 0 0 0"/>
|
||||
</keyframe>
|
||||
</mujoco>
|
||||
Binary file not shown.
|
|
@ -0,0 +1,511 @@
|
|||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<!-- 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="d1_description">
|
||||
<link
|
||||
name="base_link">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="-0.00048697 -2.9605E-12 0.017142"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.077233" />
|
||||
<inertia
|
||||
ixx="0.00010316"
|
||||
ixy="-1.0285E-14"
|
||||
ixz="2.0449E-07"
|
||||
iyy="0.00011888"
|
||||
iyz="7.3026E-15"
|
||||
izz="0.00018362" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="../stl/base_link.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="../stl/base_link.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<link
|
||||
name="Link1">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.0024649 0.00010517 0.032696"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.13174" />
|
||||
<inertia
|
||||
ixx="6.7236E-05"
|
||||
ixy="-5.5664E-08"
|
||||
ixz="7.0454E-08"
|
||||
iyy="5.416E-05"
|
||||
iyz="-3.5709E-07"
|
||||
izz="4.6637E-05" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="../stl/Link1.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="../stl/Link1.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="Joint1"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 0 0.0533"
|
||||
rpy="0 0 -3.1416" />
|
||||
<parent
|
||||
link="base_link" />
|
||||
<child
|
||||
link="Link1" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-2.35"
|
||||
upper="2.35"
|
||||
effort="0"
|
||||
velocity="0" />
|
||||
</joint>
|
||||
<link
|
||||
name="Link2">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.0002018 0.19201 -0.027007"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.20213" />
|
||||
<inertia
|
||||
ixx="0.00025682"
|
||||
ixy="-1.139E-07"
|
||||
ixz="5.5667E-08"
|
||||
iyy="6.3307E-05"
|
||||
iyz="-3.5717E-07"
|
||||
izz="0.00022968" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="../stl/Link2.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="stl/Link2.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="Joint2"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 0.028 0.0563"
|
||||
rpy="1.5708 0 -3.1416" />
|
||||
<parent
|
||||
link="Link1" />
|
||||
<child
|
||||
link="Link2" />
|
||||
<axis
|
||||
xyz="0 0 -1" />
|
||||
<limit
|
||||
lower="-1.57"
|
||||
upper="1.57"
|
||||
effort="0"
|
||||
velocity="0" />
|
||||
</joint>
|
||||
<link
|
||||
name="Link3">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.015164 0.044482 -0.027461"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.0629" />
|
||||
<inertia
|
||||
ixx="1.7232E-05"
|
||||
ixy="-2.6967E-06"
|
||||
ixz="-9.4911E-11"
|
||||
iyy="1.2606E-05"
|
||||
iyz="-9.9169E-11"
|
||||
izz="1.4964E-05" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="../stl/Link3.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="../stl/Link3.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="Joint3"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 0.2693 0.0009"
|
||||
rpy="0 0 0" />
|
||||
<parent
|
||||
link="Link2" />
|
||||
<child
|
||||
link="Link3" />
|
||||
<axis
|
||||
xyz="0 0 -1" />
|
||||
<limit
|
||||
lower="-1.57"
|
||||
upper="1.57"
|
||||
effort="0"
|
||||
velocity="0" />
|
||||
</joint>
|
||||
<link
|
||||
name="Link4">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="-0.00029556 -0.00016104 0.091339"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.083332" />
|
||||
<inertia
|
||||
ixx="3.9308E-05"
|
||||
ixy="1.0126E-08"
|
||||
ixz="-9.4316E-08"
|
||||
iyy="3.4378E-05"
|
||||
iyz="-1.6915E-09"
|
||||
izz="1.1597E-05" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="../stl/Link4.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="../stl/Link4.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="Joint4"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0.0577 0.042 -0.0275"
|
||||
rpy="-1.5708 0 -1.5708" />
|
||||
<parent
|
||||
link="Link3" />
|
||||
<child
|
||||
link="Link4" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-2.35"
|
||||
upper="2.35"
|
||||
effort="0"
|
||||
velocity="0" />
|
||||
</joint>
|
||||
<link
|
||||
name="Link5">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.040573 0.0062891 -0.023838"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.053817" />
|
||||
<inertia
|
||||
ixx="1.3072E-05"
|
||||
ixy="-3.9511E-07"
|
||||
ixz="-3.1889E-10"
|
||||
iyy="8.6301E-06"
|
||||
iyz="-1.8416E-09"
|
||||
izz="1.1049E-05" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="../stl/Link5.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="../stl/Link5.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="Joint5"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="-0.0001 -0.0237 0.14018"
|
||||
rpy="1.5708 -1.5708 0" />
|
||||
<parent
|
||||
link="Link4" />
|
||||
<child
|
||||
link="Link5" />
|
||||
<axis
|
||||
xyz="0 0 -1" />
|
||||
<limit
|
||||
lower="-1.57"
|
||||
upper="1.57"
|
||||
effort="0"
|
||||
velocity="0" />
|
||||
</joint>
|
||||
<link
|
||||
name="Link6">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="-0.0068528 -3.9973E-06 0.039705"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.077892" />
|
||||
<inertia
|
||||
ixx="3.8236E-05"
|
||||
ixy="1.3465E-08"
|
||||
ixz="-2.0614E-07"
|
||||
iyy="1.7707E-05"
|
||||
iyz="-6.7117E-10"
|
||||
izz="4.8839E-05" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="../stl/Link6.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="../stl/Link6.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="Joint6"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0.0825 -0.0010782 -0.023822"
|
||||
rpy="-1.5708 0 -1.5708" />
|
||||
<parent
|
||||
link="Link5" />
|
||||
<child
|
||||
link="Link6" />
|
||||
<axis
|
||||
xyz="0 0 -1" />
|
||||
<limit
|
||||
lower="-2.35"
|
||||
upper="2.35"
|
||||
effort="0"
|
||||
velocity="0" />
|
||||
</joint>
|
||||
<link
|
||||
name="Link7_1">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.018927 0.006 0.012082"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.015046" />
|
||||
<inertia
|
||||
ixx="1.2692E-06"
|
||||
ixy="7.7441E-20"
|
||||
ixz="-1.133E-07"
|
||||
iyy="2.0229E-06"
|
||||
iyz="1.2044E-20"
|
||||
izz="2.6177E-06" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="../stl/Link7_1.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="../stl/Link7_1.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="Joint7_1"
|
||||
type="prismatic">
|
||||
<origin
|
||||
xyz="-0.0056012 -0.029636 0.0706"
|
||||
rpy="-1.5714 -1.5708 0" />
|
||||
<parent
|
||||
link="Link6" />
|
||||
<child
|
||||
link="Link7_1" />
|
||||
<axis
|
||||
xyz="0 0 -1" />
|
||||
<limit
|
||||
lower="0"
|
||||
upper="0.03"
|
||||
effort="0"
|
||||
velocity="0" />
|
||||
</joint>
|
||||
<link
|
||||
name="Link7_2">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.018927 -0.006 0.012082"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.015046" />
|
||||
<inertia
|
||||
ixx="1.2692E-06"
|
||||
ixy="-7.8273E-20"
|
||||
ixz="-1.133E-07"
|
||||
iyy="2.0229E-06"
|
||||
iyz="-1.2122E-20"
|
||||
izz="2.6177E-06" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="../stl/Link7_2.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="../stl/Link7_2.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="Joint7_2"
|
||||
type="prismatic">
|
||||
<origin
|
||||
xyz="-0.0056388 0.02964 0.0706"
|
||||
rpy="1.5702 -1.5708 0" />
|
||||
<parent
|
||||
link="Link6" />
|
||||
<child
|
||||
link="Link7_2" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-0.03"
|
||||
upper="0"
|
||||
effort="0"
|
||||
velocity="0" />
|
||||
</joint>
|
||||
</robot>
|
||||
|
|
@ -0,0 +1,683 @@
|
|||
|
||||
<?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">
|
||||
<link name="base_link">
|
||||
<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_link"/>
|
||||
<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_HAA">
|
||||
<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_HAA_joint" type="revolute">
|
||||
<origin rpy="0 0 0" xyz="0.1934 0.0465 0"/>
|
||||
<parent link="base_link"/>
|
||||
<child link="FL_HAA"/>
|
||||
<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_HFE">
|
||||
<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_HFE_joint" type="revolute">
|
||||
<origin rpy="0 0 0" xyz="0 0.0955 0"/>
|
||||
<parent link="FL_HAA"/>
|
||||
<child link="FL_HFE"/>
|
||||
<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_KFE">
|
||||
<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_KFE_joint" type="revolute">
|
||||
<origin rpy="0 0 0" xyz="0 0 -0.213"/>
|
||||
<parent link="FL_HFE"/>
|
||||
<child link="FL_KFE"/>
|
||||
<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_KFElower">
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.065" radius="0.011"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="FL_KFElower_joint" type="fixed">
|
||||
<origin rpy="0 0.05 0" xyz="0.020 0 -0.148"/>
|
||||
<parent link="FL_KFE"/>
|
||||
<child link="FL_KFElower"/>
|
||||
<axis xyz="0 0 0"/>
|
||||
</joint>
|
||||
<link name="FL_KFElower1">
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.03" radius="0.0155"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="FL_KFElower1_joint" type="fixed">
|
||||
<origin rpy="0 0.48 0" xyz="-0.01 0 -0.04"/>
|
||||
<parent link="FL_KFElower"/>
|
||||
<child link="FL_KFElower1"/>
|
||||
<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_KFE"/>
|
||||
<child link="FL_FOOT"/>
|
||||
<axis xyz="0 0 0"/>
|
||||
</joint>
|
||||
<link name="FR_HAA">
|
||||
<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_HAA_joint" type="revolute">
|
||||
<origin rpy="0 0 0" xyz="0.1934 -0.0465 0"/>
|
||||
<parent link="base_link"/>
|
||||
<child link="FR_HAA"/>
|
||||
<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_HFE">
|
||||
<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_HFE_joint" type="revolute">
|
||||
<origin rpy="0 0 0" xyz="0 -0.0955 0"/>
|
||||
<parent link="FR_HAA"/>
|
||||
<child link="FR_HFE"/>
|
||||
<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_KFE">
|
||||
<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_KFE_joint" type="revolute">
|
||||
<origin rpy="0 0 0" xyz="0 0 -0.213"/>
|
||||
<parent link="FR_HFE"/>
|
||||
<child link="FR_KFE"/>
|
||||
<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_KFElower">
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.065" radius="0.011"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="FR_KFElower_joint" type="fixed">
|
||||
<origin rpy="0 0.05 0" xyz="0.020 0 -0.148"/>
|
||||
<parent link="FR_KFE"/>
|
||||
<child link="FR_KFElower"/>
|
||||
<axis xyz="0 0 0"/>
|
||||
</joint>
|
||||
<link name="FR_KFElower1">
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.03" radius="0.0155"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="FR_KFElower1_joint" type="fixed">
|
||||
<origin rpy="0 0.48 0" xyz="-0.01 0 -0.04"/>
|
||||
<parent link="FR_KFElower"/>
|
||||
<child link="FR_KFElower1"/>
|
||||
<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_KFE"/>
|
||||
<child link="FR_FOOT"/>
|
||||
<axis xyz="0 0 0"/>
|
||||
</joint>
|
||||
<link name="HL_HAA">
|
||||
<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="HL_HAA_joint" type="revolute">
|
||||
<origin rpy="0 0 0" xyz="-0.1934 0.0465 0"/>
|
||||
<parent link="base_link"/>
|
||||
<child link="HL_HAA"/>
|
||||
<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="HL_HFE">
|
||||
<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="HL_HFE_joint" type="revolute">
|
||||
<origin rpy="0 0 0" xyz="0 0.0955 0"/>
|
||||
<parent link="HL_HAA"/>
|
||||
<child link="HL_HFE"/>
|
||||
<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="HL_KFE">
|
||||
<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="HL_KFE_joint" type="revolute">
|
||||
<origin rpy="0 0 0" xyz="0 0 -0.213"/>
|
||||
<parent link="HL_HFE"/>
|
||||
<child link="HL_KFE"/>
|
||||
<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="HL_KFElower">
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.065" radius="0.011"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="HL_KFElower_joint" type="fixed">
|
||||
<origin rpy="0 0.05 0" xyz="0.020 0 -0.148"/>
|
||||
<parent link="HL_KFE"/>
|
||||
<child link="HL_KFElower"/>
|
||||
<axis xyz="0 0 0"/>
|
||||
</joint>
|
||||
<link name="HL_KFElower1">
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.03" radius="0.0155"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="HL_KFElower1_joint" type="fixed">
|
||||
<origin rpy="0 0.48 0" xyz="-0.01 0 -0.04"/>
|
||||
<parent link="HL_KFElower"/>
|
||||
<child link="HL_KFElower1"/>
|
||||
<axis xyz="0 0 0"/>
|
||||
</joint>
|
||||
<link name="HL_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="HL_FOOT_joint" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="0 0 -0.213"/>
|
||||
<parent link="HL_KFE"/>
|
||||
<child link="HL_FOOT"/>
|
||||
<axis xyz="0 0 0"/>
|
||||
</joint>
|
||||
<link name="HR_HAA">
|
||||
<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="HR_HAA_joint" type="revolute">
|
||||
<origin rpy="0 0 0" xyz="-0.1934 -0.0465 0"/>
|
||||
<parent link="base_link"/>
|
||||
<child link="HR_HAA"/>
|
||||
<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="HR_HFE">
|
||||
<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="HR_HFE_joint" type="revolute">
|
||||
<origin rpy="0 0 0" xyz="0 -0.0955 0"/>
|
||||
<parent link="HR_HAA"/>
|
||||
<child link="HR_HFE"/>
|
||||
<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="HR_KFE">
|
||||
<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="HR_KFE_joint" type="revolute">
|
||||
<origin rpy="0 0 0" xyz="0 0 -0.213"/>
|
||||
<parent link="HR_HFE"/>
|
||||
<child link="HR_KFE"/>
|
||||
<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="HR_KFElower">
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.065" radius="0.011"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="HR_KFElower_joint" type="fixed">
|
||||
<origin rpy="0 0.05 0" xyz="0.020 0 -0.148"/>
|
||||
<parent link="HR_KFE"/>
|
||||
<child link="HR_KFElower"/>
|
||||
<axis xyz="0 0 0"/>
|
||||
</joint>
|
||||
<link name="HR_KFElower1">
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.03" radius="0.0155"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="HR_KFElower1_joint" type="fixed">
|
||||
<origin rpy="0 0.48 0" xyz="-0.01 0 -0.04"/>
|
||||
<parent link="HR_KFElower"/>
|
||||
<child link="HR_KFElower1"/>
|
||||
<axis xyz="0 0 0"/>
|
||||
</joint>
|
||||
<link name="HR_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="HR_FOOT_joint" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="0 0 -0.213"/>
|
||||
<parent link="HR_KFE"/>
|
||||
<child link="HR_FOOT"/>
|
||||
<axis xyz="0 0 0"/>
|
||||
</joint>
|
||||
<link name="imu_link">
|
||||
<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.001697 -0.020900 -0.000007" xyz="-0.027353 -0.005499 0.039913"/>
|
||||
<parent link="base_link"/>
|
||||
<child link="imu_link"/>
|
||||
<axis xyz="0 0 0"/>
|
||||
</joint>
|
||||
|
||||
<link name="utlidar">
|
||||
<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="utlidar_joint" type="fixed">
|
||||
<origin rpy="0 2.8782 0" xyz="0.28945 0 -0.046825"/>
|
||||
<parent link="base_link"/>
|
||||
<child link="utlidar"/>
|
||||
<axis xyz="0 0 0"/>
|
||||
</joint>
|
||||
</robot>
|
||||
File diff suppressed because it is too large
Load Diff
|
|
@ -171,7 +171,7 @@ class Policy:
|
|||
if self.step_counter%50==0:
|
||||
self.perturbation = torch.rand_like(latent)*0.9
|
||||
self.step_counter +=1
|
||||
print(latent, self.perturbation)
|
||||
# print(latent, self.perturbation)
|
||||
info["latent"] = latent
|
||||
action = self.body.forward(
|
||||
torch.cat((obs["obs_history"].to("cpu"), latent), dim=-1)
|
||||
|
|
|
|||
|
|
@ -362,15 +362,22 @@ class Go2Sim:
|
|||
except:
|
||||
raise Exception(f'Could not load heightmap. Make sure the heigh_map is a 2D numpy array')
|
||||
|
||||
def reset(self):
|
||||
def reset(self, q0=None):
|
||||
if q0 is None:
|
||||
self.q_nominal = np.hstack(
|
||||
[self.pos0.squeeze(), self.rot0.squeeze(), self.q0.squeeze()]
|
||||
)
|
||||
else:
|
||||
assert q0.shape == (19,), 'Invalid q0 shape. The shape should be (19,)'
|
||||
self.q_nominal = q0
|
||||
self.data.qpos = self.q_nominal
|
||||
self.data.qvel = np.zeros(18)
|
||||
self.ex_sum=0
|
||||
self.ey_sum=0
|
||||
self.e_omega_sum=0
|
||||
mujoco.mj_step(self.model, self.data)
|
||||
if self.render:
|
||||
self.viewer.sync()
|
||||
|
||||
def standUpReset(self):
|
||||
self.q0 = self.standing_q
|
||||
|
|
|
|||
14
Makefile
14
Makefile
|
|
@ -37,6 +37,10 @@ realsense:
|
|||
hesai:
|
||||
@cd deploy && docker build --no-cache --tag go2py_hesai:latest -f docker/Dockerfile.hesai .
|
||||
|
||||
mid360:
|
||||
@cd deploy && docker build --tag go2py_mid360:latest -f docker/Dockerfile.mid360 .
|
||||
# @cd deploy && docker build --no-cache --tag go2py_mid360:latest -f docker/Dockerfile.mid360 .
|
||||
|
||||
bridge:
|
||||
@cd deploy && docker build --no-cache --tag go2py_bridge:latest -f docker/Dockerfile.bridge .
|
||||
|
||||
|
|
@ -54,6 +58,11 @@ hesai_install:
|
|||
@systemctl enable go2py-hesai.service
|
||||
@systemctl start go2py-hesai.service
|
||||
|
||||
mid360_install:
|
||||
@cp deploy/services/go2py-mid360.service /etc/systemd/system/
|
||||
@systemctl enable go2py-mid360.service
|
||||
@systemctl start go2py-mid360.service
|
||||
|
||||
bridge_install:
|
||||
@cp deploy/services/go2py-bridge.service /etc/systemd/system/
|
||||
@systemctl enable go2py-bridge.service
|
||||
|
|
@ -74,6 +83,11 @@ hesai_uninstall:
|
|||
@systemctl stop go2py-hesai.service
|
||||
@rm /etc/systemd/system/go2py-hesai.service
|
||||
|
||||
mid360_uninstall:
|
||||
@systemctl disable go2py-mid360.service
|
||||
@systemctl stop go2py-mid360.service
|
||||
@rm /etc/systemd/system/go2py-mid360.service
|
||||
|
||||
bridge_uninstall:
|
||||
@systemctl disable go2py-bridge.service
|
||||
@systemctl stop go2py-bridge.service
|
||||
|
|
|
|||
|
|
@ -0,0 +1,36 @@
|
|||
# FROM isaac_ros_dev-aarch64
|
||||
FROM robocaster/mid360:go2
|
||||
# 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 \
|
||||
# # libyaml-cpp-dev \
|
||||
# # libboost-all-dev\
|
||||
# build-essential \
|
||||
# cmake \
|
||||
# git \
|
||||
# ros-humble-pcl-ros \
|
||||
# && 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 cd /root && git clone https://github.com/Livox-SDK/Livox-SDK2.git && cd ./Livox-SDK2/ && mkdir build && cd build && cmake .. && make -j4 && make install
|
||||
# RUN cd /root && git clone https://github.com/Livox-SDK/livox_ros_driver2.git ws_livox/src/livox_ros_driver2 && cd ws_livox/src/livox_ros_driver2
|
||||
|
||||
COPY docker/scripts/MID360_config.json /root/ws_livox/src/livox_ros_driver2/config/MID360_config.json
|
||||
COPY launch_files/msg_MID360_launch.py /root/ws_livox/src/livox_ros_driver2/launch_ROS2/msg_MID360_launch.py
|
||||
RUN cd /root/ws_livox/src/livox_ros_driver2 && source /opt/ros/humble/setup.sh && /bin/bash build.sh humble
|
||||
# Clone and compile FAST-LIO
|
||||
RUN cd /root && mkdir -p fastlio-ws/src && cd fastlio-ws/src && git clone https://github.com/Ericsii/FAST_LIO.git --recursive && cd FAST_LIO && git checkout ros2 && cd /root/fastlio-ws && source /root/ws_livox/install/setup.bash && colcon build --symlink-install
|
||||
# Copy the script to start the nodes
|
||||
COPY docker/scripts/mid360_start.sh /root/mid360_start.sh
|
||||
# set the entrypoint to bash
|
||||
# ENTRYPOINT ["/bin/bash"]
|
||||
ENTRYPOINT ["/bin/bash", "/root/mid360_start.sh"]
|
||||
|
|
@ -0,0 +1,41 @@
|
|||
{
|
||||
"lidar_summary_info" : {
|
||||
"lidar_type": 8
|
||||
},
|
||||
"MID360": {
|
||||
"lidar_net_info" : {
|
||||
"cmd_data_port": 56100,
|
||||
"push_msg_port": 56200,
|
||||
"point_data_port": 56300,
|
||||
"imu_data_port": 56400,
|
||||
"log_data_port": 56500
|
||||
},
|
||||
"host_net_info" : {
|
||||
"cmd_data_ip" : "192.168.123.18",
|
||||
"cmd_data_port": 56101,
|
||||
"push_msg_ip": "192.168.123.18",
|
||||
"push_msg_port": 56201,
|
||||
"point_data_ip": "192.168.123.18",
|
||||
"point_data_port": 56301,
|
||||
"imu_data_ip" : "192.168.123.18",
|
||||
"imu_data_port": 56401,
|
||||
"log_data_ip" : "",
|
||||
"log_data_port": 56501
|
||||
}
|
||||
},
|
||||
"lidar_configs" : [
|
||||
{
|
||||
"ip" : "192.168.123.20",
|
||||
"pcl_data_type" : 1,
|
||||
"pattern_mode" : 0,
|
||||
"extrinsic_parameter" : {
|
||||
"roll": 0.0,
|
||||
"pitch": 0.0,
|
||||
"yaw": 0.0,
|
||||
"x": 0,
|
||||
"y": 0,
|
||||
"z": 0
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
|
|
@ -2,6 +2,6 @@ source /opt/ros/humble/setup.bash
|
|||
source /unitree_ros2/cyclonedds_ws/install/setup.bash
|
||||
export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
|
||||
export CYCLONEDDS_URI='<CycloneDDS><Domain><General><Interfaces>
|
||||
<NetworkInterface name="enx00e04c006390" priority="default" multicast="default" />
|
||||
<NetworkInterface name="eth0" priority="default" multicast="default" />
|
||||
</Interfaces></General></Domain></CycloneDDS>'
|
||||
source /bridge_ws/install/setup.bash && ros2 launch /root/launch/bridge.launch.py
|
||||
|
|
@ -0,0 +1,7 @@
|
|||
source /opt/ros/humble/setup.bash
|
||||
source /unitree_ros2/cyclonedds_ws/install/setup.bash
|
||||
# export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
|
||||
# export CYCLONEDDS_URI='<CycloneDDS><Domain><General><Interfaces>
|
||||
# <NetworkInterface name="eth0" priority="default" multicast="default" />
|
||||
# </Interfaces></General></Domain></CycloneDDS>'
|
||||
source /root/ws_livox/install/setup.bash && ros2 launch livox_ros_driver2 msg_MID360_launch.py
|
||||
|
|
@ -0,0 +1,48 @@
|
|||
import os.path
|
||||
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument
|
||||
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
|
||||
from launch.conditions import IfCondition
|
||||
|
||||
from launch_ros.actions import Node
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
package_path = get_package_share_directory('fast_lio')
|
||||
default_config_path = os.path.join(package_path, 'config')
|
||||
|
||||
use_sim_time = LaunchConfiguration('use_sim_time')
|
||||
config_path = LaunchConfiguration('config_path')
|
||||
config_file = LaunchConfiguration('config_file')
|
||||
|
||||
declare_use_sim_time_cmd = DeclareLaunchArgument(
|
||||
'use_sim_time', default_value='false',
|
||||
description='Use simulation (Gazebo) clock if true'
|
||||
)
|
||||
declare_config_path_cmd = DeclareLaunchArgument(
|
||||
'config_path', default_value=default_config_path,
|
||||
description='Yaml config file path'
|
||||
)
|
||||
decalre_config_file_cmd = DeclareLaunchArgument(
|
||||
'config_file', default_value='mid360.yaml',
|
||||
description='Config file'
|
||||
)
|
||||
|
||||
fast_lio_node = Node(
|
||||
package='fast_lio',
|
||||
executable='fastlio_mapping',
|
||||
parameters=[PathJoinSubstitution([config_path, config_file]),
|
||||
{'use_sim_time': use_sim_time}],
|
||||
output='screen'
|
||||
)
|
||||
|
||||
ld = LaunchDescription()
|
||||
ld.add_action(declare_use_sim_time_cmd)
|
||||
ld.add_action(declare_config_path_cmd)
|
||||
ld.add_action(decalre_config_file_cmd)
|
||||
ld.add_action(fast_lio_node)
|
||||
|
||||
return ld
|
||||
|
|
@ -0,0 +1,54 @@
|
|||
import os
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
from launch import LaunchDescription
|
||||
from launch_ros.actions import Node
|
||||
import launch
|
||||
|
||||
################### user configure parameters for ros2 start ###################
|
||||
xfer_format = 1 # 0-Pointcloud2(PointXYZRTL), 1-customized pointcloud format
|
||||
multi_topic = 0 # 0-All LiDARs share the same topic, 1-One LiDAR one topic
|
||||
data_src = 0 # 0-lidar, others-Invalid data src
|
||||
publish_freq = 10.0 # freqency of publish, 5.0, 10.0, 20.0, 50.0, etc.
|
||||
output_type = 0
|
||||
frame_id = 'livox_frame'
|
||||
lvx_file_path = '/home/livox/livox_test.lvx'
|
||||
cmdline_bd_code = 'livox0000000001'
|
||||
|
||||
cur_path = os.path.split(os.path.realpath(__file__))[0] + '/'
|
||||
cur_config_path = cur_path + '../config'
|
||||
user_config_path = os.path.join(cur_config_path, 'MID360_config.json')
|
||||
################### user configure parameters for ros2 end #####################
|
||||
|
||||
livox_ros2_params = [
|
||||
{"xfer_format": xfer_format},
|
||||
{"multi_topic": multi_topic},
|
||||
{"data_src": data_src},
|
||||
{"publish_freq": publish_freq},
|
||||
{"output_data_type": output_type},
|
||||
{"frame_id": frame_id},
|
||||
{"lvx_file_path": lvx_file_path},
|
||||
{"user_config_path": user_config_path},
|
||||
{"cmdline_input_bd_code": cmdline_bd_code}
|
||||
]
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
livox_driver = Node(
|
||||
package='livox_ros_driver2',
|
||||
executable='livox_ros_driver2_node',
|
||||
name='livox_lidar_publisher',
|
||||
output='screen',
|
||||
parameters=livox_ros2_params
|
||||
)
|
||||
|
||||
return LaunchDescription([
|
||||
livox_driver,
|
||||
# launch.actions.RegisterEventHandler(
|
||||
# event_handler=launch.event_handlers.OnProcessExit(
|
||||
# target_action=livox_rviz,
|
||||
# on_exit=[
|
||||
# launch.actions.EmitEvent(event=launch.events.Shutdown()),
|
||||
# ]
|
||||
# )
|
||||
# )
|
||||
])
|
||||
|
|
@ -185,6 +185,21 @@
|
|||
<axis xyz="0 0 0"/>
|
||||
</joint>
|
||||
|
||||
|
||||
<link name="mid360">
|
||||
<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="mid360_joint" type="fixed">
|
||||
<origin rpy="0. 0.22689284 0." xyz="0.1870 0 0.0803"/>
|
||||
<parent link="base_link"/>
|
||||
<child link="mid360"/>
|
||||
<axis xyz="0 0 0"/>
|
||||
</joint>
|
||||
|
||||
<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" />
|
||||
|
|
|
|||
|
|
@ -6,7 +6,7 @@ After=multi-user.target
|
|||
[Service]
|
||||
Restart=always
|
||||
ExecStartPre=/usr/bin/docker rm -f go2py_bridge || true
|
||||
ExecStart=/bin/bash -c '/usr/bin/docker run --rm --name go2py_bridge --privileged --network host -v /dev/*:/dev/* -v /etc/localtime:/etc/localtime:ro --runtime nvidia go2py_bridge:latest'
|
||||
ExecStart=/bin/bash -c '/usr/bin/docker run --rm --name go2py_bridge --privileged --network host -v /dev/*:/dev/* -v /etc/localtime:/etc/localtime:ro go2py_bridge:latest'
|
||||
ExecStop=/usr/bin/docker stop -t 2 go2py_bridge
|
||||
|
||||
[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_mid360 || true
|
||||
ExecStart=/bin/bash -c '/usr/bin/docker run --rm --name go2py_mid360 --privileged --network host -v /dev/*:/dev/* -v /etc/localtime:/etc/localtime:ro --runtime nvidia go2py_mid360:latest'
|
||||
ExecStop=/usr/bin/docker stop -t 2 go2py_mid360
|
||||
|
||||
[Install]
|
||||
WantedBy=default.target
|
||||
|
|
@ -1,12 +1,12 @@
|
|||
[Unit]
|
||||
Description=ROS2 device driver container
|
||||
Requires=multi-user.target
|
||||
After=multi-user.target
|
||||
Requires=go2py-robot-description.service
|
||||
After=go2py-robot-description.service
|
||||
|
||||
[Service]
|
||||
Restart=always
|
||||
ExecStartPre=/usr/bin/docker rm -f go2py_robot_description || true
|
||||
ExecStart=/bin/bash -c '/usr/bin/docker run --rm --name go2py_robot_description --privileged --network host -v /dev/*:/dev/* -v /etc/localtime:/etc/localtime:ro --runtime nvidia go2py_description:latest'
|
||||
ExecStart=/bin/bash -c '/usr/bin/docker run --rm --name go2py_robot_description --privileged --network host -v /dev/*:/dev/* -v /etc/localtime:/etc/localtime:ro go2py_description:latest'
|
||||
ExecStop=/usr/bin/docker stop -t 2 go2py_robot_description
|
||||
|
||||
[Install]
|
||||
|
|
|
|||
132
docs/setup.md
132
docs/setup.md
|
|
@ -29,6 +29,138 @@ sudo apt-get install iptables-persistent
|
|||
sudo iptables-save > /etc/iptables/rules.v4
|
||||
sudo ip6tables-save > /etc/iptables/rules.v6
|
||||
```
|
||||
### Configuring Apt Sources
|
||||
By default, the unitree comes with jetpack revision `r35.1` and Ubuntu 20.04. If you're outside China, you can change the apt source lists to the default values for faster software download. First create a backup of the old apt sources:
|
||||
|
||||
```bash
|
||||
sudo mv /etc/apt/sources.list /etc/apt/sources.list.old
|
||||
```
|
||||
|
||||
Then copy the following into the `/etc/apt/sources.list`:
|
||||
|
||||
``` bash
|
||||
# See http://help.ubuntu.com/community/UpgradeNotes for how to upgrade to
|
||||
# newer versions of the distribution.
|
||||
deb http://ports.ubuntu.com/ubuntu-ports/ bionic main restricted
|
||||
# deb-src http://ports.ubuntu.com/ubuntu-ports/ bionic main restricted
|
||||
|
||||
## Major bug fix updates produced after the final release of the
|
||||
## distribution.
|
||||
deb http://ports.ubuntu.com/ubuntu-ports/ bionic-updates main restricted
|
||||
# deb-src http://ports.ubuntu.com/ubuntu-ports/ bionic-updates main restricted
|
||||
|
||||
## N.B. software from this repository is ENTIRELY UNSUPPORTED by the Ubuntu
|
||||
## team. Also, please note that software in universe WILL NOT receive any
|
||||
## review or updates from the Ubuntu security team.
|
||||
deb http://ports.ubuntu.com/ubuntu-ports/ bionic universe
|
||||
# deb-src http://ports.ubuntu.com/ubuntu-ports/ bionic universe
|
||||
deb http://ports.ubuntu.com/ubuntu-ports/ bionic-updates universe
|
||||
# deb-src http://ports.ubuntu.com/ubuntu-ports/ bionic-updates universe
|
||||
|
||||
## N.B. software from this repository is ENTIRELY UNSUPPORTED by the Ubuntu
|
||||
## team, and may not be under a free licence. Please satisfy yourself as to
|
||||
## your rights to use the software. Also, please note that software in
|
||||
## multiverse WILL NOT receive any review or updates from the Ubuntu
|
||||
## security team.
|
||||
deb http://ports.ubuntu.com/ubuntu-ports/ bionic multiverse
|
||||
# deb-src http://ports.ubuntu.com/ubuntu-ports/ bionic multiverse
|
||||
deb http://ports.ubuntu.com/ubuntu-ports/ bionic-updates multiverse
|
||||
# deb-src http://ports.ubuntu.com/ubuntu-ports/ bionic-updates multiverse
|
||||
|
||||
## N.B. software from this repository may not have been tested as
|
||||
## extensively as that contained in the main release, although it includes
|
||||
## newer versions of some applications which may provide useful features.
|
||||
## Also, please note that software in backports WILL NOT receive any review
|
||||
## or updates from the Ubuntu security team.
|
||||
deb http://ports.ubuntu.com/ubuntu-ports/ bionic-backports main restricted universe multiverse
|
||||
# deb-src http://ports.ubuntu.com/ubuntu-ports/ bionic-backports main restricted universe multiverse
|
||||
|
||||
## Uncomment the following two lines to add software from Canonical's
|
||||
## 'partner' repository.
|
||||
## This software is not part of Ubuntu, but is offered by Canonical and the
|
||||
## respective vendors as a service to Ubuntu users.
|
||||
# deb http://archive.canonical.com/ubuntu bionic partner
|
||||
# deb-src http://archive.canonical.com/ubuntu bionic partner
|
||||
|
||||
deb http://ports.ubuntu.com/ubuntu-ports/ bionic-security main restricted
|
||||
# deb-src http://ports.ubuntu.com/ubuntu-ports/ bionic-security main restricted
|
||||
deb http://ports.ubuntu.com/ubuntu-ports/ bionic-security universe
|
||||
# deb-src http://ports.ubuntu.com/ubuntu-ports/ bionic-security universe
|
||||
deb http://ports.ubuntu.com/ubuntu-ports/ bionic-security multiverse
|
||||
# deb-src http://ports.ubuntu.com/ubuntu-ports/ bionic-security multiverse
|
||||
```
|
||||
We can also enable the Nvidia mirrors as explained [here](https://docs.nvidia.com/jetson/archives/r35.1/DeveloperGuide/text/SD/SoftwarePackagesAndTheUpdateMechanism.html). Specifically, do `sudo vi /etc/apt/sources.list.d/nvidia-l4t-apt-source.list` and copy the following inside:
|
||||
```bash
|
||||
deb https://repo.download.nvidia.com/jetson/common r35.1 main
|
||||
deb https://repo.download.nvidia.com/jetson/t234 r35.1 main
|
||||
```
|
||||
Finally, update the and upgrade:
|
||||
```bash
|
||||
sudo apt update
|
||||
sudo apt upgrade
|
||||
```
|
||||
### Installing CUDA
|
||||
|
||||
To use the onboard GPU we need to install the CUDA toolkit for Jetson [here](https://developer.nvidia.com/cuda-11-8-0-download-archive?target_os=Linux&target_arch=aarch64-jetson&Compilation=Native&Distribution=Ubuntu&target_version=20.04&target_type=deb_local). Specifically:
|
||||
|
||||
```
|
||||
wget https://developer.download.nvidia.com/compute/cuda/repos/ubuntu2004/arm64/cuda-ubuntu2004.pin
|
||||
sudo mv cuda-ubuntu2004.pin /etc/apt/preferences.d/cuda-repository-pin-600
|
||||
wget https://developer.download.nvidia.com/compute/cuda/11.8.0/local_installers/cuda-tegra-repo-ubuntu2004-11-8-local_11.8.0-1_arm64.deb
|
||||
sudo dpkg -i cuda-tegra-repo-ubuntu2004-11-8-local_11.8.0-1_arm64.deb
|
||||
sudo cp /var/cuda-tegra-repo-ubuntu2004-11-8-local/cuda-*-keyring.gpg /usr/share/keyrings/
|
||||
sudo apt-get update
|
||||
sudo apt-get -y install cuda-11-8
|
||||
```
|
||||
Additionally, we also need to install the following packages for successful CUDA kernel compilation in our projects:
|
||||
```
|
||||
sudo apt install -y libcudnn8-dev libcusolver-dev-11-8 libcublas-dev-11-8 libcublas-11-8 libcusparse-11-8 libcusparse-dev-11-8
|
||||
```
|
||||
In order to install Torch, we need to know what Jetpack revision we have. You can check it out through the following command:
|
||||
```
|
||||
dpkg-query --show nvidia-l4t-core
|
||||
```
|
||||
### Installing Conda
|
||||
|
||||
Simply run the following commands to install Conda:
|
||||
|
||||
```bash
|
||||
sudo chown $USER /opt
|
||||
wget https://github.com/conda-forge/miniforge/releases/latest/download/Miniforge3-Linux-aarch64.sh -O ~/miniconda.sh
|
||||
/bin/bash ~/miniconda.sh -b -p /opt/conda
|
||||
rm ~/miniconda.sh
|
||||
sudo ln -s /opt/conda/etc/profile.d/conda.sh /etc/profile.d/conda.sh
|
||||
echo ". /opt/conda/etc/profile.d/conda.sh" >> ~/.bashrc
|
||||
echo "conda activate base" >> ~/.bashrc
|
||||
```
|
||||
|
||||
Now create a virtual environment for the deployment of the RL policies available in Go2Py:
|
||||
|
||||
```bash
|
||||
conda create --name rl-deploy python==3.8.10
|
||||
conda activate rl-deploy
|
||||
```
|
||||
|
||||
Finally, install the deep learning libraries needed for the deployments:
|
||||
|
||||
#### DL Frameworks
|
||||
Download and install appropriate version as described [here](https://forums.developer.nvidia.com/t/pytorch-for-jetson/72048):
|
||||
|
||||
```bash
|
||||
cd ~
|
||||
wget https://developer.download.nvidia.cn/compute/redist/jp/v512/pytorch/torch-2.1.0a0+41361538.nv23.06-cp38-cp38-linux_aarch64.whl torch-2.1.0a0+41361538.nv23.06-cp38-cp38-linux_aarch64.whl
|
||||
python -m pip install torch-2.1.0a0+41361538.nv23.06-cp38-cp38-linux_aarch64.whl
|
||||
# Install warp-lang if required
|
||||
pip install https://github.com/NVIDIA/warp/releases/download/v1.5.1/warp_lang-1.5.1+cu11-py3-none-manylinux2014_aarch64.whl
|
||||
```
|
||||
If successful, the following check should return true:
|
||||
|
||||
```python
|
||||
import torch
|
||||
print(torch.cuda.is_available())
|
||||
```
|
||||
|
||||
|
||||
### Robot
|
||||
Now tell the computer on the robot to use the internet shared by the host computer. SSH into the robot's computer with IP address `192.168.123.18`, username `unitree`, and password `123`. Note that the host computer's IP range should have already been set to static mode with an IP in the `192.168.123.x` range where x is anything except IPs already used by the others (e.g. `.18`).
|
||||
|
||||
|
|
|
|||
|
|
@ -138,13 +138,6 @@
|
|||
"source": [
|
||||
"fsm.close()"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": []
|
||||
}
|
||||
],
|
||||
"metadata": {
|
||||
|
|
|
|||
File diff suppressed because one or more lines are too long
|
|
@ -0,0 +1,6 @@
|
|||
Tue Dec 3 15:37:57 2024
|
||||
WARNING: Nan, Inf or huge value in QACC at DOF 3. The simulation is unstable. Time = 0.8500.
|
||||
|
||||
Tue Dec 3 15:45:56 2024
|
||||
WARNING: Nan, Inf or huge value in QACC at DOF 20. The simulation is unstable. Time = 0.7940.
|
||||
|
||||
|
|
@ -0,0 +1,256 @@
|
|||
import pinocchio as pin
|
||||
import numpy as np
|
||||
try:
|
||||
import meshcat
|
||||
import meshcat.geometry as g
|
||||
import meshcat.transformations as tf
|
||||
except:
|
||||
print("You need to install meshcat.")
|
||||
|
||||
import crocoddyl
|
||||
|
||||
|
||||
|
||||
class ResidualForce3D(crocoddyl.ResidualModelAbstract):
|
||||
def __init__(self, state, contact_name, nu):
|
||||
crocoddyl.ResidualModelAbstract.__init__(self, state, 3, nu, True, True, True)
|
||||
self.contact_name = contact_name
|
||||
|
||||
def calc(self, data, x, u=None):
|
||||
data.r = data.shared.contacts.contacts[self.contact_name].f.vector[:3]
|
||||
|
||||
def calcDiff(self, data, x, u=None):
|
||||
data.Rx = data.shared.contacts.contacts[self.contact_name].df_dx[:3]
|
||||
data.Ru = data.shared.contacts.contacts[self.contact_name].df_du[:3]
|
||||
|
||||
|
||||
class ResidualFrictionCone(crocoddyl.ResidualModelAbstract):
|
||||
def __init__(self, state, contact_name, mu, nu):
|
||||
crocoddyl.ResidualModelAbstract.__init__(self, state, 1, nu, True, True, True)
|
||||
self.mu = mu
|
||||
self.contact_name = contact_name
|
||||
|
||||
self.dcone_df = np.zeros((1, 3))
|
||||
self.df_dx = np.zeros((3, self.state.ndx))
|
||||
self.df_du = np.zeros((3, self.nu))
|
||||
|
||||
def calc(self, data, x, u=None):
|
||||
F = data.shared.contacts.contacts[self.contact_name].f.vector[:3]
|
||||
data.r[0] = np.array([self.mu * F[2] - np.sqrt(F[0]**2 + F[1]**2)])
|
||||
|
||||
def calcDiff(self, data, x, u=None):
|
||||
F = data.shared.contacts.contacts[self.contact_name].f.vector[:3]
|
||||
|
||||
self.dcone_df[0, 0] = -F[0] / np.sqrt(F[0]**2 + F[1]**2)
|
||||
self.dcone_df[0, 1] = -F[1] / np.sqrt(F[0]**2 + F[1]**2)
|
||||
self.dcone_df[0, 2] = self.mu
|
||||
|
||||
|
||||
self.df_dx = data.shared.contacts.contacts[self.contact_name].df_dx[:3]
|
||||
self.df_du = data.shared.contacts.contacts[self.contact_name].df_du[:3]
|
||||
|
||||
data.Rx = self.dcone_df @ self.df_dx
|
||||
data.Ru = self.dcone_df @ self.df_du
|
||||
|
||||
|
||||
def meshcat_material(r, g, b, a):
|
||||
material = meshcat.geometry.MeshPhongMaterial()
|
||||
material.color = int(r * 255) * 256 ** 2 + int(g * 255) * 256 + int(b * 255)
|
||||
material.opacity = a
|
||||
material.linewidth = 5.0
|
||||
return material
|
||||
|
||||
def addViewerBox(viz, name, sizex, sizey, sizez, rgba):
|
||||
if isinstance(viz, pin.visualize.MeshcatVisualizer):
|
||||
viz.viewer[name].set_object(meshcat.geometry.Box([sizex, sizey, sizez]),
|
||||
meshcat_material(*rgba))
|
||||
|
||||
|
||||
def addLineSegment(viz, name, vertices, rgba):
|
||||
if isinstance(viz, pin.visualize.MeshcatVisualizer):
|
||||
viz.viewer[name].set_object(meshcat.geometry.LineSegments(
|
||||
meshcat.geometry.PointsGeometry(np.array(vertices)),
|
||||
meshcat_material(*rgba)
|
||||
))
|
||||
|
||||
def addPoint(viz, name, vertices, rgba):
|
||||
if isinstance(viz, pin.visualize.MeshcatVisualizer):
|
||||
viz.viewer[name].set_object(meshcat.geometry.Points(
|
||||
meshcat.geometry.PointsGeometry(np.array(vertices)),
|
||||
meshcat_material(*rgba)
|
||||
))
|
||||
|
||||
def meshcat_transform(x, y, z, q, u, a, t):
|
||||
return np.array(pin.XYZQUATToSE3([x, y, z, q, u, a, t]))
|
||||
|
||||
def applyViewerConfiguration(viz, name, xyzquat):
|
||||
if isinstance(viz, pin.visualize.MeshcatVisualizer):
|
||||
viz.viewer[name].set_transform(meshcat_transform(*xyzquat))
|
||||
|
||||
def get_solution_trajectories(solver, rmodel, rdata, supportFeetIds):
|
||||
xs, us = solver.xs, solver.us
|
||||
nq, nv, N = rmodel.nq, rmodel.nv, len(xs)
|
||||
jointPos_sol = []
|
||||
jointVel_sol = []
|
||||
jointAcc_sol = []
|
||||
jointTorques_sol = []
|
||||
centroidal_sol = []
|
||||
|
||||
x = []
|
||||
for time_idx in range (N):
|
||||
q, v = xs[time_idx][:nq], xs[time_idx][nq:]
|
||||
pin.framesForwardKinematics(rmodel, rdata, q)
|
||||
pin.computeCentroidalMomentum(rmodel, rdata, q, v)
|
||||
centroidal_sol += [
|
||||
np.concatenate(
|
||||
[pin.centerOfMass(rmodel, rdata, q, v), np.array(rdata.hg)]
|
||||
)
|
||||
]
|
||||
jointPos_sol += [q]
|
||||
jointVel_sol += [v]
|
||||
x += [xs[time_idx]]
|
||||
if time_idx < N-1:
|
||||
jointAcc_sol += [solver.problem.runningDatas[time_idx].xnext[nq::]]
|
||||
jointTorques_sol += [us[time_idx]]
|
||||
|
||||
|
||||
|
||||
|
||||
sol = {'x':x, 'centroidal':centroidal_sol, 'jointPos':jointPos_sol,
|
||||
'jointVel':jointVel_sol, 'jointAcc':jointAcc_sol,
|
||||
'jointTorques':jointTorques_sol}
|
||||
|
||||
|
||||
for frame_idx in supportFeetIds:
|
||||
# print('extract foot id ', frame_idx, "_name = ", rmodel.frames[frame_idx].name)
|
||||
ct_frame_name = rmodel.frames[frame_idx].name + "_contact"
|
||||
datas = [solver.problem.runningDatas[i].differential.multibody.contacts.contacts[ct_frame_name] for i in range(N-1)]
|
||||
ee_forces = [datas[k].f.vector for k in range(N-1)]
|
||||
sol[ct_frame_name] = [ee_forces[i] for i in range(N-1)]
|
||||
|
||||
return sol
|
||||
|
||||
|
||||
|
||||
|
||||
class Arrow(object):
|
||||
def __init__(self, meshcat_vis, name,
|
||||
location=[0,0,0],
|
||||
vector=[0,0,1],
|
||||
length_scale=1,
|
||||
color=0xff0000):
|
||||
|
||||
self.vis = meshcat_vis[name]
|
||||
self.cone = self.vis["cone"]
|
||||
self.line = self.vis["line"]
|
||||
self.material = g.MeshBasicMaterial(color=color, reflectivity=0.5)
|
||||
|
||||
self.location, self.length_scale = location, length_scale
|
||||
self.anchor_as_vector(location, vector)
|
||||
|
||||
def _update(self):
|
||||
# pass
|
||||
translation = tf.translation_matrix(self.location)
|
||||
rotation = self.orientation
|
||||
offset = tf.translation_matrix([0, self.length/2, 0])
|
||||
self.pose = translation @ rotation @ offset
|
||||
self.vis.set_transform(self.pose)
|
||||
|
||||
def set_length(self, length, update=True):
|
||||
self.length = length * self.length_scale
|
||||
cone_scale = self.length/0.08
|
||||
self.line.set_object(g.Cylinder(height=self.length, radius=0.005), self.material)
|
||||
self.cone.set_object(g.Cylinder(height=0.015,
|
||||
radius=0.01,
|
||||
radiusTop=0.,
|
||||
radiusBottom=0.01),
|
||||
self.material)
|
||||
self.cone.set_transform(tf.translation_matrix([0.,cone_scale*0.04,0]))
|
||||
if update:
|
||||
self._update()
|
||||
|
||||
def set_direction(self, direction, update=True):
|
||||
orientation = np.eye(4)
|
||||
orientation[:3, 0] = np.cross([1,0,0], direction)
|
||||
orientation[:3, 1] = direction
|
||||
orientation[:3, 2] = np.cross(orientation[:3, 0], orientation[:3, 1])
|
||||
self.orientation = orientation
|
||||
if update:
|
||||
self._update()
|
||||
|
||||
def set_location(self, location, update=True):
|
||||
self.location = location
|
||||
if update:
|
||||
self._update()
|
||||
|
||||
def anchor_as_vector(self, location, vector, update=True):
|
||||
self.set_direction(np.array(vector)/np.linalg.norm(vector), False)
|
||||
self.set_location(location, False)
|
||||
self.set_length(np.linalg.norm(vector), False)
|
||||
if update:
|
||||
self._update()
|
||||
|
||||
def delete(self):
|
||||
self.vis.delete()
|
||||
|
||||
|
||||
|
||||
|
||||
class Cone(object):
|
||||
def __init__(self, meshcat_vis, name,
|
||||
location=[0,0,0], mu=1,
|
||||
vector=[0,0,1],
|
||||
length_scale=0.06):
|
||||
|
||||
self.vis = meshcat_vis[name]
|
||||
self.cone = self.vis["cone"]
|
||||
self.material = g.MeshBasicMaterial(color=0xffffff, opacity = 0.5, reflectivity=0.5)
|
||||
|
||||
|
||||
self.mu = mu * length_scale
|
||||
self.location, self.length_scale = location, length_scale
|
||||
self.anchor_as_vector(location, vector)
|
||||
|
||||
def _update(self):
|
||||
# pass
|
||||
translation = tf.translation_matrix(self.location)
|
||||
rotation = self.orientation
|
||||
offset = tf.translation_matrix([0, self.length/2, 0])
|
||||
self.pose = translation @ rotation @ offset
|
||||
self.vis.set_transform(self.pose)
|
||||
|
||||
def set_length(self, length, update=True):
|
||||
self.length = length * self.length_scale
|
||||
cone_scale = self.length
|
||||
self.cone.set_object(g.Cylinder(height=cone_scale,
|
||||
radius=self.mu,
|
||||
radiusTop=self.mu,
|
||||
radiusBottom=0),
|
||||
self.material)
|
||||
# self.cone.set_transform(tf.translation_matrix([0.,cone_scale*0.04,0]))
|
||||
if update:
|
||||
self._update()
|
||||
|
||||
def set_direction(self, direction, update=True):
|
||||
orientation = np.eye(4)
|
||||
orientation[:3, 0] = np.cross([1,0,0], direction)
|
||||
orientation[:3, 1] = direction
|
||||
orientation[:3, 2] = np.cross(orientation[:3, 0], orientation[:3, 1])
|
||||
self.orientation = orientation
|
||||
if update:
|
||||
self._update()
|
||||
|
||||
def set_location(self, location, update=True):
|
||||
self.location = location
|
||||
if update:
|
||||
self._update()
|
||||
|
||||
def anchor_as_vector(self, location, vector, update=True):
|
||||
self.set_direction(np.array(vector)/np.linalg.norm(vector), False)
|
||||
self.set_location(location, False)
|
||||
self.set_length(np.linalg.norm(vector), False)
|
||||
if update:
|
||||
self._update()
|
||||
|
||||
def delete(self):
|
||||
self.vis.delete()
|
||||
Binary file not shown.
Loading…
Reference in New Issue