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
|
FROM ros:humble
|
||||||
ENV DEBIAN_FRONTEND noninteractive
|
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 \
|
RUN apt-get update && apt-get install -y -qq --no-install-recommends \
|
||||||
libglvnd-dev \
|
libglvnd-dev \
|
||||||
libgl1-mesa-dev \
|
libgl1-mesa-dev \
|
||||||
|
|
@ -23,10 +25,22 @@ RUN apt-get update && apt-get install -y -qq --no-install-recommends \
|
||||||
build-essential \
|
build-essential \
|
||||||
cmake \
|
cmake \
|
||||||
git \
|
git \
|
||||||
|
wget \
|
||||||
&& rm -rf /var/lib/apt/lists/*
|
&& rm -rf /var/lib/apt/lists/*
|
||||||
|
|
||||||
RUN curl -s https://packagecloud.io/install/repositories/github/git-lfs/script.deb.sh | bash
|
RUN curl -s https://packagecloud.io/install/repositories/github/git-lfs/script.deb.sh | bash
|
||||||
RUN apt-get install git-lfs
|
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
|
# 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 && \
|
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
|
cd .. && colcon build --packages-select cyclonedds && source /opt/ros/humble/setup.bash && colcon build
|
||||||
|
|
||||||
# Install Python dependencies
|
# Install Python dependencies
|
||||||
RUN pip3 install scipy ipykernel warp-lang
|
RUN pip install torch torchvision torchaudio --index-url https://download.pytorch.org/whl/cu118
|
||||||
RUN pip3 install torch torchvision torchaudio --index-url https://download.pytorch.org/whl/cu118
|
RUN pip install matplotlib opencv-python proxsuite scipy isort black
|
||||||
RUN pip3 install matplotlib opencv-python proxsuite
|
RUN pip install warp-lang scikit-learn casadi
|
||||||
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 pynput onnx onnxruntime jupyter ipykernel meshcat mujoco
|
||||||
RUN pip install cyclonedds pygame
|
RUN conda install -y -c conda-forge \
|
||||||
RUN pip install pynput pygame
|
pinocchio \
|
||||||
RUN pip install onnx onnxruntime
|
crocoddyl \
|
||||||
|
mim-solvers
|
||||||
|
|
||||||
# Set environmental variables required for using ROS
|
# Set environmental variables required for using ROS
|
||||||
RUN echo 'source /opt/ros/humble/setup.bash' >> ~/.bashrc
|
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