Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

update minitaur.py to use minitaur.urdf (instead of quadruped.urdf), also sort the legs in the same order as real hardware

Browse Source 
added test urdf files for minitaur with all fixed joints, or fixed knees.
added some stiffness/damping to minitaur legs (testing)
tiny_obj_loader, don't crash on invalid texture coordinates
btMultiBodyConstraintSolver: sweep back and forward to reduce asymmetry
This commit is contained in:
Erwin Coumans
2017-03-15 15:38:50 -07:00
parent a613911c84
commit 4db6fa9e29
11 changed files with 2043 additions and 118 deletions
Download Patch File Download Diff File Expand all files Collapse all files

16
data/quadruped/minitaur.urdf
View File

@@ -427,6 +427,8 @@
<link name="lower_leg_front_rightR_link"> <link name="lower_leg_front_rightR_link">
<contact> <contact>
<stiffness value="10000"/>
<damping value="10"/>
<lateral_friction value="1"/> <lateral_friction value="1"/>
</contact> </contact>
@@ -493,6 +495,8 @@
<link name="lower_leg_front_rightL_link"> <link name="lower_leg_front_rightL_link">
<contact> <contact>
<stiffness value="10000"/>
<damping value="10"/>
<lateral_friction value="1"/> <lateral_friction value="1"/>
</contact> </contact>
@@ -557,6 +561,8 @@
<link name="lower_leg_front_leftR_link"> <link name="lower_leg_front_leftR_link">
<contact> <contact>
<stiffness value="10000"/>
<damping value="10"/>
<lateral_friction value="1"/> <lateral_friction value="1"/>
</contact> </contact>
@@ -622,6 +628,8 @@
<link name="lower_leg_front_leftL_link"> <link name="lower_leg_front_leftL_link">
<contact> <contact>
<stiffness value="10000"/>
<damping value="10"/>
<lateral_friction value="1"/> <lateral_friction value="1"/>
</contact> </contact>
@@ -684,6 +692,8 @@
<link name="lower_leg_back_rightR_link"> <link name="lower_leg_back_rightR_link">
<contact> <contact>
<stiffness value="10000"/>
<damping value="10"/>
<lateral_friction value="1"/> <lateral_friction value="1"/>
</contact> </contact>
@@ -750,6 +760,8 @@
<link name="lower_leg_back_rightL_link"> <link name="lower_leg_back_rightL_link">
<contact> <contact>
<stiffness value="10000"/>
<damping value="10"/>
<lateral_friction value="1"/> <lateral_friction value="1"/>
</contact> </contact>
@@ -814,6 +826,8 @@
<link name="lower_leg_back_leftR_link"> <link name="lower_leg_back_leftR_link">
<contact> <contact>
<stiffness value="10000"/>
<damping value="10"/>
<lateral_friction value="1"/> <lateral_friction value="1"/>
</contact> </contact>
@@ -877,6 +891,8 @@
<link name="lower_leg_back_leftL_link"> <link name="lower_leg_back_leftL_link">
<contact> <contact>
<stiffness value="10000"/>
<damping value="10"/>
<lateral_friction value="1"/> <lateral_friction value="1"/>
</contact> </contact>

913
data/quadruped/minitaur_fixed_all.urdf Normal file
View File

