fix minitaur.urdf: move lower-leg inertia to center, add missing collision for one of the motors, add contact parameters for friction_anchor, spinning friction, compliance (stiffness/damping)

fix in indexing for maximal coordinates (unused by default, still experimental, requires many iterations for Minitaur due to extreme mass-ratio, hence use of reduces/generalized coordinates)
modify quadruped.py to test maximal coordinates
wrap angular servo (positional) target within [-PI,PI] in btGeneric6DofSpring2Constraint
add 'j' key to show body frames in wireframe/debug mode
This commit is contained in:
erwincoumans
2017-09-02 01:05:42 -07:00
parent ee30479a28
commit eb97e06280
9 changed files with 281 additions and 108 deletions

View File

@@ -267,6 +267,11 @@
<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"/>
@@ -427,8 +432,10 @@
<link name="lower_leg_front_rightR_link">
<contact>
<stiffness value="10000"/>
<damping value="10"/>
<friction_anchor/>
<stiffness value="30000.0"/>
<damping value="1000.0"/>
<spinning_friction value=".3"/>
<lateral_friction value="1"/>
</contact>
@@ -448,11 +455,12 @@
</geometry>
</collision>
<inertial>
<origin rpy="0.0 0 0" xyz="0 0 .1"/>
<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="revolute">
<joint name="knee_front_rightR_link" type="continuous">
<axis xyz="0 1 0"/>
<parent link="upper_leg_front_rightR_link"/>
<child link="lower_leg_front_rightR_link"/>
@@ -495,8 +503,10 @@
<link name="lower_leg_front_rightL_link">
<contact>
<stiffness value="10000"/>
<damping value="10"/>
<friction_anchor/>
<stiffness value="30000.0"/>
<damping value="1000.0"/>
<spinning_friction value=".3"/>
<lateral_friction value="1"/>
</contact>
@@ -516,11 +526,12 @@
</geometry>
</collision>
<inertial>
<origin rpy="0.0 0 0" xyz="0 0 .1"/>
<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="revolute">
<joint name="knee_front_rightL_link" type="continuous">
<axis xyz="0 1 0"/>
<parent link="upper_leg_front_rightL_link"/>
<child link="lower_leg_front_rightL_link"/>
@@ -561,8 +572,10 @@
<link name="lower_leg_front_leftR_link">
<contact>
<stiffness value="10000"/>
<damping value="10"/>
<friction_anchor/>
<stiffness value="30000.0"/>
<damping value="1000.0"/>
<spinning_friction value=".3"/>
<lateral_friction value="1"/>
</contact>
@@ -582,11 +595,12 @@
</geometry>
</collision>
<inertial>
<origin rpy="0.0 0.0 0" xyz="0 0 .1"/>
<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="revolute">
<joint name="knee_front_leftR_link" type="continuous">
<axis xyz="0 1 0"/>
<parent link="upper_leg_front_leftR_link"/>
<child link="lower_leg_front_leftR_link"/>
@@ -628,8 +642,10 @@
<link name="lower_leg_front_leftL_link">
<contact>
<stiffness value="10000"/>
<damping value="10"/>
<friction_anchor/>
<stiffness value="30000.0"/>
<damping value="1000.0"/>
<spinning_friction value=".3"/>
<lateral_friction value="1"/>
</contact>
@@ -649,11 +665,12 @@
</geometry>
</collision>
<inertial>
<origin rpy="0.0 0.0 0" xyz="0 0 .1"/>
<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="revolute">
<joint name="knee_front_leftL_link" type="continuous">
<axis xyz="0 1 0"/>
<parent link="upper_leg_front_leftL_link"/>
<child link="lower_leg_front_leftL_link"/>
@@ -692,8 +709,10 @@
<link name="lower_leg_back_rightR_link">
<contact>
<stiffness value="10000"/>
<damping value="10"/>
<friction_anchor/>
<stiffness value="30000.0"/>
<damping value="1000.0"/>
<spinning_friction value=".3"/>
<lateral_friction value="1"/>
</contact>
@@ -713,11 +732,12 @@
</geometry>
</collision>
<inertial>
<origin rpy="0.0 0 0" xyz="0 0 .1"/>
<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="revolute">
<joint name="knee_back_rightR_link" type="continuous">
<axis xyz="0 1 0"/>
<parent link="upper_leg_back_rightR_link"/>
<child link="lower_leg_back_rightR_link"/>
@@ -760,8 +780,10 @@
<link name="lower_leg_back_rightL_link">
<contact>
<stiffness value="10000"/>
<damping value="10"/>
<friction_anchor/>
<stiffness value="30000.0"/>
<damping value="1000.0"/>
<spinning_friction value=".3"/>
<lateral_friction value="1"/>
</contact>
@@ -781,11 +803,12 @@
</geometry>
</collision>
<inertial>
<origin rpy="0.0 0 0" xyz="0 0 .1"/>
<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="revolute">
<joint name="knee_back_rightL_link" type="continuous">
<axis xyz="0 1 0"/>
<parent link="upper_leg_back_rightL_link"/>
<child link="lower_leg_back_rightL_link"/>
@@ -826,8 +849,10 @@
<link name="lower_leg_back_leftR_link">
<contact>
<stiffness value="10000"/>
<damping value="10"/>
<friction_anchor/>
<stiffness value="30000.0"/>
<damping value="1000.0"/>
<spinning_friction value=".3"/>
<lateral_friction value="1"/>
</contact>
@@ -847,11 +872,12 @@
</geometry>
</collision>
<inertial>
<origin rpy="0.0 0 0" xyz="0 0 .1"/>
<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="revolute">
<joint name="knee_back_leftR_link" type="continuous">
<axis xyz="0 1 0"/>
<parent link="upper_leg_back_leftR_link"/>
<child link="lower_leg_back_leftR_link"/>
@@ -891,8 +917,10 @@
<link name="lower_leg_back_leftL_link">
<contact>
<stiffness value="10000"/>
<damping value="10"/>
<friction_anchor/>
<stiffness value="30000.0"/>
<damping value="1000.0"/>
<spinning_friction value=".3"/>
<lateral_friction value="1"/>
</contact>
@@ -912,11 +940,12 @@
</geometry>
</collision>
<inertial>
<origin rpy="0.0 0 0" xyz="0 0 .1"/>
<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="revolute">
<joint name="knee_back_leftL_link" type="continuous">
<axis xyz="0 1 0"/>
<parent link="upper_leg_back_leftL_link"/>
<child link="lower_leg_back_leftL_link"/>