Initial commit
This commit is contained in:
cyy
29
LICENSE
Normal file
29
LICENSE
Normal file
@@ -0,0 +1,29 @@
|
|||||||
|
BSD 3-Clause License
|
||||||
|
|
||||||
|
Copyright (c) 2016-2022 HangZhou YuShu TECHNOLOGY CO.,LTD. ("Unitree Robotics")
|
||||||
|
All rights reserved.
|
||||||
|
|
||||||
|
Redistribution and use in source and binary forms, with or without
|
||||||
|
modification, are permitted provided that the following conditions are met:
|
||||||
|
|
||||||
|
* Redistributions of source code must retain the above copyright notice, this
|
||||||
|
list of conditions and the following disclaimer.
|
||||||
|
|
||||||
|
* Redistributions in binary form must reproduce the above copyright notice,
|
||||||
|
this list of conditions and the following disclaimer in the documentation
|
||||||
|
and/or other materials provided with the distribution.
|
||||||
|
|
||||||
|
* Neither the name of the copyright holder nor the names of its
|
||||||
|
contributors may be used to endorse or promote products derived from
|
||||||
|
this software without specific prior written permission.
|
||||||
|
|
||||||
|
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||||
|
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||||
|
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||||
|
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
|
||||||
|
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
|
||||||
|
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
|
||||||
|
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||||
|
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
|
||||||
|
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
|
||||||
|
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||||
99
README.md
Normal file
99
README.md
Normal file
@@ -0,0 +1,99 @@
|
|||||||
|
# Introduction
|
||||||
|
[MuJoCo](https://mujoco.org/) stands for Multi-Joint dynamics with Contact. It is a physics engine for simulation and has been open source. To get more details, see these repositories: [MuJoCo](https://github.com/deepmind/mujoco) and [MuJuCo-py](https://github.com/openai/mujoco-py).
|
||||||
|
|
||||||
|
In this repo, some mujoco example scripts and the xml files of different Unitree robots are provided, including A1, Aliengo, Laikago and Go1. To simulate different robots, the corresponding xml files are needed. Here we introduce how to generate xml files from urdf and how to use them.
|
||||||
|
|
||||||
|
# Generate XML
|
||||||
|
In MuJoCo, robots are described by xml files. Before we generate the xml file, we need the urdf file of the robot. There are many ways to obtain urdf, and these methods are not discussed here. After generating the urdf, you must ensure that all meshes are stl files. If you are using meshes in obj or other formats, convert them to stl.
|
||||||
|
|
||||||
|
## Edit URDF
|
||||||
|
Before converting urdf to xml, some parts of urdf file need to be edited. Take "/data/urdf/a1.urdf" as an example.
|
||||||
|
|
||||||
|
1. Add MuJoCo tag
|
||||||
|
|
||||||
|
Add the following line after the robot description line:
|
||||||
|
```
|
||||||
|
<mujoco>
|
||||||
|
<compiler
|
||||||
|
meshdir="../meshes/"
|
||||||
|
balanceinertia="true"
|
||||||
|
discardvisual="false" />
|
||||||
|
</mujoco>
|
||||||
|
```
|
||||||
|
|
||||||
|
2. Add a floating joint between robot body and world frame
|
||||||
|
|
||||||
|
Since MuJoCo set the base link as fixed by default, a free joint has to be manually added between robot body and world frame:
|
||||||
|
```
|
||||||
|
<link name="world"/>
|
||||||
|
<joint name="floating_base" type="floating">
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<parent link="world"/>
|
||||||
|
<child link="trunk"/>
|
||||||
|
</joint>
|
||||||
|
```
|
||||||
|
|
||||||
|
## Generate xml
|
||||||
|
MuJoCo provides an executable file for converting urdf to XML. This file is under "/mujoco/bin". To use it:
|
||||||
|
```
|
||||||
|
./compile /path/to/model.urdf /path/to/model.xml
|
||||||
|
```
|
||||||
|
|
||||||
|
## Edit xml file
|
||||||
|
After converting the urdf to xml, some parts of xml file need to be edited.
|
||||||
|
|
||||||
|
1. Set environment
|
||||||
|
|
||||||
|
Add the following line after " size njmax="500" nconmax="100" "
|
||||||
|
|
||||||
|
```
|
||||||
|
<option gravity='0 0 -9.806' iterations='50' solver='Newton' timestep='0.002'/>
|
||||||
|
|
||||||
|
<default>
|
||||||
|
<geom contype="1" conaffinity="1" friction="0.6 0.3 0.3" rgba="0.5 0.6 0.7 1" margin="0.001" group="0"/>
|
||||||
|
|
||||||
|
<light castshadow="false" diffuse="1 1 1"/>
|
||||||
|
<motor ctrlrange="-33.5 33.5" ctrllimited="true"/>
|
||||||
|
<camera fovy="60"/>
|
||||||
|
<joint damping="0.01" armature="0.01"/>
|
||||||
|
|
||||||
|
</default>
|
||||||
|
|
||||||
|
<asset>
|
||||||
|
<mesh name="trunk" file="trunk.stl" />
|
||||||
|
<mesh name="hip" file="hip.stl" />
|
||||||
|
<mesh name="thigh_mirror" file="thigh_mirror.stl" />
|
||||||
|
<mesh name="calf" file="calf.stl" />
|
||||||
|
<mesh name="thigh" file="thigh.stl" />
|
||||||
|
</asset>
|
||||||
|
|
||||||
|
<asset>
|
||||||
|
<texture type="skybox" builtin="gradient" rgb1="1.0 1.0 1.0" rgb2="1.0 1.0 1.0" width="512" height="512"/>
|
||||||
|
<texture name="plane" type="2d" builtin="flat" rgb1="1 1 1" rgb2="1 1 1" width="512" height="512" mark="cross" markrgb="0 0 0"/>
|
||||||
|
<material name="plane" reflectance="0.0" texture="plane" texrepeat="3 3" texuniform="true"/>
|
||||||
|
</asset>
|
||||||
|
```
|
||||||
|
|
||||||
|
2. Add ground and light source:
|
||||||
|
|
||||||
|
Add these line after "worldbody" tag:
|
||||||
|
```
|
||||||
|
<light directional="true" diffuse=".8 .8 .8" pos="0 0 10" dir="0 0 -10"/>
|
||||||
|
<camera name="track" mode="trackcom" pos="0 -1.3 1.6" xyaxes="1 0 0 0 0.707 0.707"/>
|
||||||
|
<geom name='floor' type='plane' conaffinity='1' condim='3' contype='1' rgba="0.5 0.9 0.9 0.1" material='plane' pos='0 0 0' size='0 0 1'/>
|
||||||
|
|
||||||
|
```
|
||||||
|
|
||||||
|
3. Remove the visibility of the collision parts:
|
||||||
|
|
||||||
|
In the model, some parts are used as collision detection. For these parts we do not want them to show in the simulation. To make them invisible, we need to edit the rgba value.
|
||||||
|
|
||||||
|
For instance, to hide the body collision part, just set the last number of rgba to 0:
|
||||||
|
```
|
||||||
|
from
|
||||||
|
<geom size="0.1335 0.097 0.057" type="box" rgba="0.913725 0.913725 0.847059 1" />
|
||||||
|
to
|
||||||
|
<geom size="0.1335 0.097 0.057" type="box" rgba="0.913725 0.913725 0.847059 0" />
|
||||||
|
```
|
||||||
|
|
||||||
|
For now, all the preparation is done and we can simulate our robot in MuJoCo.
|
||||||
BIN
data/a1/meshes/calf.stl
Normal file
BIN
data/a1/meshes/calf.stl
Normal file
Binary file not shown.
BIN
data/a1/meshes/hip.stl
Normal file
BIN
data/a1/meshes/hip.stl
Normal file
Binary file not shown.
BIN
data/a1/meshes/thigh.stl
Normal file
BIN
data/a1/meshes/thigh.stl
Normal file
Binary file not shown.
BIN
data/a1/meshes/thigh_mirror.stl
Normal file
BIN
data/a1/meshes/thigh_mirror.stl
Normal file
Binary file not shown.
BIN
data/a1/meshes/trunk.stl
Normal file
BIN
data/a1/meshes/trunk.stl
Normal file
Binary file not shown.
718
data/a1/urdf/a1.urdf
Normal file
718
data/a1/urdf/a1.urdf
Normal file
@@ -0,0 +1,718 @@
|
|||||||
|
<?xml version="1.0" ?>
|
||||||
|
<!-- =================================================================================== -->
|
||||||
|
<!-- | This document was autogenerated by xacro from robot.xacro | -->
|
||||||
|
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
|
||||||
|
<!-- =================================================================================== -->
|
||||||
|
<robot name="a1_description" xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||||
|
|
||||||
|
<mujoco>
|
||||||
|
<compiler
|
||||||
|
meshdir="../meshes/"
|
||||||
|
balanceinertia="true"
|
||||||
|
discardvisual="false" />
|
||||||
|
</mujoco>
|
||||||
|
|
||||||
|
<material name="black">
|
||||||
|
<color rgba="0.0 0.0 0.0 1.0"/>
|
||||||
|
</material>
|
||||||
|
<material name="blue">
|
||||||
|
<color rgba="0.0 0.0 0.8 1.0"/>
|
||||||
|
</material>
|
||||||
|
<material name="green">
|
||||||
|
<color rgba="0.0 0.8 0.0 1.0"/>
|
||||||
|
</material>
|
||||||
|
<material name="grey">
|
||||||
|
<color rgba="0.2 0.2 0.2 1.0"/>
|
||||||
|
</material>
|
||||||
|
<material name="silver">
|
||||||
|
<color rgba="0.913725490196 0.913725490196 0.847058823529 1.0"/>
|
||||||
|
</material>
|
||||||
|
<material name="orange">
|
||||||
|
<color rgba="1.0 0.423529411765 0.0392156862745 1.0"/>
|
||||||
|
</material>
|
||||||
|
<material name="brown">
|
||||||
|
<color rgba="0.870588235294 0.811764705882 0.764705882353 1.0"/>
|
||||||
|
</material>
|
||||||
|
<material name="red">
|
||||||
|
<color rgba="0.8 0.0 0.0 1.0"/>
|
||||||
|
</material>
|
||||||
|
<material name="white">
|
||||||
|
<color rgba="1.0 1.0 1.0 1.0"/>
|
||||||
|
</material>
|
||||||
|
<!-- <xacro:include filename="$(find aliengo_gazebo)/launch/gazebo.xacro"/> -->
|
||||||
|
<!-- Debug mode will hung up the robot, use "true" or "false" to switch it. -->
|
||||||
|
<!-- <xacro:if value="$(arg DEBUG)">
|
||||||
|
<link name="world"/>
|
||||||
|
<joint name="base_static_joint" type="fixed">
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<parent link="world"/>
|
||||||
|
<child link="base"/>
|
||||||
|
</joint>
|
||||||
|
</xacro:if> -->
|
||||||
|
<link name="world"/>
|
||||||
|
<joint name="" type="floating">
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<parent link="world"/>
|
||||||
|
<child link="trunk"/>
|
||||||
|
</joint>
|
||||||
|
<link name="trunk">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="../meshes/trunk.stl" scale="1 1 1"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="silver"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<box size="0.267 0.194 0.114"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<!-- <collision>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="../meshes/trunk.stl" scale="1 1 1"/>
|
||||||
|
</geometry>
|
||||||
|
</collision> -->
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="0.012731 0.002186 0.000515"/>
|
||||||
|
<mass value="4.713"/>
|
||||||
|
<inertia ixx="0.01683993" ixy="8.3902e-05" ixz="0.000597679" iyy="0.056579028" iyz="2.5134e-05" izz="0.064713601"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="imu_joint" type="fixed">
|
||||||
|
<parent link="trunk"/>
|
||||||
|
<child link="imu_link"/>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
</joint>
|
||||||
|
<link name="imu_link">
|
||||||
|
<inertial>
|
||||||
|
<mass value="0.001"/>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.000001" iyz="0" izz="0.0001"/>
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<box size="0.001 0.001 0.001"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="red"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<box size=".001 .001 .001"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
<joint name="FR_hip_joint" type="revolute">
|
||||||
|
<origin rpy="0 0 0" xyz="0.183 -0.047 0"/>
|
||||||
|
<parent link="trunk"/>
|
||||||
|
<child link="FR_hip"/>
|
||||||
|
<axis xyz="1 0 0"/>
|
||||||
|
<dynamics damping="0" friction="0"/>
|
||||||
|
<limit effort="20" lower="-0.802851455917" upper="0.802851455917" velocity="52.4"/>
|
||||||
|
</joint>
|
||||||
|
<link name="FR_hip">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="3.14159265359 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="../meshes/hip.stl" scale="1 1 1"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="silver"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="1.57079632679 0 0" xyz="0 -0.055 0"/>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length="0.08" radius="0.04"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="-0.003311 -0.000635 3.1e-05"/>
|
||||||
|
<mass value="0.696"/>
|
||||||
|
<inertia ixx="0.000469246" ixy="9.409e-06" ixz="-3.42e-07" iyy="0.00080749" iyz="4.66e-07" izz="0.000552929"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<!-- <joint name="${name}_hip_fixed" type="fixed">
|
||||||
|
<origin rpy="0 0 0" xyz="0 ${(thigh_shoulder_length/2.0+hip_offset)*mirror} 0"/>
|
||||||
|
<parent link="${name}_hip"/>
|
||||||
|
<child link="${name}_thigh_shoulder"/>
|
||||||
|
</joint> -->
|
||||||
|
<!-- this link is only for collision -->
|
||||||
|
<!-- <link name="${name}_thigh_shoulder">
|
||||||
|
<collision>
|
||||||
|
<origin rpy="${PI/2.0} 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length="${thigh_shoulder_length}" radius="${thigh_shoulder_radius}"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link> -->
|
||||||
|
<joint name="FR_thigh_joint" type="continuous">
|
||||||
|
<origin rpy="0 0 0" xyz="0 -0.08505 0"/>
|
||||||
|
<parent link="FR_hip"/>
|
||||||
|
<child link="FR_thigh"/>
|
||||||
|
<axis xyz="0 1 0"/>
|
||||||
|
<dynamics damping="0" friction="0"/>
|
||||||
|
<limit effort="55" lower="-1.0471975512" upper="4.18879020479" velocity="28.6"/>
|
||||||
|
</joint>
|
||||||
|
<link name="FR_thigh">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="../meshes/thigh_mirror.stl" scale="1 1 1"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="silver"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 1.57079632679 0" xyz="0 0 -0.1"/>
|
||||||
|
<geometry>
|
||||||
|
<box size="0.2 0.0245 0.034"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="-0.003237 0.022327 -0.027326"/>
|
||||||
|
<mass value="1.013"/>
|
||||||
|
<inertia ixx="0.005529065" ixy="-4.825e-06" ixz="0.000343869" iyy="0.005139339" iyz="-2.2448e-05" izz="0.001367788"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="FR_calf_joint" type="revolute">
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 -0.2"/>
|
||||||
|
<parent link="FR_thigh"/>
|
||||||
|
<child link="FR_calf"/>
|
||||||
|
<axis xyz="0 1 0"/>
|
||||||
|
<dynamics damping="0" friction="0"/>
|
||||||
|
<limit effort="55" lower="-2.69653369433" upper="-0.916297857297" velocity="28.6"/>
|
||||||
|
</joint>
|
||||||
|
<link name="FR_calf">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="../meshes/calf.stl" scale="1 1 1"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="black"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 1.57079632679 0" xyz="0 0 -0.1"/>
|
||||||
|
<geometry>
|
||||||
|
<box size="0.2 0.016 0.016"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="0.006435 0.0 -0.107388"/>
|
||||||
|
<mass value="0.166"/>
|
||||||
|
<inertia ixx="0.002997972" ixy="0.0" ixz="-0.000141163" iyy="0.003014022" iyz="0.0" izz="3.2426e-05"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="FR_foot_fixed" type="fixed">
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 -0.2"/>
|
||||||
|
<parent link="FR_calf"/>
|
||||||
|
<child link="FR_foot"/>
|
||||||
|
</joint>
|
||||||
|
<link name="FR_foot">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<sphere radius="0.01"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="black"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<sphere radius="0.02"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="0.06"/>
|
||||||
|
<inertia ixx="9.6e-06" ixy="0.0" ixz="0.0" iyy="9.6e-06" iyz="0.0" izz="9.6e-06"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<transmission name="FR_hip_tran">
|
||||||
|
<type>transmission_interface/SimpleTransmission</type>
|
||||||
|
<joint name="FR_hip_joint">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
</joint>
|
||||||
|
<actuator name="FR_hip_motor">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
<mechanicalReduction>1</mechanicalReduction>
|
||||||
|
</actuator>
|
||||||
|
</transmission>
|
||||||
|
<transmission name="FR_thigh_tran">
|
||||||
|
<type>transmission_interface/SimpleTransmission</type>
|
||||||
|
<joint name="FR_thigh_joint">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
</joint>
|
||||||
|
<actuator name="FR_thigh_motor">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
<mechanicalReduction>1</mechanicalReduction>
|
||||||
|
</actuator>
|
||||||
|
</transmission>
|
||||||
|
<transmission name="FR_calf_tran">
|
||||||
|
<type>transmission_interface/SimpleTransmission</type>
|
||||||
|
<joint name="FR_calf_joint">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
</joint>
|
||||||
|
<actuator name="FR_calf_motor">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
<mechanicalReduction>1</mechanicalReduction>
|
||||||
|
</actuator>
|
||||||
|
</transmission>
|
||||||
|
<joint name="FL_hip_joint" type="revolute">
|
||||||
|
<origin rpy="0 0 0" xyz="0.183 0.047 0"/>
|
||||||
|
<parent link="trunk"/>
|
||||||
|
<child link="FL_hip"/>
|
||||||
|
<axis xyz="1 0 0"/>
|
||||||
|
<dynamics damping="0" friction="0"/>
|
||||||
|
<limit effort="20" lower="-0.802851455917" upper="0.802851455917" velocity="52.4"/>
|
||||||
|
</joint>
|
||||||
|
<link name="FL_hip">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="../meshes/hip.stl" scale="1 1 1"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="silver"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="1.57079632679 0 0" xyz="0 0.055 0"/>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length="0.08" radius="0.04"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="-0.003311 0.000635 3.1e-05"/>
|
||||||
|
<mass value="0.696"/>
|
||||||
|
<inertia ixx="0.000469246" ixy="-9.409e-06" ixz="-3.42e-07" iyy="0.00080749" iyz="-4.66e-07" izz="0.000552929"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<!-- <joint name="${name}_hip_fixed" type="fixed">
|
||||||
|
<origin rpy="0 0 0" xyz="0 ${(thigh_shoulder_length/2.0+hip_offset)*mirror} 0"/>
|
||||||
|
<parent link="${name}_hip"/>
|
||||||
|
<child link="${name}_thigh_shoulder"/>
|
||||||
|
</joint> -->
|
||||||
|
<!-- this link is only for collision -->
|
||||||
|
<!-- <link name="${name}_thigh_shoulder">
|
||||||
|
<collision>
|
||||||
|
<origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length="${thigh_shoulder_length}" radius="${thigh_shoulder_radius}"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link> -->
|
||||||
|
<joint name="FL_thigh_joint" type="continuous">
|
||||||
|
<origin rpy="0 0 0" xyz="0 0.08505 0"/>
|
||||||
|
<parent link="FL_hip"/>
|
||||||
|
<child link="FL_thigh"/>
|
||||||
|
<axis xyz="0 1 0"/>
|
||||||
|
<dynamics damping="0" friction="0"/>
|
||||||
|
<limit effort="55" lower="-1.0471975512" upper="4.18879020479" velocity="28.6"/>
|
||||||
|
</joint>
|
||||||
|
<link name="FL_thigh">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="../meshes/thigh.stl" scale="1 1 1"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="silver"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 1.57079632679 0" xyz="0 0 -0.1"/>
|
||||||
|
<geometry>
|
||||||
|
<box size="0.2 0.0245 0.034"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="-0.003237 -0.022327 -0.027326"/>
|
||||||
|
<mass value="1.013"/>
|
||||||
|
<inertia ixx="0.005529065" ixy="4.825e-06" ixz="0.000343869" iyy="0.005139339" iyz="2.2448e-05" izz="0.001367788"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="FL_calf_joint" type="revolute">
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 -0.2"/>
|
||||||
|
<parent link="FL_thigh"/>
|
||||||
|
<child link="FL_calf"/>
|
||||||
|
<axis xyz="0 1 0"/>
|
||||||
|
<dynamics damping="0" friction="0"/>
|
||||||
|
<limit effort="55" lower="-2.69653369433" upper="-0.916297857297" velocity="28.6"/>
|
||||||
|
</joint>
|
||||||
|
<link name="FL_calf">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="../meshes/calf.stl" scale="1 1 1"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="black"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 1.57079632679 0" xyz="0 0 -0.1"/>
|
||||||
|
<geometry>
|
||||||
|
<box size="0.2 0.016 0.016"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="0.006435 0.0 -0.107388"/>
|
||||||
|
<mass value="0.166"/>
|
||||||
|
<inertia ixx="0.002997972" ixy="0.0" ixz="-0.000141163" iyy="0.003014022" iyz="0.0" izz="3.2426e-05"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="FL_foot_fixed" type="fixed">
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 -0.2"/>
|
||||||
|
<parent link="FL_calf"/>
|
||||||
|
<child link="FL_foot"/>
|
||||||
|
</joint>
|
||||||
|
<link name="FL_foot">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<sphere radius="0.01"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="black"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<sphere radius="0.02"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="0.06"/>
|
||||||
|
<inertia ixx="9.6e-06" ixy="0.0" ixz="0.0" iyy="9.6e-06" iyz="0.0" izz="9.6e-06"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<transmission name="FL_hip_tran">
|
||||||
|
<type>transmission_interface/SimpleTransmission</type>
|
||||||
|
<joint name="FL_hip_joint">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
</joint>
|
||||||
|
<actuator name="FL_hip_motor">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
<mechanicalReduction>1</mechanicalReduction>
|
||||||
|
</actuator>
|
||||||
|
</transmission>
|
||||||
|
<transmission name="FL_thigh_tran">
|
||||||
|
<type>transmission_interface/SimpleTransmission</type>
|
||||||
|
<joint name="FL_thigh_joint">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
</joint>
|
||||||
|
<actuator name="FL_thigh_motor">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
<mechanicalReduction>1</mechanicalReduction>
|
||||||
|
</actuator>
|
||||||
|
</transmission>
|
||||||
|
<transmission name="FL_calf_tran">
|
||||||
|
<type>transmission_interface/SimpleTransmission</type>
|
||||||
|
<joint name="FL_calf_joint">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
</joint>
|
||||||
|
<actuator name="FL_calf_motor">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
<mechanicalReduction>1</mechanicalReduction>
|
||||||
|
</actuator>
|
||||||
|
</transmission>
|
||||||
|
<joint name="RR_hip_joint" type="revolute">
|
||||||
|
<origin rpy="0 0 0" xyz="-0.183 -0.047 0"/>
|
||||||
|
<parent link="trunk"/>
|
||||||
|
<child link="RR_hip"/>
|
||||||
|
<axis xyz="1 0 0"/>
|
||||||
|
<dynamics damping="0" friction="0"/>
|
||||||
|
<limit effort="20" lower="-0.802851455917" upper="0.802851455917" velocity="52.4"/>
|
||||||
|
</joint>
|
||||||
|
<link name="RR_hip">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="3.14159265359 3.14159265359 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="../meshes/hip.stl" scale="1 1 1"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="silver"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="1.57079632679 0 0" xyz="0 -0.055 0"/>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length="0.08" radius="0.04"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="0.003311 -0.000635 3.1e-05"/>
|
||||||
|
<mass value="0.696"/>
|
||||||
|
<inertia ixx="0.000469246" ixy="-9.409e-06" ixz="3.42e-07" iyy="0.00080749" iyz="4.66e-07" izz="0.000552929"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<!-- <joint name="${name}_hip_fixed" type="fixed">
|
||||||
|
<origin rpy="0 0 0" xyz="0 ${(thigh_shoulder_length/2.0+hip_offset)*mirror} 0"/>
|
||||||
|
<parent link="${name}_hip"/>
|
||||||
|
<child link="${name}_thigh_shoulder"/>
|
||||||
|
</joint> -->
|
||||||
|
<!-- this link is only for collision -->
|
||||||
|
<!-- <link name="${name}_thigh_shoulder">
|
||||||
|
<collision>
|
||||||
|
<origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length="${thigh_shoulder_length}" radius="${thigh_shoulder_radius}"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link> -->
|
||||||
|
<joint name="RR_thigh_joint" type="continuous">
|
||||||
|
<origin rpy="0 0 0" xyz="0 -0.08505 0"/>
|
||||||
|
<parent link="RR_hip"/>
|
||||||
|
<child link="RR_thigh"/>
|
||||||
|
<axis xyz="0 1 0"/>
|
||||||
|
<dynamics damping="0" friction="0"/>
|
||||||
|
<limit effort="55" lower="-1.0471975512" upper="4.18879020479" velocity="28.6"/>
|
||||||
|
</joint>
|
||||||
|
<link name="RR_thigh">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="../meshes/thigh_mirror.stl" scale="1 1 1"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="silver"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 1.57079632679 0" xyz="0 0 -0.1"/>
|
||||||
|
<geometry>
|
||||||
|
<box size="0.2 0.0245 0.034"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="-0.003237 0.022327 -0.027326"/>
|
||||||
|
<mass value="1.013"/>
|
||||||
|
<inertia ixx="0.005529065" ixy="-4.825e-06" ixz="0.000343869" iyy="0.005139339" iyz="-2.2448e-05" izz="0.001367788"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="RR_calf_joint" type="revolute">
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 -0.2"/>
|
||||||
|
<parent link="RR_thigh"/>
|
||||||
|
<child link="RR_calf"/>
|
||||||
|
<axis xyz="0 1 0"/>
|
||||||
|
<dynamics damping="0" friction="0"/>
|
||||||
|
<limit effort="55" lower="-2.69653369433" upper="-0.916297857297" velocity="28.6"/>
|
||||||
|
</joint>
|
||||||
|
<link name="RR_calf">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="../meshes/calf.stl" scale="1 1 1"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="black"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 1.57079632679 0" xyz="0 0 -0.1"/>
|
||||||
|
<geometry>
|
||||||
|
<box size="0.2 0.016 0.016"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="0.006435 0.0 -0.107388"/>
|
||||||
|
<mass value="0.166"/>
|
||||||
|
<inertia ixx="0.002997972" ixy="0.0" ixz="-0.000141163" iyy="0.003014022" iyz="0.0" izz="3.2426e-05"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="RR_foot_fixed" type="fixed">
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 -0.2"/>
|
||||||
|
<parent link="RR_calf"/>
|
||||||
|
<child link="RR_foot"/>
|
||||||
|
</joint>
|
||||||
|
<link name="RR_foot">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<sphere radius="0.01"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="black"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<sphere radius="0.02"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="0.06"/>
|
||||||
|
<inertia ixx="9.6e-06" ixy="0.0" ixz="0.0" iyy="9.6e-06" iyz="0.0" izz="9.6e-06"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<transmission name="RR_hip_tran">
|
||||||
|
<type>transmission_interface/SimpleTransmission</type>
|
||||||
|
<joint name="RR_hip_joint">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
</joint>
|
||||||
|
<actuator name="RR_hip_motor">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
<mechanicalReduction>1</mechanicalReduction>
|
||||||
|
</actuator>
|
||||||
|
</transmission>
|
||||||
|
<transmission name="RR_thigh_tran">
|
||||||
|
<type>transmission_interface/SimpleTransmission</type>
|
||||||
|
<joint name="RR_thigh_joint">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
</joint>
|
||||||
|
<actuator name="RR_thigh_motor">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
<mechanicalReduction>1</mechanicalReduction>
|
||||||
|
</actuator>
|
||||||
|
</transmission>
|
||||||
|
<transmission name="RR_calf_tran">
|
||||||
|
<type>transmission_interface/SimpleTransmission</type>
|
||||||
|
<joint name="RR_calf_joint">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
</joint>
|
||||||
|
<actuator name="RR_calf_motor">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
<mechanicalReduction>1</mechanicalReduction>
|
||||||
|
</actuator>
|
||||||
|
</transmission>
|
||||||
|
<joint name="RL_hip_joint" type="revolute">
|
||||||
|
<origin rpy="0 0 0" xyz="-0.183 0.047 0"/>
|
||||||
|
<parent link="trunk"/>
|
||||||
|
<child link="RL_hip"/>
|
||||||
|
<axis xyz="1 0 0"/>
|
||||||
|
<dynamics damping="0" friction="0"/>
|
||||||
|
<limit effort="20" lower="-0.802851455917" upper="0.802851455917" velocity="52.4"/>
|
||||||
|
</joint>
|
||||||
|
<link name="RL_hip">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 3.14159265359 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="../meshes/hip.stl" scale="1 1 1"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="silver"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="1.57079632679 0 0" xyz="0 0.055 0"/>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length="0.08" radius="0.04"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="0.003311 0.000635 3.1e-05"/>
|
||||||
|
<mass value="0.696"/>
|
||||||
|
<inertia ixx="0.000469246" ixy="9.409e-06" ixz="3.42e-07" iyy="0.00080749" iyz="-4.66e-07" izz="0.000552929"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<!-- <joint name="${name}_hip_fixed" type="fixed">
|
||||||
|
<origin rpy="0 0 0" xyz="0 ${(thigh_shoulder_length/2.0+hip_offset)*mirror} 0"/>
|
||||||
|
<parent link="${name}_hip"/>
|
||||||
|
<child link="${name}_thigh_shoulder"/>
|
||||||
|
</joint> -->
|
||||||
|
<!-- this link is only for collision -->
|
||||||
|
<!-- <link name="${name}_thigh_shoulder">
|
||||||
|
<collision>
|
||||||
|
<origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length="${thigh_shoulder_length}" radius="${thigh_shoulder_radius}"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link> -->
|
||||||
|
<joint name="RL_thigh_joint" type="continuous">
|
||||||
|
<origin rpy="0 0 0" xyz="0 0.08505 0"/>
|
||||||
|
<parent link="RL_hip"/>
|
||||||
|
<child link="RL_thigh"/>
|
||||||
|
<axis xyz="0 1 0"/>
|
||||||
|
<dynamics damping="0" friction="0"/>
|
||||||
|
<limit effort="55" lower="-1.0471975512" upper="4.18879020479" velocity="28.6"/>
|
||||||
|
</joint>
|
||||||
|
<link name="RL_thigh">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="../meshes/thigh.stl" scale="1 1 1"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="silver"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 1.57079632679 0" xyz="0 0 -0.1"/>
|
||||||
|
<geometry>
|
||||||
|
<box size="0.2 0.0245 0.034"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="-0.003237 -0.022327 -0.027326"/>
|
||||||
|
<mass value="1.013"/>
|
||||||
|
<inertia ixx="0.005529065" ixy="4.825e-06" ixz="0.000343869" iyy="0.005139339" iyz="2.2448e-05" izz="0.001367788"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="RL_calf_joint" type="revolute">
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 -0.2"/>
|
||||||
|
<parent link="RL_thigh"/>
|
||||||
|
<child link="RL_calf"/>
|
||||||
|
<axis xyz="0 1 0"/>
|
||||||
|
<dynamics damping="0" friction="0"/>
|
||||||
|
<limit effort="55" lower="-2.69653369433" upper="-0.916297857297" velocity="28.6"/>
|
||||||
|
</joint>
|
||||||
|
<link name="RL_calf">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="../meshes/calf.stl" scale="1 1 1"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="black"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 1.57079632679 0" xyz="0 0 -0.1"/>
|
||||||
|
<geometry>
|
||||||
|
<box size="0.2 0.016 0.016"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="0.006435 0.0 -0.107388"/>
|
||||||
|
<mass value="0.166"/>
|
||||||
|
<inertia ixx="0.002997972" ixy="0.0" ixz="-0.000141163" iyy="0.003014022" iyz="0.0" izz="3.2426e-05"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="RL_foot_fixed" type="fixed">
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 -0.2"/>
|
||||||
|
<parent link="RL_calf"/>
|
||||||
|
<child link="RL_foot"/>
|
||||||
|
</joint>
|
||||||
|
<link name="RL_foot">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<sphere radius="0.01"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="black"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<sphere radius="0.02"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="0.06"/>
|
||||||
|
<inertia ixx="9.6e-06" ixy="0.0" ixz="0.0" iyy="9.6e-06" iyz="0.0" izz="9.6e-06"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<transmission name="RL_hip_tran">
|
||||||
|
<type>transmission_interface/SimpleTransmission</type>
|
||||||
|
<joint name="RL_hip_joint">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
</joint>
|
||||||
|
<actuator name="RL_hip_motor">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
<mechanicalReduction>1</mechanicalReduction>
|
||||||
|
</actuator>
|
||||||
|
</transmission>
|
||||||
|
<transmission name="RL_thigh_tran">
|
||||||
|
<type>transmission_interface/SimpleTransmission</type>
|
||||||
|
<joint name="RL_thigh_joint">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
</joint>
|
||||||
|
<actuator name="RL_thigh_motor">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
<mechanicalReduction>1</mechanicalReduction>
|
||||||
|
</actuator>
|
||||||
|
</transmission>
|
||||||
|
<transmission name="RL_calf_tran">
|
||||||
|
<type>transmission_interface/SimpleTransmission</type>
|
||||||
|
<joint name="RL_calf_joint">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
</joint>
|
||||||
|
<actuator name="RL_calf_motor">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
<mechanicalReduction>1</mechanicalReduction>
|
||||||
|
</actuator>
|
||||||
|
</transmission>
|
||||||
|
</robot>
|
||||||
|
|
||||||
186
data/a1/xml/a1.xml
Normal file
186
data/a1/xml/a1.xml
Normal file
@@ -0,0 +1,186 @@
|
|||||||
|
<mujoco model="a1_description">
|
||||||
|
<compiler angle="radian" meshdir="../meshes/" />
|
||||||
|
<size njmax="500" nconmax="100" />
|
||||||
|
<option gravity='0 0 -9.806' iterations='50' solver='Newton' timestep='0.002'/>
|
||||||
|
|
||||||
|
<default>
|
||||||
|
<geom contype="1" conaffinity="1" friction="0.6 0.3 0.3" rgba="0.5 0.6 0.7 1" margin="0.001" group="0"/>
|
||||||
|
|
||||||
|
<light castshadow="false" diffuse="1 1 1"/>
|
||||||
|
<motor ctrlrange="-33.5 33.5" ctrllimited="true"/>
|
||||||
|
<camera fovy="60"/>
|
||||||
|
<joint damping="0.01" armature="0.01" frictionloss="0.2" />
|
||||||
|
|
||||||
|
</default>
|
||||||
|
|
||||||
|
<asset>
|
||||||
|
<mesh name="trunk" file="trunk.stl" />
|
||||||
|
<mesh name="hip" file="hip.stl" />
|
||||||
|
<mesh name="thigh_mirror" file="thigh_mirror.stl" />
|
||||||
|
<mesh name="calf" file="calf.stl" />
|
||||||
|
<mesh name="thigh" file="thigh.stl" />
|
||||||
|
</asset>
|
||||||
|
|
||||||
|
<asset>
|
||||||
|
<texture type="skybox" builtin="gradient" rgb1="1.0 1.0 1.0" rgb2="1.0 1.0 1.0" width="512" height="512"/>
|
||||||
|
<texture name="plane" type="2d" builtin="flat" rgb1="1 1 1" rgb2="1 1 1" width="512" height="512" mark="cross" markrgb="0 0 0"/>
|
||||||
|
<material name="plane" reflectance="0.0" texture="plane" texrepeat="3 3" texuniform="true"/>
|
||||||
|
</asset>
|
||||||
|
|
||||||
|
<visual>
|
||||||
|
<rgba com="0.502 1.0 0 0.5" contactforce="0.98 0.4 0.4 0.7" contactpoint="1.0 1.0 0.6 0.4"/>
|
||||||
|
<scale com="0.2" forcewidth="0.035" contactwidth="0.10" contactheight="0.04"/>
|
||||||
|
</visual>
|
||||||
|
|
||||||
|
<worldbody>
|
||||||
|
<light directional="true" diffuse=".8 .8 .8" pos="0 0 10" dir="0 0 -10"/>
|
||||||
|
<camera name="track" mode="trackcom" pos="0 -1.3 1.6" xyaxes="1 0 0 0 0.707 0.707"/>
|
||||||
|
<geom name='floor' type='plane' conaffinity='1' condim='3' contype='1' rgba="0.5 0.9 0.9 0.1" material='plane' pos='0 0 0' size='0 0 1'/>
|
||||||
|
|
||||||
|
<body name="trunk" pos="0 0 0.3">
|
||||||
|
<inertial pos="0.0127283 0.00218554 0.000514891" quat="0.00186575 0.711506 0.000389649 0.702677" mass="4.714" diaginertia="0.0648213 0.0565803 0.0169323" />
|
||||||
|
<joint type="free" />
|
||||||
|
<geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.913725 0.913725 0.847059 1" mesh="trunk" />
|
||||||
|
<geom size="0.1335 0.066 0.057" type="box" rgba="0.913725 0.913725 0.847059 0" />
|
||||||
|
<geom size="0.0005 0.0005 0.0005" type="box" contype="0" conaffinity="0" group="1" rgba="0.8 0 0 0" />
|
||||||
|
<geom size="0.0005 0.0005 0.0005" type="box" rgba="0.8 0 0 0" />
|
||||||
|
<site name="imu" pos="0 0 0"/>
|
||||||
|
|
||||||
|
<body name="FR_hip" pos="0.183 -0.047 0">
|
||||||
|
<inertial pos="-0.003311 -0.000635 3.1e-05" quat="0.507528 0.506268 0.491507 0.494499" mass="0.696" diaginertia="0.000807752 0.00055293 0.000468983" />
|
||||||
|
<joint name="FR_hip_joint" pos="0 0 0" axis="1 0 0" limited="true" range="-0.802851 0.802851" />
|
||||||
|
<geom quat="0 1 0 0" type="mesh" contype="0" conaffinity="0" group="1" rgba="0.913725 0.913725 0.847059 1" mesh="hip" />
|
||||||
|
<geom size="0.04 0.04" pos="0 -0.055 0" quat="0.707107 0.707107 0 0" type="cylinder" rgba="0.913725 0.913725 0.847059 0" />
|
||||||
|
<body name="FR_thigh" pos="0 -0.08505 0">
|
||||||
|
<inertial pos="-0.003237 0.022327 -0.027326" quat="0.999125 -0.00256393 -0.0409531 -0.00806091" mass="1.013" diaginertia="0.00555739 0.00513936 0.00133944" />
|
||||||
|
<joint name="FR_thigh_joint" pos="0 0 0" axis="0 1 0" limited="true" range="-1.0472 4.18879" />
|
||||||
|
<geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.913725 0.913725 0.847059 1" mesh="thigh_mirror" />
|
||||||
|
<geom size="0.1 0.01225 0.017" pos="0 0 -0.1" quat="0.707107 0 0.707107 0" type="box" rgba="0.913725 0.913725 0.847059 0" />
|
||||||
|
<body name="FR_calf" pos="0 0 -0.2">
|
||||||
|
<inertial pos="0.00472659 0 -0.131975" quat="0.706886 0.017653 0.017653 0.706886" mass="0.226" diaginertia="0.00340344 0.00339393 3.54834e-05" />
|
||||||
|
<joint name="FR_calf_joint" pos="0 0 0" axis="0 1 0" limited="true" range="-2.69653 -0.916298" />
|
||||||
|
<geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0 0 0 1" mesh="calf" />
|
||||||
|
<geom size="0.1 0.008 0.008" pos="0 0 -0.1" quat="0.707107 0 0.707107 0" type="box" rgba="0 0 0 0" />
|
||||||
|
<geom size="0.01" pos="0 0 -0.2" contype="0" conaffinity="0" group="1" rgba="0 0 0 1" />
|
||||||
|
<geom size="0.02" pos="0 0 -0.2" rgba="0 0 0 1" />
|
||||||
|
</body>
|
||||||
|
</body>
|
||||||
|
</body>
|
||||||
|
<body name="FL_hip" pos="0.183 0.047 0">
|
||||||
|
<inertial pos="-0.003311 0.000635 3.1e-05" quat="0.494499 0.491507 0.506268 0.507528" mass="0.696" diaginertia="0.000807752 0.00055293 0.000468983" />
|
||||||
|
<joint name="FL_hip_joint" pos="0 0 0" axis="1 0 0" limited="true" range="-0.802851 0.802851" />
|
||||||
|
<geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.913725 0.913725 0.847059 1" mesh="hip" />
|
||||||
|
<geom size="0.04 0.04" pos="0 0.055 0" quat="0.707107 0.707107 0 0" type="cylinder" rgba="0.913725 0.913725 0.847059 0" />
|
||||||
|
<body name="FL_thigh" pos="0 0.08505 0">
|
||||||
|
<inertial pos="-0.003237 -0.022327 -0.027326" quat="0.999125 0.00256393 -0.0409531 0.00806091" mass="1.013" diaginertia="0.00555739 0.00513936 0.00133944" />
|
||||||
|
<joint name="FL_thigh_joint" pos="0 0 0" axis="0 1 0" limited="true" range="-1.0472 4.18879" />
|
||||||
|
<geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.913725 0.913725 0.847059 1" mesh="thigh" />
|
||||||
|
<geom size="0.1 0.01225 0.017" pos="0 0 -0.1" quat="0.707107 0 0.707107 0" type="box" rgba="0.913725 0.913725 0.847059 0" />
|
||||||
|
<body name="FL_calf" pos="0 0 -0.2">
|
||||||
|
<inertial pos="0.00472659 0 -0.131975" quat="0.706886 0.017653 0.017653 0.706886" mass="0.226" diaginertia="0.00340344 0.00339393 3.54834e-05" />
|
||||||
|
<joint name="FL_calf_joint" pos="0 0 0" axis="0 1 0" limited="true" range="-2.69653 -0.916298" />
|
||||||
|
<geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0 0 0 1" mesh="calf" />
|
||||||
|
<geom size="0.1 0.008 0.008" pos="0 0 -0.1" quat="0.707107 0 0.707107 0" type="box" rgba="0 0 0 0" />
|
||||||
|
<geom size="0.01" pos="0 0 -0.2" contype="0" conaffinity="0" group="1" rgba="0 0 0 1" />
|
||||||
|
<geom size="0.02" pos="0 0 -0.2" rgba="0 0 0 1" />
|
||||||
|
</body>
|
||||||
|
</body>
|
||||||
|
</body>
|
||||||
|
<body name="RR_hip" pos="-0.183 -0.047 0">
|
||||||
|
<inertial pos="0.003311 -0.000635 3.1e-05" quat="0.491507 0.494499 0.507528 0.506268" mass="0.696" diaginertia="0.000807752 0.00055293 0.000468983" />
|
||||||
|
<joint name="RR_hip_joint" pos="0 0 0" axis="1 0 0" limited="true" range="-0.802851 0.802851" />
|
||||||
|
<geom quat="0 0 0 -1" type="mesh" contype="0" conaffinity="0" group="1" rgba="0.913725 0.913725 0.847059 1" mesh="hip" />
|
||||||
|
<geom size="0.04 0.04" pos="0 -0.055 0" quat="0.707107 0.707107 0 0" type="cylinder" rgba="0.913725 0.913725 0.847059 0" />
|
||||||
|
<body name="RR_thigh" pos="0 -0.08505 0">
|
||||||
|
<inertial pos="-0.003237 0.022327 -0.027326" quat="0.999125 -0.00256393 -0.0409531 -0.00806091" mass="1.013" diaginertia="0.00555739 0.00513936 0.00133944" />
|
||||||
|
<joint name="RR_thigh_joint" pos="0 0 0" axis="0 1 0" limited="true" range="-1.0472 4.18879" />
|
||||||
|
<geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.913725 0.913725 0.847059 1" mesh="thigh_mirror" />
|
||||||
|
<geom size="0.1 0.01225 0.017" pos="0 0 -0.1" quat="0.707107 0 0.707107 0" type="box" rgba="0.913725 0.913725 0.847059 0" />
|
||||||
|
<body name="RR_calf" pos="0 0 -0.2">
|
||||||
|
<inertial pos="0.00472659 0 -0.131975" quat="0.706886 0.017653 0.017653 0.706886" mass="0.226" diaginertia="0.00340344 0.00339393 3.54834e-05" />
|
||||||
|
<joint name="RR_calf_joint" pos="0 0 0" axis="0 1 0" limited="true" range="-2.69653 -0.916298" />
|
||||||
|
<geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0 0 0 1" mesh="calf" />
|
||||||
|
<geom size="0.1 0.008 0.008" pos="0 0 -0.1" quat="0.707107 0 0.707107 0" type="box" rgba="0 0 0 0" />
|
||||||
|
<geom size="0.01" pos="0 0 -0.2" contype="0" conaffinity="0" group="1" rgba="0 0 0 1" />
|
||||||
|
<geom size="0.02" pos="0 0 -0.2" rgba="0 0 0 1" />
|
||||||
|
</body>
|
||||||
|
</body>
|
||||||
|
</body>
|
||||||
|
<body name="RL_hip" pos="-0.183 0.047 0">
|
||||||
|
<inertial pos="0.003311 0.000635 3.1e-05" quat="0.506268 0.507528 0.494499 0.491507" mass="0.696" diaginertia="0.000807752 0.00055293 0.000468983" />
|
||||||
|
<joint name="RL_hip_joint" pos="0 0 0" axis="1 0 0" limited="true" range="-0.802851 0.802851" />
|
||||||
|
<geom quat="0 0 1 0" type="mesh" contype="0" conaffinity="0" group="1" rgba="0.913725 0.913725 0.847059 1" mesh="hip" />
|
||||||
|
<geom size="0.04 0.04" pos="0 0.055 0" quat="0.707107 0.707107 0 0" type="cylinder" rgba="0.913725 0.913725 0.847059 0" />
|
||||||
|
<body name="RL_thigh" pos="0 0.08505 0">
|
||||||
|
<inertial pos="-0.003237 -0.022327 -0.027326" quat="0.999125 0.00256393 -0.0409531 0.00806091" mass="1.013" diaginertia="0.00555739 0.00513936 0.00133944" />
|
||||||
|
<joint name="RL_thigh_joint" pos="0 0 0" axis="0 1 0" limited="true" range="-1.0472 4.18879" />
|
||||||
|
<geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.913725 0.913725 0.847059 1" mesh="thigh" />
|
||||||
|
<geom size="0.1 0.01225 0.017" pos="0 0 -0.1" quat="0.707107 0 0.707107 0" type="box" rgba="0.913725 0.913725 0.847059 0" />
|
||||||
|
<body name="RL_calf" pos="0 0 -0.2">
|
||||||
|
<inertial pos="0.00472659 0 -0.131975" quat="0.706886 0.017653 0.017653 0.706886" mass="0.226" diaginertia="0.00340344 0.00339393 3.54834e-05" />
|
||||||
|
<joint name="RL_calf_joint" pos="0 0 0" axis="0 1 0" limited="true" range="-2.69653 -0.916298" />
|
||||||
|
<geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0 0 0 1" mesh="calf" />
|
||||||
|
<geom size="0.1 0.008 0.008" pos="0 0 -0.1" quat="0.707107 0 0.707107 0" type="box" rgba="0 0 0 0" />
|
||||||
|
<geom size="0.01" pos="0 0 -0.2" contype="0" conaffinity="0" group="1" rgba="0 0 0 1" />
|
||||||
|
<geom size="0.02" pos="0 0 -0.2" rgba="0 0 0 1" />
|
||||||
|
</body>
|
||||||
|
</body>
|
||||||
|
</body>
|
||||||
|
</body>
|
||||||
|
</worldbody>
|
||||||
|
<actuator>
|
||||||
|
|
||||||
|
<motor name="FR_hip" gear="1" joint="FR_hip_joint"/>
|
||||||
|
<motor name="FR_thigh" gear="1" joint="FR_thigh_joint"/>
|
||||||
|
<motor name="FR_calf" gear="1" joint="FR_calf_joint"/>
|
||||||
|
<motor name="FL_hip" gear="1" joint="FL_hip_joint"/>
|
||||||
|
<motor name="FL_thigh" gear="1" joint="FL_thigh_joint"/>
|
||||||
|
<motor name="FL_calf" gear="1" joint="FL_calf_joint"/>
|
||||||
|
<motor name="RR_hip" gear="1" joint="RR_hip_joint"/>
|
||||||
|
<motor name="RR_thigh" gear="1" joint="RR_thigh_joint"/>
|
||||||
|
<motor name="RR_calf" gear="1" joint="RR_calf_joint" />
|
||||||
|
<motor name="RL_hip" gear="1" joint="RL_hip_joint"/>
|
||||||
|
<motor name="RL_thigh" gear="1" joint="RL_thigh_joint"/>
|
||||||
|
<motor name="RL_calf" gear="1" joint="RL_calf_joint"/>
|
||||||
|
</actuator>
|
||||||
|
|
||||||
|
<sensor>
|
||||||
|
|
||||||
|
<jointpos name="FR_hip_pos" joint="FR_hip_joint"/>
|
||||||
|
<jointpos name="FR_thigh_pos" joint="FR_thigh_joint"/>
|
||||||
|
<jointpos name="FR_calf_pos" joint="FR_calf_joint"/>
|
||||||
|
<jointpos name="FL_hip_pos" joint="FL_hip_joint"/>
|
||||||
|
<jointpos name="FL_thigh_pos" joint="FL_thigh_joint"/>
|
||||||
|
<jointpos name="FL_calf_pos" joint="FL_calf_joint"/>
|
||||||
|
<jointpos name="RR_hip_pos" joint="RR_hip_joint"/>
|
||||||
|
<jointpos name="RR_thigh_pos" joint="RR_thigh_joint"/>
|
||||||
|
<jointpos name="RR_calf_pos" joint="RR_calf_joint" />
|
||||||
|
<jointpos name="RL_hip_pos" joint="RL_hip_joint"/>
|
||||||
|
<jointpos name="RL_thigh_pos" joint="RL_thigh_joint"/>
|
||||||
|
<jointpos name="RL_calf_pos" joint="RL_calf_joint"/>
|
||||||
|
|
||||||
|
<jointvel name="FR_hip_vel" joint="FR_hip_joint"/>
|
||||||
|
<jointvel name="FR_thigh_vel" joint="FR_thigh_joint"/>
|
||||||
|
<jointvel name="FR_calf_vel" joint="FR_calf_joint"/>
|
||||||
|
<jointvel name="FL_hip_vel" joint="FL_hip_joint"/>
|
||||||
|
<jointvel name="FL_thigh_vel" joint="FL_thigh_joint"/>
|
||||||
|
<jointvel name="FL_calf_vel" joint="FL_calf_joint"/>
|
||||||
|
<jointvel name="RR_hip_vel" joint="RR_hip_joint"/>
|
||||||
|
<jointvel name="RR_thigh_vel" joint="RR_thigh_joint"/>
|
||||||
|
<jointvel name="RR_calf_vel" joint="RR_calf_joint" />
|
||||||
|
<jointvel name="RL_hip_vel" joint="RL_hip_joint"/>
|
||||||
|
<jointvel name="RL_thigh_vel" joint="RL_thigh_joint"/>
|
||||||
|
<jointvel name="RL_calf_vel" joint="RL_calf_joint"/>
|
||||||
|
|
||||||
|
<accelerometer name="Body_Acc" site="imu"/>
|
||||||
|
|
||||||
|
<gyro name="Body_Gyro" site="imu"/>
|
||||||
|
|
||||||
|
<framepos name="Body_Pos" objtype="site" objname="imu"/>
|
||||||
|
|
||||||
|
<framequat name="Body_Quat" objtype="site" objname="imu"/>
|
||||||
|
|
||||||
|
|
||||||
|
</sensor>
|
||||||
|
|
||||||
|
</mujoco>
|
||||||
BIN
data/aliengo/meshes/calf.stl
Normal file
BIN
data/aliengo/meshes/calf.stl
Normal file
Binary file not shown.
BIN
data/aliengo/meshes/hip.stl
Normal file
BIN
data/aliengo/meshes/hip.stl
Normal file
Binary file not shown.
BIN
data/aliengo/meshes/thigh.stl
Normal file
BIN
data/aliengo/meshes/thigh.stl
Normal file
Binary file not shown.
BIN
data/aliengo/meshes/thigh_mirror.stl
Normal file
BIN
data/aliengo/meshes/thigh_mirror.stl
Normal file
Binary file not shown.
BIN
data/aliengo/meshes/trunk.stl
Normal file
BIN
data/aliengo/meshes/trunk.stl
Normal file
Binary file not shown.
718
data/aliengo/urdf/aliengo.urdf
Normal file
718
data/aliengo/urdf/aliengo.urdf
Normal file
@@ -0,0 +1,718 @@
|
|||||||
|
<?xml version="1.0" ?>
|
||||||
|
<!-- =================================================================================== -->
|
||||||
|
<!-- | This document was autogenerated by xacro from robot.xacro | -->
|
||||||
|
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
|
||||||
|
<!-- =================================================================================== -->
|
||||||
|
<robot name="aliengo_description" xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||||
|
<mujoco>
|
||||||
|
<compiler
|
||||||
|
meshdir="../meshes/"
|
||||||
|
balanceinertia="true"
|
||||||
|
discardvisual="false" />
|
||||||
|
</mujoco>
|
||||||
|
|
||||||
|
<material name="black">
|
||||||
|
<color rgba="0.0 0.0 0.0 1.0"/>
|
||||||
|
</material>
|
||||||
|
<material name="blue">
|
||||||
|
<color rgba="0.0 0.0 0.8 1.0"/>
|
||||||
|
</material>
|
||||||
|
<material name="green">
|
||||||
|
<color rgba="0.0 0.8 0.0 1.0"/>
|
||||||
|
</material>
|
||||||
|
<material name="grey">
|
||||||
|
<color rgba="0.2 0.2 0.2 1.0"/>
|
||||||
|
</material>
|
||||||
|
<material name="silver">
|
||||||
|
<color rgba="0.913725490196 0.913725490196 0.847058823529 1.0"/>
|
||||||
|
</material>
|
||||||
|
<material name="orange">
|
||||||
|
<color rgba="1.0 0.423529411765 0.0392156862745 1.0"/>
|
||||||
|
</material>
|
||||||
|
<material name="brown">
|
||||||
|
<color rgba="0.870588235294 0.811764705882 0.764705882353 1.0"/>
|
||||||
|
</material>
|
||||||
|
<material name="red">
|
||||||
|
<color rgba="0.8 0.0 0.0 1.0"/>
|
||||||
|
</material>
|
||||||
|
<material name="white">
|
||||||
|
<color rgba="1.0 1.0 1.0 1.0"/>
|
||||||
|
</material>
|
||||||
|
<!-- <xacro:include filename="$(find aliengo_gazebo)/launch/gazebo.xacro"/> -->
|
||||||
|
<!-- Debug mode will hung up the robot, use "true" or "false" to switch it. -->
|
||||||
|
<!-- <xacro:if value="$(arg DEBUG)">
|
||||||
|
<link name="world"/>
|
||||||
|
<joint name="base_static_joint" type="fixed">
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<parent link="world"/>
|
||||||
|
<child link="base"/>
|
||||||
|
</joint>
|
||||||
|
</xacro:if> -->
|
||||||
|
<link name="world"/>
|
||||||
|
<joint name="floating_base" type="floating">
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<parent link="world"/>
|
||||||
|
<child link="trunk"/>
|
||||||
|
</joint>
|
||||||
|
<link name="trunk">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="../meshes/trunk.stl" scale="1 1 1"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="grey"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<box size="0.647 0.15 0.112"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<!-- <collision>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="../meshes/trunk.stl" scale="1 1 1"/>
|
||||||
|
</geometry>
|
||||||
|
</collision> -->
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="0.008465 0.004045 -0.000763"/>
|
||||||
|
<mass value="9.041"/>
|
||||||
|
<inertia ixx="0.033260231" ixy="-0.000451628" ixz="0.000487603" iyy="0.16117211" iyz="4.8356e-05" izz="0.17460442"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="imu_joint" type="fixed">
|
||||||
|
<parent link="trunk"/>
|
||||||
|
<child link="imu_link"/>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
</joint>
|
||||||
|
<link name="imu_link">
|
||||||
|
<inertial>
|
||||||
|
<mass value="0.001"/>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.000001" iyz="0" izz="0.0001"/>
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<box size="0.001 0.001 0.001"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="red"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<box size=".001 .001 .001"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
<joint name="FR_hip_joint" type="revolute">
|
||||||
|
<origin rpy="0 0 0" xyz="0.2399 -0.051 0"/>
|
||||||
|
<parent link="trunk"/>
|
||||||
|
<child link="FR_hip"/>
|
||||||
|
<axis xyz="1 0 0"/>
|
||||||
|
<dynamics damping="0" friction="0"/>
|
||||||
|
<limit effort="44.4" lower="-1.2217304764" upper="1.2217304764" velocity="40"/>
|
||||||
|
</joint>
|
||||||
|
<link name="FR_hip">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="3.14159265359 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="../meshes/hip.stl" scale="1 1 1"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="grey"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="1.57079632679 0 0" xyz="0 -0.083 0"/>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length="0.0418" radius="0.046"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="-0.022191 -0.015144 -1.5e-05"/>
|
||||||
|
<mass value="1.993"/>
|
||||||
|
<inertia ixx="0.002903894" ixy="7.185e-05" ixz="-1.262e-06" iyy="0.004907517" iyz="1.75e-06" izz="0.005586944"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<!-- <joint name="${name}_hip_fixed" type="fixed">
|
||||||
|
<origin rpy="0 0 0" xyz="0 ${(thigh_shoulder_length/2.0+hip_offset)*mirror} 0"/>
|
||||||
|
<parent link="${name}_hip"/>
|
||||||
|
<child link="${name}_thigh_shoulder"/>
|
||||||
|
</joint> -->
|
||||||
|
<!-- this link is only for collision -->
|
||||||
|
<!-- <link name="${name}_thigh_shoulder">
|
||||||
|
<collision>
|
||||||
|
<origin rpy="${PI/2.0} 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length="${thigh_shoulder_length}" radius="${thigh_shoulder_radius}"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link> -->
|
||||||
|
<joint name="FR_thigh_joint" type="continuous">
|
||||||
|
<origin rpy="0 0 0" xyz="0 -0.083 0"/>
|
||||||
|
<parent link="FR_hip"/>
|
||||||
|
<child link="FR_thigh"/>
|
||||||
|
<axis xyz="0 1 0"/>
|
||||||
|
<dynamics damping="0" friction="0"/>
|
||||||
|
<limit effort="44.4" velocity="40"/>
|
||||||
|
</joint>
|
||||||
|
<link name="FR_thigh">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="../meshes/thigh_mirror.stl" scale="1 1 1"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="grey"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 1.57079632679 0" xyz="0 0 -0.125"/>
|
||||||
|
<geometry>
|
||||||
|
<box size="0.25 0.0374 0.043"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="-0.005607 0.003877 -0.048199"/>
|
||||||
|
<mass value="0.639"/>
|
||||||
|
<inertia ixx="0.005666803" ixy="-3.597e-06" ixz="0.000491446" iyy="0.005847229" iyz="-1.0086e-05" izz="0.000369811"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="FR_calf_joint" type="revolute">
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 -0.25"/>
|
||||||
|
<parent link="FR_thigh"/>
|
||||||
|
<child link="FR_calf"/>
|
||||||
|
<axis xyz="0 1 0"/>
|
||||||
|
<dynamics damping="0" friction="0"/>
|
||||||
|
<limit effort="44.4" lower="-2.77507351067" upper="-0.645771823238" velocity="40"/>
|
||||||
|
</joint>
|
||||||
|
<link name="FR_calf">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="../meshes/calf.stl" scale="1 1 1"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="black"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 1.57079632679 0" xyz="0 0 -0.125"/>
|
||||||
|
<geometry>
|
||||||
|
<box size="0.25 0.0208 0.016"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="0.002781 6.3e-05 -0.142518"/>
|
||||||
|
<mass value="0.207"/>
|
||||||
|
<inertia ixx="0.006341369" ixy="-3e-09" ixz="-8.7951e-05" iyy="0.006355157" iyz="-1.336e-06" izz="3.9188e-05"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="FR_foot_fixed" type="fixed">
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 -0.25"/>
|
||||||
|
<parent link="FR_calf"/>
|
||||||
|
<child link="FR_foot"/>
|
||||||
|
</joint>
|
||||||
|
<link name="FR_foot">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<sphere radius="0.0165"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="black"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<sphere radius="0.0265"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="0.06"/>
|
||||||
|
<inertia ixx="1.6854e-05" ixy="0.0" ixz="0.0" iyy="1.6854e-05" iyz="0.0" izz="1.6854e-05"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<transmission name="FR_hip_tran">
|
||||||
|
<type>transmission_interface/SimpleTransmission</type>
|
||||||
|
<joint name="FR_hip_joint">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
</joint>
|
||||||
|
<actuator name="FR_hip_motor">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
<mechanicalReduction>1</mechanicalReduction>
|
||||||
|
</actuator>
|
||||||
|
</transmission>
|
||||||
|
<transmission name="FR_thigh_tran">
|
||||||
|
<type>transmission_interface/SimpleTransmission</type>
|
||||||
|
<joint name="FR_thigh_joint">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
</joint>
|
||||||
|
<actuator name="FR_thigh_motor">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
<mechanicalReduction>1</mechanicalReduction>
|
||||||
|
</actuator>
|
||||||
|
</transmission>
|
||||||
|
<transmission name="FR_calf_tran">
|
||||||
|
<type>transmission_interface/SimpleTransmission</type>
|
||||||
|
<joint name="FR_calf_joint">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
</joint>
|
||||||
|
<actuator name="FR_calf_motor">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
<mechanicalReduction>1</mechanicalReduction>
|
||||||
|
</actuator>
|
||||||
|
</transmission>
|
||||||
|
<joint name="FL_hip_joint" type="revolute">
|
||||||
|
<origin rpy="0 0 0" xyz="0.2399 0.051 0"/>
|
||||||
|
<parent link="trunk"/>
|
||||||
|
<child link="FL_hip"/>
|
||||||
|
<axis xyz="1 0 0"/>
|
||||||
|
<dynamics damping="0" friction="0"/>
|
||||||
|
<limit effort="44.4" lower="-1.2217304764" upper="1.2217304764" velocity="40"/>
|
||||||
|
</joint>
|
||||||
|
<link name="FL_hip">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="../meshes/hip.stl" scale="1 1 1"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="grey"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="1.57079632679 0 0" xyz="0 0.083 0"/>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length="0.0418" radius="0.046"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="-0.022191 0.015144 -1.5e-05"/>
|
||||||
|
<mass value="1.993"/>
|
||||||
|
<inertia ixx="0.002903894" ixy="-7.185e-05" ixz="-1.262e-06" iyy="0.004907517" iyz="-1.75e-06" izz="0.005586944"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<!-- <joint name="${name}_hip_fixed" type="fixed">
|
||||||
|
<origin rpy="0 0 0" xyz="0 ${(thigh_shoulder_length/2.0+hip_offset)*mirror} 0"/>
|
||||||
|
<parent link="${name}_hip"/>
|
||||||
|
<child link="${name}_thigh_shoulder"/>
|
||||||
|
</joint> -->
|
||||||
|
<!-- this link is only for collision -->
|
||||||
|
<!-- <link name="${name}_thigh_shoulder">
|
||||||
|
<collision>
|
||||||
|
<origin rpy="${PI/2.0} 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length="${thigh_shoulder_length}" radius="${thigh_shoulder_radius}"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link> -->
|
||||||
|
<joint name="FL_thigh_joint" type="continuous">
|
||||||
|
<origin rpy="0 0 0" xyz="0 0.083 0"/>
|
||||||
|
<parent link="FL_hip"/>
|
||||||
|
<child link="FL_thigh"/>
|
||||||
|
<axis xyz="0 1 0"/>
|
||||||
|
<dynamics damping="0" friction="0"/>
|
||||||
|
<limit effort="44.4" velocity="40"/>
|
||||||
|
</joint>
|
||||||
|
<link name="FL_thigh">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="../meshes/thigh.stl" scale="1 1 1"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="grey"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 1.57079632679 0" xyz="0 0 -0.125"/>
|
||||||
|
<geometry>
|
||||||
|
<box size="0.25 0.0374 0.043"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="-0.005607 -0.003877 -0.048199"/>
|
||||||
|
<mass value="0.639"/>
|
||||||
|
<inertia ixx="0.005666803" ixy="3.597e-06" ixz="0.000491446" iyy="0.005847229" iyz="1.0086e-05" izz="0.000369811"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="FL_calf_joint" type="revolute">
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 -0.25"/>
|
||||||
|
<parent link="FL_thigh"/>
|
||||||
|
<child link="FL_calf"/>
|
||||||
|
<axis xyz="0 1 0"/>
|
||||||
|
<dynamics damping="0" friction="0"/>
|
||||||
|
<limit effort="44.4" lower="-2.77507351067" upper="-0.645771823238" velocity="40"/>
|
||||||
|
</joint>
|
||||||
|
<link name="FL_calf">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="../meshes/calf.stl" scale="1 1 1"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="black"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 1.57079632679 0" xyz="0 0 -0.125"/>
|
||||||
|
<geometry>
|
||||||
|
<box size="0.25 0.0208 0.016"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="0.002781 6.3e-05 -0.142518"/>
|
||||||
|
<mass value="0.207"/>
|
||||||
|
<inertia ixx="0.006341369" ixy="-3e-09" ixz="-8.7951e-05" iyy="0.006355157" iyz="-1.336e-06" izz="3.9188e-05"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="FL_foot_fixed" type="fixed">
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 -0.25"/>
|
||||||
|
<parent link="FL_calf"/>
|
||||||
|
<child link="FL_foot"/>
|
||||||
|
</joint>
|
||||||
|
<link name="FL_foot">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<sphere radius="0.0165"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="black"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<sphere radius="0.0265"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="0.06"/>
|
||||||
|
<inertia ixx="1.6854e-05" ixy="0.0" ixz="0.0" iyy="1.6854e-05" iyz="0.0" izz="1.6854e-05"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<transmission name="FL_hip_tran">
|
||||||
|
<type>transmission_interface/SimpleTransmission</type>
|
||||||
|
<joint name="FL_hip_joint">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
</joint>
|
||||||
|
<actuator name="FL_hip_motor">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
<mechanicalReduction>1</mechanicalReduction>
|
||||||
|
</actuator>
|
||||||
|
</transmission>
|
||||||
|
<transmission name="FL_thigh_tran">
|
||||||
|
<type>transmission_interface/SimpleTransmission</type>
|
||||||
|
<joint name="FL_thigh_joint">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
</joint>
|
||||||
|
<actuator name="FL_thigh_motor">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
<mechanicalReduction>1</mechanicalReduction>
|
||||||
|
</actuator>
|
||||||
|
</transmission>
|
||||||
|
<transmission name="FL_calf_tran">
|
||||||
|
<type>transmission_interface/SimpleTransmission</type>
|
||||||
|
<joint name="FL_calf_joint">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
</joint>
|
||||||
|
<actuator name="FL_calf_motor">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
<mechanicalReduction>1</mechanicalReduction>
|
||||||
|
</actuator>
|
||||||
|
</transmission>
|
||||||
|
<joint name="RR_hip_joint" type="revolute">
|
||||||
|
<origin rpy="0 0 0" xyz="-0.2399 -0.051 0"/>
|
||||||
|
<parent link="trunk"/>
|
||||||
|
<child link="RR_hip"/>
|
||||||
|
<axis xyz="1 0 0"/>
|
||||||
|
<dynamics damping="0" friction="0"/>
|
||||||
|
<limit effort="44.4" lower="-1.2217304764" upper="1.2217304764" velocity="40"/>
|
||||||
|
</joint>
|
||||||
|
<link name="RR_hip">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="3.14159265359 3.14159265359 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="../meshes/hip.stl" scale="1 1 1"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="grey"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="1.57079632679 0 0" xyz="0 -0.083 0"/>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length="0.0418" radius="0.046"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="0.022191 -0.015144 -1.5e-05"/>
|
||||||
|
<mass value="1.993"/>
|
||||||
|
<inertia ixx="0.002903894" ixy="-7.185e-05" ixz="1.262e-06" iyy="0.004907517" iyz="1.75e-06" izz="0.005586944"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<!-- <joint name="${name}_hip_fixed" type="fixed">
|
||||||
|
<origin rpy="0 0 0" xyz="0 ${(thigh_shoulder_length/2.0+hip_offset)*mirror} 0"/>
|
||||||
|
<parent link="${name}_hip"/>
|
||||||
|
<child link="${name}_thigh_shoulder"/>
|
||||||
|
</joint> -->
|
||||||
|
<!-- this link is only for collision -->
|
||||||
|
<!-- <link name="${name}_thigh_shoulder">
|
||||||
|
<collision>
|
||||||
|
<origin rpy="${PI/2.0} 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length="${thigh_shoulder_length}" radius="${thigh_shoulder_radius}"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link> -->
|
||||||
|
<joint name="RR_thigh_joint" type="continuous">
|
||||||
|
<origin rpy="0 0 0" xyz="0 -0.083 0"/>
|
||||||
|
<parent link="RR_hip"/>
|
||||||
|
<child link="RR_thigh"/>
|
||||||
|
<axis xyz="0 1 0"/>
|
||||||
|
<dynamics damping="0" friction="0"/>
|
||||||
|
<limit effort="44.4" velocity="40"/>
|
||||||
|
</joint>
|
||||||
|
<link name="RR_thigh">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="../meshes/thigh_mirror.stl" scale="1 1 1"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="grey"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 1.57079632679 0" xyz="0 0 -0.125"/>
|
||||||
|
<geometry>
|
||||||
|
<box size="0.25 0.0374 0.043"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="-0.005607 0.003877 -0.048199"/>
|
||||||
|
<mass value="0.639"/>
|
||||||
|
<inertia ixx="0.005666803" ixy="-3.597e-06" ixz="0.000491446" iyy="0.005847229" iyz="-1.0086e-05" izz="0.000369811"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="RR_calf_joint" type="revolute">
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 -0.25"/>
|
||||||
|
<parent link="RR_thigh"/>
|
||||||
|
<child link="RR_calf"/>
|
||||||
|
<axis xyz="0 1 0"/>
|
||||||
|
<dynamics damping="0" friction="0"/>
|
||||||
|
<limit effort="44.4" lower="-2.77507351067" upper="-0.645771823238" velocity="40"/>
|
||||||
|
</joint>
|
||||||
|
<link name="RR_calf">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="../meshes/calf.stl" scale="1 1 1"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="black"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 1.57079632679 0" xyz="0 0 -0.125"/>
|
||||||
|
<geometry>
|
||||||
|
<box size="0.25 0.0208 0.016"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="0.002781 6.3e-05 -0.142518"/>
|
||||||
|
<mass value="0.207"/>
|
||||||
|
<inertia ixx="0.006341369" ixy="-3e-09" ixz="-8.7951e-05" iyy="0.006355157" iyz="-1.336e-06" izz="3.9188e-05"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="RR_foot_fixed" type="fixed">
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 -0.25"/>
|
||||||
|
<parent link="RR_calf"/>
|
||||||
|
<child link="RR_foot"/>
|
||||||
|
</joint>
|
||||||
|
<link name="RR_foot">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<sphere radius="0.0165"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="black"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<sphere radius="0.0265"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="0.06"/>
|
||||||
|
<inertia ixx="1.6854e-05" ixy="0.0" ixz="0.0" iyy="1.6854e-05" iyz="0.0" izz="1.6854e-05"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<transmission name="RR_hip_tran">
|
||||||
|
<type>transmission_interface/SimpleTransmission</type>
|
||||||
|
<joint name="RR_hip_joint">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
</joint>
|
||||||
|
<actuator name="RR_hip_motor">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
<mechanicalReduction>1</mechanicalReduction>
|
||||||
|
</actuator>
|
||||||
|
</transmission>
|
||||||
|
<transmission name="RR_thigh_tran">
|
||||||
|
<type>transmission_interface/SimpleTransmission</type>
|
||||||
|
<joint name="RR_thigh_joint">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
</joint>
|
||||||
|
<actuator name="RR_thigh_motor">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
<mechanicalReduction>1</mechanicalReduction>
|
||||||
|
</actuator>
|
||||||
|
</transmission>
|
||||||
|
<transmission name="RR_calf_tran">
|
||||||
|
<type>transmission_interface/SimpleTransmission</type>
|
||||||
|
<joint name="RR_calf_joint">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
</joint>
|
||||||
|
<actuator name="RR_calf_motor">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
<mechanicalReduction>1</mechanicalReduction>
|
||||||
|
</actuator>
|
||||||
|
</transmission>
|
||||||
|
<joint name="RL_hip_joint" type="revolute">
|
||||||
|
<origin rpy="0 0 0" xyz="-0.2399 0.051 0"/>
|
||||||
|
<parent link="trunk"/>
|
||||||
|
<child link="RL_hip"/>
|
||||||
|
<axis xyz="1 0 0"/>
|
||||||
|
<dynamics damping="0" friction="0"/>
|
||||||
|
<limit effort="44.4" lower="-1.2217304764" upper="1.2217304764" velocity="40"/>
|
||||||
|
</joint>
|
||||||
|
<link name="RL_hip">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 3.14159265359 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="../meshes/hip.stl" scale="1 1 1"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="grey"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="1.57079632679 0 0" xyz="0 0.083 0"/>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length="0.0418" radius="0.046"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="0.022191 0.015144 -1.5e-05"/>
|
||||||
|
<mass value="1.993"/>
|
||||||
|
<inertia ixx="0.002903894" ixy="7.185e-05" ixz="1.262e-06" iyy="0.004907517" iyz="-1.75e-06" izz="0.005586944"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<!-- <joint name="${name}_hip_fixed" type="fixed">
|
||||||
|
<origin rpy="0 0 0" xyz="0 ${(thigh_shoulder_length/2.0+hip_offset)*mirror} 0"/>
|
||||||
|
<parent link="${name}_hip"/>
|
||||||
|
<child link="${name}_thigh_shoulder"/>
|
||||||
|
</joint> -->
|
||||||
|
<!-- this link is only for collision -->
|
||||||
|
<!-- <link name="${name}_thigh_shoulder">
|
||||||
|
<collision>
|
||||||
|
<origin rpy="${PI/2.0} 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length="${thigh_shoulder_length}" radius="${thigh_shoulder_radius}"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link> -->
|
||||||
|
<joint name="RL_thigh_joint" type="continuous">
|
||||||
|
<origin rpy="0 0 0" xyz="0 0.083 0"/>
|
||||||
|
<parent link="RL_hip"/>
|
||||||
|
<child link="RL_thigh"/>
|
||||||
|
<axis xyz="0 1 0"/>
|
||||||
|
<dynamics damping="0" friction="0"/>
|
||||||
|
<limit effort="44.4" velocity="40"/>
|
||||||
|
</joint>
|
||||||
|
<link name="RL_thigh">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="../meshes/thigh.stl" scale="1 1 1"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="grey"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 1.57079632679 0" xyz="0 0 -0.125"/>
|
||||||
|
<geometry>
|
||||||
|
<box size="0.25 0.0374 0.043"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="-0.005607 -0.003877 -0.048199"/>
|
||||||
|
<mass value="0.639"/>
|
||||||
|
<inertia ixx="0.005666803" ixy="3.597e-06" ixz="0.000491446" iyy="0.005847229" iyz="1.0086e-05" izz="0.000369811"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="RL_calf_joint" type="revolute">
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 -0.25"/>
|
||||||
|
<parent link="RL_thigh"/>
|
||||||
|
<child link="RL_calf"/>
|
||||||
|
<axis xyz="0 1 0"/>
|
||||||
|
<dynamics damping="0" friction="0"/>
|
||||||
|
<limit effort="44.4" lower="-2.77507351067" upper="-0.645771823238" velocity="40"/>
|
||||||
|
</joint>
|
||||||
|
<link name="RL_calf">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="../meshes/calf.stl" scale="1 1 1"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="black"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 1.57079632679 0" xyz="0 0 -0.125"/>
|
||||||
|
<geometry>
|
||||||
|
<box size="0.25 0.0208 0.016"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="0.002781 6.3e-05 -0.142518"/>
|
||||||
|
<mass value="0.207"/>
|
||||||
|
<inertia ixx="0.006341369" ixy="-3e-09" ixz="-8.7951e-05" iyy="0.006355157" iyz="-1.336e-06" izz="3.9188e-05"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="RL_foot_fixed" type="fixed">
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 -0.25"/>
|
||||||
|
<parent link="RL_calf"/>
|
||||||
|
<child link="RL_foot"/>
|
||||||
|
</joint>
|
||||||
|
<link name="RL_foot">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<sphere radius="0.0165"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="black"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<sphere radius="0.0265"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="0.06"/>
|
||||||
|
<inertia ixx="1.6854e-05" ixy="0.0" ixz="0.0" iyy="1.6854e-05" iyz="0.0" izz="1.6854e-05"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<transmission name="RL_hip_tran">
|
||||||
|
<type>transmission_interface/SimpleTransmission</type>
|
||||||
|
<joint name="RL_hip_joint">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
</joint>
|
||||||
|
<actuator name="RL_hip_motor">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
<mechanicalReduction>1</mechanicalReduction>
|
||||||
|
</actuator>
|
||||||
|
</transmission>
|
||||||
|
<transmission name="RL_thigh_tran">
|
||||||
|
<type>transmission_interface/SimpleTransmission</type>
|
||||||
|
<joint name="RL_thigh_joint">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
</joint>
|
||||||
|
<actuator name="RL_thigh_motor">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
<mechanicalReduction>1</mechanicalReduction>
|
||||||
|
</actuator>
|
||||||
|
</transmission>
|
||||||
|
<transmission name="RL_calf_tran">
|
||||||
|
<type>transmission_interface/SimpleTransmission</type>
|
||||||
|
<joint name="RL_calf_joint">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
</joint>
|
||||||
|
<actuator name="RL_calf_motor">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
<mechanicalReduction>1</mechanicalReduction>
|
||||||
|
</actuator>
|
||||||
|
</transmission>
|
||||||
|
<!-- <xacro:stairs stairs="15" xpos="0" ypos="2.0" zpos="0" /> -->
|
||||||
|
</robot>
|
||||||
|
|
||||||
185
data/aliengo/xml/aliengo.xml
Normal file
185
data/aliengo/xml/aliengo.xml
Normal file
@@ -0,0 +1,185 @@
|
|||||||
|
<mujoco model="aliengo_description">
|
||||||
|
<compiler angle="radian" meshdir="../meshes/" />
|
||||||
|
<size njmax="500" nconmax="100" />
|
||||||
|
<option gravity='0 0 -9.806' iterations='50' solver='Newton' timestep='0.002'/>
|
||||||
|
|
||||||
|
<default>
|
||||||
|
<geom contype="1" conaffinity="1" friction="0.6 0.3 0.3" rgba="0.5 0.6 0.7 1" margin="0.001" group="0"/>
|
||||||
|
|
||||||
|
<light castshadow="false" diffuse="1 1 1"/>
|
||||||
|
<motor ctrlrange="-44.4 44.4" ctrllimited="true"/>
|
||||||
|
<camera fovy="60"/>
|
||||||
|
<joint damping="0.01" armature="0.01" frictionloss="0.2" />
|
||||||
|
|
||||||
|
</default>
|
||||||
|
|
||||||
|
<asset>
|
||||||
|
<mesh name="trunk" file="trunk.stl" />
|
||||||
|
<mesh name="hip" file="hip.stl" />
|
||||||
|
<mesh name="thigh_mirror" file="thigh_mirror.stl" />
|
||||||
|
<mesh name="calf" file="calf.stl" />
|
||||||
|
<mesh name="thigh" file="thigh.stl" />
|
||||||
|
</asset>
|
||||||
|
|
||||||
|
<asset>
|
||||||
|
<texture type="skybox" builtin="gradient" rgb1="1.0 1.0 1.0" rgb2="1.0 1.0 1.0" width="512" height="512"/>
|
||||||
|
<texture name="plane" type="2d" builtin="flat" rgb1="1 1 1" rgb2="1 1 1" width="512" height="512" mark="cross" markrgb="0 0 0"/>
|
||||||
|
<material name="plane" reflectance="0.0" texture="plane" texrepeat="3 3" texuniform="true"/>
|
||||||
|
</asset>
|
||||||
|
|
||||||
|
<visual>
|
||||||
|
<rgba com="0.502 1.0 0 0.5" contactforce="0.98 0.4 0.4 0.7" contactpoint="1.0 1.0 0.6 0.4"/>
|
||||||
|
<scale com="0.2" forcewidth="0.035" contactwidth="0.10" contactheight="0.04"/>
|
||||||
|
</visual>
|
||||||
|
|
||||||
|
<worldbody>
|
||||||
|
<light directional="true" diffuse=".8 .8 .8" pos="0 0 10" dir="0 0 -10"/>
|
||||||
|
<camera name="track" mode="trackcom" pos="0 -1.3 1.6" xyaxes="1 0 0 0 0.707 0.707"/>
|
||||||
|
<geom name='floor' type='plane' conaffinity='1' condim='3' contype='1' rgba="0.5 0.9 0.9 0.1" material='plane' pos='0 0 0' size='0 0 1'/>
|
||||||
|
|
||||||
|
<body name="trunk" pos="0 0 0.4">
|
||||||
|
<inertial pos="0.00846406 0.00404455 -0.000762916" quat="-3.12625e-05 0.708321 0.00247254 0.705886" mass="9.042" diaginertia="0.174706 0.161175 0.033357" />
|
||||||
|
<joint type="free" />
|
||||||
|
<geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.2 0.2 0.2 1" mesh="trunk" />
|
||||||
|
<geom size="0.18 0.075 0.056" type="box" rgba="0.2 0.2 0.2 0" />
|
||||||
|
<geom size="0.0005 0.0005 0.0005" type="box" contype="0" conaffinity="0" group="1" rgba="0.8 0 0 0" />
|
||||||
|
<geom size="0.0005 0.0005 0.0005" type="box" rgba="0.8 0 0 0" />
|
||||||
|
<site name="imu" pos="0 0 0"/>
|
||||||
|
|
||||||
|
<body name="FR_hip" pos="0.2399 -0.051 0">
|
||||||
|
<inertial pos="-0.022191 -0.015144 -1.5e-05" quat="0.0135493 0.706835 -0.0117635 0.707151" mass="1.993" diaginertia="0.00558695 0.00491009 0.00290132" />
|
||||||
|
<joint name="FR_hip_joint" pos="0 0 0" axis="1 0 0" limited="true" range="-1.22173 1.22173" />
|
||||||
|
<geom quat="0 1 0 0" type="mesh" contype="0" conaffinity="0" group="1" rgba="0.2 0.2 0.2 1" mesh="hip" />
|
||||||
|
<geom size="0.046 0.0209" pos="0 -0.083 0" quat="0.707107 0.707107 0 0" type="cylinder" rgba="0.2 0.2 0.2 0" />
|
||||||
|
<body name="FR_thigh" pos="0 -0.083 0">
|
||||||
|
<inertial pos="-0.005607 0.003877 -0.048199" quat="0.694544 -0.0335669 -0.031246 0.717987" mass="0.639" diaginertia="0.0058474 0.00571186 0.000324584" />
|
||||||
|
<joint name="FR_thigh_joint" pos="0 0 0" axis="0 1 0" />
|
||||||
|
<geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.2 0.2 0.2 1" mesh="thigh_mirror" />
|
||||||
|
<geom size="0.125 0.0187 0.0215" pos="0 0 -0.125" quat="0.707107 0 0.707107 0" type="box" rgba="0.2 0.2 0.2 0" />
|
||||||
|
<body name="FR_calf" pos="0 0 -0.25">
|
||||||
|
<inertial pos="0.00215606 4.88427e-05 -0.166671" quat="0.707462 0.00517582 0.00535172 0.706712" mass="0.267" diaginertia="0.00690975 0.00689712 5.4885e-05" />
|
||||||
|
<joint name="FR_calf_joint" pos="0 0 0" axis="0 1 0" limited="true" range="-2.77507 -0.645772" />
|
||||||
|
<geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0 0 0 1" mesh="calf" />
|
||||||
|
<geom size="0.125 0.0104 0.008" pos="0 0 -0.125" quat="0.707107 0 0.707107 0" type="box" rgba="0 0 0 0" />
|
||||||
|
<geom size="0.0165" pos="0 0 -0.25" contype="0" conaffinity="0" group="1" rgba="0 0 0 1" />
|
||||||
|
<geom size="0.0265" pos="0 0 -0.25" rgba="0 0 0 1" />
|
||||||
|
</body>
|
||||||
|
</body>
|
||||||
|
</body>
|
||||||
|
<body name="FL_hip" pos="0.2399 0.051 0">
|
||||||
|
<inertial pos="-0.022191 0.015144 -1.5e-05" quat="-0.0135493 0.706835 0.0117635 0.707151" mass="1.993" diaginertia="0.00558695 0.00491009 0.00290132" />
|
||||||
|
<joint name="FL_hip_joint" pos="0 0 0" axis="1 0 0" limited="true" range="-1.22173 1.22173" />
|
||||||
|
<geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.2 0.2 0.2 1" mesh="hip" />
|
||||||
|
<geom size="0.046 0.0209" pos="0 0.083 0" quat="0.707107 0.707107 0 0" type="cylinder" rgba="0.2 0.2 0.2 0" />
|
||||||
|
<body name="FL_thigh" pos="0 0.083 0">
|
||||||
|
<inertial pos="-0.005607 -0.003877 -0.048199" quat="0.717987 -0.031246 -0.0335669 0.694544" mass="0.639" diaginertia="0.0058474 0.00571186 0.000324584" />
|
||||||
|
<joint name="FL_thigh_joint" pos="0 0 0" axis="0 1 0" />
|
||||||
|
<geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.2 0.2 0.2 1" mesh="thigh" />
|
||||||
|
<geom size="0.125 0.0187 0.0215" pos="0 0 -0.125" quat="0.707107 0 0.707107 0" type="box" rgba="0.2 0.2 0.2 0" />
|
||||||
|
<body name="FL_calf" pos="0 0 -0.25">
|
||||||
|
<inertial pos="0.00215606 4.88427e-05 -0.166671" quat="0.707462 0.00517582 0.00535172 0.706712" mass="0.267" diaginertia="0.00690975 0.00689712 5.4885e-05" />
|
||||||
|
<joint name="FL_calf_joint" pos="0 0 0" axis="0 1 0" limited="true" range="-2.77507 -0.645772" />
|
||||||
|
<geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0 0 0 1" mesh="calf" />
|
||||||
|
<geom size="0.125 0.0104 0.008" pos="0 0 -0.125" quat="0.707107 0 0.707107 0" type="box" rgba="0 0 0 0" />
|
||||||
|
<geom size="0.0165" pos="0 0 -0.25" contype="0" conaffinity="0" group="1" rgba="0 0 0 1" />
|
||||||
|
<geom size="0.0265" pos="0 0 -0.25" rgba="0 0 0 1" />
|
||||||
|
</body>
|
||||||
|
</body>
|
||||||
|
</body>
|
||||||
|
<body name="RR_hip" pos="-0.2399 -0.051 0">
|
||||||
|
<inertial pos="0.022191 -0.015144 -1.5e-05" quat="-0.0117635 0.707151 0.0135493 0.706835" mass="1.993" diaginertia="0.00558695 0.00491009 0.00290132" />
|
||||||
|
<joint name="RR_hip_joint" pos="0 0 0" axis="1 0 0" limited="true" range="-1.22173 1.22173" />
|
||||||
|
<geom quat="0 0 0 -1" type="mesh" contype="0" conaffinity="0" group="1" rgba="0.2 0.2 0.2 1" mesh="hip" />
|
||||||
|
<geom size="0.046 0.0209" pos="0 -0.083 0" quat="0.707107 0.707107 0 0" type="cylinder" rgba="0.2 0.2 0.2 0" />
|
||||||
|
<body name="RR_thigh" pos="0 -0.083 0">
|
||||||
|
<inertial pos="-0.005607 0.003877 -0.048199" quat="0.694544 -0.0335669 -0.031246 0.717987" mass="0.639" diaginertia="0.0058474 0.00571186 0.000324584" />
|
||||||
|
<joint name="RR_thigh_joint" pos="0 0 0" axis="0 1 0" />
|
||||||
|
<geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.2 0.2 0.2 1" mesh="thigh_mirror" />
|
||||||
|
<geom size="0.125 0.0187 0.0215" pos="0 0 -0.125" quat="0.707107 0 0.707107 0" type="box" rgba="0.2 0.2 0.2 0" />
|
||||||
|
<body name="RR_calf" pos="0 0 -0.25">
|
||||||
|
<inertial pos="0.00215606 4.88427e-05 -0.166671" quat="0.707462 0.00517582 0.00535172 0.706712" mass="0.267" diaginertia="0.00690975 0.00689712 5.4885e-05" />
|
||||||
|
<joint name="RR_calf_joint" pos="0 0 0" axis="0 1 0" limited="true" range="-2.77507 -0.645772" />
|
||||||
|
<geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0 0 0 1" mesh="calf" />
|
||||||
|
<geom size="0.125 0.0104 0.008" pos="0 0 -0.125" quat="0.707107 0 0.707107 0" type="box" rgba="0 0 0 0" />
|
||||||
|
<geom size="0.0165" pos="0 0 -0.25" contype="0" conaffinity="0" group="1" rgba="0 0 0 1" />
|
||||||
|
<geom size="0.0265" pos="0 0 -0.25" rgba="0 0 0 1" />
|
||||||
|
</body>
|
||||||
|
</body>
|
||||||
|
</body>
|
||||||
|
<body name="RL_hip" pos="-0.2399 0.051 0">
|
||||||
|
<inertial pos="0.022191 0.015144 -1.5e-05" quat="0.0117635 0.707151 -0.0135493 0.706835" mass="1.993" diaginertia="0.00558695 0.00491009 0.00290132" />
|
||||||
|
<joint name="RL_hip_joint" pos="0 0 0" axis="1 0 0" limited="true" range="-1.22173 1.22173" />
|
||||||
|
<geom quat="0 0 1 0" type="mesh" contype="0" conaffinity="0" group="1" rgba="0.2 0.2 0.2 1" mesh="hip" />
|
||||||
|
<geom size="0.046 0.0209" pos="0 0.083 0" quat="0.707107 0.707107 0 0" type="cylinder" rgba="0.2 0.2 0.2 0" />
|
||||||
|
<body name="RL_thigh" pos="0 0.083 0">
|
||||||
|
<inertial pos="-0.005607 -0.003877 -0.048199" quat="0.717987 -0.031246 -0.0335669 0.694544" mass="0.639" diaginertia="0.0058474 0.00571186 0.000324584" />
|
||||||
|
<joint name="RL_thigh_joint" pos="0 0 0" axis="0 1 0" />
|
||||||
|
<geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.2 0.2 0.2 1" mesh="thigh" />
|
||||||
|
<geom size="0.125 0.0187 0.0215" pos="0 0 -0.125" quat="0.707107 0 0.707107 0" type="box" rgba="0.2 0.2 0.2 0" />
|
||||||
|
<body name="RL_calf" pos="0 0 -0.25">
|
||||||
|
<inertial pos="0.00215606 4.88427e-05 -0.166671" quat="0.707462 0.00517582 0.00535172 0.706712" mass="0.267" diaginertia="0.00690975 0.00689712 5.4885e-05" />
|
||||||
|
<joint name="RL_calf_joint" pos="0 0 0" axis="0 1 0" limited="true" range="-2.77507 -0.645772" />
|
||||||
|
<geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0 0 0 1" mesh="calf" />
|
||||||
|
<geom size="0.125 0.0104 0.008" pos="0 0 -0.125" quat="0.707107 0 0.707107 0" type="box" rgba="0 0 0 0" />
|
||||||
|
<geom size="0.0165" pos="0 0 -0.25" contype="0" conaffinity="0" group="1" rgba="0 0 0 1" />
|
||||||
|
<geom size="0.0265" pos="0 0 -0.25" rgba="0 0 0 1" />
|
||||||
|
</body>
|
||||||
|
</body>
|
||||||
|
</body>
|
||||||
|
</body>
|
||||||
|
</worldbody>
|
||||||
|
|
||||||
|
<actuator>
|
||||||
|
|
||||||
|
<motor name="FR_hip" gear="1" joint="FR_hip_joint"/>
|
||||||
|
<motor name="FR_thigh" gear="1" joint="FR_thigh_joint"/>
|
||||||
|
<motor name="FR_calf" gear="1" joint="FR_calf_joint"/>
|
||||||
|
<motor name="FL_hip" gear="1" joint="FL_hip_joint"/>
|
||||||
|
<motor name="FL_thigh" gear="1" joint="FL_thigh_joint"/>
|
||||||
|
<motor name="FL_calf" gear="1" joint="FL_calf_joint"/>
|
||||||
|
<motor name="RR_hip" gear="1" joint="RR_hip_joint"/>
|
||||||
|
<motor name="RR_thigh" gear="1" joint="RR_thigh_joint"/>
|
||||||
|
<motor name="RR_calf" gear="1" joint="RR_calf_joint" />
|
||||||
|
<motor name="RL_hip" gear="1" joint="RL_hip_joint"/>
|
||||||
|
<motor name="RL_thigh" gear="1" joint="RL_thigh_joint"/>
|
||||||
|
<motor name="RL_calf" gear="1" joint="RL_calf_joint"/>
|
||||||
|
</actuator>
|
||||||
|
|
||||||
|
<sensor>
|
||||||
|
|
||||||
|
<jointpos name="FR_hip_pos" joint="FR_hip_joint"/>
|
||||||
|
<jointpos name="FR_thigh_pos" joint="FR_thigh_joint"/>
|
||||||
|
<jointpos name="FR_calf_pos" joint="FR_calf_joint"/>
|
||||||
|
<jointpos name="FL_hip_pos" joint="FL_hip_joint"/>
|
||||||
|
<jointpos name="FL_thigh_pos" joint="FL_thigh_joint"/>
|
||||||
|
<jointpos name="FL_calf_pos" joint="FL_calf_joint"/>
|
||||||
|
<jointpos name="RR_hip_pos" joint="RR_hip_joint"/>
|
||||||
|
<jointpos name="RR_thigh_pos" joint="RR_thigh_joint"/>
|
||||||
|
<jointpos name="RR_calf_pos" joint="RR_calf_joint" />
|
||||||
|
<jointpos name="RL_hip_pos" joint="RL_hip_joint"/>
|
||||||
|
<jointpos name="RL_thigh_pos" joint="RL_thigh_joint"/>
|
||||||
|
<jointpos name="RL_calf_pos" joint="RL_calf_joint"/>
|
||||||
|
|
||||||
|
<jointvel name="FR_hip_vel" joint="FR_hip_joint"/>
|
||||||
|
<jointvel name="FR_thigh_vel" joint="FR_thigh_joint"/>
|
||||||
|
<jointvel name="FR_calf_vel" joint="FR_calf_joint"/>
|
||||||
|
<jointvel name="FL_hip_vel" joint="FL_hip_joint"/>
|
||||||
|
<jointvel name="FL_thigh_vel" joint="FL_thigh_joint"/>
|
||||||
|
<jointvel name="FL_calf_vel" joint="FL_calf_joint"/>
|
||||||
|
<jointvel name="RR_hip_vel" joint="RR_hip_joint"/>
|
||||||
|
<jointvel name="RR_thigh_vel" joint="RR_thigh_joint"/>
|
||||||
|
<jointvel name="RR_calf_vel" joint="RR_calf_joint" />
|
||||||
|
<jointvel name="RL_hip_vel" joint="RL_hip_joint"/>
|
||||||
|
<jointvel name="RL_thigh_vel" joint="RL_thigh_joint"/>
|
||||||
|
<jointvel name="RL_calf_vel" joint="RL_calf_joint"/>
|
||||||
|
|
||||||
|
<accelerometer name="Body_Acc" site="imu"/>
|
||||||
|
|
||||||
|
<gyro name="Body_Gyro" site="imu"/>
|
||||||
|
|
||||||
|
<framepos name="Body_Pos" objtype="site" objname="imu"/>
|
||||||
|
|
||||||
|
<framequat name="Body_Quat" objtype="site" objname="imu"/>
|
||||||
|
|
||||||
|
</sensor>
|
||||||
|
</mujoco>
|
||||||
BIN
data/go1/meshes/calf.stl
Normal file
BIN
data/go1/meshes/calf.stl
Normal file
Binary file not shown.
BIN
data/go1/meshes/hip.stl
Normal file
BIN
data/go1/meshes/hip.stl
Normal file
Binary file not shown.
BIN
data/go1/meshes/thigh.stl
Normal file
BIN
data/go1/meshes/thigh.stl
Normal file
Binary file not shown.
BIN
data/go1/meshes/thigh_mirror.stl
Normal file
BIN
data/go1/meshes/thigh_mirror.stl
Normal file
Binary file not shown.
BIN
data/go1/meshes/trunk.stl
Normal file
BIN
data/go1/meshes/trunk.stl
Normal file
Binary file not shown.
704
data/go1/urdf/go1.urdf
Normal file
704
data/go1/urdf/go1.urdf
Normal file
@@ -0,0 +1,704 @@
|
|||||||
|
<?xml version="1.0" ?>
|
||||||
|
<!-- =================================================================================== -->
|
||||||
|
<!-- | This document was autogenerated by xacro from robot.xacro | -->
|
||||||
|
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
|
||||||
|
<!-- =================================================================================== -->
|
||||||
|
<robot name="go1_description">
|
||||||
|
|
||||||
|
<mujoco>
|
||||||
|
<compiler
|
||||||
|
meshdir="../meshes/"
|
||||||
|
balanceinertia="true"
|
||||||
|
discardvisual="false" />
|
||||||
|
</mujoco>
|
||||||
|
|
||||||
|
<material name="black">
|
||||||
|
<color rgba="0.0 0.0 0.0 1.0"/>
|
||||||
|
</material>
|
||||||
|
<material name="blue">
|
||||||
|
<color rgba="0.0 0.0 0.8 1.0"/>
|
||||||
|
</material>
|
||||||
|
<material name="green">
|
||||||
|
<color rgba="0.0 0.8 0.0 1.0"/>
|
||||||
|
</material>
|
||||||
|
<material name="grey">
|
||||||
|
<color rgba="0.2 0.2 0.2 1.0"/>
|
||||||
|
</material>
|
||||||
|
<material name="silver">
|
||||||
|
<color rgba="0.913725490196 0.913725490196 0.847058823529 1.0"/>
|
||||||
|
</material>
|
||||||
|
<material name="orange">
|
||||||
|
<color rgba="1.0 0.423529411765 0.0392156862745 1.0"/>
|
||||||
|
</material>
|
||||||
|
<material name="brown">
|
||||||
|
<color rgba="0.870588235294 0.811764705882 0.764705882353 1.0"/>
|
||||||
|
</material>
|
||||||
|
<material name="red">
|
||||||
|
<color rgba="0.8 0.0 0.0 1.0"/>
|
||||||
|
</material>
|
||||||
|
<material name="white">
|
||||||
|
<color rgba="1.0 1.0 1.0 1.0"/>
|
||||||
|
</material>
|
||||||
|
|
||||||
|
|
||||||
|
<link name="world"/>
|
||||||
|
<joint name="" type="floating">
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<parent link="world"/>
|
||||||
|
<child link="trunk"/>
|
||||||
|
</joint>
|
||||||
|
<link name="trunk">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="../meshes/trunk.stl" scale="1 1 1"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="silver"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<box size="0.3762 0.0935 0.114"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="0.011611 0.004437 0.000108"/>
|
||||||
|
<mass value="4.8"/>
|
||||||
|
<inertia ixx="0.016130741919" ixy="0.000593180607" ixz="7.324662e-06" iyy="0.036507810812" iyz="2.0969537e-05" izz="0.044693872053"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="imu_joint" type="fixed">
|
||||||
|
<parent link="trunk"/>
|
||||||
|
<child link="imu_link"/>
|
||||||
|
<origin rpy="0 0 0" xyz="-0.01592 -0.06659 -0.00617"/>
|
||||||
|
</joint>
|
||||||
|
<link name="imu_link">
|
||||||
|
<inertial>
|
||||||
|
<mass value="0.001"/>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001"/>
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<box size="0.001 0.001 0.001"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="red"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<box size=".001 .001 .001"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
<joint name="FR_hip_joint" type="revolute">
|
||||||
|
<origin rpy="0 0 0" xyz="0.1881 -0.04675 0"/>
|
||||||
|
<parent link="trunk"/>
|
||||||
|
<child link="FR_hip"/>
|
||||||
|
<axis xyz="1 0 0"/>
|
||||||
|
<dynamics damping="0.01" friction="0.2"/>
|
||||||
|
<limit effort="33.5" lower="-0.802851455917" upper="0.802851455917" velocity="21"/>
|
||||||
|
</joint>
|
||||||
|
<link name="FR_hip">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="3.14159265359 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="../meshes/hip.stl" scale="1 1 1"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="silver"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="1.57079632679 0 0" xyz="0 -0.045 0"/>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length="0.04" radius="0.046"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="-0.00541 0.00074 6e-06"/>
|
||||||
|
<mass value="0.510299"/>
|
||||||
|
<inertia ixx="0.00030528937" ixy="7.788013e-06" ixz="2.2016e-07" iyy="0.000590894859" iyz="1.7175e-08" izz="0.000396594572"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="FR_hip_fixed" type="fixed">
|
||||||
|
<origin rpy="0 0 0" xyz="0 -0.08 0"/>
|
||||||
|
<parent link="FR_hip"/>
|
||||||
|
<child link="FR_thigh_shoulder"/>
|
||||||
|
</joint>
|
||||||
|
<!-- this link is only for collision -->
|
||||||
|
<link name="FR_thigh_shoulder">
|
||||||
|
<collision>
|
||||||
|
<origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length="0.032" radius="0.041"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
<joint name="FR_thigh_joint" type="revolute">
|
||||||
|
<origin rpy="0 0 0" xyz="0 -0.08 0"/>
|
||||||
|
<parent link="FR_hip"/>
|
||||||
|
<child link="FR_thigh"/>
|
||||||
|
<axis xyz="0 1 0"/>
|
||||||
|
<dynamics damping="0.01" friction="0.2"/>
|
||||||
|
<limit effort="33.5" lower="-1.0471975512" upper="4.18879020479" velocity="21"/>
|
||||||
|
</joint>
|
||||||
|
<link name="FR_thigh">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="../meshes/thigh_mirror.stl" scale="1 1 1"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="silver"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 1.57079632679 0" xyz="0 0 -0.1065"/>
|
||||||
|
<geometry>
|
||||||
|
<box size="0.213 0.0245 0.034"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="-0.003468 0.018947 -0.032736"/>
|
||||||
|
<mass value="0.898919"/>
|
||||||
|
<inertia ixx="0.005395867678" ixy="-1.02809e-07" ixz="0.000337529085" iyy="0.005142451046" iyz="5.816563e-06" izz="0.00102478732"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="FR_calf_joint" type="revolute">
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 -0.213"/>
|
||||||
|
<parent link="FR_thigh"/>
|
||||||
|
<child link="FR_calf"/>
|
||||||
|
<axis xyz="0 1 0"/>
|
||||||
|
<dynamics damping="0.01" friction="0.2"/>
|
||||||
|
<limit effort="33.5" lower="-2.69653369433" upper="-0.916297857297" velocity="21"/>
|
||||||
|
</joint>
|
||||||
|
<link name="FR_calf">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="../meshes/calf.stl" scale="1 1 1"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="black"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 1.57079632679 0" xyz="0 0 -0.1065"/>
|
||||||
|
<geometry>
|
||||||
|
<box size="0.213 0.016 0.016"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="0.006286 0.001307 -0.122269"/>
|
||||||
|
<mass value="0.158015"/>
|
||||||
|
<inertia ixx="0.003607648222" ixy="1.494971e-06" ixz="-0.000132778525" iyy="0.003626771492" iyz="-2.8638535e-05" izz="3.5148003e-05"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="FR_foot_fixed" type="fixed">
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 -0.213"/>
|
||||||
|
<parent link="FR_calf"/>
|
||||||
|
<child link="FR_foot"/>
|
||||||
|
</joint>
|
||||||
|
<link name="FR_foot">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<sphere radius="0.01"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="black"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<sphere radius="0.02"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="0.06"/>
|
||||||
|
<inertia ixx="9.6e-06" ixy="0.0" ixz="0.0" iyy="9.6e-06" iyz="0.0" izz="9.6e-06"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<transmission name="FR_hip_tran">
|
||||||
|
<type>transmission_interface/SimpleTransmission</type>
|
||||||
|
<joint name="FR_hip_joint">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
</joint>
|
||||||
|
<actuator name="FR_hip_motor">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
<mechanicalReduction>1</mechanicalReduction>
|
||||||
|
</actuator>
|
||||||
|
</transmission>
|
||||||
|
<transmission name="FR_thigh_tran">
|
||||||
|
<type>transmission_interface/SimpleTransmission</type>
|
||||||
|
<joint name="FR_thigh_joint">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
</joint>
|
||||||
|
<actuator name="FR_thigh_motor">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
<mechanicalReduction>1</mechanicalReduction>
|
||||||
|
</actuator>
|
||||||
|
</transmission>
|
||||||
|
<transmission name="FR_calf_tran">
|
||||||
|
<type>transmission_interface/SimpleTransmission</type>
|
||||||
|
<joint name="FR_calf_joint">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
</joint>
|
||||||
|
<actuator name="FR_calf_motor">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
<mechanicalReduction>1</mechanicalReduction>
|
||||||
|
</actuator>
|
||||||
|
</transmission>
|
||||||
|
<joint name="FL_hip_joint" type="revolute">
|
||||||
|
<origin rpy="0 0 0" xyz="0.1881 0.04675 0"/>
|
||||||
|
<parent link="trunk"/>
|
||||||
|
<child link="FL_hip"/>
|
||||||
|
<axis xyz="1 0 0"/>
|
||||||
|
<dynamics damping="0.01" friction="0.2"/>
|
||||||
|
<limit effort="33.5" lower="-0.802851455917" upper="0.802851455917" velocity="21"/>
|
||||||
|
</joint>
|
||||||
|
<link name="FL_hip">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="../meshes/hip.stl" scale="1 1 1"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="silver"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="1.57079632679 0 0" xyz="0 0.045 0"/>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length="0.04" radius="0.046"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="-0.00541 -0.00074 6e-06"/>
|
||||||
|
<mass value="0.510299"/>
|
||||||
|
<inertia ixx="0.00030528937" ixy="-7.788013e-06" ixz="2.2016e-07" iyy="0.000590894859" iyz="-1.7175e-08" izz="0.000396594572"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="FL_hip_fixed" type="fixed">
|
||||||
|
<origin rpy="0 0 0" xyz="0 0.08 0"/>
|
||||||
|
<parent link="FL_hip"/>
|
||||||
|
<child link="FL_thigh_shoulder"/>
|
||||||
|
</joint>
|
||||||
|
<!-- this link is only for collision -->
|
||||||
|
<link name="FL_thigh_shoulder">
|
||||||
|
<collision>
|
||||||
|
<origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length="0.032" radius="0.041"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
<joint name="FL_thigh_joint" type="revolute">
|
||||||
|
<origin rpy="0 0 0" xyz="0 0.08 0"/>
|
||||||
|
<parent link="FL_hip"/>
|
||||||
|
<child link="FL_thigh"/>
|
||||||
|
<axis xyz="0 1 0"/>
|
||||||
|
<dynamics damping="0.01" friction="0.2"/>
|
||||||
|
<limit effort="33.5" lower="-1.0471975512" upper="4.18879020479" velocity="21"/>
|
||||||
|
</joint>
|
||||||
|
<link name="FL_thigh">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="../meshes/thigh.stl" scale="1 1 1"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="silver"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 1.57079632679 0" xyz="0 0 -0.1065"/>
|
||||||
|
<geometry>
|
||||||
|
<box size="0.213 0.0245 0.034"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="-0.003468 -0.018947 -0.032736"/>
|
||||||
|
<mass value="0.898919"/>
|
||||||
|
<inertia ixx="0.005395867678" ixy="1.02809e-07" ixz="0.000337529085" iyy="0.005142451046" iyz="-5.816563e-06" izz="0.00102478732"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="FL_calf_joint" type="revolute">
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 -0.213"/>
|
||||||
|
<parent link="FL_thigh"/>
|
||||||
|
<child link="FL_calf"/>
|
||||||
|
<axis xyz="0 1 0"/>
|
||||||
|
<dynamics damping="0.01" friction="0.2"/>
|
||||||
|
<limit effort="33.5" lower="-2.69653369433" upper="-0.916297857297" velocity="21"/>
|
||||||
|
</joint>
|
||||||
|
<link name="FL_calf">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="../meshes/calf.stl" scale="1 1 1"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="silver"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 1.57079632679 0" xyz="0 0 -0.1065"/>
|
||||||
|
<geometry>
|
||||||
|
<box size="0.213 0.016 0.016"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="0.006286 0.001307 -0.122269"/>
|
||||||
|
<mass value="0.158015"/>
|
||||||
|
<inertia ixx="0.003607648222" ixy="1.494971e-06" ixz="-0.000132778525" iyy="0.003626771492" iyz="-2.8638535e-05" izz="3.5148003e-05"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="FL_foot_fixed" type="fixed">
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 -0.213"/>
|
||||||
|
<parent link="FL_calf"/>
|
||||||
|
<child link="FL_foot"/>
|
||||||
|
</joint>
|
||||||
|
<link name="FL_foot">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<sphere radius="0.01"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="black"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<sphere radius="0.02"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="0.06"/>
|
||||||
|
<inertia ixx="9.6e-06" ixy="0.0" ixz="0.0" iyy="9.6e-06" iyz="0.0" izz="9.6e-06"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<transmission name="FL_hip_tran">
|
||||||
|
<type>transmission_interface/SimpleTransmission</type>
|
||||||
|
<joint name="FL_hip_joint">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
</joint>
|
||||||
|
<actuator name="FL_hip_motor">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
<mechanicalReduction>1</mechanicalReduction>
|
||||||
|
</actuator>
|
||||||
|
</transmission>
|
||||||
|
<transmission name="FL_thigh_tran">
|
||||||
|
<type>transmission_interface/SimpleTransmission</type>
|
||||||
|
<joint name="FL_thigh_joint">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
</joint>
|
||||||
|
<actuator name="FL_thigh_motor">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
<mechanicalReduction>1</mechanicalReduction>
|
||||||
|
</actuator>
|
||||||
|
</transmission>
|
||||||
|
<transmission name="FL_calf_tran">
|
||||||
|
<type>transmission_interface/SimpleTransmission</type>
|
||||||
|
<joint name="FL_calf_joint">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
</joint>
|
||||||
|
<actuator name="FL_calf_motor">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
<mechanicalReduction>1</mechanicalReduction>
|
||||||
|
</actuator>
|
||||||
|
</transmission>
|
||||||
|
<joint name="RR_hip_joint" type="revolute">
|
||||||
|
<origin rpy="0 0 0" xyz="-0.1881 -0.04675 0"/>
|
||||||
|
<parent link="trunk"/>
|
||||||
|
<child link="RR_hip"/>
|
||||||
|
<axis xyz="1 0 0"/>
|
||||||
|
<dynamics damping="0.01" friction="0.2"/>
|
||||||
|
<limit effort="33.5" lower="-0.802851455917" upper="0.802851455917" velocity="21"/>
|
||||||
|
</joint>
|
||||||
|
<link name="RR_hip">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="3.14159265359 3.14159265359 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="../meshes/hip.stl" scale="1 1 1"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="silver"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="1.57079632679 0 0" xyz="0 -0.045 0"/>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length="0.04" radius="0.046"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="0.00541 0.00074 6e-06"/>
|
||||||
|
<mass value="0.510299"/>
|
||||||
|
<inertia ixx="0.00030528937" ixy="-7.788013e-06" ixz="-2.2016e-07" iyy="0.000590894859" iyz="1.7175e-08" izz="0.000396594572"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="RR_hip_fixed" type="fixed">
|
||||||
|
<origin rpy="0 0 0" xyz="0 -0.08 0"/>
|
||||||
|
<parent link="RR_hip"/>
|
||||||
|
<child link="RR_thigh_shoulder"/>
|
||||||
|
</joint>
|
||||||
|
<!-- this link is only for collision -->
|
||||||
|
<link name="RR_thigh_shoulder">
|
||||||
|
<collision>
|
||||||
|
<origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length="0.032" radius="0.041"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
<joint name="RR_thigh_joint" type="revolute">
|
||||||
|
<origin rpy="0 0 0" xyz="0 -0.08 0"/>
|
||||||
|
<parent link="RR_hip"/>
|
||||||
|
<child link="RR_thigh"/>
|
||||||
|
<axis xyz="0 1 0"/>
|
||||||
|
<dynamics damping="0.01" friction="0.2"/>
|
||||||
|
<limit effort="33.5" lower="-1.0471975512" upper="4.18879020479" velocity="21"/>
|
||||||
|
</joint>
|
||||||
|
<link name="RR_thigh">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="../meshes/thigh_mirror.stl" scale="1 1 1"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="silver"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 1.57079632679 0" xyz="0 0 -0.1065"/>
|
||||||
|
<geometry>
|
||||||
|
<box size="0.213 0.0245 0.034"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="-0.003468 0.018947 -0.032736"/>
|
||||||
|
<mass value="0.898919"/>
|
||||||
|
<inertia ixx="0.005395867678" ixy="-1.02809e-07" ixz="0.000337529085" iyy="0.005142451046" iyz="5.816563e-06" izz="0.00102478732"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="RR_calf_joint" type="revolute">
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 -0.213"/>
|
||||||
|
<parent link="RR_thigh"/>
|
||||||
|
<child link="RR_calf"/>
|
||||||
|
<axis xyz="0 1 0"/>
|
||||||
|
<dynamics damping="0.01" friction="0.2"/>
|
||||||
|
<limit effort="33.5" lower="-2.69653369433" upper="-0.916297857297" velocity="21"/>
|
||||||
|
</joint>
|
||||||
|
<link name="RR_calf">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="../meshes/calf.stl" scale="1 1 1"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="black"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 1.57079632679 0" xyz="0 0 -0.1065"/>
|
||||||
|
<geometry>
|
||||||
|
<box size="0.213 0.016 0.016"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="0.006286 0.001307 -0.122269"/>
|
||||||
|
<mass value="0.158015"/>
|
||||||
|
<inertia ixx="0.003607648222" ixy="1.494971e-06" ixz="-0.000132778525" iyy="0.003626771492" iyz="-2.8638535e-05" izz="3.5148003e-05"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="RR_foot_fixed" type="fixed">
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 -0.213"/>
|
||||||
|
<parent link="RR_calf"/>
|
||||||
|
<child link="RR_foot"/>
|
||||||
|
</joint>
|
||||||
|
<link name="RR_foot">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<sphere radius="0.01"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="black"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<sphere radius="0.02"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="0.06"/>
|
||||||
|
<inertia ixx="9.6e-06" ixy="0.0" ixz="0.0" iyy="9.6e-06" iyz="0.0" izz="9.6e-06"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<transmission name="RR_hip_tran">
|
||||||
|
<type>transmission_interface/SimpleTransmission</type>
|
||||||
|
<joint name="RR_hip_joint">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
</joint>
|
||||||
|
<actuator name="RR_hip_motor">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
<mechanicalReduction>1</mechanicalReduction>
|
||||||
|
</actuator>
|
||||||
|
</transmission>
|
||||||
|
<transmission name="RR_thigh_tran">
|
||||||
|
<type>transmission_interface/SimpleTransmission</type>
|
||||||
|
<joint name="RR_thigh_joint">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
</joint>
|
||||||
|
<actuator name="RR_thigh_motor">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
<mechanicalReduction>1</mechanicalReduction>
|
||||||
|
</actuator>
|
||||||
|
</transmission>
|
||||||
|
<transmission name="RR_calf_tran">
|
||||||
|
<type>transmission_interface/SimpleTransmission</type>
|
||||||
|
<joint name="RR_calf_joint">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
</joint>
|
||||||
|
<actuator name="RR_calf_motor">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
<mechanicalReduction>1</mechanicalReduction>
|
||||||
|
</actuator>
|
||||||
|
</transmission>
|
||||||
|
<joint name="RL_hip_joint" type="revolute">
|
||||||
|
<origin rpy="0 0 0" xyz="-0.1881 0.04675 0"/>
|
||||||
|
<parent link="trunk"/>
|
||||||
|
<child link="RL_hip"/>
|
||||||
|
<axis xyz="1 0 0"/>
|
||||||
|
<dynamics damping="0.01" friction="0.2"/>
|
||||||
|
<limit effort="33.5" lower="-0.802851455917" upper="0.802851455917" velocity="21"/>
|
||||||
|
</joint>
|
||||||
|
<link name="RL_hip">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 3.14159265359 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="../meshes/hip.stl" scale="1 1 1"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="silver"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="1.57079632679 0 0" xyz="0 0.045 0"/>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length="0.04" radius="0.046"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="0.00541 -0.00074 6e-06"/>
|
||||||
|
<mass value="0.510299"/>
|
||||||
|
<inertia ixx="0.00030528937" ixy="7.788013e-06" ixz="-2.2016e-07" iyy="0.000590894859" iyz="-1.7175e-08" izz="0.000396594572"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="RL_hip_fixed" type="fixed">
|
||||||
|
<origin rpy="0 0 0" xyz="0 0.08 0"/>
|
||||||
|
<parent link="RL_hip"/>
|
||||||
|
<child link="RL_thigh_shoulder"/>
|
||||||
|
</joint>
|
||||||
|
<!-- this link is only for collision -->
|
||||||
|
<link name="RL_thigh_shoulder">
|
||||||
|
<collision>
|
||||||
|
<origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length="0.032" radius="0.041"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
<joint name="RL_thigh_joint" type="revolute">
|
||||||
|
<origin rpy="0 0 0" xyz="0 0.08 0"/>
|
||||||
|
<parent link="RL_hip"/>
|
||||||
|
<child link="RL_thigh"/>
|
||||||
|
<axis xyz="0 1 0"/>
|
||||||
|
<dynamics damping="0.01" friction="0.2"/>
|
||||||
|
<limit effort="33.5" lower="-1.0471975512" upper="4.18879020479" velocity="21"/>
|
||||||
|
</joint>
|
||||||
|
<link name="RL_thigh">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="../meshes/thigh.stl" scale="1 1 1"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="silver"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 1.57079632679 0" xyz="0 0 -0.1065"/>
|
||||||
|
<geometry>
|
||||||
|
<box size="0.213 0.0245 0.034"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="-0.003468 -0.018947 -0.032736"/>
|
||||||
|
<mass value="0.898919"/>
|
||||||
|
<inertia ixx="0.005395867678" ixy="1.02809e-07" ixz="0.000337529085" iyy="0.005142451046" iyz="-5.816563e-06" izz="0.00102478732"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="RL_calf_joint" type="revolute">
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 -0.213"/>
|
||||||
|
<parent link="RL_thigh"/>
|
||||||
|
<child link="RL_calf"/>
|
||||||
|
<axis xyz="0 1 0"/>
|
||||||
|
<dynamics damping="0.01" friction="0.2"/>
|
||||||
|
<limit effort="33.5" lower="-2.69653369433" upper="-0.916297857297" velocity="21"/>
|
||||||
|
</joint>
|
||||||
|
<link name="RL_calf">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="../meshes/calf.stl" scale="1 1 1"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="black"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 1.57079632679 0" xyz="0 0 -0.1065"/>
|
||||||
|
<geometry>
|
||||||
|
<box size="0.213 0.016 0.016"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="0.006286 0.001307 -0.122269"/>
|
||||||
|
<mass value="0.158015"/>
|
||||||
|
<inertia ixx="0.003607648222" ixy="1.494971e-06" ixz="-0.000132778525" iyy="0.003626771492" iyz="-2.8638535e-05" izz="3.5148003e-05"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="RL_foot_fixed" type="fixed">
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 -0.213"/>
|
||||||
|
<parent link="RL_calf"/>
|
||||||
|
<child link="RL_foot"/>
|
||||||
|
</joint>
|
||||||
|
<link name="RL_foot">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<sphere radius="0.01"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="black"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<sphere radius="0.02"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="0.06"/>
|
||||||
|
<inertia ixx="9.6e-06" ixy="0.0" ixz="0.0" iyy="9.6e-06" iyz="0.0" izz="9.6e-06"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<transmission name="RL_hip_tran">
|
||||||
|
<type>transmission_interface/SimpleTransmission</type>
|
||||||
|
<joint name="RL_hip_joint">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
</joint>
|
||||||
|
<actuator name="RL_hip_motor">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
<mechanicalReduction>1</mechanicalReduction>
|
||||||
|
</actuator>
|
||||||
|
</transmission>
|
||||||
|
<transmission name="RL_thigh_tran">
|
||||||
|
<type>transmission_interface/SimpleTransmission</type>
|
||||||
|
<joint name="RL_thigh_joint">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
</joint>
|
||||||
|
<actuator name="RL_thigh_motor">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
<mechanicalReduction>1</mechanicalReduction>
|
||||||
|
</actuator>
|
||||||
|
</transmission>
|
||||||
|
<transmission name="RL_calf_tran">
|
||||||
|
<type>transmission_interface/SimpleTransmission</type>
|
||||||
|
<joint name="RL_calf_joint">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
</joint>
|
||||||
|
<actuator name="RL_calf_motor">
|
||||||
|
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||||
|
<mechanicalReduction>1</mechanicalReduction>
|
||||||
|
</actuator>
|
||||||
|
</transmission>
|
||||||
|
</robot>
|
||||||
|
|
||||||
195
data/go1/xml/go1.xml
Normal file
195
data/go1/xml/go1.xml
Normal file
@@ -0,0 +1,195 @@
|
|||||||
|
<mujoco model="go1_description">
|
||||||
|
<compiler angle="radian" meshdir="../meshes/" />
|
||||||
|
<size njmax="500" nconmax="100" />
|
||||||
|
<option gravity='0 0 -9.806' iterations='50' solver='Newton' timestep='0.002'/>
|
||||||
|
|
||||||
|
<default>
|
||||||
|
<geom contype="1" conaffinity="1" friction="0.6 0.3 0.3" rgba="0.5 0.6 0.7 1" margin="0.001" group="0"/>
|
||||||
|
|
||||||
|
<light castshadow="false" diffuse="1 1 1"/>
|
||||||
|
<motor ctrlrange="-33.5 33.5" ctrllimited="true"/>
|
||||||
|
<camera fovy="60"/>
|
||||||
|
<joint damping="1.0" armature="0.01" frictionloss="0.5" />
|
||||||
|
|
||||||
|
</default>
|
||||||
|
=
|
||||||
|
<asset>
|
||||||
|
<mesh name="trunk" file="trunk.stl" />
|
||||||
|
<mesh name="hip" file="hip.stl" />
|
||||||
|
<mesh name="thigh_mirror" file="thigh_mirror.stl" />
|
||||||
|
<mesh name="calf" file="calf.stl" />
|
||||||
|
<mesh name="thigh" file="thigh.stl" />
|
||||||
|
</asset>
|
||||||
|
|
||||||
|
<asset>
|
||||||
|
<texture type="skybox" builtin="gradient" rgb1="1.0 1.0 1.0" rgb2="1.0 1.0 1.0" width="512" height="512"/>
|
||||||
|
<texture name="plane" type="2d" builtin="flat" rgb1="1 1 1" rgb2="1 1 1" width="512" height="512" mark="cross" markrgb="0 0 0"/>
|
||||||
|
<material name="plane" reflectance="0.0" texture="plane" texrepeat="3 3" texuniform="true"/>
|
||||||
|
</asset>
|
||||||
|
|
||||||
|
<visual>
|
||||||
|
<rgba com="0.502 1.0 0 0.5" contactforce="0.98 0.4 0.4 0.7" contactpoint="1.0 1.0 0.6 0.4"/>
|
||||||
|
<scale com="0.2" forcewidth="0.035" contactwidth="0.10" contactheight="0.04"/>
|
||||||
|
</visual>
|
||||||
|
|
||||||
|
<worldbody>
|
||||||
|
<light directional="true" diffuse=".8 .8 .8" pos="0 0 10" dir="0 0 -10"/>
|
||||||
|
<camera name="track" mode="trackcom" pos="0 -1.3 1.6" xyaxes="1 0 0 0 0.707 0.707"/>
|
||||||
|
<geom name='floor' type='plane' conaffinity='1' condim='3' contype='1' rgba="0.5 0.9 0.9 0.1" material='plane' pos='0 0 0' size='0 0 1'/>
|
||||||
|
|
||||||
|
<body name="trunk" pos="0 0 0.35">
|
||||||
|
<inertial pos="0.0116053 0.00442221 0.000106692" quat="0.0111438 0.707126 -0.00935374 0.706938" mass="4.801" diaginertia="0.0447997 0.0366257 0.0162187" />
|
||||||
|
<joint type="free" />
|
||||||
|
<geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.913725 0.913725 0.847059 1" mesh="trunk" />
|
||||||
|
<geom size="0.13 0.04675 0.057" type="box" rgba="0.913725 0.913725 0.847059 1" />
|
||||||
|
<geom size="0.0005 0.0005 0.0005" pos="-0.01592 -0.06659 -0.00617" type="box" contype="0" conaffinity="0" group="1" rgba="0.8 0 0 0" />
|
||||||
|
<geom size="0.0005 0.0005 0.0005" pos="-0.01592 -0.06659 -0.00617" type="box" rgba="0.8 0 0 0" />
|
||||||
|
<site name="imu" pos="0 0 0"/>
|
||||||
|
|
||||||
|
<body name="FR_hip" pos="0.1881 -0.04675 0">
|
||||||
|
<inertial pos="-0.00406411 -0.0193463 4.50733e-06" quat="0.467526 0.531662 -0.466259 0.530431" mass="0.679292" diaginertia="0.00131334 0.00122648 0.000728484" />
|
||||||
|
<joint name="FR_hip_joint" pos="0 0 0" axis="1 0 0" limited="true" range="-0.802851 0.802851" />
|
||||||
|
<geom quat="0 1 0 0" type="mesh" contype="0" conaffinity="0" group="1" rgba="0.913725 0.913725 0.847059 1" mesh="hip" />
|
||||||
|
<geom size="0.046 0.02" pos="0 -0.045 0" quat="0.707107 0.707107 0 0" type="cylinder" rgba="0.913725 0.913725 0.847059 0" />
|
||||||
|
<geom size="0.031 0.02" pos="0 -0.07 0" quat="0.707107 0.707107 0 0" type="cylinder" rgba="0.913725 0.913725 0.847059 0" />
|
||||||
|
|
||||||
|
<body name="FR_thigh" pos="0 -0.08 0">
|
||||||
|
<inertial pos="-0.003468 0.018947 -0.032736" quat="0.999266 0.00067676 -0.0382978 0.000639813" mass="0.898919" diaginertia="0.00542178 0.00514246 0.000998869" />
|
||||||
|
<joint name="FR_thigh_joint" pos="0 0 0" axis="0 1 0" limited="true" range="-1.0472 4.18879" />
|
||||||
|
<geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.913725 0.913725 0.847059 1" mesh="thigh_mirror" />
|
||||||
|
<geom size="0.1065 0.01225 0.017" pos="0 0 -0.1065" quat="0.707107 0 0.707107 0" type="box" rgba="0.913725 0.913725 0.847059 0" />
|
||||||
|
<body name="FR_calf" pos="0 0 -0.213">
|
||||||
|
<inertial pos="0.00455603 0.0009473 -0.147239" quat="0.762045 0.00970173 0.0180098 0.647201" mass="0.218015" diaginertia="0.00399678 0.00398122 3.99428e-05" />
|
||||||
|
<joint name="FR_calf_joint" pos="0 0 0" axis="0 1 0" limited="true" range="-2.69653 -0.916298" />
|
||||||
|
<geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0 0 0 1" mesh="calf" />
|
||||||
|
<geom size="0.1065 0.008 0.008" pos="0 0 -0.1065" quat="0.707107 0 0.707107 0" type="box" rgba="0 0 0 0" />
|
||||||
|
<geom size="0.01" pos="0 0 -0.213" contype="0" conaffinity="0" group="1" rgba="0 0 0 1" />
|
||||||
|
<geom size="0.02" pos="0 0 -0.213" rgba="0 0 0 1" />
|
||||||
|
</body>
|
||||||
|
</body>
|
||||||
|
</body>
|
||||||
|
<body name="FL_hip" pos="0.1881 0.04675 0">
|
||||||
|
<inertial pos="-0.00406411 0.0193463 4.50733e-06" quat="0.531662 0.467526 -0.530431 0.466259" mass="0.679292" diaginertia="0.00131334 0.00122648 0.000728484" />
|
||||||
|
<joint name="FL_hip_joint" pos="0 0 0" axis="1 0 0" limited="true" range="-0.802851 0.802851" />
|
||||||
|
<geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.913725 0.913725 0.847059 1" mesh="hip" />
|
||||||
|
<geom size="0.046 0.02" pos="0 0.045 0" quat="0.707107 0.707107 0 0" type="cylinder" rgba="0.913725 0.913725 0.847059 0" />
|
||||||
|
<geom size="0.031 0.02" pos="0 0.07 0" quat="0.707107 0.707107 0 0" type="cylinder" rgba="0.913725 0.913725 0.847059 0" />
|
||||||
|
|
||||||
|
<body name="FL_thigh" pos="0 0.08 0">
|
||||||
|
<inertial pos="-0.003468 -0.018947 -0.032736" quat="0.999266 -0.00067676 -0.0382978 -0.000639813" mass="0.898919" diaginertia="0.00542178 0.00514246 0.000998869" />
|
||||||
|
<joint name="FL_thigh_joint" pos="0 0 0" axis="0 1 0" limited="true" range="-1.0472 4.18879" />
|
||||||
|
<geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.913725 0.913725 0.847059 1" mesh="thigh" />
|
||||||
|
<geom size="0.1065 0.01225 0.017" pos="0 0 -0.1065" quat="0.707107 0 0.707107 0" type="box" rgba="0.913725 0.913725 0.847059 0" />
|
||||||
|
<body name="FL_calf" pos="0 0 -0.213">
|
||||||
|
<inertial pos="0.00455603 0.0009473 -0.147239" quat="0.762045 0.00970173 0.0180098 0.647201" mass="0.218015" diaginertia="0.00399678 0.00398122 3.99428e-05" />
|
||||||
|
<joint name="FL_calf_joint" pos="0 0 0" axis="0 1 0" limited="true" range="-2.69653 -0.916298" />
|
||||||
|
<geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0 0 0 1" mesh="calf" />
|
||||||
|
<geom size="0.1065 0.008 0.008" pos="0 0 -0.1065" quat="0.707107 0 0.707107 0" type="box" rgba="0.913725 0.913725 0.847059 0" />
|
||||||
|
<geom size="0.01" pos="0 0 -0.213" contype="0" conaffinity="0" group="1" rgba="0 0 0 1" />
|
||||||
|
<geom size="0.02" pos="0 0 -0.213" rgba="0 0 0 1" />
|
||||||
|
</body>
|
||||||
|
</body>
|
||||||
|
</body>
|
||||||
|
<body name="RR_hip" pos="-0.1881 -0.04675 0">
|
||||||
|
<inertial pos="0.00406411 -0.0193463 4.50733e-06" quat="0.530431 0.466259 -0.531662 0.467526" mass="0.679292" diaginertia="0.00131334 0.00122648 0.000728484" />
|
||||||
|
<joint name="RR_hip_joint" pos="0 0 0" axis="1 0 0" limited="true" range="-0.802851 0.802851" />
|
||||||
|
<geom quat="0 0 0 -1" type="mesh" contype="0" conaffinity="0" group="1" rgba="0.913725 0.913725 0.847059 1" mesh="hip" />
|
||||||
|
<geom size="0.046 0.02" quat="0.707107 0.707107 0 0" type="cylinder" rgba="0.913725 0.913725 0.847059 1" />
|
||||||
|
<geom size="0.046 0.02" pos="0 -0.045 0" quat="0.707107 0.707107 0 0" type="cylinder" rgba="0.913725 0.913725 0.847059 0" />
|
||||||
|
<geom size="0.031 0.02" pos="0 -0.07 0" quat="0.707107 0.707107 0 0" type="cylinder" rgba="0.913725 0.913725 0.847059 0" />
|
||||||
|
|
||||||
|
<body name="RR_thigh" pos="0 -0.08 0">
|
||||||
|
<inertial pos="-0.003468 0.018947 -0.032736" quat="0.999266 0.00067676 -0.0382978 0.000639813" mass="0.898919" diaginertia="0.00542178 0.00514246 0.000998869" />
|
||||||
|
<joint name="RR_thigh_joint" pos="0 0 0" axis="0 1 0" limited="true" range="-1.0472 4.18879" />
|
||||||
|
<geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.913725 0.913725 0.847059 1" mesh="thigh_mirror" />
|
||||||
|
<geom size="0.1065 0.01225 0.017" pos="0 0 -0.1065" quat="0.707107 0 0.707107 0" type="box" rgba="0.913725 0.913725 0.847059 0" />
|
||||||
|
<body name="RR_calf" pos="0 0 -0.213">
|
||||||
|
<inertial pos="0.00455603 0.0009473 -0.147239" quat="0.762045 0.00970173 0.0180098 0.647201" mass="0.218015" diaginertia="0.00399678 0.00398122 3.99428e-05" />
|
||||||
|
<joint name="RR_calf_joint" pos="0 0 0" axis="0 1 0" limited="true" range="-2.69653 -0.916298" />
|
||||||
|
<geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0 0 0 1" mesh="calf" />
|
||||||
|
<geom size="0.1065 0.008 0.008" pos="0 0 -0.1065" quat="0.707107 0 0.707107 0" type="box" rgba="0 0 0 0" />
|
||||||
|
<geom size="0.01" pos="0 0 -0.213" contype="0" conaffinity="0" group="1" rgba="0 0 0 1" />
|
||||||
|
<geom size="0.02" pos="0 0 -0.213" rgba="0 0 0 1" />
|
||||||
|
</body>
|
||||||
|
</body>
|
||||||
|
</body>
|
||||||
|
<body name="RL_hip" pos="-0.1881 0.04675 0">
|
||||||
|
<inertial pos="0.00406411 0.0193463 4.50733e-06" quat="0.466259 0.530431 -0.467526 0.531662" mass="0.679292" diaginertia="0.00131334 0.00122648 0.000728484" />
|
||||||
|
<joint name="RL_hip_joint" pos="0 0 0" axis="1 0 0" limited="true" range="-0.802851 0.802851" />
|
||||||
|
<geom quat="0 0 1 0" type="mesh" contype="0" conaffinity="0" group="1" rgba="0.913725 0.913725 0.847059 1" mesh="hip" />
|
||||||
|
<geom size="0.046 0.02" quat="0.707107 0.707107 0 0" type="cylinder" rgba="0.913725 0.913725 0.847059 1" />
|
||||||
|
<geom size="0.046 0.02" pos="0 0.045 0" quat="0.707107 0.707107 0 0" type="cylinder" rgba="0.913725 0.913725 0.847059 0" />
|
||||||
|
<geom size="0.031 0.02" pos="0 0.07 0" quat="0.707107 0.707107 0 0" type="cylinder" rgba="0.913725 0.913725 0.847059 0" />
|
||||||
|
|
||||||
|
<body name="RL_thigh" pos="0 0.08 0">
|
||||||
|
<inertial pos="-0.003468 -0.018947 -0.032736" quat="0.999266 -0.00067676 -0.0382978 -0.000639813" mass="0.898919" diaginertia="0.00542178 0.00514246 0.000998869" />
|
||||||
|
<joint name="RL_thigh_joint" pos="0 0 0" axis="0 1 0" limited="true" range="-1.0472 4.18879" />
|
||||||
|
<geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.913725 0.913725 0.847059 1" mesh="thigh" />
|
||||||
|
<geom size="0.1065 0.01225 0.017" pos="0 0 -0.1065" quat="0.707107 0 0.707107 0" type="box" rgba="0.913725 0.913725 0.847059 0" />
|
||||||
|
<body name="RL_calf" pos="0 0 -0.213">
|
||||||
|
<inertial pos="0.00455603 0.0009473 -0.147239" quat="0.762045 0.00970173 0.0180098 0.647201" mass="0.218015" diaginertia="0.00399678 0.00398122 3.99428e-05" />
|
||||||
|
<joint name="RL_calf_joint" pos="0 0 0" axis="0 1 0" limited="true" range="-2.69653 -0.916298" />
|
||||||
|
<geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0 0 0 1" mesh="calf" />
|
||||||
|
<geom size="0.1065 0.008 0.008" pos="0 0 -0.1065" quat="0.707107 0 0.707107 0" type="box" rgba="0 0 0 0" />
|
||||||
|
<geom size="0.01" pos="0 0 -0.213" contype="0" conaffinity="0" group="1" rgba="0 0 0 1" />
|
||||||
|
<geom size="0.02" pos="0 0 -0.213" rgba="0 0 0 1" />
|
||||||
|
</body>
|
||||||
|
</body>
|
||||||
|
</body>
|
||||||
|
</body>
|
||||||
|
</worldbody>
|
||||||
|
<actuator>
|
||||||
|
|
||||||
|
<motor name="FR_hip" gear="1" joint="FR_hip_joint"/>
|
||||||
|
<motor name="FR_thigh" gear="1" joint="FR_thigh_joint"/>
|
||||||
|
<motor name="FR_calf" gear="1" joint="FR_calf_joint"/>
|
||||||
|
<motor name="FL_hip" gear="1" joint="FL_hip_joint"/>
|
||||||
|
<motor name="FL_thigh" gear="1" joint="FL_thigh_joint"/>
|
||||||
|
<motor name="FL_calf" gear="1" joint="FL_calf_joint"/>
|
||||||
|
<motor name="RR_hip" gear="1" joint="RR_hip_joint"/>
|
||||||
|
<motor name="RR_thigh" gear="1" joint="RR_thigh_joint"/>
|
||||||
|
<motor name="RR_calf" gear="1" joint="RR_calf_joint" />
|
||||||
|
<motor name="RL_hip" gear="1" joint="RL_hip_joint"/>
|
||||||
|
<motor name="RL_thigh" gear="1" joint="RL_thigh_joint"/>
|
||||||
|
<motor name="RL_calf" gear="1" joint="RL_calf_joint"/>
|
||||||
|
</actuator>
|
||||||
|
|
||||||
|
<sensor>
|
||||||
|
|
||||||
|
<jointpos name="FR_hip_pos" joint="FR_hip_joint"/>
|
||||||
|
<jointpos name="FR_thigh_pos" joint="FR_thigh_joint"/>
|
||||||
|
<jointpos name="FR_calf_pos" joint="FR_calf_joint"/>
|
||||||
|
<jointpos name="FL_hip_pos" joint="FL_hip_joint"/>
|
||||||
|
<jointpos name="FL_thigh_pos" joint="FL_thigh_joint"/>
|
||||||
|
<jointpos name="FL_calf_pos" joint="FL_calf_joint"/>
|
||||||
|
<jointpos name="RR_hip_pos" joint="RR_hip_joint"/>
|
||||||
|
<jointpos name="RR_thigh_pos" joint="RR_thigh_joint"/>
|
||||||
|
<jointpos name="RR_calf_pos" joint="RR_calf_joint" />
|
||||||
|
<jointpos name="RL_hip_pos" joint="RL_hip_joint"/>
|
||||||
|
<jointpos name="RL_thigh_pos" joint="RL_thigh_joint"/>
|
||||||
|
<jointpos name="RL_calf_pos" joint="RL_calf_joint"/>
|
||||||
|
|
||||||
|
<jointvel name="FR_hip_vel" joint="FR_hip_joint"/>
|
||||||
|
<jointvel name="FR_thigh_vel" joint="FR_thigh_joint"/>
|
||||||
|
<jointvel name="FR_calf_vel" joint="FR_calf_joint"/>
|
||||||
|
<jointvel name="FL_hip_vel" joint="FL_hip_joint"/>
|
||||||
|
<jointvel name="FL_thigh_vel" joint="FL_thigh_joint"/>
|
||||||
|
<jointvel name="FL_calf_vel" joint="FL_calf_joint"/>
|
||||||
|
<jointvel name="RR_hip_vel" joint="RR_hip_joint"/>
|
||||||
|
<jointvel name="RR_thigh_vel" joint="RR_thigh_joint"/>
|
||||||
|
<jointvel name="RR_calf_vel" joint="RR_calf_joint" />
|
||||||
|
<jointvel name="RL_hip_vel" joint="RL_hip_joint"/>
|
||||||
|
<jointvel name="RL_thigh_vel" joint="RL_thigh_joint"/>
|
||||||
|
<jointvel name="RL_calf_vel" joint="RL_calf_joint"/>
|
||||||
|
|
||||||
|
<accelerometer name="Body_Acc" site="imu"/>
|
||||||
|
|
||||||
|
<gyro name="Body_Gyro" site="imu"/>
|
||||||
|
|
||||||
|
<framepos name="Body_Pos" objtype="site" objname="imu"/>
|
||||||
|
|
||||||
|
<framequat name="Body_Quat" objtype="site" objname="imu"/>
|
||||||
|
|
||||||
|
|
||||||
|
</sensor>
|
||||||
|
</mujoco>
|
||||||
1
data/go1/xml/terrain/plane.xml
Normal file
1
data/go1/xml/terrain/plane.xml
Normal file
@@ -0,0 +1 @@
|
|||||||
|
<geom name='floor' type='plane' conaffinity='1' condim='3' contype='1' rgba="0.5 0.9 0.9 0.1" material='plane' pos='0 0 0' size='0 0 1' friction="0.6 0.3 0.3"/>
|
||||||
12
data/go1/xml/terrain/rough.xml
Normal file
12
data/go1/xml/terrain/rough.xml
Normal file
@@ -0,0 +1,12 @@
|
|||||||
|
<!-- 模拟碎地形 -->
|
||||||
|
<geom name='floor' type='plane' conaffinity='1' condim='3' contype='1' rgba="0.4 0.6 0.3 1" pos='0 0 0' size='0 0 1' friction="0.6 0.3 0.3"/>
|
||||||
|
<geom name='r1' type='box' size='0.15 0.1 0.03' pos='0.5 0.2 0' rgba="0.5 0.4 0.3 1" friction="0.8 0.3 0.3"/>
|
||||||
|
<geom name='r2' type='box' size='0.1 0.15 0.05' pos='0.8 -0.1 0' rgba="0.5 0.4 0.3 1" friction="0.8 0.3 0.3"/>
|
||||||
|
<geom name='r3' type='box' size='0.12 0.08 0.04' pos='1.1 0.15 0' rgba="0.5 0.4 0.3 1" friction="0.8 0.3 0.3"/>
|
||||||
|
<geom name='r4' type='box' size='0.08 0.12 0.06' pos='1.4 -0.2 0' rgba="0.5 0.4 0.3 1" friction="0.8 0.3 0.3"/>
|
||||||
|
<geom name='r5' type='box' size='0.1 0.1 0.035' pos='1.7 0.1 0' rgba="0.5 0.4 0.3 1" friction="0.8 0.3 0.3"/>
|
||||||
|
<geom name='r6' type='box' size='0.14 0.09 0.045' pos='2.0 -0.05 0' rgba="0.5 0.4 0.3 1" friction="0.8 0.3 0.3"/>
|
||||||
|
<geom name='r7' type='box' size='0.09 0.14 0.055' pos='0.3 0.25 0' rgba="0.5 0.4 0.3 1" friction="0.8 0.3 0.3"/>
|
||||||
|
<geom name='r8' type='box' size='0.11 0.07 0.025' pos='0.6 -0.15 0' rgba="0.5 0.4 0.3 1" friction="0.8 0.3 0.3"/>
|
||||||
|
<geom name='r9' type='box' size='0.07 0.11 0.05' pos='0.9 0.05 0' rgba="0.5 0.4 0.3 1" friction="0.8 0.3 0.3"/>
|
||||||
|
<geom name='r10' type='box' size='0.13 0.08 0.04' pos='1.2 -0.1 0' rgba="0.5 0.4 0.3 1" friction="0.8 0.3 0.3"/>
|
||||||
6
data/go1/xml/terrain/slope.xml
Normal file
6
data/go1/xml/terrain/slope.xml
Normal file
@@ -0,0 +1,6 @@
|
|||||||
|
<!-- 斜坡地形-->
|
||||||
|
<geom name='start_platform' type='box' size='10.0 10.0 0.1' pos='0 0 -0.06' rgba="0.5 0.6 0.4 1" friction="0.8 0.3 0.3"/>
|
||||||
|
<geom name='start_fill' type='box' size='10.0 10.0 0.2' pos='0 0 -0.26' rgba="0.4 0.35 0.3 1" friction="0.8 0.3 0.3"/>
|
||||||
|
<geom name='slope' type='box' size='2.5 0.3 0.1' pos='6.8 0 0.10' quat='0.258819 0 0 0.965926' rgba="0.5 0.6 0.4 1" friction="0.8 0.3 0.3"/>
|
||||||
|
<geom name='top_platform' type='box' size='1.0 0.3 0.06' pos='8.8 0 0.16' rgba="0.5 0.6 0.4 1" friction="0.8 0.3 0.3"/>
|
||||||
|
<geom name='slope_fill' type='box' size='2.5 0.3 0.25' pos='6.8 0 -0.15' rgba="0.4 0.35 0.3 1" friction="0.8 0.3 0.3"/>
|
||||||
13
data/go1/xml/terrain/stairs.xml
Normal file
13
data/go1/xml/terrain/stairs.xml
Normal file
@@ -0,0 +1,13 @@
|
|||||||
|
<!-- 楼梯地形 -->
|
||||||
|
<geom name='step0' type='box' size='0.5 10.4 0.06' pos='0 0 -0.06' rgba="0.6 0.5 0.4 1" friction="0.8 0.3 0.3"/>
|
||||||
|
<geom name='step1' type='box' size='0.5 10.4 0.06' pos='0.3 0 -0.06' rgba="0.6 0.5 0.4 1" friction="0.8 0.3 0.3"/>
|
||||||
|
<geom name='step2' type='box' size='0.5 10.4 0.06' pos='0.6 0 0.00' rgba="0.6 0.5 0.4 1" friction="0.8 0.3 0.3"/>
|
||||||
|
<geom name='step3' type='box' size='0.5 10.4 0.06' pos='0.9 0 0.06' rgba="0.6 0.5 0.4 1" friction="0.8 0.3 0.3"/>
|
||||||
|
<geom name='step4' type='box' size='0.5 10.4 0.06' pos='1.2 0 0.12' rgba="0.6 0.5 0.4 1" friction="0.8 0.3 0.3"/>
|
||||||
|
<geom name='step5' type='box' size='0.5 10.4 0.06' pos='1.5 0 0.18' rgba="0.6 0.5 0.4 1" friction="0.8 0.3 0.3"/>
|
||||||
|
<geom name='step6' type='box' size='0.5 10.4 0.06' pos='1.8 0 0.24' rgba="0.6 0.5 0.4 1" friction="0.8 0.3 0.3"/>
|
||||||
|
<geom name='step7' type='box' size='0.5 10.4 0.06' pos='2.1 0 0.30' rgba="0.6 0.5 0.4 1" friction="0.8 0.3 0.3"/>
|
||||||
|
<geom name='step8' type='box' size='0.5 10.4 0.06' pos='2.4 0 0.36' rgba="0.6 0.5 0.4 1" friction="0.8 0.3 0.3"/>
|
||||||
|
<geom name='step9' type='box' size='0.5 10.4 0.06' pos='2.7 0 0.42' rgba="0.6 0.5 0.4 1" friction="0.8 0.3 0.3"/>
|
||||||
|
<geom name='platform' type='box' size='0.8 10.4 0.06' pos='3.3 0 0.45' rgba="0.6 0.5 0.4 1" friction="0.8 0.3 0.3"/>
|
||||||
|
<geom name='fill' type='box' size='2.0 10.4 0.45' pos='1.5 0 -0.45' rgba="0.5 0.4 0.3 1" friction="0.8 0.3 0.3"/>
|
||||||
BIN
data/laikago/meshes/calf.stl
Normal file
BIN
data/laikago/meshes/calf.stl
Normal file
Binary file not shown.
BIN
data/laikago/meshes/hip.stl
Normal file
BIN
data/laikago/meshes/hip.stl
Normal file
Binary file not shown.
BIN
data/laikago/meshes/thigh.stl
Normal file
BIN
data/laikago/meshes/thigh.stl
Normal file
Binary file not shown.
BIN
data/laikago/meshes/thigh_mirror.stl
Normal file
BIN
data/laikago/meshes/thigh_mirror.stl
Normal file
Binary file not shown.
BIN
data/laikago/meshes/trunk.stl
Normal file
BIN
data/laikago/meshes/trunk.stl
Normal file
Binary file not shown.
408
data/laikago/urdf/laikago.urdf
Normal file
408
data/laikago/urdf/laikago.urdf
Normal file
@@ -0,0 +1,408 @@
|
|||||||
|
<?xml version="1.0" ?>
|
||||||
|
<!-- =================================================================================== -->
|
||||||
|
<!-- | This document was autogenerated by xacro from robot.xacro | -->
|
||||||
|
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
|
||||||
|
<!-- =================================================================================== -->
|
||||||
|
<robot name="laikago_description" xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||||
|
|
||||||
|
<mujoco>
|
||||||
|
<compiler
|
||||||
|
meshdir="../meshes/"
|
||||||
|
balanceinertia="true"
|
||||||
|
discardvisual="false" />
|
||||||
|
</mujoco>
|
||||||
|
|
||||||
|
<material name="black">
|
||||||
|
<color rgba="0.0 0.0 0.0 1.0"/>
|
||||||
|
</material>
|
||||||
|
<material name="blue">
|
||||||
|
<color rgba="0.0 0.0 0.8 1.0"/>
|
||||||
|
</material>
|
||||||
|
<material name="green">
|
||||||
|
<color rgba="0.0 0.8 0.0 1.0"/>
|
||||||
|
</material>
|
||||||
|
<material name="grey">
|
||||||
|
<color rgba="0.2 0.2 0.2 1.0"/>
|
||||||
|
</material>
|
||||||
|
<material name="silver">
|
||||||
|
<color rgba="0.913725490196 0.913725490196 0.847058823529 1.0"/>
|
||||||
|
</material>
|
||||||
|
<material name="orange">
|
||||||
|
<color rgba="1.0 0.423529411765 0.0392156862745 1.0"/>
|
||||||
|
</material>
|
||||||
|
<material name="brown">
|
||||||
|
<color rgba="0.870588235294 0.811764705882 0.764705882353 1.0"/>
|
||||||
|
</material>
|
||||||
|
<material name="red">
|
||||||
|
<color rgba="0.8 0.0 0.0 1.0"/>
|
||||||
|
</material>
|
||||||
|
<material name="white">
|
||||||
|
<color rgba="1.0 1.0 1.0 1.0"/>
|
||||||
|
</material>
|
||||||
|
|
||||||
|
<link name="world"/>
|
||||||
|
<joint name="floating_base" type="floating">
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<parent link="world"/>
|
||||||
|
<child link="trunk"/>
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<link name="trunk">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="../meshes/trunk.stl" scale="1 1 1"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="grey"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0.01675"/>
|
||||||
|
<geometry>
|
||||||
|
<box size="0.5616 0.172 0.1875"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="0.002284 -4.1e-05 0.025165"/>
|
||||||
|
<mass value="13.733"/>
|
||||||
|
<inertia ixx="0.073348887" ixy="0.00030338" ixz="0.001918218" iyy="0.250684593" iyz="-7.5402e-05" izz="0.254469458"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="FR_hip_joint" type="revolute">
|
||||||
|
<origin rpy="0 0 0" xyz="0.21935 -0.0875 0"/>
|
||||||
|
<parent link="trunk"/>
|
||||||
|
<child link="FR_hip"/>
|
||||||
|
<axis xyz="1 0 0"/>
|
||||||
|
<dynamics damping="0" friction="0"/>
|
||||||
|
<limit effort="20" lower="-1.0471975512" upper="0.872664625997" velocity="52.4"/>
|
||||||
|
</joint>
|
||||||
|
<link name="FR_hip">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="3.14159265359 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="../meshes/hip.stl" scale="1 1 1"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="grey"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="1.57079632679 0 0" xyz="0 0.021 0"/>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length="0.08" radius="0.041"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="-0.001568 0.008134 0.000864"/>
|
||||||
|
<mass value="1.096"/>
|
||||||
|
<inertia ixx="0.000822113" ixy="4.982e-06" ixz="-3.672e-05" iyy="0.000983196" iyz="-2.811e-06" izz="0.000864753"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="FR_thigh_joint" type="revolute">
|
||||||
|
<origin rpy="0 0 0" xyz="0 -0.037 0"/>
|
||||||
|
<parent link="FR_hip"/>
|
||||||
|
<child link="FR_thigh"/>
|
||||||
|
<axis xyz="0 1 0"/>
|
||||||
|
<dynamics damping="0" friction="0"/>
|
||||||
|
<limit effort="55" lower="-0.523598775598" upper="3.92699081699" velocity="28.6"/>
|
||||||
|
</joint>
|
||||||
|
<link name="FR_thigh">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="../meshes/thigh_mirror.stl" scale="1 1 1"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="grey"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 1.57079632679 0" xyz="0 0 -0.125"/>
|
||||||
|
<geometry>
|
||||||
|
<box size="0.25 0.034 0.043"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="-0.000482 -0.02001 -0.031996"/>
|
||||||
|
<mass value="1.528"/>
|
||||||
|
<inertia ixx="0.00991611" ixy="-1.0388e-05" ixz="0.000250428" iyy="0.009280083" iyz="8.511e-05" izz="0.00178256"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="FR_calf_joint" type="revolute">
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 -0.25"/>
|
||||||
|
<parent link="FR_thigh"/>
|
||||||
|
<child link="FR_calf"/>
|
||||||
|
<axis xyz="0 1 0"/>
|
||||||
|
<dynamics damping="0" friction="0"/>
|
||||||
|
<limit effort="55" lower="-2.77507351067" upper="-0.610865238198" velocity="28.6"/>
|
||||||
|
</joint>
|
||||||
|
<link name="FR_calf">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="../meshes/calf.stl" scale="1 1 1"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="silver"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 1.57079632679 0" xyz="0 0 -0.125"/>
|
||||||
|
<geometry>
|
||||||
|
<box size="0.25 0.016 0.016"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="-0.002196 -0.000381 -0.12338"/>
|
||||||
|
<mass value="0.241"/>
|
||||||
|
<inertia ixx="0.006181961" ixy="2.37e-07" ixz="-2.985e-06" iyy="0.006196546" iyz="5.138e-06" izz="3.4774e-05"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="FL_hip_joint" type="revolute">
|
||||||
|
<origin rpy="0 0 0" xyz="0.21935 0.0875 0"/>
|
||||||
|
<parent link="trunk"/>
|
||||||
|
<child link="FL_hip"/>
|
||||||
|
<axis xyz="1 0 0"/>
|
||||||
|
<dynamics damping="0" friction="0"/>
|
||||||
|
<limit effort="20" lower="-0.872664625997" upper="1.0471975512" velocity="52.4"/>
|
||||||
|
</joint>
|
||||||
|
<link name="FL_hip">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="../meshes/hip.stl" scale="1 1 1"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="grey"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="1.57079632679 0 0" xyz="0 -0.021 0"/>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length="0.08" radius="0.041"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="-0.001568 -0.008134 0.000864"/>
|
||||||
|
<mass value="1.096"/>
|
||||||
|
<inertia ixx="0.000822113" ixy="-4.982e-06" ixz="-3.672e-05" iyy="0.000983196" iyz="2.811e-06" izz="0.000864753"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="FL_thigh_joint" type="revolute">
|
||||||
|
<origin rpy="0 0 0" xyz="0 0.037 0"/>
|
||||||
|
<parent link="FL_hip"/>
|
||||||
|
<child link="FL_thigh"/>
|
||||||
|
<axis xyz="0 1 0"/>
|
||||||
|
<dynamics damping="0" friction="0"/>
|
||||||
|
<limit effort="55" lower="-0.523598775598" upper="3.92699081699" velocity="28.6"/>
|
||||||
|
</joint>
|
||||||
|
<link name="FL_thigh">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="../meshes/thigh.stl" scale="1 1 1"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="grey"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 1.57079632679 0" xyz="0 0 -0.125"/>
|
||||||
|
<geometry>
|
||||||
|
<box size="0.25 0.034 0.043"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="-0.000482 0.02001 -0.031996"/>
|
||||||
|
<mass value="1.528"/>
|
||||||
|
<inertia ixx="0.00991611" ixy="1.0388e-05" ixz="0.000250428" iyy="0.009280083" iyz="-8.511e-05" izz="0.00178256"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="FL_calf_joint" type="revolute">
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 -0.25"/>
|
||||||
|
<parent link="FL_thigh"/>
|
||||||
|
<child link="FL_calf"/>
|
||||||
|
<axis xyz="0 1 0"/>
|
||||||
|
<dynamics damping="0" friction="0"/>
|
||||||
|
<limit effort="55" lower="-2.77507351067" upper="-0.610865238198" velocity="28.6"/>
|
||||||
|
</joint>
|
||||||
|
<link name="FL_calf">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="../meshes/calf.stl" scale="1 1 1"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="silver"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 1.57079632679 0" xyz="0 0 -0.125"/>
|
||||||
|
<geometry>
|
||||||
|
<box size="0.25 0.016 0.016"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="-0.002196 -0.000381 -0.12338"/>
|
||||||
|
<mass value="0.241"/>
|
||||||
|
<inertia ixx="0.006181961" ixy="2.37e-07" ixz="-2.985e-06" iyy="0.006196546" iyz="5.138e-06" izz="3.4774e-05"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="RR_hip_joint" type="revolute">
|
||||||
|
<origin rpy="0 0 0" xyz="-0.21935 -0.0875 0"/>
|
||||||
|
<parent link="trunk"/>
|
||||||
|
<child link="RR_hip"/>
|
||||||
|
<axis xyz="1 0 0"/>
|
||||||
|
<dynamics damping="0" friction="0"/>
|
||||||
|
<limit effort="20" lower="-1.0471975512" upper="0.872664625997" velocity="52.4"/>
|
||||||
|
</joint>
|
||||||
|
<link name="RR_hip">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="3.14159265359 3.14159265359 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="../meshes/hip.stl" scale="1 1 1"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="grey"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="1.57079632679 0 0" xyz="0 0.021 0"/>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length="0.08" radius="0.041"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="0.001568 0.008134 0.000864"/>
|
||||||
|
<mass value="1.096"/>
|
||||||
|
<inertia ixx="0.000822113" ixy="-4.982e-06" ixz="3.672e-05" iyy="0.000983196" iyz="-2.811e-06" izz="0.000864753"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="RR_thigh_joint" type="revolute">
|
||||||
|
<origin rpy="0 0 0" xyz="0 -0.037 0"/>
|
||||||
|
<parent link="RR_hip"/>
|
||||||
|
<child link="RR_thigh"/>
|
||||||
|
<axis xyz="0 1 0"/>
|
||||||
|
<dynamics damping="0" friction="0"/>
|
||||||
|
<limit effort="55" lower="-0.523598775598" upper="3.92699081699" velocity="28.6"/>
|
||||||
|
</joint>
|
||||||
|
<link name="RR_thigh">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="../meshes/thigh_mirror.stl" scale="1 1 1"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="grey"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 1.57079632679 0" xyz="0 0 -0.125"/>
|
||||||
|
<geometry>
|
||||||
|
<box size="0.25 0.034 0.043"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="-0.000482 -0.02001 -0.031996"/>
|
||||||
|
<mass value="1.528"/>
|
||||||
|
<inertia ixx="0.00991611" ixy="-1.0388e-05" ixz="0.000250428" iyy="0.009280083" iyz="8.511e-05" izz="0.00178256"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="RR_calf_joint" type="revolute">
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 -0.25"/>
|
||||||
|
<parent link="RR_thigh"/>
|
||||||
|
<child link="RR_calf"/>
|
||||||
|
<axis xyz="0 1 0"/>
|
||||||
|
<dynamics damping="0" friction="0"/>
|
||||||
|
<limit effort="55" lower="-2.77507351067" upper="-0.610865238198" velocity="28.6"/>
|
||||||
|
</joint>
|
||||||
|
<link name="RR_calf">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="../meshes/calf.stl" scale="1 1 1"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="silver"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 1.57079632679 0" xyz="0 0 -0.125"/>
|
||||||
|
<geometry>
|
||||||
|
<box size="0.25 0.016 0.016"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="-0.002196 -0.000381 -0.12338"/>
|
||||||
|
<mass value="0.241"/>
|
||||||
|
<inertia ixx="0.006181961" ixy="2.37e-07" ixz="-2.985e-06" iyy="0.006196546" iyz="5.138e-06" izz="3.4774e-05"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="RL_hip_joint" type="revolute">
|
||||||
|
<origin rpy="0 0 0" xyz="-0.21935 0.0875 0"/>
|
||||||
|
<parent link="trunk"/>
|
||||||
|
<child link="RL_hip"/>
|
||||||
|
<axis xyz="1 0 0"/>
|
||||||
|
<dynamics damping="0" friction="0"/>
|
||||||
|
<limit effort="20" lower="-0.872664625997" upper="1.0471975512" velocity="52.4"/>
|
||||||
|
</joint>
|
||||||
|
<link name="RL_hip">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 3.14159265359 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="../meshes/hip.stl" scale="1 1 1"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="grey"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="1.57079632679 0 0" xyz="0 -0.021 0"/>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length="0.08" radius="0.041"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="0.001568 -0.008134 0.000864"/>
|
||||||
|
<mass value="1.096"/>
|
||||||
|
<inertia ixx="0.000822113" ixy="4.982e-06" ixz="3.672e-05" iyy="0.000983196" iyz="2.811e-06" izz="0.000864753"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="RL_thigh_joint" type="revolute">
|
||||||
|
<origin rpy="0 0 0" xyz="0 0.037 0"/>
|
||||||
|
<parent link="RL_hip"/>
|
||||||
|
<child link="RL_thigh"/>
|
||||||
|
<axis xyz="0 1 0"/>
|
||||||
|
<dynamics damping="0" friction="0"/>
|
||||||
|
<limit effort="55" lower="-0.523598775598" upper="3.92699081699" velocity="28.6"/>
|
||||||
|
</joint>
|
||||||
|
<link name="RL_thigh">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="../meshes/thigh.stl" scale="1 1 1"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="grey"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 1.57079632679 0" xyz="0 0 -0.125"/>
|
||||||
|
<geometry>
|
||||||
|
<box size="0.25 0.034 0.043"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="-0.000482 0.02001 -0.031996"/>
|
||||||
|
<mass value="1.528"/>
|
||||||
|
<inertia ixx="0.00991611" ixy="1.0388e-05" ixz="0.000250428" iyy="0.009280083" iyz="-8.511e-05" izz="0.00178256"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="RL_calf_joint" type="revolute">
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 -0.25"/>
|
||||||
|
<parent link="RL_thigh"/>
|
||||||
|
<child link="RL_calf"/>
|
||||||
|
<axis xyz="0 1 0"/>
|
||||||
|
<dynamics damping="0" friction="0"/>
|
||||||
|
<limit effort="55" lower="-2.77507351067" upper="-0.610865238198" velocity="28.6"/>
|
||||||
|
</joint>
|
||||||
|
<link name="RL_calf">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="../meshes/calf.stl" scale="1 1 1"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="silver"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 1.57079632679 0" xyz="0 0 -0.125"/>
|
||||||
|
<geometry>
|
||||||
|
<box size="0.25 0.016 0.016"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="-0.002196 -0.000381 -0.12338"/>
|
||||||
|
<mass value="0.241"/>
|
||||||
|
<inertia ixx="0.006181961" ixy="2.37e-07" ixz="-2.985e-06" iyy="0.006196546" iyz="5.138e-06" izz="3.4774e-05"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
</robot>
|
||||||
|
|
||||||
178
data/laikago/xml/laikago.xml
Normal file
178
data/laikago/xml/laikago.xml
Normal file
@@ -0,0 +1,178 @@
|
|||||||
|
<mujoco model="laikago_description">
|
||||||
|
<compiler angle="radian" meshdir="../meshes/" />
|
||||||
|
<size njmax="500" nconmax="100" />
|
||||||
|
<option gravity='0 0 -9.806' iterations='50' solver='Newton' timestep='0.002'/>
|
||||||
|
|
||||||
|
<default>
|
||||||
|
<geom contype="1" conaffinity="1" friction="0.6 0.3 0.3" rgba="0.5 0.6 0.7 1" margin="0.001" group="0"/>
|
||||||
|
|
||||||
|
<light castshadow="false" diffuse="1 1 1"/>
|
||||||
|
<motor ctrlrange="-40.0 40.0" ctrllimited="true"/>
|
||||||
|
<camera fovy="60"/>
|
||||||
|
<joint damping="0.01" armature="0.01" frictionloss="0.2" />
|
||||||
|
|
||||||
|
</default>
|
||||||
|
|
||||||
|
<asset>
|
||||||
|
<mesh name="trunk" file="trunk.stl" />
|
||||||
|
<mesh name="hip" file="hip.stl" />
|
||||||
|
<mesh name="thigh_mirror" file="thigh_mirror.stl" />
|
||||||
|
<mesh name="calf" file="calf.stl" />
|
||||||
|
<mesh name="thigh" file="thigh.stl" />
|
||||||
|
</asset>
|
||||||
|
|
||||||
|
<visual>
|
||||||
|
<rgba com="0.502 1.0 0 0.5" contactforce="0.98 0.4 0.4 0.7" contactpoint="1.0 1.0 0.6 0.4"/>
|
||||||
|
<scale com="0.2" forcewidth="0.035" contactwidth="0.10" contactheight="0.04"/>
|
||||||
|
</visual>
|
||||||
|
|
||||||
|
<asset>
|
||||||
|
<texture type="skybox" builtin="gradient" rgb1="1.0 1.0 1.0" rgb2="1.0 1.0 1.0" width="512" height="512"/>
|
||||||
|
<texture name="plane" type="2d" builtin="flat" rgb1="1 1 1" rgb2="1 1 1" width="512" height="512" mark="cross" markrgb="0 0 0"/>
|
||||||
|
<material name="plane" reflectance="0.0" texture="plane" texrepeat="3 3" texuniform="true"/>
|
||||||
|
</asset>
|
||||||
|
<worldbody>
|
||||||
|
<light directional="true" diffuse=".8 .8 .8" pos="0 0 10" dir="0 0 -10"/>
|
||||||
|
<camera name="track" mode="trackcom" pos="0 -1.3 1.6" xyaxes="1 0 0 0 0.707 0.707"/>
|
||||||
|
<geom name='floor' type='plane' conaffinity='1' condim='3' contype='1' rgba="0.5 0.9 0.9 0.1" material='plane' pos='0 0 0' size='0 0 1'/>
|
||||||
|
|
||||||
|
<body name="trunk" pos="0 0 0.4">
|
||||||
|
<inertial pos="0.002284 -4.1e-05 0.025165" quat="-0.00605949 0.710803 -0.00734309 0.703327" mass="13.733" diaginertia="0.254491 0.250684 0.0733281" />
|
||||||
|
<joint type="free" />
|
||||||
|
<geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.2 0.2 0.2 1" mesh="trunk" />
|
||||||
|
<geom size="0.2808 0.086 0.09375" pos="0 0 0.01675" type="box" rgba="0.2 0.2 0.2 0" />
|
||||||
|
<site name="imu" pos="0 0 0"/>
|
||||||
|
|
||||||
|
<body name="FR_hip" pos="0.21935 -0.0875 0">
|
||||||
|
<inertial pos="-0.001568 0.008134 0.000864" quat="0.629953 0.350191 0.35823 0.593462" mass="1.096" diaginertia="0.000983491 0.000885646 0.000800926" />
|
||||||
|
<joint name="FR_hip_joint" pos="0 0 0" axis="1 0 0" limited="true" range="-1.0472 0.872665" />
|
||||||
|
<geom quat="0 1 0 0" type="mesh" contype="0" conaffinity="0" group="1" rgba="0.2 0.2 0.2 1" mesh="hip" />
|
||||||
|
<geom size="0.041 0.04" pos="0 0.021 0" quat="0.707107 0.707107 0 0" type="cylinder" rgba="0.2 0.2 0.2 0" />
|
||||||
|
<body name="FR_thigh" pos="0 -0.037 0">
|
||||||
|
<inertial pos="-0.000482 -0.02001 -0.031996" quat="0.999848 0.00577968 -0.0153453 -0.00595156" mass="1.528" diaginertia="0.00992391 0.00928096 0.00177389" />
|
||||||
|
<joint name="FR_thigh_joint" pos="0 0 0" axis="0 1 0" limited="true" range="-0.523599 3.92699" />
|
||||||
|
<geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.2 0.2 0.2 1" mesh="thigh_mirror" />
|
||||||
|
<geom size="0.125 0.017 0.0215" pos="0 0 -0.125" quat="0.707107 0 0.707107 0" type="box" rgba="0.2 0.2 0.2 0" />
|
||||||
|
<body name="FR_calf" pos="0 0 -0.25">
|
||||||
|
<inertial pos="-0.002196 -0.000381 -0.12338" quat="0.712765 0.000467477 -0.000119366 0.701402" mass="0.241" diaginertia="0.00619655 0.00618196 3.47683e-05" />
|
||||||
|
<joint name="FR_calf_joint" pos="0 0 0" axis="0 1 0" limited="true" range="-2.77507 -0.610865" />
|
||||||
|
<geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.913725 0.913725 0.847059 1" mesh="calf" />
|
||||||
|
<geom size="0.125 0.008 0.008" pos="0 0 -0.125" quat="0.707107 0 0.707107 0" type="box" rgba="0.913725 0.913725 0.847059 0" />
|
||||||
|
<geom size="0.027 0.014" pos="0 0 -0.25" quat="0.707107 0.707107 0 0" rgba="0.913725 0.913725 0.847059 0" />
|
||||||
|
</body>
|
||||||
|
</body>
|
||||||
|
</body>
|
||||||
|
<body name="FL_hip" pos="0.21935 0.0875 0">
|
||||||
|
<inertial pos="-0.001568 -0.008134 0.000864" quat="0.593462 0.35823 0.350191 0.629953" mass="1.096" diaginertia="0.000983491 0.000885646 0.000800926" />
|
||||||
|
<joint name="FL_hip_joint" pos="0 0 0" axis="1 0 0" limited="true" range="-0.872665 1.0472" />
|
||||||
|
<geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.2 0.2 0.2 1" mesh="hip" />
|
||||||
|
<geom size="0.041 0.04" pos="0 -0.021 0" quat="0.707107 0.707107 0 0" type="cylinder" rgba="0.2 0.2 0.2 0" />
|
||||||
|
<body name="FL_thigh" pos="0 0.037 0">
|
||||||
|
<inertial pos="-0.000482 0.02001 -0.031996" quat="0.999848 -0.00577968 -0.0153453 0.00595156" mass="1.528" diaginertia="0.00992391 0.00928096 0.00177389" />
|
||||||
|
<joint name="FL_thigh_joint" pos="0 0 0" axis="0 1 0" limited="true" range="-0.523599 3.92699" />
|
||||||
|
<geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.2 0.2 0.2 1" mesh="thigh" />
|
||||||
|
<geom size="0.125 0.017 0.0215" pos="0 0 -0.125" quat="0.707107 0 0.707107 0" type="box" rgba="0.2 0.2 0.2 0" />
|
||||||
|
<body name="FL_calf" pos="0 0 -0.25">
|
||||||
|
<inertial pos="-0.002196 -0.000381 -0.12338" quat="0.712765 0.000467477 -0.000119366 0.701402" mass="0.241" diaginertia="0.00619655 0.00618196 3.47683e-05" />
|
||||||
|
<joint name="FL_calf_joint" pos="0 0 0" axis="0 1 0" limited="true" range="-2.77507 -0.610865" />
|
||||||
|
<geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.913725 0.913725 0.847059 1" mesh="calf" />
|
||||||
|
<geom size="0.125 0.008 0.008" pos="0 0 -0.125" quat="0.707107 0 0.707107 0" type="box" rgba="0.913725 0.913725 0.847059 0" />
|
||||||
|
<geom size="0.027 0.014" pos="0 0 -0.25" quat="0.707107 0.707107 0 0" rgba="0.913725 0.913725 0.847059 0" />
|
||||||
|
</body>
|
||||||
|
</body>
|
||||||
|
</body>
|
||||||
|
<body name="RR_hip" pos="-0.21935 -0.0875 0">
|
||||||
|
<inertial pos="0.001568 0.008134 0.000864" quat="0.35823 0.593462 0.629953 0.350191" mass="1.096" diaginertia="0.000983491 0.000885646 0.000800926" />
|
||||||
|
<joint name="RR_hip_joint" pos="0 0 0" axis="1 0 0" limited="true" range="-1.0472 0.872665" />
|
||||||
|
<geom quat="0 0 0 -1" type="mesh" contype="0" conaffinity="0" group="1" rgba="0.2 0.2 0.2 1" mesh="hip" />
|
||||||
|
<geom size="0.041 0.04" pos="0 0.021 0" quat="0.707107 0.707107 0 0" type="cylinder" rgba="0.2 0.2 0.2 0" />
|
||||||
|
<body name="RR_thigh" pos="0 -0.037 0">
|
||||||
|
<inertial pos="-0.000482 -0.02001 -0.031996" quat="0.999848 0.00577968 -0.0153453 -0.00595156" mass="1.528" diaginertia="0.00992391 0.00928096 0.00177389" />
|
||||||
|
<joint name="RR_thigh_joint" pos="0 0 0" axis="0 1 0" limited="true" range="-0.523599 3.92699" />
|
||||||
|
<geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.2 0.2 0.2 1" mesh="thigh_mirror" />
|
||||||
|
<geom size="0.125 0.017 0.0215" pos="0 0 -0.125" quat="0.707107 0 0.707107 0" type="box" rgba="0.2 0.2 0.2 0" />
|
||||||
|
<body name="RR_calf" pos="0 0 -0.25">
|
||||||
|
<inertial pos="-0.002196 -0.000381 -0.12338" quat="0.712765 0.000467477 -0.000119366 0.701402" mass="0.241" diaginertia="0.00619655 0.00618196 3.47683e-05" />
|
||||||
|
<joint name="RR_calf_joint" pos="0 0 0" axis="0 1 0" limited="true" range="-2.77507 -0.610865" />
|
||||||
|
<geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.913725 0.913725 0.847059 1" mesh="calf" />
|
||||||
|
<geom size="0.125 0.008 0.008" pos="0 0 -0.125" quat="0.707107 0 0.707107 0" type="box" rgba="0.913725 0.913725 0.847059 0" />
|
||||||
|
<geom size="0.027 0.014" pos="0 0 -0.25" quat="0.707107 0.707107 0 0" rgba="0.913725 0.913725 0.847059 0" />
|
||||||
|
</body>
|
||||||
|
</body>
|
||||||
|
</body>
|
||||||
|
<body name="RL_hip" pos="-0.21935 0.0875 0">
|
||||||
|
<inertial pos="0.001568 -0.008134 0.000864" quat="0.350191 0.629953 0.593462 0.35823" mass="1.096" diaginertia="0.000983491 0.000885646 0.000800926" />
|
||||||
|
<joint name="RL_hip_joint" pos="0 0 0" axis="1 0 0" limited="true" range="-0.872665 1.0472" />
|
||||||
|
<geom quat="0 0 1 0" type="mesh" contype="0" conaffinity="0" group="1" rgba="0.2 0.2 0.2 1" mesh="hip" />
|
||||||
|
<geom size="0.041 0.04" pos="0 -0.021 0" quat="0.707107 0.707107 0 0" type="cylinder" rgba="0.2 0.2 0.2 0" />
|
||||||
|
<body name="RL_thigh" pos="0 0.037 0">
|
||||||
|
<inertial pos="-0.000482 0.02001 -0.031996" quat="0.999848 -0.00577968 -0.0153453 0.00595156" mass="1.528" diaginertia="0.00992391 0.00928096 0.00177389" />
|
||||||
|
<joint name="RL_thigh_joint" pos="0 0 0" axis="0 1 0" limited="true" range="-0.523599 3.92699" />
|
||||||
|
<geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.2 0.2 0.2 1" mesh="thigh" />
|
||||||
|
<geom size="0.125 0.017 0.0215" pos="0 0 -0.125" quat="0.707107 0 0.707107 0" type="box" rgba="0.2 0.2 0.2 0" />
|
||||||
|
<body name="RL_calf" pos="0 0 -0.25">
|
||||||
|
<inertial pos="-0.002196 -0.000381 -0.12338" quat="0.712765 0.000467477 -0.000119366 0.701402" mass="0.241" diaginertia="0.00619655 0.00618196 3.47683e-05" />
|
||||||
|
<joint name="RL_calf_joint" pos="0 0 0" axis="0 1 0" limited="true" range="-2.77507 -0.610865" />
|
||||||
|
<geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.913725 0.913725 0.847059 1" mesh="calf" />
|
||||||
|
<geom size="0.125 0.008 0.008" pos="0 0 -0.125" quat="0.707107 0 0.707107 0" type="box" rgba="0.913725 0.913725 0.847059 0" />
|
||||||
|
<geom size="0.027 0.014" pos="0 0 -0.25" quat="0.707107 0.707107 0 0" rgba="0.913725 0.913725 0.847059 0" />
|
||||||
|
</body>
|
||||||
|
</body>
|
||||||
|
</body>
|
||||||
|
</body>
|
||||||
|
</worldbody>
|
||||||
|
<actuator>
|
||||||
|
|
||||||
|
<motor name="FR_hip" gear="1" joint="FR_hip_joint"/>
|
||||||
|
<motor name="FR_thigh" gear="1" joint="FR_thigh_joint"/>
|
||||||
|
<motor name="FR_calf" gear="1" joint="FR_calf_joint"/>
|
||||||
|
<motor name="FL_hip" gear="1" joint="FL_hip_joint"/>
|
||||||
|
<motor name="FL_thigh" gear="1" joint="FL_thigh_joint"/>
|
||||||
|
<motor name="FL_calf" gear="1" joint="FL_calf_joint"/>
|
||||||
|
<motor name="RR_hip" gear="1" joint="RR_hip_joint"/>
|
||||||
|
<motor name="RR_thigh" gear="1" joint="RR_thigh_joint"/>
|
||||||
|
<motor name="RR_calf" gear="1" joint="RR_calf_joint" />
|
||||||
|
<motor name="RL_hip" gear="1" joint="RL_hip_joint"/>
|
||||||
|
<motor name="RL_thigh" gear="1" joint="RL_thigh_joint"/>
|
||||||
|
<motor name="RL_calf" gear="1" joint="RL_calf_joint"/>
|
||||||
|
</actuator>
|
||||||
|
|
||||||
|
<sensor>
|
||||||
|
|
||||||
|
<jointpos name="FR_hip_pos" joint="FR_hip_joint"/>
|
||||||
|
<jointpos name="FR_thigh_pos" joint="FR_thigh_joint"/>
|
||||||
|
<jointpos name="FR_calf_pos" joint="FR_calf_joint"/>
|
||||||
|
<jointpos name="FL_hip_pos" joint="FL_hip_joint"/>
|
||||||
|
<jointpos name="FL_thigh_pos" joint="FL_thigh_joint"/>
|
||||||
|
<jointpos name="FL_calf_pos" joint="FL_calf_joint"/>
|
||||||
|
<jointpos name="RR_hip_pos" joint="RR_hip_joint"/>
|
||||||
|
<jointpos name="RR_thigh_pos" joint="RR_thigh_joint"/>
|
||||||
|
<jointpos name="RR_calf_pos" joint="RR_calf_joint" />
|
||||||
|
<jointpos name="RL_hip_pos" joint="RL_hip_joint"/>
|
||||||
|
<jointpos name="RL_thigh_pos" joint="RL_thigh_joint"/>
|
||||||
|
<jointpos name="RL_calf_pos" joint="RL_calf_joint"/>
|
||||||
|
|
||||||
|
<jointvel name="FR_hip_vel" joint="FR_hip_joint"/>
|
||||||
|
<jointvel name="FR_thigh_vel" joint="FR_thigh_joint"/>
|
||||||
|
<jointvel name="FR_calf_vel" joint="FR_calf_joint"/>
|
||||||
|
<jointvel name="FL_hip_vel" joint="FL_hip_joint"/>
|
||||||
|
<jointvel name="FL_thigh_vel" joint="FL_thigh_joint"/>
|
||||||
|
<jointvel name="FL_calf_vel" joint="FL_calf_joint"/>
|
||||||
|
<jointvel name="RR_hip_vel" joint="RR_hip_joint"/>
|
||||||
|
<jointvel name="RR_thigh_vel" joint="RR_thigh_joint"/>
|
||||||
|
<jointvel name="RR_calf_vel" joint="RR_calf_joint" />
|
||||||
|
<jointvel name="RL_hip_vel" joint="RL_hip_joint"/>
|
||||||
|
<jointvel name="RL_thigh_vel" joint="RL_thigh_joint"/>
|
||||||
|
<jointvel name="RL_calf_vel" joint="RL_calf_joint"/>
|
||||||
|
|
||||||
|
<accelerometer name="Body_Acc" site="imu"/>
|
||||||
|
|
||||||
|
<gyro name="Body_Gyro" site="imu"/>
|
||||||
|
|
||||||
|
<framepos name="Body_Pos" objtype="site" objname="imu"/>
|
||||||
|
|
||||||
|
<framequat name="Body_Quat" objtype="site" objname="imu"/>
|
||||||
|
|
||||||
|
</sensor>
|
||||||
|
|
||||||
|
</mujoco>
|
||||||
Binary file not shown.
Binary file not shown.
185
demo/export_skrl_policy_to_onnx.py
Normal file
185
demo/export_skrl_policy_to_onnx.py
Normal file
@@ -0,0 +1,185 @@
|
|||||||
|
"""
|
||||||
|
Export SKRL PyTorch policy to ONNX format.
|
||||||
|
|
||||||
|
Usage:
|
||||||
|
python demo/export_skrl_policy_to_onnx.py --checkpoint <path_to_checkpoint.pt> --output <output_dir>
|
||||||
|
|
||||||
|
Example:
|
||||||
|
python demo/export_skrl_policy_to_onnx.py \
|
||||||
|
--checkpoint runs/go1-rough-terrain-walk/skrl/[time]_PPO/checkpoints/best_agent.pt \
|
||||||
|
--output exports_go1_rough
|
||||||
|
"""
|
||||||
|
|
||||||
|
import argparse
|
||||||
|
import os
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
import torch
|
||||||
|
import torch.nn as nn
|
||||||
|
|
||||||
|
|
||||||
|
# ============== PyTorch Checkpoint Export ==============
|
||||||
|
|
||||||
|
class SKRLPolicyTorch(nn.Module):
|
||||||
|
"""PyTorch policy matching SKRL GaussianMixin architecture.
|
||||||
|
|
||||||
|
The architecture is inferred from the checkpoint keys:
|
||||||
|
- net.N: Hidden layers (Linear -> ELU)
|
||||||
|
- mean_layer: Output layer (no activation)
|
||||||
|
"""
|
||||||
|
|
||||||
|
def __init__(self, obs_dim, action_dim, hidden_dims=[512, 256, 128]):
|
||||||
|
super().__init__()
|
||||||
|
self.obs_dim = obs_dim
|
||||||
|
self.action_dim = action_dim
|
||||||
|
self.hidden_dims = hidden_dims
|
||||||
|
|
||||||
|
layers = []
|
||||||
|
in_dim = obs_dim
|
||||||
|
for hidden_dim in hidden_dims:
|
||||||
|
layers.extend([
|
||||||
|
nn.Linear(in_dim, hidden_dim),
|
||||||
|
nn.ELU(),
|
||||||
|
])
|
||||||
|
in_dim = hidden_dim
|
||||||
|
|
||||||
|
self.net = nn.Sequential(*layers)
|
||||||
|
self.mean_layer = nn.Linear(in_dim, action_dim)
|
||||||
|
self.log_std = nn.Parameter(torch.zeros(action_dim))
|
||||||
|
|
||||||
|
def forward(self, x):
|
||||||
|
return self.mean_layer(self.net(x))
|
||||||
|
|
||||||
|
|
||||||
|
class ONNXPolicyExporterTorch(nn.Module):
|
||||||
|
"""Exporter wrapper that applies normalization and policy.
|
||||||
|
|
||||||
|
Matches SKRL's RunningStandardScaler behavior:
|
||||||
|
- Normalize: (x - mean) / sqrt(var + eps)
|
||||||
|
- Clip to [-5.0, 5.0] (clip_threshold)
|
||||||
|
"""
|
||||||
|
|
||||||
|
def __init__(self, policy, normalizer_mean, normalizer_std):
|
||||||
|
super().__init__()
|
||||||
|
self.policy = policy
|
||||||
|
# Register normalizer buffers as float32 (matching RunningStandardScaler)
|
||||||
|
self.register_buffer('mean', normalizer_mean.float())
|
||||||
|
self.register_buffer('std', normalizer_std.float())
|
||||||
|
self.clip_threshold = 5.0
|
||||||
|
|
||||||
|
def forward(self, x):
|
||||||
|
# Normalize (RunningStandardScaler._compute)
|
||||||
|
x = (x - self.mean) / (self.std + 1e-8)
|
||||||
|
# Clip to RunningStandardScaler clip_threshold
|
||||||
|
x = torch.clamp(x, min=-self.clip_threshold, max=self.clip_threshold)
|
||||||
|
return self.policy(x)
|
||||||
|
|
||||||
|
|
||||||
|
def infer_architecture_from_state_dict(state_dict):
|
||||||
|
"""Infer the policy architecture from state dict keys.
|
||||||
|
|
||||||
|
Args:
|
||||||
|
state_dict: Policy state dict
|
||||||
|
|
||||||
|
Returns:
|
||||||
|
tuple: (obs_dim, action_dim, hidden_dims)
|
||||||
|
"""
|
||||||
|
# Find observation dimension from first layer
|
||||||
|
obs_dim = state_dict['net.0.weight'].shape[1]
|
||||||
|
|
||||||
|
# Find action dimension from mean_layer
|
||||||
|
action_dim = state_dict['mean_layer.weight'].shape[0]
|
||||||
|
|
||||||
|
# Infer hidden dimensions from net layers
|
||||||
|
hidden_dims = []
|
||||||
|
net_keys = sorted([k for k in state_dict.keys() if k.startswith('net.') and k.endswith('.weight')])
|
||||||
|
for key in net_keys:
|
||||||
|
if 'mean_layer' not in key: # Skip output layer
|
||||||
|
layer_idx = int(key.split('.')[1])
|
||||||
|
if layer_idx % 2 == 0: # Only Linear layers, not activation layers
|
||||||
|
hidden_dims.append(state_dict[key].shape[0])
|
||||||
|
|
||||||
|
return obs_dim, action_dim, hidden_dims
|
||||||
|
|
||||||
|
|
||||||
|
def export_torch_policy_to_onnx(checkpoint_path, output_path, verbose=False):
|
||||||
|
"""Export PyTorch SKRL policy to ONNX.
|
||||||
|
|
||||||
|
Args:
|
||||||
|
checkpoint_path: Path to .pt checkpoint file
|
||||||
|
output_path: Directory to save ONNX file
|
||||||
|
verbose: Whether to print verbose output
|
||||||
|
"""
|
||||||
|
print(f"Loading PyTorch checkpoint from: {checkpoint_path}")
|
||||||
|
state = torch.load(checkpoint_path, map_location='cpu', weights_only=False)
|
||||||
|
|
||||||
|
policy_state = state['policy']
|
||||||
|
normalizer_state = state['state_preprocessor']
|
||||||
|
|
||||||
|
# Infer architecture
|
||||||
|
obs_dim, action_dim, hidden_dims = infer_architecture_from_state_dict(policy_state)
|
||||||
|
print(f"Inferred architecture: obs={obs_dim}, action={action_dim}, hidden={hidden_dims}")
|
||||||
|
|
||||||
|
# Create model
|
||||||
|
print("Creating PyTorch model...")
|
||||||
|
policy = SKRLPolicyTorch(obs_dim, action_dim, hidden_dims)
|
||||||
|
|
||||||
|
# Remove value layers from state dict (keep only policy)
|
||||||
|
policy_state_policy_only = {k: v for k, v in policy_state.items()
|
||||||
|
if not k.startswith('value') and 'value_layer' not in k}
|
||||||
|
policy.load_state_dict(policy_state_policy_only, strict=False)
|
||||||
|
policy.eval()
|
||||||
|
|
||||||
|
# Load normalizer
|
||||||
|
mean = normalizer_state['running_mean']
|
||||||
|
std = torch.sqrt(normalizer_state['running_variance'])
|
||||||
|
|
||||||
|
# Create exporter
|
||||||
|
exporter = ONNXPolicyExporterTorch(policy, mean, std)
|
||||||
|
exporter.eval()
|
||||||
|
|
||||||
|
# Create output directory
|
||||||
|
os.makedirs(output_path, exist_ok=True)
|
||||||
|
output_file = os.path.join(output_path, "policy.onnx")
|
||||||
|
|
||||||
|
# Export to ONNX
|
||||||
|
print(f"Exporting policy to: {output_file}")
|
||||||
|
obs = torch.zeros(1, obs_dim, dtype=torch.float32)
|
||||||
|
|
||||||
|
torch.onnx.export(
|
||||||
|
exporter,
|
||||||
|
obs,
|
||||||
|
output_file,
|
||||||
|
export_params=True,
|
||||||
|
opset_version=11,
|
||||||
|
verbose=verbose,
|
||||||
|
input_names=["observations"],
|
||||||
|
output_names=["actions"],
|
||||||
|
dynamic_axes={},
|
||||||
|
)
|
||||||
|
|
||||||
|
print(f"Successfully exported policy to ONNX: {output_file}")
|
||||||
|
|
||||||
|
# Save PyTorch model and normalizer
|
||||||
|
torch.save(policy.state_dict(), os.path.join(output_path, "policy.pt"))
|
||||||
|
np.savez(os.path.join(output_path, "normalizer.npz"),
|
||||||
|
mean=mean.numpy(), std=std.numpy())
|
||||||
|
|
||||||
|
return output_file
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
parser = argparse.ArgumentParser(description="Export SKRL PyTorch policy to ONNX")
|
||||||
|
parser.add_argument("--checkpoint", type=str, required=True, help="Path to checkpoint file (.pt)")
|
||||||
|
parser.add_argument("--output", type=str, default="exports", help="Output directory")
|
||||||
|
parser.add_argument("--verbose", action="store_true", help="Verbose output")
|
||||||
|
|
||||||
|
args = parser.parse_args()
|
||||||
|
|
||||||
|
output_file = export_torch_policy_to_onnx(
|
||||||
|
checkpoint_path=args.checkpoint,
|
||||||
|
output_path=args.output,
|
||||||
|
verbose=args.verbose
|
||||||
|
)
|
||||||
|
|
||||||
|
print(f"\nExported to: {output_file}")
|
||||||
BIN
demo/exports/normalizer.npz
Normal file
BIN
demo/exports/normalizer.npz
Normal file
Binary file not shown.
BIN
demo/exports/policy.onnx
Normal file
BIN
demo/exports/policy.onnx
Normal file
Binary file not shown.
BIN
demo/exports/policy.pt
Normal file
BIN
demo/exports/policy.pt
Normal file
Binary file not shown.
371
demo/go1_sim2sim_mujoco.py
Normal file
371
demo/go1_sim2sim_mujoco.py
Normal file
@@ -0,0 +1,371 @@
|
|||||||
|
#!/usr/bin/env python3
|
||||||
|
"""
|
||||||
|
MOTRIXLAB_UNTRIEE_GO1_SIM2SIM
|
||||||
|
source /opt/mujoco/venv/bin/activate
|
||||||
|
cd /opt/unitree_mujoco
|
||||||
|
python demo/go1_sim2sim_mujoco.py
|
||||||
|
|
||||||
|
"""
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
import mujoco
|
||||||
|
from mujoco import viewer
|
||||||
|
import os
|
||||||
|
import threading
|
||||||
|
import signal
|
||||||
|
import queue
|
||||||
|
import argparse
|
||||||
|
import time
|
||||||
|
|
||||||
|
g_exit_requested = False
|
||||||
|
|
||||||
|
def signal_handler(signum, frame):
|
||||||
|
global g_exit_requested
|
||||||
|
g_exit_requested = True
|
||||||
|
|
||||||
|
signal.signal(signal.SIGINT, signal_handler)
|
||||||
|
|
||||||
|
# ============================================================
|
||||||
|
# 配置
|
||||||
|
# ============================================================
|
||||||
|
DEFAULT_ONNX_PATH = "/opt/motrixlab/repo/export_flat/policy.onnx"
|
||||||
|
MOTRIX_XML_DIR = "/opt/motrixlab/repo/motrix_envs/src/motrix_envs/locomotion/go1/xmls"
|
||||||
|
XML_PATH = f"{MOTRIX_XML_DIR}/go1_motor_actuator.xml"
|
||||||
|
|
||||||
|
TERRAIN = "none"
|
||||||
|
|
||||||
|
# ============================================================
|
||||||
|
# MotrixLab 参数 (来自 cfg.py)
|
||||||
|
# ============================================================
|
||||||
|
NUM_OBS = 48
|
||||||
|
NUM_ACTIONS = 12
|
||||||
|
OBS_SCALES = {'lin_vel': 2.0, 'ang_vel': 0.25, 'dof_pos': 1.0, 'dof_vel': 0.05}
|
||||||
|
ACTION_SCALE = 0.05
|
||||||
|
KP = 80.0
|
||||||
|
KD = 1.0
|
||||||
|
CLIP_ACTIONS = 23.7
|
||||||
|
CLIP_OBSERVATIONS = 100.0
|
||||||
|
MAX_LIN_VEL_X = 1.0
|
||||||
|
MAX_LIN_VEL_Y = 1.0
|
||||||
|
MAX_ANG_VEL = 0.5
|
||||||
|
|
||||||
|
# ============================================================
|
||||||
|
# 关节名称和顺序
|
||||||
|
# ============================================================
|
||||||
|
POLICY_JOINT_NAMES = [
|
||||||
|
"FR_hip", "FR_thigh", "FR_calf",
|
||||||
|
"FL_hip", "FL_thigh", "FL_calf",
|
||||||
|
"RR_hip", "RR_thigh", "RR_calf",
|
||||||
|
"RL_hip", "RL_thigh", "RL_calf",
|
||||||
|
]
|
||||||
|
|
||||||
|
DEFAULT_JOINT_ANGLES = np.array([
|
||||||
|
-0.0, 0.9, -1.8, # FR_hip, FR_thigh, FR_calf
|
||||||
|
0.0, 0.9, -1.8, # FL_hip, FL_thigh, FL_calf
|
||||||
|
-0.0, 0.9, -1.8, # RR_hip, RR_thigh, RR_calf
|
||||||
|
0.0, 0.9, -1.8, # RL_hip, RL_thigh, RL_calf
|
||||||
|
], dtype=np.float32)
|
||||||
|
|
||||||
|
MUJOCO_TO_POLICY = np.arange(12, dtype=np.int64)
|
||||||
|
POLICY_TO_MUJOCO = np.arange(12, dtype=np.int64)
|
||||||
|
|
||||||
|
# ============================================================
|
||||||
|
# 键盘输入
|
||||||
|
# ============================================================
|
||||||
|
from pynput import keyboard
|
||||||
|
|
||||||
|
class KeyboardReader:
|
||||||
|
def __init__(self):
|
||||||
|
self._event_queue = queue.Queue()
|
||||||
|
self.running = True
|
||||||
|
self.shared_keys_held = set()
|
||||||
|
self.shared_one_shot = set()
|
||||||
|
self._reader_thread = None
|
||||||
|
self._listener = None
|
||||||
|
|
||||||
|
def _normalize_key(self, key):
|
||||||
|
try:
|
||||||
|
if hasattr(key, 'char') and key.char is not None:
|
||||||
|
return key.char.lower()
|
||||||
|
except:
|
||||||
|
pass
|
||||||
|
key_str = str(key)
|
||||||
|
if key_str == 'Key.esc':
|
||||||
|
return 'escape'
|
||||||
|
elif key_str == 'Key.space':
|
||||||
|
return 'space'
|
||||||
|
elif key_str.startswith('Key.'):
|
||||||
|
return key_str.lower()
|
||||||
|
return key_str.lower()
|
||||||
|
|
||||||
|
def _reader_worker(self):
|
||||||
|
while self.running:
|
||||||
|
try:
|
||||||
|
event_type, key = self._event_queue.get(timeout=0.05)
|
||||||
|
k = self._normalize_key(key)
|
||||||
|
if event_type == 'press':
|
||||||
|
self.shared_keys_held.add(k)
|
||||||
|
self.shared_one_shot.discard(k)
|
||||||
|
elif event_type == 'release':
|
||||||
|
self.shared_keys_held.discard(k)
|
||||||
|
self.shared_one_shot.discard(k)
|
||||||
|
except queue.Empty:
|
||||||
|
pass
|
||||||
|
|
||||||
|
def init(self):
|
||||||
|
def on_press(key):
|
||||||
|
self._event_queue.put(('press', key))
|
||||||
|
def on_release(key):
|
||||||
|
self._event_queue.put(('release', key))
|
||||||
|
try:
|
||||||
|
self._listener = keyboard.Listener(on_press=on_press, on_release=on_release)
|
||||||
|
self._listener.start()
|
||||||
|
self._reader_thread = threading.Thread(target=self._reader_worker, daemon=True)
|
||||||
|
self._reader_thread.start()
|
||||||
|
print("[INFO] 键盘监听已启动")
|
||||||
|
except Exception as e:
|
||||||
|
print(f"[WARN] 无法初始化键盘监听: {e}")
|
||||||
|
|
||||||
|
def is_key_pressed(self, key):
|
||||||
|
k = self._normalize_key(key) if isinstance(key, str) else self._normalize_key(key)
|
||||||
|
if k not in self.shared_keys_held or k in self.shared_one_shot:
|
||||||
|
return False
|
||||||
|
self.shared_one_shot.add(k)
|
||||||
|
return True
|
||||||
|
|
||||||
|
def is_key_held(self, key):
|
||||||
|
k = self._normalize_key(key) if isinstance(key, str) else self._normalize_key(key)
|
||||||
|
return k in self.shared_keys_held
|
||||||
|
|
||||||
|
def restore(self):
|
||||||
|
self.running = False
|
||||||
|
if self._listener:
|
||||||
|
self._listener.stop()
|
||||||
|
|
||||||
|
|
||||||
|
# ============================================================
|
||||||
|
# Sensor 读取
|
||||||
|
# ============================================================
|
||||||
|
def get_sensor(model, data, name):
|
||||||
|
sid = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SENSOR, name)
|
||||||
|
if sid < 0:
|
||||||
|
return None
|
||||||
|
adr = model.sensor_adr[sid]
|
||||||
|
dim = model.sensor_dim[sid]
|
||||||
|
return data.sensordata[adr:adr + dim].copy()
|
||||||
|
|
||||||
|
|
||||||
|
def compute_observations_motrix(model, data, commands, last_actions):
|
||||||
|
"""计算 48 维观测"""
|
||||||
|
obs = np.zeros(NUM_OBS, dtype=np.float32)
|
||||||
|
|
||||||
|
# 局部线速度 (来自 sensor)
|
||||||
|
local_vel = get_sensor(model, data, "local_linvel")
|
||||||
|
if local_vel is not None:
|
||||||
|
obs[0:3] = local_vel * OBS_SCALES['lin_vel']
|
||||||
|
else:
|
||||||
|
base_rot = data.xmat[1].reshape(3, 3)
|
||||||
|
world_linvel = data.qvel[0:3]
|
||||||
|
local_linvel = base_rot.T @ world_linvel
|
||||||
|
obs[0:3] = local_linvel * OBS_SCALES['lin_vel']
|
||||||
|
|
||||||
|
# 陀螺仪
|
||||||
|
gyro = get_sensor(model, data, "gyro")
|
||||||
|
if gyro is not None:
|
||||||
|
obs[3:6] = gyro * OBS_SCALES['ang_vel']
|
||||||
|
else:
|
||||||
|
obs[3:6] = data.qvel[3:6] * OBS_SCALES['ang_vel']
|
||||||
|
|
||||||
|
# 重力向量 (躯干坐标系
|
||||||
|
base_rot = data.xmat[1].reshape(3, 3)
|
||||||
|
gravity_world = np.array([0., 0., -1.], dtype=np.float64)
|
||||||
|
local_gravity = base_rot.T @ gravity_world
|
||||||
|
obs[6:9] = local_gravity.astype(np.float32)
|
||||||
|
|
||||||
|
# 关节位置偏差
|
||||||
|
joint_pos = data.qpos[7:19]
|
||||||
|
dof_pos_rel = (joint_pos - DEFAULT_JOINT_ANGLES) * OBS_SCALES['dof_pos']
|
||||||
|
obs[9:21] = dof_pos_rel
|
||||||
|
|
||||||
|
# 关节速度
|
||||||
|
joint_vel = data.qvel[6:18]
|
||||||
|
obs[21:33] = joint_vel * OBS_SCALES['dof_vel']
|
||||||
|
|
||||||
|
# 上一步动作
|
||||||
|
obs[33:45] = last_actions
|
||||||
|
|
||||||
|
# 命令
|
||||||
|
obs[45:48] = commands * np.array([OBS_SCALES['lin_vel'], OBS_SCALES['lin_vel'], OBS_SCALES['ang_vel']], dtype=np.float32)
|
||||||
|
|
||||||
|
# 限幅
|
||||||
|
obs = np.clip(obs, -CLIP_OBSERVATIONS, CLIP_OBSERVATIONS)
|
||||||
|
return obs
|
||||||
|
|
||||||
|
|
||||||
|
def main():
|
||||||
|
import re
|
||||||
|
import onnxruntime as ort
|
||||||
|
|
||||||
|
parser = argparse.ArgumentParser(description="MotrixLab Go1 Policy Inference in MuJoCo")
|
||||||
|
parser.add_argument("--onnx", type=str, default=DEFAULT_ONNX_PATH)
|
||||||
|
parser.add_argument("--terrain", type=str, default=TERRAIN, choices=["none", "rough", "stairs"])
|
||||||
|
args = parser.parse_args()
|
||||||
|
|
||||||
|
os.chdir(MOTRIX_XML_DIR)
|
||||||
|
|
||||||
|
if args.terrain == "rough":
|
||||||
|
xml_file = f"{MOTRIX_XML_DIR}/scene_rough_terrain.xml"
|
||||||
|
elif args.terrain == "stairs":
|
||||||
|
xml_file = f"{MOTRIX_XML_DIR}/scene_stairs_terrain.xml"
|
||||||
|
else:
|
||||||
|
xml_file = f"{MOTRIX_XML_DIR}/scene_motor_actuator.xml" # flat floor
|
||||||
|
|
||||||
|
with open(xml_file, 'r') as f:
|
||||||
|
xml_content = f.read()
|
||||||
|
|
||||||
|
model = mujoco.MjModel.from_xml_string(xml_content)
|
||||||
|
data = mujoco.MjData(model)
|
||||||
|
|
||||||
|
print(f"[INFO] Model: {model.nbody} bodies, {model.nq} DoF, {model.nu} actuators")
|
||||||
|
print(f"[INFO] MuJoCo timestep: {model.opt.timestep}")
|
||||||
|
print(f"[INFO] 关节顺序 (qpos[7:19]): {[mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_JOINT, i) for i in range(1, 13)]}")
|
||||||
|
|
||||||
|
for sensor_name in ["gyro", "local_linvel"]:
|
||||||
|
sid = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SENSOR, sensor_name)
|
||||||
|
print(f"[SENSOR] {sensor_name}: {'存在' if sid >= 0 else '不存在'}")
|
||||||
|
|
||||||
|
# 初始化
|
||||||
|
data.qpos[0:3] = np.array([0.0, 0.0, 0.42])
|
||||||
|
data.qpos[3:7] = np.array([1.0, 0.0, 0.0, 0.0]) # 默认四元数
|
||||||
|
data.qpos[7:19] = DEFAULT_JOINT_ANGLES
|
||||||
|
data.qvel[:] = 0.0
|
||||||
|
data.ctrl[:] = 0.0
|
||||||
|
mujoco.mj_forward(model, data)
|
||||||
|
|
||||||
|
print(f"[INIT] qpos[2]={data.qpos[2]:.3f}")
|
||||||
|
print(f"[INIT] qpos[7:19]={data.qpos[7:19]}")
|
||||||
|
print(f"[INIT] DEFAULT_JOINT_ANGLES={DEFAULT_JOINT_ANGLES}")
|
||||||
|
|
||||||
|
# 加载onnx
|
||||||
|
session = ort.InferenceSession(args.onnx, providers=['CPUExecutionProvider'])
|
||||||
|
print(f"[INFO] loaded")
|
||||||
|
|
||||||
|
# 主循环
|
||||||
|
ctrl_dt = 0.01 # 100Hz
|
||||||
|
num_steps_per_inference = int(ctrl_dt / model.opt.timestep)
|
||||||
|
print(f"[INFO] 每 {num_steps_per_inference} 步推理一次 ")
|
||||||
|
|
||||||
|
step_count = 0
|
||||||
|
inference_step = 0
|
||||||
|
|
||||||
|
x_vel_cmd = 0.0
|
||||||
|
y_vel_cmd = 0.0
|
||||||
|
yaw_vel_cmd = 0.0
|
||||||
|
commands = np.array([x_vel_cmd, y_vel_cmd, yaw_vel_cmd], dtype=np.float32)
|
||||||
|
last_actions = np.zeros(NUM_ACTIONS, dtype=np.float32)
|
||||||
|
action = np.zeros(NUM_ACTIONS, dtype=np.float32)
|
||||||
|
|
||||||
|
# 键盘
|
||||||
|
keyboard_reader = KeyboardReader()
|
||||||
|
keyboard_reader.init()
|
||||||
|
|
||||||
|
view = viewer.launch_passive(model, data)
|
||||||
|
print("[INFO] 已启动!")
|
||||||
|
|
||||||
|
loop_start_time = time.time()
|
||||||
|
|
||||||
|
while view.is_running() and not g_exit_requested:
|
||||||
|
# 键盘命令
|
||||||
|
if keyboard_reader.is_key_pressed(' '):
|
||||||
|
x_vel_cmd = 0.0
|
||||||
|
y_vel_cmd = 0.0
|
||||||
|
yaw_vel_cmd = 0.0
|
||||||
|
|
||||||
|
if keyboard_reader.is_key_held('w'):
|
||||||
|
x_vel_cmd = MAX_LIN_VEL_X
|
||||||
|
elif keyboard_reader.is_key_held('s'):
|
||||||
|
x_vel_cmd = -MAX_LIN_VEL_X
|
||||||
|
else:
|
||||||
|
x_vel_cmd = 0.0
|
||||||
|
|
||||||
|
if keyboard_reader.is_key_held('q'):
|
||||||
|
y_vel_cmd = MAX_LIN_VEL_Y
|
||||||
|
elif keyboard_reader.is_key_held('e'):
|
||||||
|
y_vel_cmd = -MAX_LIN_VEL_Y
|
||||||
|
else:
|
||||||
|
y_vel_cmd = 0.0
|
||||||
|
|
||||||
|
if keyboard_reader.is_key_held('a'):
|
||||||
|
yaw_vel_cmd = MAX_ANG_VEL
|
||||||
|
elif keyboard_reader.is_key_held('d'):
|
||||||
|
yaw_vel_cmd = -MAX_ANG_VEL
|
||||||
|
else:
|
||||||
|
yaw_vel_cmd = 0.0
|
||||||
|
|
||||||
|
if keyboard_reader.is_key_pressed('r'):
|
||||||
|
# 重置机器人到初始位置
|
||||||
|
data.qpos[0:3] = np.array([0.0, 0.0, 0.42])
|
||||||
|
data.qpos[3:7] = np.array([1.0, 0.0, 0.0, 0.0])
|
||||||
|
data.qpos[7:19] = DEFAULT_JOINT_ANGLES
|
||||||
|
data.qvel[:] = 0.0
|
||||||
|
data.ctrl[:] = 0.0
|
||||||
|
last_actions = np.zeros(NUM_ACTIONS, dtype=np.float32)
|
||||||
|
mujoco.mj_forward(model, data)
|
||||||
|
print("[RESET] 机器人已重置")
|
||||||
|
|
||||||
|
if keyboard_reader.is_key_pressed('escape'):
|
||||||
|
break
|
||||||
|
|
||||||
|
# 推理 (每 N 步一次)
|
||||||
|
if inference_step == 0:
|
||||||
|
commands[0] = x_vel_cmd
|
||||||
|
commands[1] = y_vel_cmd
|
||||||
|
commands[2] = yaw_vel_cmd
|
||||||
|
|
||||||
|
obs = compute_observations_motrix(model, data, commands, last_actions)
|
||||||
|
|
||||||
|
# 推理
|
||||||
|
action = session.run(None, {'observations': obs.reshape(1, -1).astype(np.float32)})[0][0]
|
||||||
|
action = np.clip(action, -CLIP_ACTIONS, CLIP_ACTIONS)
|
||||||
|
last_actions = action.copy()
|
||||||
|
|
||||||
|
# PD 控制
|
||||||
|
# joint_targets = action * action_scale + default_angles
|
||||||
|
joint_targets = DEFAULT_JOINT_ANGLES + action * ACTION_SCALE
|
||||||
|
|
||||||
|
current_pos = data.qpos[7:19]
|
||||||
|
current_vel = data.qvel[6:18]
|
||||||
|
torques = KP * (joint_targets - current_pos) - KD * current_vel
|
||||||
|
torques = np.clip(torques, -CLIP_ACTIONS, CLIP_ACTIONS)
|
||||||
|
data.ctrl[:] = torques
|
||||||
|
|
||||||
|
mujoco.mj_step(model, data)
|
||||||
|
view.sync()
|
||||||
|
|
||||||
|
expected_time = step_count * ctrl_dt
|
||||||
|
elapsed = time.time() - loop_start_time
|
||||||
|
sleep_time = expected_time - elapsed
|
||||||
|
if sleep_time > 0:
|
||||||
|
time.sleep(sleep_time)
|
||||||
|
|
||||||
|
step_count += 1
|
||||||
|
inference_step = (inference_step + 1) % num_steps_per_inference
|
||||||
|
|
||||||
|
if step_count % 200 == 0:
|
||||||
|
trunk_z = data.qpos[2]
|
||||||
|
lin_vel = np.linalg.norm(data.qvel[0:3])
|
||||||
|
print(f"\n========== Step {step_count} ==========")
|
||||||
|
print(f"[CMD] x={x_vel_cmd:.2f}, y={y_vel_cmd:.2f}, yaw={yaw_vel_cmd:.2f}")
|
||||||
|
print(f"[OBS] lin_vel={obs[0:3]}, ang_vel={obs[3:6]}, grav={obs[6:9]}")
|
||||||
|
print(f"[ACTION] raw={action[:4]}... scaled={action[:4]*ACTION_SCALE}...")
|
||||||
|
print(f"[TARGET] {joint_targets[:4]}...")
|
||||||
|
print(f"[TORQUE] {torques[:4]}...")
|
||||||
|
print(f"[STATE] z={trunk_z:.3f}m, vel={lin_vel:.3f}m/s")
|
||||||
|
print(f"==========================================\n")
|
||||||
|
|
||||||
|
keyboard_reader.restore()
|
||||||
|
view.close()
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
main()
|
||||||
Reference in New Issue
Block a user