@@ -0,0 +1,913 @@
<?xml version="0.0" ?>
<!-- ======================================================================= -->
<!--LICENSE: -->
<!--Copyright (c) 2017, Erwin Coumans -->
<!--Google Inc. -->
<!--All rights reserved. -->
<!-- -->
<!--Redistribution and use in source and binary forms, with or without -->
<!--modification, are permitted provided that the following conditions are -->
<!--met: -->
<!-- -->
<!--1. Redistributions or derived work must retain this copyright notice, -->
<!-- this list of conditions and the following disclaimer. -->
<!-- -->
<!--2. 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. -->
<!-- -->
<!--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. -->
<robot name="quadruped">
<link name="base_chassis_link">
<visual>
<geometry>
<box size=".33 0.14 .07"/>
</geometry>
<material name="black">
<color rgba="0.3 0.3 0.3 1"/>
</material>
</visual>
<visual>
<origin rpy="0 0 0" xyz="0 0.10 0"/>
<geometry>
<box size=".17 0.10 .05"/>
</geometry>
<material name="black">
<color rgba="0.3 0.3 0.3 1"/>
</material>
</visual>
<visual>
<origin rpy="0 0 0" xyz="0 -0.10 0"/>
<geometry>
<box size=".17 0.10 .05"/>
</geometry>
<material name="black">
<color rgba="0.3 0.3 0.3 1"/>
</material>
</visual>
<collision>
<geometry>
<box size=".34 0.14 .07"/>
</geometry>
</collision>
<collision>
<origin rpy="0 0 0" xyz="0 0.10 0"/>
<geometry>
<box size=".17 0.10 .05"/>
</geometry>
</collision>
<collision>
<origin rpy="0 0 0" xyz="0 -0.10 0"/>
<geometry>
<box size=".17 0.10 .05"/>
</geometry>
</collision>
<inertial>
<mass value="3"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<link name="chassis_right">
<visual>
<origin rpy="0 0 0" xyz="0 0.035 0"/>
<geometry>
<box size=".34 0.01 .04"/>
</geometry>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material> </visual>
<visual>
<origin rpy="0 0 0" xyz="0 -0.035 0"/>
<geometry>
<box size=".34 0.01 .04"/>
</geometry>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material> </visual>
<collision>
<origin rpy="0 0 0" xyz="0 0.035 0"/>
<geometry>
<box size=".34 0.01 .04"/>
</geometry>
</collision>
<collision>
<origin rpy="0 0 0" xyz="0 -0.035 0"/>
<geometry>
<box size=".34 0.01 .04"/>
</geometry>
</collision>
<inertial>
<mass value=".1"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="chassis_right_center" type="fixed">
<axis xyz="0 0 1"/>
<parent link="base_chassis_link"/>
<child link="chassis_right"/>
<origin rpy="-0.0872665 0 0" xyz="0.0 -0.10 0.0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="chassis_left">
<visual>
<origin rpy="0 0 0" xyz="0 0.035 0"/>
<geometry>
<box size=".34 0.01 .04"/>
</geometry>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material> </visual>
<visual>
<origin rpy="0 0 0" xyz="0 -0.035 0"/>
<geometry>
<box size=".34 0.01 .04"/>
</geometry>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material> </visual>
<collision>
<origin rpy="0 0 0" xyz="0 0.035 0"/>
<geometry>
<box size=".34 0.01 .04"/>
</geometry>
</collision>
<collision>
<origin rpy="0 0 0" xyz="0 -0.035 0"/>
<geometry>
<box size=".34 0.01 .04"/>
</geometry>
</collision>
<inertial>
<mass value=".1"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="chassis_left_center" type="fixed">
<axis xyz="0 0 1"/>
<parent link="base_chassis_link"/>
<child link="chassis_left"/>
<origin rpy="0.0872665 0 0" xyz="0.0 0.10 0.0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="motor_front_rightR_link">
<visual>
<geometry>
<mesh filename="tmotor3.obj" scale="1 1 1"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder length="0.026" radius="0.0434"/>
</geometry>
</collision>
<inertial>
<mass value="0.25"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="motor_front_rightR_joint" type="fixed">
<axis xyz="0 0 1"/>
<parent link="chassis_right"/>
<child link="motor_front_rightR_link"/>
<origin rpy="1.57075 -1.570796 0" xyz="0.21 -0.025 0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="motor_front_rightL_link">
<visual>
<geometry>
<mesh filename="tmotor3.obj" scale="1 1 1"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder length="0.026" radius="0.0434"/>
</geometry>
</collision>
<inertial>
<mass value="0.25"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="motor_front_rightL_joint" type="fixed">
<axis xyz="0 0 1"/>
<parent link="chassis_right"/>
<child link="motor_front_rightL_link"/>
<origin rpy="1.57075 -1.570796 3.141592" xyz="0.21 0.04 0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="motor_front_leftR_link">
<visual>
<geometry>
<mesh filename="tmotor3.obj" scale="1 1 1"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder length="0.026" radius="0.0434"/>
</geometry>
</collision>
<inertial>
<mass value="0.25"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="motor_front_leftR_joint" type="fixed">
<axis xyz="0 0 1"/>
<parent link="chassis_left"/>
<child link="motor_front_leftR_link"/>
<origin rpy="1.57075 1.570796 3.141592" xyz="0.21 0.025 0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="motor_front_leftL_link">
<visual>
<geometry>
<mesh filename="tmotor3.obj" scale="1 1 1"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<inertial>
<mass value="0.25"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="motor_front_leftL_joint" type="fixed">
<axis xyz="0 0 1"/>
<parent link="chassis_left"/>
<child link="motor_front_leftL_link"/>
<origin rpy="1.57075 1.570796 0" xyz="0.21 -0.04 0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="motor_back_rightR_link">
<visual>
<geometry>
<mesh filename="tmotor3.obj" scale="1 1 1"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder length="0.026" radius="0.0434"/>
</geometry>
</collision>
<inertial>
<mass value="0.25"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="motor_back_rightR_joint" type="fixed">
<axis xyz="0 0 1"/>
<parent link="chassis_right"/>
<child link="motor_back_rightR_link"/>
<origin rpy="1.57075 -1.570796 0" xyz="-0.21 -0.025 0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="motor_back_rightL_link">
<visual>
<geometry>
<mesh filename="tmotor3.obj" scale="1 1 1"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder length="0.026" radius="0.0434"/>
</geometry>
</collision>
<inertial>
<mass value="0.25"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="motor_back_rightL_joint" type="fixed">
<axis xyz="0 0 1"/>
<parent link="chassis_right"/>
<child link="motor_back_rightL_link"/>
<origin rpy="1.57075 -1.570796 3.141592" xyz="-0.21 0.04 0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="motor_back_leftR_link">
<visual>
<geometry>
<mesh filename="tmotor3.obj" scale="1 1 1"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder length="0.026" radius="0.0434"/>
</geometry>
</collision>
<inertial>
<mass value="0.25"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="motor_back_leftR_joint" type="fixed">
<axis xyz="0 0 1"/>
<parent link="chassis_left"/>
<child link="motor_back_leftR_link"/>
<origin rpy="1.57075 1.570796 3.141592" xyz="-0.21 0.025 0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="motor_back_leftL_link">
<visual>
<geometry>
<mesh filename="tmotor3.obj" scale="1 1 1"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder length="0.026" radius="0.0434"/>
</geometry>
</collision>
<inertial>
<mass value="0.25"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="motor_back_leftL_joint" type="fixed">
<axis xyz="0 0 1"/>
<parent link="chassis_left"/>
<child link="motor_back_leftL_link"/>
<origin rpy="1.57075 1.570796 0" xyz="-0.21 -0.04 0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="upper_leg_front_rightR_link">
<visual>
<geometry>
<box size=".01 0.01 .11"/>
</geometry>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material>
</visual>
<collision>
<geometry>
<box size=".01 0.01 .11"/>
</geometry>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="hip_front_rightR_link" type="fixed">
<axis xyz="0 0 1"/>
<parent link="motor_front_rightR_link"/>
<child link="upper_leg_front_rightR_link"/>
<origin rpy="-1.57075 0 0" xyz="0.0 0.06 -0.015"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="lower_leg_front_rightR_link">
<contact>
<lateral_friction value="1"/>
</contact>
<visual>
<origin rpy="0.0 0 0" xyz="0 0 .1"/>
<geometry>
<box size=".01 0.01 .2"/>
</geometry>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material>
</visual>
<collision>
<origin rpy="0.0 0 0" xyz="0 0 .1"/>
<geometry>
<box size=".01 0.01 .2"/>
</geometry>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="knee_front_rightR_link" type="fixed">
<axis xyz="0 1 0"/>
<parent link="upper_leg_front_rightR_link"/>
<child link="lower_leg_front_rightR_link"/>
<origin rpy="0 -2.1834 0" xyz="0.0 0.01 .055"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="upper_leg_front_rightL_link">
<visual>
<geometry>
<box size=".01 0.01 .11"/>
</geometry>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material>
</visual>
<collision>
<geometry>
<box size=".01 0.01 .11"/>
</geometry>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="motor_front_rightL_link" type="fixed">
<axis xyz="0 0 1"/>
<parent link="motor_front_rightL_link"/>
<child link="upper_leg_front_rightL_link"/>
<origin rpy="-1.57075 0 0" xyz="0.0 0.06 -0.015"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="lower_leg_front_rightL_link">
<contact>
<lateral_friction value="1"/>
</contact>
<visual>
<origin rpy="0.0 0 0" xyz="0 0 .1"/>
<geometry>
<box size=".01 0.01 .198"/>
</geometry>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material>
</visual>
<collision>
<origin rpy="0.0 0 0" xyz="0 0 .1"/>
<geometry>
<box size=".01 0.01 .198"/>
</geometry>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="knee_front_rightL_link" type="fixed">
<axis xyz="0 1 0"/>
<parent link="upper_leg_front_rightL_link"/>
<child link="lower_leg_front_rightL_link"/>
<origin rpy="0 -2.1834 0" xyz="0.0 0.01 .055"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="upper_leg_front_leftR_link">
<visual>
<geometry>
<box size=".01 0.01 .11"/>
</geometry>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material>
</visual>
<collision>
<geometry>
<box size=".01 0.01 .11"/>
</geometry>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="hip_front_leftR_link" type="fixed">
<axis xyz="0 0 1"/>
<parent link="motor_front_leftR_link"/>
<child link="upper_leg_front_leftR_link"/>
<origin rpy="-1.57075 0 0" xyz="0.0 0.06 -0.015"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="lower_leg_front_leftR_link">
<contact>
<lateral_friction value="1"/>
</contact>
<visual>
<origin rpy="0.0 0.0 0" xyz="0 0 .1"/>
<geometry>
<box size=".01 0.01 .2"/>
</geometry>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material>
</visual>
<collision>
<origin rpy="0.0 0.0 0" xyz="0 0 .1"/>
<geometry>
<box size=".01 0.01 .2"/>
</geometry>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="knee_front_leftR_link" type="fixed">
<axis xyz="0 1 0"/>
<parent link="upper_leg_front_leftR_link"/>
<child link="lower_leg_front_leftR_link"/>
<origin rpy="0 2.1834 0" xyz="0.0 0.01 .055"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="upper_leg_front_leftL_link">
<visual>
<geometry>
<box size=".01 0.01 .11"/>
</geometry>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material>
</visual>
<collision>
<geometry>
<box size=".01 0.01 .11"/>
</geometry>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="motor_front_leftL_link" type="fixed">
<axis xyz="0 0 1"/>
<parent link="motor_front_leftL_link"/>
<child link="upper_leg_front_leftL_link"/>
<origin rpy="-1.57075 0 0" xyz="0.0 0.06 -0.015"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="lower_leg_front_leftL_link">
<contact>
<lateral_friction value="1"/>
</contact>
<visual>
<origin rpy="0.0 0.0 0" xyz="0 0 .1"/>
<geometry>
<box size=".01 0.01 .198"/>
</geometry>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material>
</visual>
<collision>
<origin rpy="0.0 0.0 0" xyz="0 0 .1"/>
<geometry>
<box size=".01 0.01 .198"/>
</geometry>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="knee_front_leftL_link" type="fixed">
<axis xyz="0 1 0"/>
<parent link="upper_leg_front_leftL_link"/>
<child link="lower_leg_front_leftL_link"/>
<origin rpy="0 2.1834 0" xyz="0.0 0.01 .055"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="upper_leg_back_rightR_link">
<visual>
<geometry>
<box size=".01 0.01 .11"/>
</geometry>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material>
</visual>
<collision>
<geometry>
<box size=".01 0.01 .11"/>
</geometry>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="hip_rightR_link" type="fixed">
<axis xyz="0 0 1"/>
<parent link="motor_back_rightR_link"/>
<child link="upper_leg_back_rightR_link"/>
<origin rpy="-1.57075 0 0" xyz="0.0 0.06 -0.015"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="lower_leg_back_rightR_link">
<contact>
<lateral_friction value="1"/>
</contact>
<visual>
<origin rpy="0.0 0 0" xyz="0 0 .1"/>
<geometry>
<box size=".01 0.01 .2032"/>
</geometry>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material>
</visual>
<collision>
<origin rpy="0.0 0 0" xyz="0 0 .1"/>
<geometry>
<box size=".01 0.01 .2032"/>
</geometry>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="knee_back_rightR_link" type="fixed">
<axis xyz="0 1 0"/>
<parent link="upper_leg_back_rightR_link"/>
<child link="lower_leg_back_rightR_link"/>
<origin rpy="0 -2.1834 0" xyz="0.0 0.01 .055"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="upper_leg_back_rightL_link">
<visual>
<geometry>
<box size=".01 0.01 .11"/>
</geometry>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material>
</visual>
<collision>
<geometry>
<box size=".01 0.01 .11"/>
</geometry>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="motor_back_rightL_link" type="fixed">
<axis xyz="0 0 1"/>
<parent link="motor_back_rightL_link"/>
<child link="upper_leg_back_rightL_link"/>
<origin rpy="-1.57075 0 0" xyz="0.0 0.06 -0.015"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="lower_leg_back_rightL_link">
<contact>
<lateral_friction value="1"/>
</contact>
<visual>
<origin rpy="0.0 0 0" xyz="0 0 .1"/>
<geometry>
<box size=".01 0.01 .2"/>
</geometry>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material>
</visual>
<collision>
<origin rpy="0.0 0 0" xyz="0 0 .1"/>
<geometry>
<box size=".01 0.01 .2"/>
</geometry>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="knee_back_rightL_link" type="fixed">
<axis xyz="0 1 0"/>
<parent link="upper_leg_back_rightL_link"/>
<child link="lower_leg_back_rightL_link"/>
<origin rpy="0 -2.1834 0" xyz="0.0 0.01 .055"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="upper_leg_back_leftR_link">
<visual>
<geometry>
<box size=".01 0.01 .11"/>
</geometry>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material>
</visual>
<collision>
<geometry>
<box size=".01 0.01 .11"/>
</geometry>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="hip_leftR_link" type="fixed">
<axis xyz="0 0 1"/>
<parent link="motor_back_leftR_link"/>
<child link="upper_leg_back_leftR_link"/>
<origin rpy="-1.57075 0 0" xyz="0.0 0.06 -0.015"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="lower_leg_back_leftR_link">
<contact>
<lateral_friction value="1"/>
</contact>
<visual>
<origin rpy="0.0 0 0" xyz="0 0 .1"/>
<geometry>
<box size=".01 0.01 .2032"/>
</geometry>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material>
</visual>
<collision>
<origin rpy="0.0 0 0" xyz="0 0 .1"/>
<geometry>
<box size=".01 0.01 .2032"/>
</geometry>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="knee_back_leftR_link" type="fixed">
<axis xyz="0 1 0"/>
<parent link="upper_leg_back_leftR_link"/>
<child link="lower_leg_back_leftR_link"/>
<origin rpy="0 2.1834 0" xyz="0.0 0.01 .055"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="upper_leg_back_leftL_link">
<visual>
<geometry>
<box size=".01 0.01 .11"/>
</geometry>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material>
</visual>
<collision>
<geometry>
<box size=".01 0.01 .11"/>
</geometry>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="motor_back_leftL_link" type="fixed">
<axis xyz="0 0 1"/>
<parent link="motor_back_leftL_link"/>
<child link="upper_leg_back_leftL_link"/>
<origin rpy="-1.57075 0 0" xyz="0.0 0.06 -0.015"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="lower_leg_back_leftL_link">
<contact>
<lateral_friction value="1"/>
</contact>
<visual>
<origin rpy="0.0 0 0" xyz="0 0 .1"/>
<geometry>
<box size=".01 0.01 .2"/>
</geometry>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material>
</visual>
<collision>
<origin rpy="0.0 0 0" xyz="0 0 .1"/>
<geometry>
<box size=".01 0.01 .2"/>
</geometry>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="knee_back_leftL_link" type="fixed">
<axis xyz="0 1 0"/>
<parent link="upper_leg_back_leftL_link"/>
<child link="lower_leg_back_leftL_link"/>
<origin rpy="0 2.1834 0" xyz="0.0 0.01 .055"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
</robot>

