dockerfile updated and basic ocp example added
This commit is contained in:
Rooholla-Khorrambakht
parent
ef07e54121
commit
4aca24c569
|
|
@ -1,6 +1,8 @@
|
|||
FROM ros:humble
|
||||
ENV DEBIAN_FRONTEND noninteractive
|
||||
SHELL ["/bin/bash", "-c"]
|
||||
ARG CONDA_VER=latest
|
||||
ARG OS_TYPE=x86_64
|
||||
|
||||
RUN apt-get update && apt-get install -y -qq --no-install-recommends \
|
||||
libglvnd-dev \
|
||||
libgl1-mesa-dev \
|
||||
|
|
@ -23,10 +25,22 @@ RUN apt-get update && apt-get install -y -qq --no-install-recommends \
|
|||
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
|
||||
# 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
|
||||
SHELL ["conda", "run", "-n", "base", "/bin/bash", "-c"]
|
||||
ENV CONDA_PREFIX /opt/conda
|
||||
|
||||
# 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 && \
|
||||
|
|
@ -34,16 +48,15 @@ git clone https://github.com/ros2/rmw_cyclonedds -b humble && git clone https://
|
|||
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 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 jupyter ipykernel
|
||||
RUN pip install cyclonedds pygame
|
||||
RUN pip install pynput pygame
|
||||
RUN pip install onnx onnxruntime
|
||||
RUN pip install cyclonedds pygame pynput onnx onnxruntime jupyter ipykernel 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
|
||||
|
|
|
|||
|
|
@ -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_hip">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="-0.0054 0.00194 -0.000105"/>
|
||||
<mass value="0.678"/>
|
||||
<inertia ixx="0.00048" ixy="-3.01E-06" ixz="1.11E-06" iyy="0.000884" iyz="-1.42E-06" izz="0.000596"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="dae/hip.dae"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="1 1 1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<!-- <collision>
|
||||
<origin rpy="1.5707963267948966 0 0" xyz="0 0.08 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.04" radius="0.046"/>
|
||||
</geometry>
|
||||
</collision> -->
|
||||
</link>
|
||||
<joint name="FL_hip_joint" type="revolute">
|
||||
<origin rpy="0 0 0" xyz="0.1934 0.0465 0"/>
|
||||
<parent link="base_link"/>
|
||||
<child link="FL_hip"/>
|
||||
<axis xyz="1 0 0"/>
|
||||
<dynamics damping="0" friction="0"/>
|
||||
<limit effort="23.7" lower="-1.0472" upper="1.0472" velocity="30.1"/>
|
||||
</joint>
|
||||
<link name="FL_thigh">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="-0.00374 -0.0223 -0.0327"/>
|
||||
<mass value="1.152"/>
|
||||
<inertia ixx="0.00584" ixy="8.72E-05" ixz="-0.000289" iyy="0.0058" iyz="0.000808" izz="0.00103"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="dae/thigh.dae"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="1 1 1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 1.5707963267948966 0" xyz="0 0 -0.1065"/>
|
||||
<geometry>
|
||||
<box size="0.213 0.0245 0.034"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="FL_thigh_joint" type="revolute">
|
||||
<origin rpy="0 0 0" xyz="0 0.0955 0"/>
|
||||
<parent link="FL_hip"/>
|
||||
<child link="FL_thigh"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
<dynamics damping="0" friction="0"/>
|
||||
<limit effort="23.7" lower="-1.5708" upper="3.4907" velocity="30.1"/>
|
||||
</joint>
|
||||
<link name="FL_calf">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0.00548 -0.000975 -0.115"/>
|
||||
<mass value="0.154"/>
|
||||
<inertia ixx="0.00108" ixy="3.4E-07" ixz="1.72E-05" iyy="0.0011" iyz="8.28E-06" izz="3.29E-05"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="dae/calf.dae"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="1 1 1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 -0.21 0" xyz="0.008 0 -0.06"/>
|
||||
<geometry>
|
||||
<cylinder length="0.12" radius="0.012"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="FL_calf_joint" type="revolute">
|
||||
<origin rpy="0 0 0" xyz="0 0 -0.213"/>
|
||||
<parent link="FL_thigh"/>
|
||||
<child link="FL_calf"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
<dynamics damping="0" friction="0"/>
|
||||
<limit effort="45.43" lower="-2.7227" upper="-0.83776" velocity="15.70"/>
|
||||
</joint>
|
||||
<link name="FL_calflower">
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.065" radius="0.011"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="FL_calflower_joint" type="fixed">
|
||||
<origin rpy="0 0.05 0" xyz="0.020 0 -0.148"/>
|
||||
<parent link="FL_calf"/>
|
||||
<child link="FL_calflower"/>
|
||||
<axis xyz="0 0 0"/>
|
||||
</joint>
|
||||
<link name="FL_calflower1">
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.03" radius="0.0155"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="FL_calflower1_joint" type="fixed">
|
||||
<origin rpy="0 0.48 0" xyz="-0.01 0 -0.04"/>
|
||||
<parent link="FL_calflower"/>
|
||||
<child link="FL_calflower1"/>
|
||||
<axis xyz="0 0 0"/>
|
||||
</joint>
|
||||
<link name="FL_foot">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<mass value="0.04"/>
|
||||
<inertia ixx="9.6e-06" ixy="0" ixz="0" iyy="9.6e-06" iyz="0" izz="9.6e-06"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="dae/foot.dae"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="1 1 1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="-0.002 0 0"/>
|
||||
<geometry>
|
||||
<sphere radius="0.022"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint dont_collapse="true" name="FL_foot_joint" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="0 0 -0.213"/>
|
||||
<parent link="FL_calf"/>
|
||||
<child link="FL_foot"/>
|
||||
<axis xyz="0 0 0"/>
|
||||
</joint>
|
||||
<link name="FR_hip">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="-0.0054 -0.00194 -0.000105"/>
|
||||
<mass value="0.678"/>
|
||||
<inertia ixx="0.00048" ixy="3.01E-06" ixz="1.11E-06" iyy="0.000884" iyz="1.42E-06" izz="0.000596"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="3.1415 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="dae/hip.dae"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="1 1 1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<!-- <collision>
|
||||
<origin rpy="1.5707963267948966 0 0" xyz="0 -0.08 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.04" radius="0.046"/>
|
||||
</geometry>
|
||||
</collision> -->
|
||||
</link>
|
||||
<joint name="FR_hip_joint" type="revolute">
|
||||
<origin rpy="0 0 0" xyz="0.1934 -0.0465 0"/>
|
||||
<parent link="base_link"/>
|
||||
<child link="FR_hip"/>
|
||||
<axis xyz="1 0 0"/>
|
||||
<dynamics damping="0" friction="0"/>
|
||||
<limit effort="23.7" lower="-1.0472" upper="1.0472" velocity="30.1"/>
|
||||
</joint>
|
||||
<link name="FR_thigh">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="-0.00374 0.0223 -0.0327"/>
|
||||
<mass value="1.152"/>
|
||||
<inertia ixx="0.00584" ixy="-8.72E-05" ixz="-0.000289" iyy="0.0058" iyz="-0.000808" izz="0.00103"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="dae/thigh_mirror.dae"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="1 1 1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 1.5707963267948966 0" xyz="0 0 -0.1065"/>
|
||||
<geometry>
|
||||
<box size="0.213 0.0245 0.034"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="FR_thigh_joint" type="revolute">
|
||||
<origin rpy="0 0 0" xyz="0 -0.0955 0"/>
|
||||
<parent link="FR_hip"/>
|
||||
<child link="FR_thigh"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
<dynamics damping="0" friction="0"/>
|
||||
<limit effort="23.7" lower="-1.5708" upper="3.4907" velocity="30.1"/>
|
||||
</joint>
|
||||
<link name="FR_calf">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0.00548 0.000975 -0.115"/>
|
||||
<mass value="0.154"/>
|
||||
<inertia ixx="0.00108" ixy="-3.4E-07" ixz="1.72E-05" iyy="0.0011" iyz="-8.28E-06" izz="3.29E-05"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="dae/calf_mirror.dae"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="1 1 1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 -0.2 0" xyz="0.01 0 -0.06"/>
|
||||
<geometry>
|
||||
<cylinder length="0.12" radius="0.013"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="FR_calf_joint" type="revolute">
|
||||
<origin rpy="0 0 0" xyz="0 0 -0.213"/>
|
||||
<parent link="FR_thigh"/>
|
||||
<child link="FR_calf"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
<dynamics damping="0" friction="0"/>
|
||||
<limit effort="45.43" lower="-2.7227" upper="-0.83776" velocity="15.70"/>
|
||||
</joint>
|
||||
<link name="FR_calflower">
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.065" radius="0.011"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="FR_calflower_joint" type="fixed">
|
||||
<origin rpy="0 0.05 0" xyz="0.020 0 -0.148"/>
|
||||
<parent link="FR_calf"/>
|
||||
<child link="FR_calflower"/>
|
||||
<axis xyz="0 0 0"/>
|
||||
</joint>
|
||||
<link name="FR_calflower1">
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.03" radius="0.0155"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="FR_calflower1_joint" type="fixed">
|
||||
<origin rpy="0 0.48 0" xyz="-0.01 0 -0.04"/>
|
||||
<parent link="FR_calflower"/>
|
||||
<child link="FR_calflower1"/>
|
||||
<axis xyz="0 0 0"/>
|
||||
</joint>
|
||||
<link name="FR_foot">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<mass value="0.04"/>
|
||||
<inertia ixx="9.6e-06" ixy="0" ixz="0" iyy="9.6e-06" iyz="0" izz="9.6e-06"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="dae/foot.dae"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="1 1 1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="-0.002 0 0"/>
|
||||
<geometry>
|
||||
<sphere radius="0.022"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint dont_collapse="true" name="FR_foot_joint" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="0 0 -0.213"/>
|
||||
<parent link="FR_calf"/>
|
||||
<child link="FR_foot"/>
|
||||
<axis xyz="0 0 0"/>
|
||||
</joint>
|
||||
<link name="RL_hip">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0.0054 0.00194 -0.000105"/>
|
||||
<mass value="0.678"/>
|
||||
<inertia ixx="0.00048" ixy="3.01E-06" ixz="-1.11E-06" iyy="0.000884" iyz="-1.42E-06" izz="0.000596"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 3.1415 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="dae/hip.dae"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="1 1 1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<!-- <collision>
|
||||
<origin rpy="1.5707963267948966 0 0" xyz="0 0.08 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.04" radius="0.046"/>
|
||||
</geometry>
|
||||
</collision> -->
|
||||
</link>
|
||||
<joint name="RL_hip_joint" type="revolute">
|
||||
<origin rpy="0 0 0" xyz="-0.1934 0.0465 0"/>
|
||||
<parent link="base_link"/>
|
||||
<child link="RL_hip"/>
|
||||
<axis xyz="1 0 0"/>
|
||||
<dynamics damping="0" friction="0"/>
|
||||
<limit effort="23.7" lower="-1.0472" upper="1.0472" velocity="30.1"/>
|
||||
</joint>
|
||||
<link name="RL_thigh">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="-0.00374 -0.0223 -0.0327"/>
|
||||
<mass value="1.152"/>
|
||||
<inertia ixx="0.00584" ixy="8.72E-05" ixz="-0.000289" iyy="0.0058" iyz="0.000808" izz="0.00103"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="dae/thigh.dae"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="1 1 1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 1.5707963267948966 0" xyz="0 0 -0.1065"/>
|
||||
<geometry>
|
||||
<box size="0.213 0.0245 0.034"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="RL_thigh_joint" type="revolute">
|
||||
<origin rpy="0 0 0" xyz="0 0.0955 0"/>
|
||||
<parent link="RL_hip"/>
|
||||
<child link="RL_thigh"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
<dynamics damping="0" friction="0"/>
|
||||
<limit effort="23.7" lower="-0.5236" upper="4.5379" velocity="30.1"/>
|
||||
</joint>
|
||||
<link name="RL_calf">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0.00548 -0.000975 -0.115"/>
|
||||
<mass value="0.154"/>
|
||||
<inertia ixx="0.00108" ixy="3.4E-07" ixz="1.72E-05" iyy="0.0011" iyz="8.28E-06" izz="3.29E-05"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="dae/calf.dae"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="1 1 1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 -0.2 0" xyz="0.01 0 -0.06"/>
|
||||
<geometry>
|
||||
<cylinder length="0.12" radius="0.013"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="RL_calf_joint" type="revolute">
|
||||
<origin rpy="0 0 0" xyz="0 0 -0.213"/>
|
||||
<parent link="RL_thigh"/>
|
||||
<child link="RL_calf"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
<dynamics damping="0" friction="0"/>
|
||||
<limit effort="45.43" lower="-2.7227" upper="-0.83776" velocity="15.70"/>
|
||||
</joint>
|
||||
<link name="RL_calflower">
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.065" radius="0.011"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="RL_calflower_joint" type="fixed">
|
||||
<origin rpy="0 0.05 0" xyz="0.020 0 -0.148"/>
|
||||
<parent link="RL_calf"/>
|
||||
<child link="RL_calflower"/>
|
||||
<axis xyz="0 0 0"/>
|
||||
</joint>
|
||||
<link name="RL_calflower1">
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.03" radius="0.0155"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="RL_calflower1_joint" type="fixed">
|
||||
<origin rpy="0 0.48 0" xyz="-0.01 0 -0.04"/>
|
||||
<parent link="RL_calflower"/>
|
||||
<child link="RL_calflower1"/>
|
||||
<axis xyz="0 0 0"/>
|
||||
</joint>
|
||||
<link name="RL_foot">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<mass value="0.04"/>
|
||||
<inertia ixx="9.6e-06" ixy="0" ixz="0" iyy="9.6e-06" iyz="0" izz="9.6e-06"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="dae/foot.dae"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="1 1 1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="-0.002 0 0"/>
|
||||
<geometry>
|
||||
<sphere radius="0.022"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint dont_collapse="true" name="RL_foot_joint" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="0 0 -0.213"/>
|
||||
<parent link="RL_calf"/>
|
||||
<child link="RL_foot"/>
|
||||
<axis xyz="0 0 0"/>
|
||||
</joint>
|
||||
<link name="RR_hip">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0.0054 -0.00194 -0.000105"/>
|
||||
<mass value="0.678"/>
|
||||
<inertia ixx="0.00048" ixy="-3.01E-06" ixz="-1.11E-06" iyy="0.000884" iyz="1.42E-06" izz="0.000596"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="3.1415 3.1415 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="dae/hip.dae"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="1 1 1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<!-- <collision>
|
||||
<origin rpy="1.5707963267948966 0 0" xyz="0 -0.08 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.04" radius="0.046"/>
|
||||
</geometry>
|
||||
</collision> -->
|
||||
</link>
|
||||
<joint name="RR_hip_joint" type="revolute">
|
||||
<origin rpy="0 0 0" xyz="-0.1934 -0.0465 0"/>
|
||||
<parent link="base_link"/>
|
||||
<child link="RR_hip"/>
|
||||
<axis xyz="1 0 0"/>
|
||||
<dynamics damping="0" friction="0"/>
|
||||
<limit effort="23.7" lower="-1.0472" upper="1.0472" velocity="30.1"/>
|
||||
</joint>
|
||||
<link name="RR_thigh">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="-0.00374 0.0223 -0.0327"/>
|
||||
<mass value="1.152"/>
|
||||
<inertia ixx="0.00584" ixy="-8.72E-05" ixz="-0.000289" iyy="0.0058" iyz="-0.000808" izz="0.00103"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="dae/thigh_mirror.dae"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="1 1 1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 1.5707963267948966 0" xyz="0 0 -0.1065"/>
|
||||
<geometry>
|
||||
<box size="0.213 0.0245 0.034"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="RR_thigh_joint" type="revolute">
|
||||
<origin rpy="0 0 0" xyz="0 -0.0955 0"/>
|
||||
<parent link="RR_hip"/>
|
||||
<child link="RR_thigh"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
<dynamics damping="0" friction="0"/>
|
||||
<limit effort="23.7" lower="-0.5236" upper="4.5379" velocity="30.1"/>
|
||||
</joint>
|
||||
<link name="RR_calf">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0.00548 0.000975 -0.115"/>
|
||||
<mass value="0.154"/>
|
||||
<inertia ixx="0.00108" ixy="-3.4E-07" ixz="1.72E-05" iyy="0.0011" iyz="-8.28E-06" izz="3.29E-05"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="dae/calf_mirror.dae"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="1 1 1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 -0.2 0" xyz="0.01 0 -0.06"/>
|
||||
<geometry>
|
||||
<cylinder length="0.12" radius="0.013"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="RR_calf_joint" type="revolute">
|
||||
<origin rpy="0 0 0" xyz="0 0 -0.213"/>
|
||||
<parent link="RR_thigh"/>
|
||||
<child link="RR_calf"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
<dynamics damping="0" friction="0"/>
|
||||
<limit effort="45.43" lower="-2.7227" upper="-0.83776" velocity="15.70"/>
|
||||
</joint>
|
||||
<link name="RR_calflower">
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.065" radius="0.011"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="RR_calflower_joint" type="fixed">
|
||||
<origin rpy="0 0.05 0" xyz="0.020 0 -0.148"/>
|
||||
<parent link="RR_calf"/>
|
||||
<child link="RR_calflower"/>
|
||||
<axis xyz="0 0 0"/>
|
||||
</joint>
|
||||
<link name="RR_calflower1">
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.03" radius="0.0155"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="RR_calflower1_joint" type="fixed">
|
||||
<origin rpy="0 0.48 0" xyz="-0.01 0 -0.04"/>
|
||||
<parent link="RR_calflower"/>
|
||||
<child link="RR_calflower1"/>
|
||||
<axis xyz="0 0 0"/>
|
||||
</joint>
|
||||
<link name="RR_foot">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<mass value="0.04"/>
|
||||
<inertia ixx="9.6e-06" ixy="0" ixz="0" iyy="9.6e-06" iyz="0" izz="9.6e-06"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="dae/foot.dae"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="1 1 1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="-0.002 0 0"/>
|
||||
<geometry>
|
||||
<sphere radius="0.022"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint dont_collapse="true" name="RR_foot_joint" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="0 0 -0.213"/>
|
||||
<parent link="RR_calf"/>
|
||||
<child link="RR_foot"/>
|
||||
<axis xyz="0 0 0"/>
|
||||
</joint>
|
||||
<link name="imu_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 one or more lines are too long
|
|
@ -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()
|
||||
Loading…
Reference in New Issue