929
data/quadruped/minitaur_fixed_knees.urdf Normal file
View File

@@ -0,0 +1,929 @@
<?xml version="0.0" ?>
<!-- ======================================================================= -->
<!--LICENSE: -->
<!--Copyright (c) 2017, Erwin Coumans -->
<!--Google Inc. -->
<!--All rights reserved. -->
<!-- -->
<!--Redistribution and use in source and binary forms, with or without -->
<!--modification, are permitted provided that the following conditions are -->
<!--met: -->
<!-- -->
<!--1. Redistributions or derived work must retain this copyright notice, -->
<!-- this list of conditions and the following disclaimer. -->
<!-- -->
<!--2. 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. -->
<!-- -->
<!--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. -->
<robot name="quadruped">
<link name="base_chassis_link">
<visual>
<geometry>
<box size=".33 0.14 .07"/>
</geometry>
<material name="black">
<color rgba="0.3 0.3 0.3 1"/>
</material>
</visual>
<visual>
<origin rpy="0 0 0" xyz="0 0.10 0"/>
<geometry>
<box size=".17 0.10 .05"/>
</geometry>
<material name="black">
<color rgba="0.3 0.3 0.3 1"/>
</material>
</visual>
<visual>
<origin rpy="0 0 0" xyz="0 -0.10 0"/>
<geometry>
<box size=".17 0.10 .05"/>
</geometry>
<material name="black">
<color rgba="0.3 0.3 0.3 1"/>
</material>
</visual>
<collision>
<geometry>
<box size=".34 0.14 .07"/>
</geometry>
</collision>
<collision>
<origin rpy="0 0 0" xyz="0 0.10 0"/>
<geometry>
<box size=".17 0.10 .05"/>
</geometry>
</collision>
<collision>
<origin rpy="0 0 0" xyz="0 -0.10 0"/>
<geometry>
<box size=".17 0.10 .05"/>
</geometry>
</collision>
<inertial>
<mass value="3"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<link name="chassis_right">
<visual>
<origin rpy="0 0 0" xyz="0 0.035 0"/>
<geometry>
<box size=".34 0.01 .04"/>
</geometry>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material> </visual>
<visual>
<origin rpy="0 0 0" xyz="0 -0.035 0"/>
<geometry>
<box size=".34 0.01 .04"/>
</geometry>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material> </visual>
<collision>
<origin rpy="0 0 0" xyz="0 0.035 0"/>
<geometry>
<box size=".34 0.01 .04"/>
</geometry>
</collision>
<collision>
<origin rpy="0 0 0" xyz="0 -0.035 0"/>
<geometry>
<box size=".34 0.01 .04"/>
</geometry>
</collision>
<inertial>
<mass value=".1"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="chassis_right_center" type="fixed">
<axis xyz="0 0 1"/>
<parent link="base_chassis_link"/>
<child link="chassis_right"/>
<origin rpy="-0.0872665 0 0" xyz="0.0 -0.10 0.0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="chassis_left">
<visual>
<origin rpy="0 0 0" xyz="0 0.035 0"/>
<geometry>
<box size=".34 0.01 .04"/>
</geometry>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material> </visual>
<visual>
<origin rpy="0 0 0" xyz="0 -0.035 0"/>
<geometry>
<box size=".34 0.01 .04"/>
</geometry>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material> </visual>
<collision>
<origin rpy="0 0 0" xyz="0 0.035 0"/>
<geometry>
<box size=".34 0.01 .04"/>
</geometry>
</collision>
<collision>
<origin rpy="0 0 0" xyz="0 -0.035 0"/>
<geometry>
<box size=".34 0.01 .04"/>
</geometry>
</collision>
<inertial>
<mass value=".1"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="chassis_left_center" type="fixed">
<axis xyz="0 0 1"/>
<parent link="base_chassis_link"/>
<child link="chassis_left"/>
<origin rpy="0.0872665 0 0" xyz="0.0 0.10 0.0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="motor_front_rightR_link">
<visual>
<geometry>
<mesh filename="tmotor3.obj" scale="1 1 1"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder length="0.026" radius="0.0434"/>
</geometry>
</collision>
<inertial>
<mass value="0.25"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="motor_front_rightR_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="chassis_right"/>
<child link="motor_front_rightR_link"/>
<origin rpy="1.57075 0 0" xyz="0.21 -0.025 0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="motor_front_rightL_link">
<visual>
<geometry>
<mesh filename="tmotor3.obj" scale="1 1 1"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder length="0.026" radius="0.0434"/>
</geometry>
</collision>
<inertial>
<mass value="0.25"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="motor_front_rightL_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="chassis_right"/>
<child link="motor_front_rightL_link"/>
<origin rpy="1.57075 0 3.141592" xyz="0.21 0.04 0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="motor_front_leftR_link">
<visual>
<geometry>
<mesh filename="tmotor3.obj" scale="1 1 1"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder length="0.026" radius="0.0434"/>
</geometry>
</collision>
<inertial>
<mass value="0.25"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="motor_front_leftR_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="chassis_left"/>
<child link="motor_front_leftR_link"/>
<origin rpy="1.57075 0 3.141592" xyz="0.21 0.025 0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="motor_front_leftL_link">
<visual>
<geometry>
<mesh filename="tmotor3.obj" scale="1 1 1"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<inertial>
<mass value="0.25"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="motor_front_leftL_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="chassis_left"/>
<child link="motor_front_leftL_link"/>
<origin rpy="1.57075 0 0" xyz="0.21 -0.04 0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="motor_back_rightR_link">
<visual>
<geometry>
<mesh filename="tmotor3.obj" scale="1 1 1"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder length="0.026" radius="0.0434"/>
</geometry>
</collision>
<inertial>
<mass value="0.25"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="motor_back_rightR_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="chassis_right"/>
<child link="motor_back_rightR_link"/>
<origin rpy="1.57075 0 0" xyz="-0.21 -0.025 0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="motor_back_rightL_link">
<visual>
<geometry>
<mesh filename="tmotor3.obj" scale="1 1 1"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder length="0.026" radius="0.0434"/>
</geometry>
</collision>
<inertial>
<mass value="0.25"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="motor_back_rightL_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="chassis_right"/>
<child link="motor_back_rightL_link"/>
<origin rpy="1.57075 0 3.141592" xyz="-0.21 0.04 0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="motor_back_leftR_link">
<visual>
<geometry>
<mesh filename="tmotor3.obj" scale="1 1 1"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder length="0.026" radius="0.0434"/>
</geometry>
</collision>
<inertial>
<mass value="0.25"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="motor_back_leftR_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="chassis_left"/>
<child link="motor_back_leftR_link"/>
<origin rpy="1.57075 0 3.141592" xyz="-0.21 0.025 0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="motor_back_leftL_link">
<visual>
<geometry>
<mesh filename="tmotor3.obj" scale="1 1 1"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder length="0.026" radius="0.0434"/>
</geometry>
</collision>
<inertial>
<mass value="0.25"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="motor_back_leftL_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="chassis_left"/>
<child link="motor_back_leftL_link"/>
<origin rpy="1.57075 0 0" xyz="-0.21 -0.04 0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="upper_leg_front_rightR_link">
<visual>
<geometry>
<box size=".01 0.01 .11"/>
</geometry>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material>
</visual>
<collision>
<geometry>
<box size=".01 0.01 .11"/>
</geometry>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="hip_front_rightR_link" type="fixed">
<axis xyz="0 0 1"/>
<parent link="motor_front_rightR_link"/>
<child link="upper_leg_front_rightR_link"/>
<origin rpy="-1.57075 0 0" xyz="0.0 0.06 -0.015"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="lower_leg_front_rightR_link">
<contact>
<stiffness value="10000"/>
<damping value="10"/>
<lateral_friction value="1"/>
</contact>
<visual>
<origin rpy="0.0 0 0" xyz="0 0 .1"/>
<geometry>
<box size=".01 0.01 .2"/>
</geometry>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material>
</visual>
<collision>
<origin rpy="0.0 0 0" xyz="0 0 .1"/>
<geometry>
<box size=".01 0.01 .2"/>
</geometry>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="knee_front_rightR_link" type="fixed">
<axis xyz="0 1 0"/>
<parent link="upper_leg_front_rightR_link"/>
<child link="lower_leg_front_rightR_link"/>
<origin rpy="0 -2.1834 0" xyz="0.0 0.01 .055"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="upper_leg_front_rightL_link">
<visual>
<geometry>
<box size=".01 0.01 .11"/>
</geometry>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material>
</visual>
<collision>
<geometry>
<box size=".01 0.01 .11"/>
</geometry>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="motor_front_rightL_link" type="fixed">
<axis xyz="0 0 1"/>
<parent link="motor_front_rightL_link"/>
<child link="upper_leg_front_rightL_link"/>
<origin rpy="-1.57075 0 0" xyz="0.0 0.06 -0.015"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="lower_leg_front_rightL_link">
<contact>
<stiffness value="10000"/>
<damping value="10"/>
<lateral_friction value="1"/>
</contact>
<visual>
<origin rpy="0.0 0 0" xyz="0 0 .1"/>
<geometry>
<box size=".01 0.01 .198"/>
</geometry>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material>
</visual>
<collision>
<origin rpy="0.0 0 0" xyz="0 0 .1"/>
<geometry>
<box size=".01 0.01 .198"/>
</geometry>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="knee_front_rightL_link" type="fixed">
<axis xyz="0 1 0"/>
<parent link="upper_leg_front_rightL_link"/>
<child link="lower_leg_front_rightL_link"/>
<origin rpy="0 -2.1834 0" xyz="0.0 0.01 .055"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="upper_leg_front_leftR_link">
<visual>
<geometry>
<box size=".01 0.01 .11"/>
</geometry>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material>
</visual>
<collision>
<geometry>
<box size=".01 0.01 .11"/>
</geometry>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="hip_front_leftR_link" type="fixed">
<axis xyz="0 0 1"/>
<parent link="motor_front_leftR_link"/>
<child link="upper_leg_front_leftR_link"/>
<origin rpy="-1.57075 0 0" xyz="0.0 0.06 -0.015"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="lower_leg_front_leftR_link">
<contact>
<stiffness value="10000"/>
<damping value="10"/>
<lateral_friction value="1"/>
</contact>
<visual>
<origin rpy="0.0 0.0 0" xyz="0 0 .1"/>
<geometry>
<box size=".01 0.01 .2"/>
</geometry>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material>
</visual>
<collision>
<origin rpy="0.0 0.0 0" xyz="0 0 .1"/>
<geometry>
<box size=".01 0.01 .2"/>
</geometry>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="knee_front_leftR_link" type="fixed">
<axis xyz="0 1 0"/>
<parent link="upper_leg_front_leftR_link"/>
<child link="lower_leg_front_leftR_link"/>
<origin rpy="0 2.1834 0" xyz="0.0 0.01 .055"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="upper_leg_front_leftL_link">
<visual>
<geometry>
<box size=".01 0.01 .11"/>
</geometry>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material>
</visual>
<collision>
<geometry>
<box size=".01 0.01 .11"/>
</geometry>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="motor_front_leftL_link" type="fixed">
<axis xyz="0 0 1"/>
<parent link="motor_front_leftL_link"/>
<child link="upper_leg_front_leftL_link"/>
<origin rpy="-1.57075 0 0" xyz="0.0 0.06 -0.015"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="lower_leg_front_leftL_link">
<contact>
<stiffness value="10000"/>
<damping value="10"/>
<lateral_friction value="1"/>
</contact>
<visual>
<origin rpy="0.0 0.0 0" xyz="0 0 .1"/>
<geometry>
<box size=".01 0.01 .198"/>
</geometry>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material>
</visual>
<collision>
<origin rpy="0.0 0.0 0" xyz="0 0 .1"/>
<geometry>
<box size=".01 0.01 .198"/>
</geometry>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="knee_front_leftL_link" type="fixed">
<axis xyz="0 1 0"/>
<parent link="upper_leg_front_leftL_link"/>
<child link="lower_leg_front_leftL_link"/>
<origin rpy="0 2.1834 0" xyz="0.0 0.01 .055"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="upper_leg_back_rightR_link">
<visual>
<geometry>
<box size=".01 0.01 .11"/>
</geometry>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material>
</visual>
<collision>
<geometry>
<box size=".01 0.01 .11"/>
</geometry>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="hip_rightR_link" type="fixed">
<axis xyz="0 0 1"/>
<parent link="motor_back_rightR_link"/>
<child link="upper_leg_back_rightR_link"/>
<origin rpy="-1.57075 0 0" xyz="0.0 0.06 -0.015"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="lower_leg_back_rightR_link">
<contact>
<stiffness value="10000"/>
<damping value="10"/>
<lateral_friction value="1"/>
</contact>
<visual>
<origin rpy="0.0 0 0" xyz="0 0 .1"/>
<geometry>
<box size=".01 0.01 .2032"/>
</geometry>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material>
</visual>
<collision>
<origin rpy="0.0 0 0" xyz="0 0 .1"/>
<geometry>
<box size=".01 0.01 .2032"/>
</geometry>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="knee_back_rightR_link" type="fixed">
<axis xyz="0 1 0"/>
<parent link="upper_leg_back_rightR_link"/>
<child link="lower_leg_back_rightR_link"/>
<origin rpy="0 -2.1834 0" xyz="0.0 0.01 .055"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="upper_leg_back_rightL_link">
<visual>
<geometry>
<box size=".01 0.01 .11"/>
</geometry>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material>
</visual>
<collision>
<geometry>
<box size=".01 0.01 .11"/>
</geometry>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="motor_back_rightL_link" type="fixed">
<axis xyz="0 0 1"/>
<parent link="motor_back_rightL_link"/>
<child link="upper_leg_back_rightL_link"/>
<origin rpy="-1.57075 0 0" xyz="0.0 0.06 -0.015"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="lower_leg_back_rightL_link">
<contact>
<stiffness value="10000"/>
<damping value="10"/>
<lateral_friction value="1"/>
</contact>
<visual>
<origin rpy="0.0 0 0" xyz="0 0 .1"/>
<geometry>
<box size=".01 0.01 .2"/>
</geometry>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material>
</visual>
<collision>
<origin rpy="0.0 0 0" xyz="0 0 .1"/>
<geometry>
<box size=".01 0.01 .2"/>
</geometry>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="knee_back_rightL_link" type="fixed">
<axis xyz="0 1 0"/>
<parent link="upper_leg_back_rightL_link"/>
<child link="lower_leg_back_rightL_link"/>
<origin rpy="0 -2.1834 0" xyz="0.0 0.01 .055"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="upper_leg_back_leftR_link">
<visual>
<geometry>
<box size=".01 0.01 .11"/>
</geometry>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material>
</visual>
<collision>
<geometry>
<box size=".01 0.01 .11"/>
</geometry>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="hip_leftR_link" type="fixed">
<axis xyz="0 0 1"/>
<parent link="motor_back_leftR_link"/>
<child link="upper_leg_back_leftR_link"/>
<origin rpy="-1.57075 0 0" xyz="0.0 0.06 -0.015"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="lower_leg_back_leftR_link">
<contact>
<stiffness value="10000"/>
<damping value="10"/>
<lateral_friction value="1"/>
</contact>
<visual>
<origin rpy="0.0 0 0" xyz="0 0 .1"/>
<geometry>
<box size=".01 0.01 .2032"/>
</geometry>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material>
</visual>
<collision>
<origin rpy="0.0 0 0" xyz="0 0 .1"/>
<geometry>
<box size=".01 0.01 .2032"/>
</geometry>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="knee_back_leftR_link" type="fixed">
<axis xyz="0 1 0"/>
<parent link="upper_leg_back_leftR_link"/>
<child link="lower_leg_back_leftR_link"/>
<origin rpy="0 2.1834 0" xyz="0.0 0.01 .055"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="upper_leg_back_leftL_link">
<visual>
<geometry>
<box size=".01 0.01 .11"/>
</geometry>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material>
</visual>
<collision>
<geometry>
<box size=".01 0.01 .11"/>
</geometry>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="motor_back_leftL_link" type="fixed">
<axis xyz="0 0 1"/>
<parent link="motor_back_leftL_link"/>
<child link="upper_leg_back_leftL_link"/>
<origin rpy="-1.57075 0 0" xyz="0.0 0.06 -0.015"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="lower_leg_back_leftL_link">
<contact>
<stiffness value="10000"/>
<damping value="10"/>
<lateral_friction value="1"/>
</contact>
<visual>
<origin rpy="0.0 0 0" xyz="0 0 .1"/>
<geometry>
<box size=".01 0.01 .2"/>
</geometry>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material>
</visual>
<collision>
<origin rpy="0.0 0 0" xyz="0 0 .1"/>
<geometry>
<box size=".01 0.01 .2"/>
</geometry>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="knee_back_leftL_link" type="fixed">
<axis xyz="0 1 0"/>
<parent link="upper_leg_back_leftL_link"/>
<child link="lower_leg_back_leftL_link"/>
<origin rpy="0 2.1834 0" xyz="0.0 0.01 .055"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
</robot>

2
examples/RobotSimulator/b3RobotSimulatorClientAPI.h
View File

@@ -146,7 +146,7 @@ public:
b3Vector3 getEulerFromQuaternion(const b3Quaternion& quat); b3Vector3 getEulerFromQuaternion(const b3Quaternion& quat);
int loadURDF(const std::string& fileName, const struct b3RobotSimulatorLoadUrdfFileArgs& args=b3RobotSimulatorLoadUrdfFileArgs()); int loadURDF(const std::string& fileName, const struct b3RobotSimulatorLoadUrdfFileArgs& args=b3RobotSimulatorLoadUrdfFileArgs());
bool loadSDF(const std::string& fileName, b3RobotSimulatorLoadFileResults& results, const struct b3RobotSimulatorLoadSdfFileArgs& args=b3RobotSimulatorLoadSdfFileArgs()); bool loadSDF(const std::string& fileName, b3RobotSimulatorLoadFileResults& results=b3RobotSimulatorLoadFileResults(), const struct b3RobotSimulatorLoadSdfFileArgs& args=b3RobotSimulatorLoadSdfFileArgs());
bool loadMJCF(const std::string& fileName, b3RobotSimulatorLoadFileResults& results); bool loadMJCF(const std::string& fileName, b3RobotSimulatorLoadFileResults& results);
bool loadBullet(const std::string& fileName, b3RobotSimulatorLoadFileResults& results); bool loadBullet(const std::string& fileName, b3RobotSimulatorLoadFileResults& results);

14
examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp
View File

@@ -218,8 +218,18 @@ updateVertex(
} }
if (i.vt_idx >= 0) { if (i.vt_idx >= 0) {
texcoords.push_back(in_texcoords[2*i.vt_idx+0]); int numTexCoords = in_texcoords.size();
texcoords.push_back(in_texcoords[2*i.vt_idx+1]); int index0 = 2*i.vt_idx+0;
int index1 = 2*i.vt_idx+1;
if (index0>=0 && (index0)<numTexCoords)
{
texcoords.push_back(in_texcoords[index0]);
}
if (index1>=0 && (index1)<numTexCoords)
{
texcoords.push_back(in_texcoords[index1]);
}
} }
unsigned int idx = positions.size() / 3 - 1; unsigned int idx = positions.size() / 3 - 1;

89
examples/pybullet/minitaur.py
View File

@@ -17,10 +17,10 @@ class Minitaur:
p.stepSimulation() p.stepSimulation()
def buildMotorIdList(self): def buildMotorIdList(self):
self.motorIdList.append(self.jointNameToId['motor_front_leftR_joint'])
self.motorIdList.append(self.jointNameToId['motor_front_leftL_joint']) self.motorIdList.append(self.jointNameToId['motor_front_leftL_joint'])
self.motorIdList.append(self.jointNameToId['motor_back_leftR_joint']) self.motorIdList.append(self.jointNameToId['motor_front_leftR_joint'])
self.motorIdList.append(self.jointNameToId['motor_back_leftL_joint']) self.motorIdList.append(self.jointNameToId['motor_back_leftL_joint'])
self.motorIdList.append(self.jointNameToId['motor_back_leftR_joint'])
self.motorIdList.append(self.jointNameToId['motor_front_rightL_joint']) self.motorIdList.append(self.jointNameToId['motor_front_rightL_joint'])
self.motorIdList.append(self.jointNameToId['motor_front_rightR_joint']) self.motorIdList.append(self.jointNameToId['motor_front_rightR_joint'])
self.motorIdList.append(self.jointNameToId['motor_back_rightL_joint']) self.motorIdList.append(self.jointNameToId['motor_back_rightL_joint'])
@@ -28,21 +28,17 @@ class Minitaur:
def reset(self): def reset(self):
self.quadruped = p.loadURDF("%s/quadruped/quadruped.urdf" % self.urdfRootPath,0,0,.3) self.quadruped = p.loadURDF("%s/quadruped/minitaur.urdf" % self.urdfRootPath,0,0,.2)
self.kp = 1 self.kp = 1
self.kd = 0.1 self.kd = 0.1
self.maxForce = 3.5 self.maxForce = 3.5
self.nMotors = 8 self.nMotors = 8
self.motorIdList = [] self.motorIdList = []
self.motorDir = [1, -1, 1, -1, -1, 1, -1, 1] self.motorDir = [1, 1, 1, 1, 1, 1, 1, 1]
self.buildJointNameToIdDict() self.buildJointNameToIdDict()
self.buildMotorIdList() self.buildMotorIdList()
def disableAllMotors(self):
nJoints = p.getNumJoints(self.quadruped)
for i in range(nJoints):
p.setJointMotorControl2(bodyIndex=self.quadruped, jointIndex=i, controlMode=p.VELOCITY_CONTROL, force=0)
def setMotorAngleById(self, motorId, desiredAngle): def setMotorAngleById(self, motorId, desiredAngle):
p.setJointMotorControl2(bodyIndex=self.quadruped, jointIndex=motorId, controlMode=p.POSITION_CONTROL, targetPosition=desiredAngle, positionGain=self.kp, velocityGain=self.kd, force=self.maxForce) p.setJointMotorControl2(bodyIndex=self.quadruped, jointIndex=motorId, controlMode=p.POSITION_CONTROL, targetPosition=desiredAngle, positionGain=self.kp, velocityGain=self.kd, force=self.maxForce)
@@ -51,42 +47,55 @@ class Minitaur:
self.setMotorAngleById(self.jointNameToId[motorName], desiredAngle) self.setMotorAngleById(self.jointNameToId[motorName], desiredAngle)
def resetPose(self): def resetPose(self):
#right front leg kneeFrictionForce = 0
self.disableAllMotors() halfpi = 1.57079632679
p.resetJointState(self.quadruped,self.jointNameToId['motor_front_rightR_joint'],1.57) kneeangle = -2.1834 #halfpi - acos(upper_leg_length / lower_leg_length)
p.resetJointState(self.quadruped,self.jointNameToId['knee_front_rightR_link'],-2.2)
p.resetJointState(self.quadruped,self.jointNameToId['motor_front_rightL_joint'],-1.57)
p.resetJointState(self.quadruped,self.jointNameToId['knee_front_rightL_link'],2.2)
p.createConstraint(self.quadruped,self.jointNameToId['knee_front_rightR_link'],self.quadruped,self.jointNameToId['knee_front_rightL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,0.01,0.2],[0,-0.015,0.2])
self.setMotorAngleByName('motor_front_rightR_joint', 1.57)
self.setMotorAngleByName('motor_front_rightL_joint',-1.57)
#left front leg #left front leg
p.resetJointState(self.quadruped,self.jointNameToId['motor_front_leftR_joint'],1.57) p.resetJointState(self.quadruped,self.jointNameToId['motor_front_leftL_joint'],self.motorDir[0]*halfpi)
p.resetJointState(self.quadruped,self.jointNameToId['knee_front_leftR_link'],-2.2) p.resetJointState(self.quadruped,self.jointNameToId['knee_front_leftL_link'],self.motorDir[0]*kneeangle)
p.resetJointState(self.quadruped,self.jointNameToId['motor_front_leftL_joint'],-1.57) p.resetJointState(self.quadruped,self.jointNameToId['motor_front_leftR_joint'],self.motorDir[1]*halfpi)
p.resetJointState(self.quadruped,self.jointNameToId['knee_front_leftL_link'],2.2) p.resetJointState(self.quadruped,self.jointNameToId['knee_front_leftR_link'],self.motorDir[1]*kneeangle)
p.createConstraint(self.quadruped,self.jointNameToId['knee_front_leftR_link'],self.quadruped,self.jointNameToId['knee_front_leftL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,-0.01,0.2],[0,0.015,0.2]) p.createConstraint(self.quadruped,self.jointNameToId['knee_front_leftR_link'],self.quadruped,self.jointNameToId['knee_front_leftL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2])
self.setMotorAngleByName('motor_front_leftR_joint', 1.57) self.setMotorAngleByName('motor_front_leftL_joint', self.motorDir[0]*halfpi)
self.setMotorAngleByName('motor_front_leftL_joint',-1.57) self.setMotorAngleByName('motor_front_leftR_joint', self.motorDir[1]*halfpi)
p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_front_leftL_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce)
#right back leg p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_front_leftR_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce)
p.resetJointState(self.quadruped,self.jointNameToId['motor_back_rightR_joint'],1.57)
p.resetJointState(self.quadruped,self.jointNameToId['knee_back_rightR_link'],-2.2)
p.resetJointState(self.quadruped,self.jointNameToId['motor_back_rightL_joint'],-1.57)
p.resetJointState(self.quadruped,self.jointNameToId['knee_back_rightL_link'],2.2)
p.createConstraint(self.quadruped,self.jointNameToId['knee_back_rightR_link'],self.quadruped,self.jointNameToId['knee_back_rightL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,0.01,0.2],[0,-0.015,0.2])
self.setMotorAngleByName('motor_back_rightR_joint', 1.57)
self.setMotorAngleByName('motor_back_rightL_joint',-1.57)
#left back leg #left back leg
p.resetJointState(self.quadruped,self.jointNameToId['motor_back_leftR_joint'],1.57) p.resetJointState(self.quadruped,self.jointNameToId['motor_back_leftL_joint'],self.motorDir[2]*halfpi)
p.resetJointState(self.quadruped,self.jointNameToId['knee_back_leftR_link'],-2.2) p.resetJointState(self.quadruped,self.jointNameToId['knee_back_leftL_link'],self.motorDir[2]*kneeangle)
p.resetJointState(self.quadruped,self.jointNameToId['motor_back_leftL_joint'],-1.57) p.resetJointState(self.quadruped,self.jointNameToId['motor_back_leftR_joint'],self.motorDir[3]*halfpi)
p.resetJointState(self.quadruped,self.jointNameToId['knee_back_leftL_link'],2.2) p.resetJointState(self.quadruped,self.jointNameToId['knee_back_leftR_link'],self.motorDir[3]*kneeangle)
p.createConstraint(self.quadruped,self.jointNameToId['knee_back_leftR_link'],self.quadruped,self.jointNameToId['knee_back_leftL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,-0.01,0.2],[0,0.015,0.2]) p.createConstraint(self.quadruped,self.jointNameToId['knee_back_leftR_link'],self.quadruped,self.jointNameToId['knee_back_leftL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2])
self.setMotorAngleByName('motor_back_leftR_joint', 1.57) self.setMotorAngleByName('motor_back_leftL_joint',self.motorDir[2]*halfpi)
self.setMotorAngleByName('motor_back_leftL_joint',-1.57) self.setMotorAngleByName('motor_back_leftR_joint',self.motorDir[3]*halfpi)
p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_back_leftL_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce)
p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_back_leftR_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce)
#right front leg
p.resetJointState(self.quadruped,self.jointNameToId['motor_front_rightL_joint'],self.motorDir[4]*halfpi)
p.resetJointState(self.quadruped,self.jointNameToId['knee_front_rightL_link'],self.motorDir[4]*kneeangle)
p.resetJointState(self.quadruped,self.jointNameToId['motor_front_rightR_joint'],self.motorDir[5]*halfpi)
p.resetJointState(self.quadruped,self.jointNameToId['knee_front_rightR_link'],self.motorDir[5]*kneeangle)
p.createConstraint(self.quadruped,self.jointNameToId['knee_front_rightR_link'],self.quadruped,self.jointNameToId['knee_front_rightL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2])
self.setMotorAngleByName('motor_front_rightL_joint',self.motorDir[4]*halfpi)
self.setMotorAngleByName('motor_front_rightR_joint',self.motorDir[5]*halfpi)
p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_front_rightL_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce)
p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_front_rightR_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce)
#right back leg
p.resetJointState(self.quadruped,self.jointNameToId['motor_back_rightL_joint'],self.motorDir[6]*halfpi)
p.resetJointState(self.quadruped,self.jointNameToId['knee_back_rightL_link'],self.motorDir[6]*kneeangle)
p.resetJointState(self.quadruped,self.jointNameToId['motor_back_rightR_joint'],self.motorDir[7]*halfpi)
p.resetJointState(self.quadruped,self.jointNameToId['knee_back_rightR_link'],self.motorDir[7]*kneeangle)
p.createConstraint(self.quadruped,self.jointNameToId['knee_back_rightR_link'],self.quadruped,self.jointNameToId['knee_back_rightL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2])
self.setMotorAngleByName('motor_back_rightL_joint',self.motorDir[6]*halfpi)
self.setMotorAngleByName('motor_back_rightR_joint',self.motorDir[7]*halfpi)
p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_back_rightL_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce)
p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_back_rightR_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce)
def getBasePosition(self): def getBasePosition(self):
position, orientation = p.getBasePositionAndOrientation(self.quadruped) position, orientation = p.getBasePositionAndOrientation(self.quadruped)

2
examples/pybullet/minitaur_evaluate.py
View File

@@ -58,7 +58,7 @@ evaluate_func_map['evaluate_desired_motorAngle_hop'] = evaluate_desired_motorAng
def evaluate_params(evaluateFunc, params, objectiveParams, urdfRoot='', timeStep=0.01, maxNumSteps=1000, sleepTime=0): def evaluate_params(evaluateFunc, params, objectiveParams, urdfRoot='', timeStep=0.01, maxNumSteps=10000, sleepTime=0):
print('start evaluation') print('start evaluation')
beforeTime = time.time() beforeTime = time.time()
p.resetSimulation() p.resetSimulation()

4
examples/pybullet/minitaur_test.py
View File

@@ -1,3 +1,7 @@
import sys
#some python interpreters need '.' added
sys.path.append(".")
import pybullet as p import pybullet as p
from minitaur import Minitaur from minitaur import Minitaur
from minitaur_evaluate import * from minitaur_evaluate import *

21
examples/pybullet/mylittleminitaur.py Normal file
View File

@@ -0,0 +1,21 @@
import pybullet as p
import time
import math
import sys
sys.path.append(".")
from minitaur import Minitaur
p.connect(p.GUI)
p.setTimeOut(5)
#p.setPhysicsEngineParameter(numSolverIterations=50)
p.setGravity(0,0,-10)
p.setTimeStep(0.01)
urdfRoot = ''
p.loadURDF("%s/plane.urdf" % urdfRoot)
minitaur = Minitaur(urdfRoot)
while (True):
p.stepSimulation()
time.sleep(0.01)

121
examples/pybullet/quadruped.py
View File

@@ -3,28 +3,31 @@ import time
import math import math
useRealTime = 0 useRealTime = 0
fixedTimeStep = 0.001 fixedTimeStep = 0.01
speed = 10 speed = 10
amplitude = 0.8 amplitude = 0.8
jump_amp = 0.5 jump_amp = 0.5
maxForce = 3.5 maxForce = 3.5
kneeFrictionForce = 0.00 kneeFrictionForce = 0.00
kp = .05 kp = 1
kd = .5 kd = .1
physId = p.connect(p.SHARED_MEMORY) physId = p.connect(p.SHARED_MEMORY)
if (physId<0): if (physId<0):
p.connect(p.GUI) p.connect(p.GUI)
#p.resetSimulation() #p.resetSimulation()
p.loadURDF("plane.urdf",0,0,0)
p.setPhysicsEngineParameter(numSolverIterations=50)
p.setTimeOut(4) p.setTimeOut(4)
p.loadURDF("plane.urdf",0,0,0.1)
#p.loadSDF("kitchens/1.sdf")
p.setGravity(0,0,0) p.setGravity(0,0,0)
p.setTimeStep(fixedTimeStep) p.setTimeStep(fixedTimeStep)
orn = p.getQuaternionFromEuler([0,0,0.4])
p.setRealTimeSimulation(0) p.setRealTimeSimulation(0)
quadruped = p.loadURDF("quadruped/minitaur.urdf",[0,0,0.2],useFixedBase=False) quadruped = p.loadURDF("quadruped/minitaur.urdf",[1,0,0.2],orn,useFixedBase=False)
nJoints = p.getNumJoints(quadruped) nJoints = p.getNumJoints(quadruped)
jointNameToId = {} jointNameToId = {}
@@ -58,47 +61,68 @@ motor_back_leftL_joint = jointNameToId['motor_back_leftL_joint']
motor_back_leftL_link = jointNameToId['motor_back_leftL_link'] motor_back_leftL_link = jointNameToId['motor_back_leftL_link']
knee_back_leftL_link = jointNameToId['knee_back_leftL_link'] knee_back_leftL_link = jointNameToId['knee_back_leftL_link']
motordir=[-1,-1,-1,-1,1,1,1,1]
halfpi = 1.57079632679
kneeangle = -2.1834
p.resetJointState(quadruped,motor_front_leftL_joint,motordir[0]*halfpi)
p.resetJointState(quadruped,knee_front_leftL_link,motordir[0]*kneeangle)
p.resetJointState(quadruped,motor_front_leftR_joint,motordir[1]*halfpi)
p.resetJointState(quadruped,knee_front_leftR_link,motordir[1]*kneeangle)
cid = p.createConstraint(quadruped,knee_front_leftR_link,quadruped,knee_front_leftL_link,p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2])
p.changeConstraint(cid,maxForce=10000)
p.setJointMotorControl(quadruped,knee_front_leftL_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
p.setJointMotorControl(quadruped,knee_front_leftR_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
p.resetJointState(quadruped,motor_back_leftL_joint,motordir[2]*halfpi)
p.resetJointState(quadruped,knee_back_leftL_link,motordir[2]*kneeangle)
p.resetJointState(quadruped,motor_back_leftR_joint,motordir[3]*halfpi)
p.resetJointState(quadruped,knee_back_leftR_link,motordir[3]*kneeangle)
cid = p.createConstraint(quadruped,knee_back_leftR_link,quadruped,knee_back_leftL_link,p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2])
p.changeConstraint(cid,maxForce=10000)
p.setJointMotorControl(quadruped,knee_back_leftL_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
p.setJointMotorControl(quadruped,knee_back_leftR_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
#p.getNumJoints(1) #p.getNumJoints(1)
p.resetJointState(quadruped,motor_front_rightR_joint,1.57)
p.resetJointState(quadruped,knee_front_rightR_link,-2.2)
p.resetJointState(quadruped,motor_front_rightL_joint,1.57) p.resetJointState(quadruped,motor_front_rightL_joint,motordir[4]*halfpi)
p.resetJointState(quadruped,knee_front_rightL_link,-2.2) p.resetJointState(quadruped,knee_front_rightL_link,motordir[4]*kneeangle)
p.createConstraint(quadruped,knee_front_rightR_link,quadruped,knee_front_rightL_link,p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2]) p.resetJointState(quadruped,motor_front_rightR_joint,motordir[5]*halfpi)
p.setJointMotorControl(quadruped,knee_front_rightR_link,p.VELOCITY_CONTROL,0,kneeFrictionForce) p.resetJointState(quadruped,knee_front_rightR_link,motordir[5]*kneeangle)
cid = p.createConstraint(quadruped,knee_front_rightR_link,quadruped,knee_front_rightL_link,p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2])
p.changeConstraint(cid,maxForce=10000)
p.setJointMotorControl(quadruped,knee_front_rightL_link,p.VELOCITY_CONTROL,0,kneeFrictionForce) p.setJointMotorControl(quadruped,knee_front_rightL_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
p.setJointMotorControl(quadruped,knee_front_rightR_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
p.resetJointState(quadruped,motor_front_leftR_joint,-1.57) p.resetJointState(quadruped,motor_back_rightL_joint,motordir[6]*halfpi)
p.resetJointState(quadruped,knee_front_leftR_link,2.2) p.resetJointState(quadruped,knee_back_rightL_link,motordir[6]*kneeangle)
p.resetJointState(quadruped,motor_front_leftL_joint,-1.57) p.resetJointState(quadruped,motor_back_rightR_joint,motordir[7]*halfpi)
p.resetJointState(quadruped,knee_front_leftL_link,2.2) p.resetJointState(quadruped,knee_back_rightR_link,motordir[7]*kneeangle)
p.createConstraint(quadruped,knee_front_leftR_link,quadruped,knee_front_leftL_link,p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2]) cid = p.createConstraint(quadruped,knee_back_rightR_link,quadruped,knee_back_rightL_link,p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2])
p.setJointMotorControl(quadruped,knee_front_leftR_link,p.VELOCITY_CONTROL,0,kneeFrictionForce) p.changeConstraint(cid,maxForce=10000)
p.setJointMotorControl(quadruped,knee_front_leftL_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
p.resetJointState(quadruped,motor_back_rightR_joint,1.57)
p.resetJointState(quadruped,knee_back_rightR_link,-2.2)
p.resetJointState(quadruped,motor_back_rightL_joint,1.57)
p.resetJointState(quadruped,knee_back_rightL_link,-2.2)
p.createConstraint(quadruped,knee_back_rightR_link,quadruped,knee_back_rightL_link,p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2])
p.setJointMotorControl(quadruped,knee_back_rightR_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
p.setJointMotorControl(quadruped,knee_back_rightL_link,p.VELOCITY_CONTROL,0,kneeFrictionForce) p.setJointMotorControl(quadruped,knee_back_rightL_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
p.setJointMotorControl(quadruped,knee_back_rightR_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
p.resetJointState(quadruped,motor_back_leftR_joint,-1.57) p.setGravity(0,0,-10)
p.resetJointState(quadruped,knee_back_leftR_link,2.2)
p.resetJointState(quadruped,motor_back_leftL_joint,-1.57)
p.resetJointState(quadruped,knee_back_leftL_link,2.2)
p.createConstraint(quadruped,knee_back_leftR_link,quadruped,knee_back_leftL_link,p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2])
p.setJointMotorControl(quadruped,motor_back_leftR_joint,p.POSITION_CONTROL,-1.57,maxForce)
p.setJointMotorControl(quadruped,knee_back_leftR_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
p.setJointMotorControl(quadruped,motor_back_leftL_joint,p.POSITION_CONTROL,-1.57,maxForce)
p.setJointMotorControl(quadruped,knee_back_leftL_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
motordir=[-1,-1,-1,-1,1,1,1,1] legnumbering=[
legnumbering=[motor_front_leftR_joint,motor_front_leftL_joint,motor_back_leftR_joint,motor_back_leftL_joint,motor_front_rightR_joint,motor_front_rightL_joint,motor_back_rightR_joint,motor_back_rightL_joint] motor_front_leftL_joint,
motor_front_leftR_joint,
motor_back_leftL_joint,
motor_back_leftR_joint,
motor_front_rightL_joint,
motor_front_rightR_joint,
motor_back_rightL_joint,
motor_back_rightR_joint]
for i in range (8):
print (legnumbering[i])
#use the Minitaur leg numbering #use the Minitaur leg numbering
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[0],controlMode=p.POSITION_CONTROL,targetPosition=motordir[0]*1.57,positionGain=kp, velocityGain=kd, force=maxForce) p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[0],controlMode=p.POSITION_CONTROL,targetPosition=motordir[0]*1.57,positionGain=kp, velocityGain=kd, force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[1],controlMode=p.POSITION_CONTROL,targetPosition=motordir[1]*1.57,positionGain=kp, velocityGain=kd, force=maxForce) p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[1],controlMode=p.POSITION_CONTROL,targetPosition=motordir[1]*1.57,positionGain=kp, velocityGain=kd, force=maxForce)
@@ -108,9 +132,12 @@ p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[4],controlMo
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[5],controlMode=p.POSITION_CONTROL,targetPosition=motordir[5]*1.57,positionGain=kp, velocityGain=kd, force=maxForce) p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[5],controlMode=p.POSITION_CONTROL,targetPosition=motordir[5]*1.57,positionGain=kp, velocityGain=kd, force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[6],controlMode=p.POSITION_CONTROL,targetPosition=motordir[6]*1.57,positionGain=kp, velocityGain=kd, force=maxForce) p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[6],controlMode=p.POSITION_CONTROL,targetPosition=motordir[6]*1.57,positionGain=kp, velocityGain=kd, force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[7],controlMode=p.POSITION_CONTROL,targetPosition=motordir[7]*1.57,positionGain=kp, velocityGain=kd, force=maxForce) p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[7],controlMode=p.POSITION_CONTROL,targetPosition=motordir[7]*1.57,positionGain=kp, velocityGain=kd, force=maxForce)
#stand still
p.setRealTimeSimulation(useRealTime)
#while (True):
p.stepSimulation() # time.sleep(0.01)
#p.stepSimulation()
print("quadruped Id = ") print("quadruped Id = ")
@@ -118,15 +145,14 @@ print(quadruped)
p.saveWorld("quadru.py") p.saveWorld("quadru.py")
logId = p.startStateLogging(p.STATE_LOGGING_MINITAUR,"quadrupedLog.txt",[quadruped]) logId = p.startStateLogging(p.STATE_LOGGING_MINITAUR,"quadrupedLog.txt",[quadruped])
p.setGravity(0,0,-10)
#stand still
p.setRealTimeSimulation(useRealTime)
#jump #jump
t = 0.0 t = 0.0
t_end = t + 10 t_end = t + 100
i=0 i=0
ref_time = time.time() ref_time = time.time()
@@ -135,15 +161,7 @@ while t < t_end:
t = time.time()-ref_time t = time.time()-ref_time
else: else:
t = t+fixedTimeStep t = t+fixedTimeStep
if (True):
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=motor_front_rightR_joint, controlMode=p.POSITION_CONTROL, targetPosition=math.sin(t*speed)*jump_amp+1.57, positionGain=kp, velocityGain=kd, force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=motor_front_rightL_joint, controlMode=p.POSITION_CONTROL, targetPosition=-math.sin(t*speed)*jump_amp-1.57, positionGain=kp, velocityGain=kd, force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=motor_front_leftR_joint, controlMode=p.POSITION_CONTROL, targetPosition=math.sin(t*speed)*jump_amp+1.57, positionGain=kp, velocityGain=kd, force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=motor_front_leftL_joint, controlMode=p.POSITION_CONTROL, targetPosition=-math.sin(t*speed)*jump_amp-1.57, positionGain=kp, velocityGain=kd, force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=motor_back_rightR_joint, controlMode=p.POSITION_CONTROL, targetPosition=math.sin(t*speed)*jump_amp+1.57, positionGain=kp, velocityGain=kd, force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=motor_back_rightL_joint, controlMode=p.POSITION_CONTROL, targetPosition=-math.sin(t*speed)*jump_amp-1.57, positionGain=kp, velocityGain=kd, force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=motor_back_leftR_joint, controlMode=p.POSITION_CONTROL, targetPosition=math.sin(t*speed)*jump_amp+1.57, positionGain=kp, velocityGain=kd, force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=motor_back_leftL_joint, controlMode=p.POSITION_CONTROL, targetPosition=-math.sin(t*speed)*jump_amp-1.57, positionGain=kp, velocityGain=kd, force=maxForce)
target = math.sin(t*speed)*jump_amp+1.57; target = math.sin(t*speed)*jump_amp+1.57;
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[0],controlMode=p.POSITION_CONTROL,targetPosition=motordir[0]*target,positionGain=kp, velocityGain=kd, force=maxForce) p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[0],controlMode=p.POSITION_CONTROL,targetPosition=motordir[0]*target,positionGain=kp, velocityGain=kd, force=maxForce)
@@ -157,4 +175,5 @@ while t < t_end:
if (useRealTime==0): if (useRealTime==0):
p.stepSimulation() p.stepSimulation()
time.sleep(fixedTimeStep)

12
src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp
View File

@@ -48,9 +48,11 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl
} }
//solve featherstone normal contact //solve featherstone normal contact
for (int j=0;j<m_multiBodyNormalContactConstraints.size();j++) for (int j0=0;j0<m_multiBodyNormalContactConstraints.size();j0++)
{ {
btMultiBodySolverConstraint& constraint = m_multiBodyNormalContactConstraints[j]; int index = iteration&1? j0 : m_multiBodyNormalContactConstraints.size()-1-j0;
btMultiBodySolverConstraint& constraint = m_multiBodyNormalContactConstraints[index];
btScalar residual = 0.f; btScalar residual = 0.f;
if (iteration < infoGlobal.m_numIterations) if (iteration < infoGlobal.m_numIterations)
@@ -68,11 +70,13 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl
//solve featherstone frictional contact //solve featherstone frictional contact
for (int j=0;j<this->m_multiBodyFrictionContactConstraints.size();j++) for (int j1=0;j1<this->m_multiBodyFrictionContactConstraints.size();j1++)
{ {
if (iteration < infoGlobal.m_numIterations) if (iteration < infoGlobal.m_numIterations)
{ {
btMultiBodySolverConstraint& frictionConstraint = m_multiBodyFrictionContactConstraints[j]; int index = iteration&1? j1 : m_multiBodyFrictionContactConstraints.size()-1-j1;
btMultiBodySolverConstraint& frictionConstraint = m_multiBodyFrictionContactConstraints[index];
btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse; btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse;
//adjust friction limits here //adjust friction limits here
if (totalImpulse>btScalar(0)) if (totalImpulse>btScalar(0))