Merge remote-tracking branch 'remotes/bulletphysics/master'
# Conflicts: # src/BulletCollision/CollisionDispatch/btCollisionWorldImporter.h
This commit is contained in:
nicolaichuk
91
_clang-format
Normal file
91
_clang-format
Normal file
@@ -0,0 +1,91 @@
|
||||
---
|
||||
Language: Cpp
|
||||
# BasedOnStyle: Google
|
||||
AccessModifierOffset: -1
|
||||
AlignAfterOpenBracket: Align
|
||||
AlignConsecutiveAssignments: false
|
||||
AlignConsecutiveDeclarations: false
|
||||
AlignEscapedNewlinesLeft: true
|
||||
AlignOperands: true
|
||||
AlignTrailingComments: true
|
||||
AllowAllParametersOfDeclarationOnNextLine: true
|
||||
AllowShortBlocksOnASingleLine: false
|
||||
AllowShortCaseLabelsOnASingleLine: false
|
||||
AllowShortFunctionsOnASingleLine: All
|
||||
AllowShortIfStatementsOnASingleLine: true
|
||||
AllowShortLoopsOnASingleLine: true
|
||||
AlwaysBreakAfterDefinitionReturnType: None
|
||||
AlwaysBreakAfterReturnType: None
|
||||
AlwaysBreakBeforeMultilineStrings: true
|
||||
AlwaysBreakTemplateDeclarations: true
|
||||
BinPackArguments: true
|
||||
BinPackParameters: true
|
||||
BraceWrapping:
|
||||
AfterClass: false
|
||||
AfterControlStatement: false
|
||||
AfterEnum: false
|
||||
AfterFunction: false
|
||||
AfterNamespace: false
|
||||
AfterObjCDeclaration: false
|
||||
AfterStruct: false
|
||||
AfterUnion: false
|
||||
BeforeCatch: false
|
||||
BeforeElse: false
|
||||
IndentBraces: false
|
||||
BreakBeforeBinaryOperators: None
|
||||
BreakBeforeBraces: Allman
|
||||
BreakBeforeTernaryOperators: true
|
||||
BreakConstructorInitializersBeforeComma: false
|
||||
ColumnLimit: 0
|
||||
CommentPragmas: '^ IWYU pragma:'
|
||||
ConstructorInitializerAllOnOneLineOrOnePerLine: true
|
||||
ConstructorInitializerIndentWidth: 4
|
||||
ContinuationIndentWidth: 4
|
||||
Cpp11BracedListStyle: true
|
||||
DerivePointerAlignment: true
|
||||
DisableFormat: false
|
||||
ExperimentalAutoDetectBinPacking: false
|
||||
ForEachMacros: [ foreach, Q_FOREACH, BOOST_FOREACH ]
|
||||
IncludeCategories:
|
||||
- Regex: '^<.*\.h>'
|
||||
Priority: 1
|
||||
- Regex: '^<.*'
|
||||
Priority: 2
|
||||
- Regex: '.*'
|
||||
Priority: 3
|
||||
IndentCaseLabels: true
|
||||
IndentWidth: 4
|
||||
IndentWrappedFunctionNames: false
|
||||
KeepEmptyLinesAtTheStartOfBlocks: false
|
||||
MacroBlockBegin: ''
|
||||
MacroBlockEnd: ''
|
||||
MaxEmptyLinesToKeep: 1
|
||||
NamespaceIndentation: None
|
||||
ObjCBlockIndentWidth: 2
|
||||
ObjCSpaceAfterProperty: false
|
||||
ObjCSpaceBeforeProtocolList: false
|
||||
PenaltyBreakBeforeFirstCallParameter: 1
|
||||
PenaltyBreakComment: 300
|
||||
PenaltyBreakFirstLessLess: 120
|
||||
PenaltyBreakString: 1000
|
||||
PenaltyExcessCharacter: 1000000
|
||||
PenaltyReturnTypeOnItsOwnLine: 200
|
||||
PointerAlignment: Left
|
||||
ReflowComments: false
|
||||
SortIncludes: false
|
||||
SpaceAfterCStyleCast: false
|
||||
SpaceBeforeAssignmentOperators: true
|
||||
SpaceBeforeParens: ControlStatements
|
||||
SpaceInEmptyParentheses: false
|
||||
SpacesBeforeTrailingComments: 2
|
||||
SpacesInAngles: false
|
||||
SpacesInContainerLiterals: true
|
||||
SpacesInCStyleCastParentheses: false
|
||||
SpacesInParentheses: false
|
||||
SpacesInSquareBrackets: false
|
||||
AccessModifierOffset: -4
|
||||
Standard: Auto
|
||||
TabWidth: 4
|
||||
UseTab: ForContinuationAndIndentation
|
||||
...
|
||||
|
||||
BIN
build3/premake4_arm64
Executable file
BIN
build3/premake4_arm64
Executable file
Binary file not shown.
@@ -49,30 +49,30 @@
|
||||
</visual>
|
||||
|
||||
<asset>
|
||||
<mesh name="index0" file="index0.stl"/>
|
||||
<mesh name="index1" file="index1.stl"/>
|
||||
<mesh name="index2" file="index2.stl"/>
|
||||
<mesh name="index3" file="index3.stl"/>
|
||||
<mesh name="middle0" file="middle0.stl"/>
|
||||
<mesh name="middle1" file="middle1.stl"/>
|
||||
<mesh name="middle2" file="middle2.stl"/>
|
||||
<mesh name="middle3" file="middle3.stl"/>
|
||||
<mesh name="index0" file="index0_collision.stl"/>
|
||||
<mesh name="index1" file="index1_collision.stl"/>
|
||||
<mesh name="index2" file="index2_collision.stl"/>
|
||||
<mesh name="index3" file="index3_collision.stl"/>
|
||||
<mesh name="middle0" file="middle0_collision.stl"/>
|
||||
<mesh name="middle1" file="middle1_collision.stl"/>
|
||||
<mesh name="middle2" file="middle2_collision.stl"/>
|
||||
<mesh name="middle3" file="middle3_collision.stl"/>
|
||||
<mesh name="palm" file="palm.stl"/>
|
||||
<mesh name="pinky0" file="pinky0.stl"/>
|
||||
<mesh name="pinky1" file="pinky1.stl"/>
|
||||
<mesh name="pinky2" file="pinky2.stl"/>
|
||||
<mesh name="pinky3" file="pinky3.stl"/>
|
||||
<mesh name="ring0" file="ring0.stl"/>
|
||||
<mesh name="ring1" file="ring1.stl"/>
|
||||
<mesh name="ring2" file="ring2.stl"/>
|
||||
<mesh name="ring3" file="ring3.stl"/>
|
||||
<mesh name="thumb0" file="thumb0.stl"/>
|
||||
<mesh name="thumb1" file="thumb1.stl"/>
|
||||
<mesh name="thumb2" file="thumb2.stl"/>
|
||||
<mesh name="thumb3" file="thumb3.stl"/>
|
||||
<mesh name="wristx" file="wristx.stl"/>
|
||||
<mesh name="wristy" file="wristy.stl"/>
|
||||
<mesh name="wristz" file="wristz.stl"/>
|
||||
<mesh name="pinky0" file="pinky0_collision.stl"/>
|
||||
<mesh name="pinky1" file="pinky1_collision.stl"/>
|
||||
<mesh name="pinky2" file="pinky2_collision.stl"/>
|
||||
<mesh name="pinky3" file="pinky3_collision.stl"/>
|
||||
<mesh name="ring0" file="ring0_collision.stl"/>
|
||||
<mesh name="ring1" file="ring1_collision.stl"/>
|
||||
<mesh name="ring2" file="ring2_collision.stl"/>
|
||||
<mesh name="ring3" file="ring3_collision.stl"/>
|
||||
<mesh name="thumb0" file="thumb0_collision.stl"/>
|
||||
<mesh name="thumb1" file="thumb1_collision.stl"/>
|
||||
<mesh name="thumb2" file="thumb2_collision.stl"/>
|
||||
<mesh name="thumb3" file="thumb3_collision.stl"/>
|
||||
<mesh name="wristx" file="wristx_collision.stl"/>
|
||||
<mesh name="wristy" file="wristy_collision.stl"/>
|
||||
<mesh name="wristz" file="wristz_collision.stl"/>
|
||||
|
||||
<texture type="skybox" builtin="gradient" rgb1=".4 .6 .8" rgb2="0 0 0"
|
||||
width="100" height="100"/>
|
||||
|
||||
BIN
data/MPL/mesh/index0_collision.stl
Normal file
BIN
data/MPL/mesh/index0_collision.stl
Normal file
Binary file not shown.
BIN
data/MPL/mesh/index1_collision.stl
Normal file
BIN
data/MPL/mesh/index1_collision.stl
Normal file
Binary file not shown.
BIN
data/MPL/mesh/index2_collision.stl
Normal file
BIN
data/MPL/mesh/index2_collision.stl
Normal file
Binary file not shown.
BIN
data/MPL/mesh/index3_collision.stl
Normal file
BIN
data/MPL/mesh/index3_collision.stl
Normal file
Binary file not shown.
BIN
data/MPL/mesh/middle0_collision.stl
Normal file
BIN
data/MPL/mesh/middle0_collision.stl
Normal file
Binary file not shown.
BIN
data/MPL/mesh/middle1_collision.stl
Normal file
BIN
data/MPL/mesh/middle1_collision.stl
Normal file
Binary file not shown.
BIN
data/MPL/mesh/middle2_collision.stl
Normal file
BIN
data/MPL/mesh/middle2_collision.stl
Normal file
Binary file not shown.
BIN
data/MPL/mesh/middle3_collision.stl
Normal file
BIN
data/MPL/mesh/middle3_collision.stl
Normal file
Binary file not shown.
BIN
data/MPL/mesh/palm_collision.stl
Normal file
BIN
data/MPL/mesh/palm_collision.stl
Normal file
Binary file not shown.
BIN
data/MPL/mesh/pinky0_collision.stl
Normal file
BIN
data/MPL/mesh/pinky0_collision.stl
Normal file
Binary file not shown.
BIN
data/MPL/mesh/pinky1_collision.stl
Normal file
BIN
data/MPL/mesh/pinky1_collision.stl
Normal file
Binary file not shown.
BIN
data/MPL/mesh/pinky2_collision.stl
Normal file
BIN
data/MPL/mesh/pinky2_collision.stl
Normal file
Binary file not shown.
BIN
data/MPL/mesh/pinky3_collision.stl
Normal file
BIN
data/MPL/mesh/pinky3_collision.stl
Normal file
Binary file not shown.
BIN
data/MPL/mesh/ring0_collision.stl
Normal file
BIN
data/MPL/mesh/ring0_collision.stl
Normal file
Binary file not shown.
BIN
data/MPL/mesh/ring1_collision.stl
Normal file
BIN
data/MPL/mesh/ring1_collision.stl
Normal file
Binary file not shown.
BIN
data/MPL/mesh/ring2_collision.stl
Normal file
BIN
data/MPL/mesh/ring2_collision.stl
Normal file
Binary file not shown.
BIN
data/MPL/mesh/ring3_collision.stl
Normal file
BIN
data/MPL/mesh/ring3_collision.stl
Normal file
Binary file not shown.
BIN
data/MPL/mesh/thumb0_collision.stl
Normal file
BIN
data/MPL/mesh/thumb0_collision.stl
Normal file
Binary file not shown.
BIN
data/MPL/mesh/thumb1_collision.stl
Normal file
BIN
data/MPL/mesh/thumb1_collision.stl
Normal file
Binary file not shown.
BIN
data/MPL/mesh/thumb2_collision.stl
Normal file
BIN
data/MPL/mesh/thumb2_collision.stl
Normal file
Binary file not shown.
BIN
data/MPL/mesh/thumb3_collision.stl
Normal file
BIN
data/MPL/mesh/thumb3_collision.stl
Normal file
Binary file not shown.
BIN
data/MPL/mesh/wristx_collision.stl
Normal file
BIN
data/MPL/mesh/wristx_collision.stl
Normal file
Binary file not shown.
BIN
data/MPL/mesh/wristy_collision.stl
Normal file
BIN
data/MPL/mesh/wristy_collision.stl
Normal file
Binary file not shown.
BIN
data/MPL/mesh/wristz_collision.stl
Normal file
BIN
data/MPL/mesh/wristz_collision.stl
Normal file
Binary file not shown.
@@ -49,30 +49,30 @@
|
||||
</visual>
|
||||
|
||||
<asset>
|
||||
<mesh name="index0" file="index0.stl"/>
|
||||
<mesh name="index1" file="index1.stl"/>
|
||||
<mesh name="index2" file="index2.stl"/>
|
||||
<mesh name="index3" file="index3.stl"/>
|
||||
<mesh name="middle0" file="middle0.stl"/>
|
||||
<mesh name="middle1" file="middle1.stl"/>
|
||||
<mesh name="middle2" file="middle2.stl"/>
|
||||
<mesh name="middle3" file="middle3.stl"/>
|
||||
<mesh name="palm" file="palm.stl"/>
|
||||
<mesh name="pinky0" file="pinky0.stl"/>
|
||||
<mesh name="pinky1" file="pinky1.stl"/>
|
||||
<mesh name="pinky2" file="pinky2.stl"/>
|
||||
<mesh name="pinky3" file="pinky3.stl"/>
|
||||
<mesh name="ring0" file="ring0.stl"/>
|
||||
<mesh name="ring1" file="ring1.stl"/>
|
||||
<mesh name="ring2" file="ring2.stl"/>
|
||||
<mesh name="ring3" file="ring3.stl"/>
|
||||
<mesh name="thumb0" file="thumb0.stl"/>
|
||||
<mesh name="thumb1" file="thumb1.stl"/>
|
||||
<mesh name="thumb2" file="thumb2.stl"/>
|
||||
<mesh name="thumb3" file="thumb3.stl"/>
|
||||
<mesh name="wristx" file="wristx.stl"/>
|
||||
<mesh name="wristy" file="wristy.stl"/>
|
||||
<mesh name="wristz" file="wristz.stl"/>
|
||||
<mesh name="index0" file="index0_collision.stl"/>
|
||||
<mesh name="index1" file="index1_collision.stl"/>
|
||||
<mesh name="index2" file="index2_collision.stl"/>
|
||||
<mesh name="index3" file="index3_collision.stl"/>
|
||||
<mesh name="middle0" file="middle0_collision.stl"/>
|
||||
<mesh name="middle1" file="middle1_collision.stl"/>
|
||||
<mesh name="middle2" file="middle2_collision.stl"/>
|
||||
<mesh name="middle3" file="middle3_collision.stl"/>
|
||||
<mesh name="palm" file="palm_collision.stl"/>
|
||||
<mesh name="pinky0" file="pinky0_collision.stl"/>
|
||||
<mesh name="pinky1" file="pinky1_collision.stl"/>
|
||||
<mesh name="pinky2" file="pinky2_collision.stl"/>
|
||||
<mesh name="pinky3" file="pinky3_collision.stl"/>
|
||||
<mesh name="ring0" file="ring0_collision.stl"/>
|
||||
<mesh name="ring1" file="ring1_collision.stl"/>
|
||||
<mesh name="ring2" file="ring2_collision.stl"/>
|
||||
<mesh name="ring3" file="ring3_collision.stl"/>
|
||||
<mesh name="thumb0" file="thumb0_collision.stl"/>
|
||||
<mesh name="thumb1" file="thumb1_collision.stl"/>
|
||||
<mesh name="thumb2" file="thumb2_collision.stl"/>
|
||||
<mesh name="thumb3" file="thumb3_collision.stl"/>
|
||||
<mesh name="wristx" file="wristx_collision.stl"/>
|
||||
<mesh name="wristy" file="wristy_collision.stl"/>
|
||||
<mesh name="wristz" file="wristz_collision.stl"/>
|
||||
|
||||
<texture type="skybox" builtin="gradient" rgb1=".4 .6 .8" rgb2="0 0 0"
|
||||
width="100" height="100"/>
|
||||
|
||||
74
data/cartpole.urdf
Normal file
74
data/cartpole.urdf
Normal file
@@ -0,0 +1,74 @@
|
||||
<?xml version="1.0"?>
|
||||
<robot name="physics">
|
||||
|
||||
<link name="slideBar">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size="30 0.05 0.05"/>
|
||||
</geometry>
|
||||
<origin xyz="0 0 0"/>
|
||||
<material name="green">
|
||||
<color rgba="0 0.8 .8 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<inertial>
|
||||
<mass value="0"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<link name="cart">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size="0.5 0.5 0.2"/>
|
||||
</geometry>
|
||||
<origin xyz="0 0 0"/>
|
||||
<material name="blue">
|
||||
<color rgba="0 0 .8 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<box size="0.5 0.5 0.2"/>
|
||||
</geometry>
|
||||
<origin xyz="0 0 0"/>
|
||||
</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="slider_to_cart" type="prismatic">
|
||||
<axis xyz="1 0 0"/>
|
||||
<origin xyz="0.0 0.0 0.0"/>
|
||||
<parent link="slideBar"/>
|
||||
<child link="cart"/>
|
||||
<limit effort="1000.0" lower="-15" upper="15" velocity="5"/>
|
||||
</joint>
|
||||
|
||||
<link name="pole">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size="0.05 0.05 1.0"/>
|
||||
</geometry>
|
||||
<origin rpy="0 0 0" xyz="0 0 0.5"/>
|
||||
<material name="white">
|
||||
<color rgba="1 1 1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<inertial>
|
||||
<origin xyz="0 0 0.5"/>
|
||||
<mass value="10"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="cart_to_pole" type="continuous">
|
||||
<axis xyz="0 1 0"/>
|
||||
<origin xyz="0.0 0.0 0"/>
|
||||
<parent link="cart"/>
|
||||
<child link="pole"/>
|
||||
</joint>
|
||||
|
||||
</robot>
|
||||
@@ -2,7 +2,8 @@
|
||||
<robot name="cube.urdf">
|
||||
<link name="baseLink">
|
||||
<contact>
|
||||
<lateral_friction value="1.0"/>
|
||||
<friction_anchor/>
|
||||
<lateral_friction value="1.3"/>
|
||||
<rolling_friction value="0.0"/>
|
||||
<stiffness value="300"/>
|
||||
<damping value="10"/>
|
||||
|
||||
@@ -215,6 +215,10 @@
|
||||
</joint>
|
||||
|
||||
<link name='finger_right'>
|
||||
<contact>
|
||||
<lateral_friction>1.0</lateral_friction>
|
||||
<spinning_friction>1.5</spinning_friction>
|
||||
</contact>
|
||||
<pose frame=''>0.062 0 0.145 0 0 1.5708</pose>
|
||||
<inertial>
|
||||
<mass>0.2</mass>
|
||||
@@ -255,6 +259,10 @@
|
||||
</joint>
|
||||
|
||||
<link name='finger_left'>
|
||||
<contact>
|
||||
<lateral_friction>1.0</lateral_friction>
|
||||
<spinning_friction>1.5</spinning_friction>
|
||||
</contact>
|
||||
<pose frame=''>-0.062 0 0.145 0 0 4.71239</pose>
|
||||
<inertial>
|
||||
<mass>0.2</mass>
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -2,7 +2,7 @@
|
||||
<robot name="cube.urdf">
|
||||
<link name="planeLink">
|
||||
<contact>
|
||||
<lateral_friction value="2"/>
|
||||
<lateral_friction value="1"/>
|
||||
</contact>
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
|
||||
BIN
data/quadruped/LOG00076.TXT
Normal file
BIN
data/quadruped/LOG00076.TXT
Normal file
Binary file not shown.
929
data/quadruped/minitaur.urdf
Normal file
929
data/quadruped/minitaur.urdf
Normal 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="revolute">
|
||||
<axis xyz="0 1 0"/>
|
||||
<parent link="upper_leg_front_rightR_link"/>
|
||||
<child link="lower_leg_front_rightR_link"/>
|
||||
<origin rpy="0 0 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="revolute">
|
||||
<axis xyz="0 1 0"/>
|
||||
<parent link="upper_leg_front_rightL_link"/>
|
||||
<child link="lower_leg_front_rightL_link"/>
|
||||
<origin rpy="0 0 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="revolute">
|
||||
<axis xyz="0 1 0"/>
|
||||
<parent link="upper_leg_front_leftR_link"/>
|
||||
<child link="lower_leg_front_leftR_link"/>
|
||||
<origin rpy="0 0 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="revolute">
|
||||
<axis xyz="0 1 0"/>
|
||||
<parent link="upper_leg_front_leftL_link"/>
|
||||
<child link="lower_leg_front_leftL_link"/>
|
||||
<origin rpy="0 0 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="revolute">
|
||||
<axis xyz="0 1 0"/>
|
||||
<parent link="upper_leg_back_rightR_link"/>
|
||||
<child link="lower_leg_back_rightR_link"/>
|
||||
<origin rpy="0 0 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="revolute">
|
||||
<axis xyz="0 1 0"/>
|
||||
<parent link="upper_leg_back_rightL_link"/>
|
||||
<child link="lower_leg_back_rightL_link"/>
|
||||
<origin rpy="0 0 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="revolute">
|
||||
<axis xyz="0 1 0"/>
|
||||
<parent link="upper_leg_back_leftR_link"/>
|
||||
<child link="lower_leg_back_leftR_link"/>
|
||||
<origin rpy="0 0 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="revolute">
|
||||
<axis xyz="0 1 0"/>
|
||||
<parent link="upper_leg_back_leftL_link"/>
|
||||
<child link="lower_leg_back_leftL_link"/>
|
||||
<origin rpy="0 0 0" xyz="0.0 0.01 .055"/>
|
||||
<limit effort="100" velocity="100"/>
|
||||
<joint_properties damping="0.0" friction="0.0"/>
|
||||
</joint>
|
||||
|
||||
</robot>
|
||||
|
||||
913
data/quadruped/minitaur_fixed_all.urdf
Normal file
913
data/quadruped/minitaur_fixed_all.urdf
Normal 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
929
data/quadruped/minitaur_fixed_knees.urdf
Normal 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>
|
||||
|
||||
BIN
data/quadruped/t-motor.jpg
Normal file
BIN
data/quadruped/t-motor.jpg
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 153 KiB |
BIN
data/quadruped/tmotor.blend
Normal file
BIN
data/quadruped/tmotor.blend
Normal file
Binary file not shown.
19
data/quadruped/tmotor3.mtl
Normal file
19
data/quadruped/tmotor3.mtl
Normal file
@@ -0,0 +1,19 @@
|
||||
# Blender MTL File: 'tmotor.blend'
|
||||
# Material Count: 2
|
||||
|
||||
newmtl None
|
||||
Ns 0
|
||||
Ka 0.000000 0.000000 0.000000
|
||||
Kd 0.8 0.8 0.8
|
||||
Ks 0.8 0.8 0.8
|
||||
d 1
|
||||
illum 2
|
||||
map_Kd t-motor.jpg
|
||||
|
||||
newmtl None_NONE
|
||||
Ns 0
|
||||
Ka 0.000000 0.000000 0.000000
|
||||
Kd 0.8 0.8 0.8
|
||||
Ks 0.8 0.8 0.8
|
||||
d 1
|
||||
illum 2
|
||||
325
data/quadruped/tmotor3.obj
Normal file
325
data/quadruped/tmotor3.obj
Normal file
@@ -0,0 +1,325 @@
|
||||
# Blender v2.78 (sub 0) OBJ File: 'tmotor.blend'
|
||||
# www.blender.org
|
||||
mtllib tmotor3.mtl
|
||||
o Cylinder
|
||||
v 0.000000 0.043000 -0.006500
|
||||
v 0.000000 0.043000 0.006500
|
||||
v 0.008389 0.042174 -0.006500
|
||||
v 0.008389 0.042174 0.006500
|
||||
v 0.016455 0.039727 -0.006500
|
||||
v 0.016455 0.039727 0.006500
|
||||
v 0.023890 0.035753 -0.006500
|
||||
v 0.023890 0.035753 0.006500
|
||||
v 0.030406 0.030406 -0.006500
|
||||
v 0.030406 0.030406 0.006500
|
||||
v 0.035753 0.023890 -0.006500
|
||||
v 0.035753 0.023890 0.006500
|
||||
v 0.039727 0.016455 -0.006500
|
||||
v 0.039727 0.016455 0.006500
|
||||
v 0.042174 0.008389 -0.006500
|
||||
v 0.042174 0.008389 0.006500
|
||||
v 0.043000 0.000000 -0.006500
|
||||
v 0.043000 0.000000 0.006500
|
||||
v 0.042174 -0.008389 -0.006500
|
||||
v 0.042174 -0.008389 0.006500
|
||||
v 0.039727 -0.016455 -0.006500
|
||||
v 0.039727 -0.016455 0.006500
|
||||
v 0.035753 -0.023890 -0.006500
|
||||
v 0.035753 -0.023890 0.006500
|
||||
v 0.030406 -0.030406 -0.006500
|
||||
v 0.030406 -0.030406 0.006500
|
||||
v 0.023890 -0.035753 -0.006500
|
||||
v 0.023890 -0.035753 0.006500
|
||||
v 0.016455 -0.039727 -0.006500
|
||||
v 0.016455 -0.039727 0.006500
|
||||
v 0.008389 -0.042174 -0.006500
|
||||
v 0.008389 -0.042174 0.006500
|
||||
v -0.000000 -0.043000 -0.006500
|
||||
v -0.000000 -0.043000 0.006500
|
||||
v -0.008389 -0.042174 -0.006500
|
||||
v -0.008389 -0.042174 0.006500
|
||||
v -0.016455 -0.039727 -0.006500
|
||||
v -0.016455 -0.039727 0.006500
|
||||
v -0.023890 -0.035753 -0.006500
|
||||
v -0.023890 -0.035753 0.006500
|
||||
v -0.030406 -0.030406 -0.006500
|
||||
v -0.030406 -0.030406 0.006500
|
||||
v -0.035753 -0.023889 -0.006500
|
||||
v -0.035753 -0.023889 0.006500
|
||||
v -0.039727 -0.016455 -0.006500
|
||||
v -0.039727 -0.016455 0.006500
|
||||
v -0.042174 -0.008389 -0.006500
|
||||
v -0.042174 -0.008389 0.006500
|
||||
v -0.043000 0.000000 -0.006500
|
||||
v -0.043000 0.000000 0.006500
|
||||
v -0.042174 0.008389 -0.006500
|
||||
v -0.042174 0.008389 0.006500
|
||||
v -0.039727 0.016455 -0.006500
|
||||
v -0.039727 0.016455 0.006500
|
||||
v -0.035753 0.023890 -0.006500
|
||||
v -0.035753 0.023890 0.006500
|
||||
v -0.030406 0.030406 -0.006500
|
||||
v -0.030406 0.030406 0.006500
|
||||
v -0.023889 0.035753 -0.006500
|
||||
v -0.023889 0.035753 0.006500
|
||||
v -0.016455 0.039727 -0.006500
|
||||
v -0.016455 0.039727 0.006500
|
||||
v -0.008389 0.042174 -0.006500
|
||||
v -0.008389 0.042174 0.006500
|
||||
vt 0.6520 0.1657
|
||||
vt 0.8624 0.3762
|
||||
vt 0.6520 0.8843
|
||||
vt 0.5790 0.9064
|
||||
vt 0.3543 0.8843
|
||||
vt 0.5031 0.9139
|
||||
vt 0.4273 0.9064
|
||||
vt 0.2871 0.8484
|
||||
vt 0.2281 0.8000
|
||||
vt 0.1798 0.7411
|
||||
vt 0.1438 0.6738
|
||||
vt 0.1217 0.6009
|
||||
vt 0.1142 0.5250
|
||||
vt 0.1217 0.4491
|
||||
vt 0.1438 0.3762
|
||||
vt 0.1798 0.3089
|
||||
vt 0.2281 0.2500
|
||||
vt 0.2871 0.2016
|
||||
vt 0.3543 0.1657
|
||||
vt 0.4273 0.1436
|
||||
vt 0.5031 0.1361
|
||||
vt 0.5790 0.1436
|
||||
vt 0.7192 0.2016
|
||||
vt 0.7781 0.2500
|
||||
vt 0.8265 0.3089
|
||||
vt 0.8846 0.4491
|
||||
vt 0.8920 0.5250
|
||||
vt 0.8846 0.6009
|
||||
vt 0.8624 0.6738
|
||||
vt 0.8265 0.7411
|
||||
vt 0.7781 0.8000
|
||||
vt 0.7192 0.8484
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vt 0.0000 0.0000
|
||||
vn 0.0000 0.0000 1.0000
|
||||
vn 0.0980 0.9952 0.0000
|
||||
vn 0.2903 0.9569 0.0000
|
||||
vn 0.4714 0.8819 0.0000
|
||||
vn 0.6344 0.7730 0.0000
|
||||
vn 0.7730 0.6344 0.0000
|
||||
vn 0.8819 0.4714 0.0000
|
||||
vn 0.9569 0.2903 0.0000
|
||||
vn 0.9952 0.0980 0.0000
|
||||
vn 0.9952 -0.0980 0.0000
|
||||
vn 0.9569 -0.2903 0.0000
|
||||
vn 0.8819 -0.4714 0.0000
|
||||
vn 0.7730 -0.6344 0.0000
|
||||
vn 0.6344 -0.7730 0.0000
|
||||
vn 0.4714 -0.8819 0.0000
|
||||
vn 0.2903 -0.9569 0.0000
|
||||
vn 0.0980 -0.9952 0.0000
|
||||
vn -0.0980 -0.9952 0.0000
|
||||
vn -0.2903 -0.9569 0.0000
|
||||
vn -0.4714 -0.8819 0.0000
|
||||
vn -0.6344 -0.7730 0.0000
|
||||
vn -0.7730 -0.6344 0.0000
|
||||
vn -0.8819 -0.4714 0.0000
|
||||
vn -0.9569 -0.2903 0.0000
|
||||
vn -0.9952 -0.0980 0.0000
|
||||
vn -0.9952 0.0980 0.0000
|
||||
vn -0.9569 0.2903 0.0000
|
||||
vn -0.8819 0.4714 0.0000
|
||||
vn -0.7730 0.6344 0.0000
|
||||
vn -0.6344 0.7730 0.0000
|
||||
vn -0.4714 0.8819 0.0000
|
||||
vn -0.2903 0.9569 0.0000
|
||||
vn -0.0980 0.9952 0.0000
|
||||
vn 0.0000 0.0000 -1.0000
|
||||
usemtl None
|
||||
s off
|
||||
f 30/1/1 22/2/1 6/3/1
|
||||
f 6/3/1 4/4/1 62/5/1
|
||||
f 2/6/1 64/7/1 62/5/1
|
||||
f 62/5/1 60/8/1 58/9/1
|
||||
f 58/9/1 56/10/1 54/11/1
|
||||
f 54/11/1 52/12/1 50/13/1
|
||||
f 50/13/1 48/14/1 54/11/1
|
||||
f 46/15/1 44/16/1 42/17/1
|
||||
f 42/17/1 40/18/1 38/19/1
|
||||
f 38/19/1 36/20/1 34/21/1
|
||||
f 34/21/1 32/22/1 38/19/1
|
||||
f 30/1/1 28/23/1 26/24/1
|
||||
f 26/24/1 24/25/1 22/2/1
|
||||
f 22/2/1 20/26/1 18/27/1
|
||||
f 18/27/1 16/28/1 22/2/1
|
||||
f 14/29/1 12/30/1 10/31/1
|
||||
f 10/31/1 8/32/1 6/3/1
|
||||
f 4/4/1 2/6/1 62/5/1
|
||||
f 62/5/1 58/9/1 6/3/1
|
||||
f 54/11/1 48/14/1 46/15/1
|
||||
f 46/15/1 42/17/1 54/11/1
|
||||
f 38/19/1 32/22/1 30/1/1
|
||||
f 30/1/1 26/24/1 22/2/1
|
||||
f 22/2/1 16/28/1 14/29/1
|
||||
f 14/29/1 10/31/1 22/2/1
|
||||
f 6/3/1 58/9/1 54/11/1
|
||||
f 54/11/1 42/17/1 38/19/1
|
||||
f 38/19/1 30/1/1 6/3/1
|
||||
f 22/2/1 10/31/1 6/3/1
|
||||
f 6/3/1 54/11/1 38/19/1
|
||||
usemtl None
|
||||
f 2/33/2 3/34/2 1/35/2
|
||||
f 4/36/3 5/37/3 3/34/3
|
||||
f 6/38/4 7/39/4 5/37/4
|
||||
f 8/40/5 9/41/5 7/39/5
|
||||
f 10/42/6 11/43/6 9/41/6
|
||||
f 12/44/7 13/45/7 11/43/7
|
||||
f 14/46/8 15/47/8 13/45/8
|
||||
f 16/48/9 17/49/9 15/47/9
|
||||
f 18/50/10 19/51/10 17/49/10
|
||||
f 20/52/11 21/53/11 19/51/11
|
||||
f 22/54/12 23/55/12 21/53/12
|
||||
f 24/56/13 25/57/13 23/55/13
|
||||
f 26/58/14 27/59/14 25/57/14
|
||||
f 28/60/15 29/61/15 27/59/15
|
||||
f 30/62/16 31/63/16 29/61/16
|
||||
f 32/64/17 33/65/17 31/63/17
|
||||
f 34/66/18 35/67/18 33/65/18
|
||||
f 36/68/19 37/69/19 35/67/19
|
||||
f 38/70/20 39/71/20 37/69/20
|
||||
f 40/72/21 41/73/21 39/71/21
|
||||
f 42/74/22 43/75/22 41/73/22
|
||||
f 44/76/23 45/77/23 43/75/23
|
||||
f 46/78/24 47/79/24 45/77/24
|
||||
f 48/80/25 49/81/25 47/79/25
|
||||
f 50/82/26 51/83/26 49/81/26
|
||||
f 52/84/27 53/85/27 51/83/27
|
||||
f 54/86/28 55/87/28 53/85/28
|
||||
f 56/88/29 57/89/29 55/87/29
|
||||
f 58/90/30 59/91/30 57/89/30
|
||||
f 60/92/31 61/93/31 59/91/31
|
||||
f 62/94/32 63/95/32 61/93/32
|
||||
f 64/96/33 1/35/33 63/95/33
|
||||
f 31/63/34 55/87/34 63/95/34
|
||||
f 2/33/2 4/36/2 3/34/2
|
||||
f 4/36/3 6/38/3 5/37/3
|
||||
f 6/38/4 8/40/4 7/39/4
|
||||
f 8/40/5 10/42/5 9/41/5
|
||||
f 10/42/6 12/44/6 11/43/6
|
||||
f 12/44/7 14/46/7 13/45/7
|
||||
f 14/46/8 16/48/8 15/47/8
|
||||
f 16/48/9 18/50/9 17/49/9
|
||||
f 18/50/10 20/52/10 19/51/10
|
||||
f 20/52/11 22/54/11 21/53/11
|
||||
f 22/54/12 24/56/12 23/55/12
|
||||
f 24/56/13 26/58/13 25/57/13
|
||||
f 26/58/14 28/60/14 27/59/14
|
||||
f 28/60/15 30/62/15 29/61/15
|
||||
f 30/62/16 32/64/16 31/63/16
|
||||
f 32/64/17 34/66/17 33/65/17
|
||||
f 34/66/18 36/68/18 35/67/18
|
||||
f 36/68/19 38/70/19 37/69/19
|
||||
f 38/70/20 40/72/20 39/71/20
|
||||
f 40/72/21 42/74/21 41/73/21
|
||||
f 42/74/22 44/76/22 43/75/22
|
||||
f 44/76/23 46/78/23 45/77/23
|
||||
f 46/78/24 48/80/24 47/79/24
|
||||
f 48/80/25 50/82/25 49/81/25
|
||||
f 50/82/26 52/84/26 51/83/26
|
||||
f 52/84/27 54/86/27 53/85/27
|
||||
f 54/86/28 56/88/28 55/87/28
|
||||
f 56/88/29 58/90/29 57/89/29
|
||||
f 58/90/30 60/92/30 59/91/30
|
||||
f 60/92/31 62/94/31 61/93/31
|
||||
f 62/94/32 64/96/32 63/95/32
|
||||
f 64/96/33 2/33/33 1/35/33
|
||||
f 63/95/34 1/35/34 3/34/34
|
||||
f 3/34/34 5/37/34 7/39/34
|
||||
f 7/39/34 9/41/34 15/47/34
|
||||
f 11/43/34 13/45/34 15/47/34
|
||||
f 15/47/34 17/49/34 19/51/34
|
||||
f 19/51/34 21/53/34 23/55/34
|
||||
f 23/55/34 25/57/34 31/63/34
|
||||
f 27/59/34 29/61/34 31/63/34
|
||||
f 31/63/34 33/65/34 35/67/34
|
||||
f 35/67/34 37/69/34 31/63/34
|
||||
f 39/71/34 41/73/34 47/79/34
|
||||
f 43/75/34 45/77/34 47/79/34
|
||||
f 47/79/34 49/81/34 51/83/34
|
||||
f 51/83/34 53/85/34 55/87/34
|
||||
f 55/87/34 57/89/34 63/95/34
|
||||
f 59/91/34 61/93/34 63/95/34
|
||||
f 63/95/34 3/34/34 15/47/34
|
||||
f 9/41/34 11/43/34 15/47/34
|
||||
f 15/47/34 19/51/34 31/63/34
|
||||
f 25/57/34 27/59/34 31/63/34
|
||||
f 31/63/34 37/69/34 39/71/34
|
||||
f 41/73/34 43/75/34 47/79/34
|
||||
f 47/79/34 51/83/34 55/87/34
|
||||
f 57/89/34 59/91/34 63/95/34
|
||||
f 3/34/34 7/39/34 15/47/34
|
||||
f 19/51/34 23/55/34 31/63/34
|
||||
f 31/63/34 39/71/34 47/79/34
|
||||
f 47/79/34 55/87/34 31/63/34
|
||||
f 63/95/34 15/47/34 31/63/34
|
||||
Binary file not shown.
@@ -85,6 +85,9 @@ struct GUIHelperInterface
|
||||
virtual void removeAllUserDebugItems( ){};
|
||||
virtual void setVisualizerFlagCallback(VisualizerFlagCallback callback){}
|
||||
|
||||
//empty name stops dumping video
|
||||
virtual void dumpFramesToVideo(const char* mp4FileName) {};
|
||||
|
||||
};
|
||||
|
||||
|
||||
|
||||
@@ -191,6 +191,8 @@ SET(BulletExampleBrowser_SRCS
|
||||
../SharedMemory/PhysicsLoopBackC_API.h
|
||||
../SharedMemory/PhysicsServerCommandProcessor.cpp
|
||||
../SharedMemory/PhysicsServerCommandProcessor.h
|
||||
../SharedMemory/SharedMemoryCommands.h
|
||||
../SharedMemory/SharedMemoryPublic.h
|
||||
../BasicDemo/BasicExample.cpp
|
||||
../BasicDemo/BasicExample.h
|
||||
../InverseDynamics/InverseDynamicsExample.cpp
|
||||
|
||||
@@ -371,6 +371,7 @@ void OpenGLExampleBrowserVisualizerFlagCallback(int flag, bool enable)
|
||||
if (flag == COV_ENABLE_GUI)
|
||||
{
|
||||
renderGui = enable;
|
||||
renderGrid = enable;
|
||||
}
|
||||
|
||||
if (flag == COV_ENABLE_WIREFRAME)
|
||||
|
||||
@@ -427,47 +427,67 @@ void OpenGLGuiHelper::copyCameraImageData(const float viewMatrix[16], const floa
|
||||
SimpleCamera tempCam;
|
||||
getRenderInterface()->setActiveCamera(&tempCam);
|
||||
getRenderInterface()->getActiveCamera()->setVRCamera(viewMatrix,projectionMatrix);
|
||||
getRenderInterface()->renderScene();
|
||||
{
|
||||
BT_PROFILE("renderScene");
|
||||
getRenderInterface()->renderScene();
|
||||
}
|
||||
getRenderInterface()->setActiveCamera(oldCam);
|
||||
|
||||
{
|
||||
BT_PROFILE("copy pixels");
|
||||
btAlignedObjectArray<unsigned char> sourceRgbaPixelBuffer;
|
||||
btAlignedObjectArray<float> sourceDepthBuffer;
|
||||
//copy the image into our local cache
|
||||
sourceRgbaPixelBuffer.resize(sourceWidth*sourceHeight*numBytesPerPixel);
|
||||
sourceDepthBuffer.resize(sourceWidth*sourceHeight);
|
||||
m_data->m_glApp->getScreenPixels(&(sourceRgbaPixelBuffer[0]),sourceRgbaPixelBuffer.size(), &sourceDepthBuffer[0],sizeof(float)*sourceDepthBuffer.size());
|
||||
{
|
||||
BT_PROFILE("getScreenPixels");
|
||||
m_data->m_glApp->getScreenPixels(&(sourceRgbaPixelBuffer[0]),sourceRgbaPixelBuffer.size(), &sourceDepthBuffer[0],sizeof(float)*sourceDepthBuffer.size());
|
||||
}
|
||||
|
||||
m_data->m_rgbaPixelBuffer1.resize(destinationWidth*destinationHeight*numBytesPerPixel);
|
||||
m_data->m_depthBuffer1.resize(destinationWidth*destinationHeight);
|
||||
//rescale and flip
|
||||
|
||||
for (int i=0;i<destinationWidth;i++)
|
||||
{
|
||||
for (int j=0;j<destinationHeight;j++)
|
||||
{
|
||||
int xIndex = int(float(i)*(float(sourceWidth)/float(destinationWidth)));
|
||||
int yIndex = int(float(destinationHeight-1-j)*(float(sourceHeight)/float(destinationHeight)));
|
||||
btClamp(xIndex,0,sourceWidth);
|
||||
btClamp(yIndex,0,sourceHeight);
|
||||
int bytesPerPixel = 4; //RGBA
|
||||
{
|
||||
BT_PROFILE("resize and flip");
|
||||
for (int j=0;j<destinationHeight;j++)
|
||||
{
|
||||
for (int i=0;i<destinationWidth;i++)
|
||||
{
|
||||
int xIndex = int(float(i)*(float(sourceWidth)/float(destinationWidth)));
|
||||
int yIndex = int(float(destinationHeight-1-j)*(float(sourceHeight)/float(destinationHeight)));
|
||||
btClamp(xIndex,0,sourceWidth);
|
||||
btClamp(yIndex,0,sourceHeight);
|
||||
int bytesPerPixel = 4; //RGBA
|
||||
|
||||
int sourcePixelIndex = (xIndex+yIndex*sourceWidth)*bytesPerPixel;
|
||||
int sourceDepthIndex = xIndex+yIndex*sourceWidth;
|
||||
int sourcePixelIndex = (xIndex+yIndex*sourceWidth)*bytesPerPixel;
|
||||
int sourceDepthIndex = xIndex+yIndex*sourceWidth;
|
||||
#define COPY4PIXELS 1
|
||||
#ifdef COPY4PIXELS
|
||||
int* dst = (int*)&m_data->m_rgbaPixelBuffer1[(i+j*destinationWidth)*4+0];
|
||||
int* src = (int*)&sourceRgbaPixelBuffer[sourcePixelIndex+0];
|
||||
*dst = *src;
|
||||
|
||||
#else
|
||||
m_data->m_rgbaPixelBuffer1[(i+j*destinationWidth)*4+0] = sourceRgbaPixelBuffer[sourcePixelIndex+0];
|
||||
m_data->m_rgbaPixelBuffer1[(i+j*destinationWidth)*4+1] = sourceRgbaPixelBuffer[sourcePixelIndex+1];
|
||||
m_data->m_rgbaPixelBuffer1[(i+j*destinationWidth)*4+2] = sourceRgbaPixelBuffer[sourcePixelIndex+2];
|
||||
m_data->m_rgbaPixelBuffer1[(i+j*destinationWidth)*4+3] = 255;
|
||||
#endif
|
||||
if (depthBuffer)
|
||||
{
|
||||
m_data->m_depthBuffer1[i+j*destinationWidth] = sourceDepthBuffer[sourceDepthIndex];
|
||||
}
|
||||
|
||||
m_data->m_rgbaPixelBuffer1[(i+j*destinationWidth)*4+0] = sourceRgbaPixelBuffer[sourcePixelIndex+0];
|
||||
m_data->m_rgbaPixelBuffer1[(i+j*destinationWidth)*4+1] = sourceRgbaPixelBuffer[sourcePixelIndex+1];
|
||||
m_data->m_rgbaPixelBuffer1[(i+j*destinationWidth)*4+2] = sourceRgbaPixelBuffer[sourcePixelIndex+2];
|
||||
m_data->m_rgbaPixelBuffer1[(i+j*destinationWidth)*4+3] = 255;
|
||||
|
||||
m_data->m_depthBuffer1[i+j*destinationWidth] = sourceDepthBuffer[sourceDepthIndex];
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
if (pixelsRGBA)
|
||||
{
|
||||
BT_PROFILE("copy rgba pixels");
|
||||
|
||||
for (int i=0;i<numRequestedPixels*numBytesPerPixel;i++)
|
||||
{
|
||||
pixelsRGBA[i] = m_data->m_rgbaPixelBuffer1[i+startPixelIndex*numBytesPerPixel];
|
||||
@@ -475,6 +495,8 @@ void OpenGLGuiHelper::copyCameraImageData(const float viewMatrix[16], const floa
|
||||
}
|
||||
if (depthBuffer)
|
||||
{
|
||||
BT_PROFILE("copy depth buffer pixels");
|
||||
|
||||
for (int i=0;i<numRequestedPixels;i++)
|
||||
{
|
||||
depthBuffer[i] = m_data->m_depthBuffer1[i+startPixelIndex];
|
||||
@@ -546,3 +568,10 @@ struct CommonGraphicsApp* OpenGLGuiHelper::getAppInterface()
|
||||
return m_data->m_glApp;
|
||||
}
|
||||
|
||||
void OpenGLGuiHelper::dumpFramesToVideo(const char* mp4FileName)
|
||||
{
|
||||
if (m_data->m_glApp)
|
||||
{
|
||||
m_data->m_glApp->dumpFramesToVideo(mp4FileName);
|
||||
}
|
||||
}
|
||||
@@ -85,6 +85,8 @@ struct OpenGLGuiHelper : public GUIHelperInterface
|
||||
|
||||
virtual void setVisualizerFlagCallback(VisualizerFlagCallback callback);
|
||||
|
||||
virtual void dumpFramesToVideo(const char* mp4FileName);
|
||||
|
||||
};
|
||||
|
||||
#endif //OPENGL_GUI_HELPER_H
|
||||
|
||||
@@ -88,6 +88,8 @@ project "App_BulletExampleBrowser"
|
||||
"../SharedMemory/PhysicsServerCommandProcessor.h",
|
||||
"../SharedMemory/TinyRendererVisualShapeConverter.cpp",
|
||||
"../SharedMemory/TinyRendererVisualShapeConverter.h",
|
||||
"../SharedMemory/SharedMemoryCommands.h",
|
||||
"../SharedMemory/SharedMemoryPublic.h",
|
||||
"../MultiThreading/MultiThreadingExample.cpp",
|
||||
"../MultiThreading/b3PosixThreadSupport.cpp",
|
||||
"../MultiThreading/b3Win32ThreadSupport.cpp",
|
||||
|
||||
@@ -30,7 +30,7 @@ int main(int argc, char** argv)
|
||||
btDefaultCollisionConfiguration* collisionConfiguration = new btDefaultCollisionConfiguration();
|
||||
|
||||
///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
|
||||
btCollisionDispatcher* dispatcher = new btCollisionDispatcher(collisionConfiguration);
|
||||
btCollisionDispatcher* dispatcher = new btCollisionDispatcher(collisionConfiguration);
|
||||
|
||||
///btDbvtBroadphase is a good general purpose broadphase. You can also try out btAxis3Sweep.
|
||||
btBroadphaseInterface* overlappingPairCache = new btDbvtBroadphase();
|
||||
@@ -38,9 +38,9 @@ int main(int argc, char** argv)
|
||||
///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded)
|
||||
btSequentialImpulseConstraintSolver* solver = new btSequentialImpulseConstraintSolver;
|
||||
|
||||
btDiscreteDynamicsWorld* dynamicsWorld = new btDiscreteDynamicsWorld(dispatcher,overlappingPairCache,solver,collisionConfiguration);
|
||||
btDiscreteDynamicsWorld* dynamicsWorld = new btDiscreteDynamicsWorld(dispatcher, overlappingPairCache, solver, collisionConfiguration);
|
||||
|
||||
dynamicsWorld->setGravity(btVector3(0,-10,0));
|
||||
dynamicsWorld->setGravity(btVector3(0, -10, 0));
|
||||
|
||||
///-----initialization_end-----
|
||||
|
||||
@@ -48,39 +48,37 @@ int main(int argc, char** argv)
|
||||
//make sure to re-use collision shapes among rigid bodies whenever possible!
|
||||
btAlignedObjectArray<btCollisionShape*> collisionShapes;
|
||||
|
||||
|
||||
///create a few basic rigid bodies
|
||||
|
||||
//the ground is a cube of side 100 at position y = -56.
|
||||
//the sphere will hit it at y = -6, with center at -5
|
||||
{
|
||||
btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(50.),btScalar(50.),btScalar(50.)));
|
||||
btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(50.), btScalar(50.), btScalar(50.)));
|
||||
|
||||
collisionShapes.push_back(groundShape);
|
||||
|
||||
btTransform groundTransform;
|
||||
groundTransform.setIdentity();
|
||||
groundTransform.setOrigin(btVector3(0,-56,0));
|
||||
groundTransform.setOrigin(btVector3(0, -56, 0));
|
||||
|
||||
btScalar mass(0.);
|
||||
|
||||
//rigidbody is dynamic if and only if mass is non zero, otherwise static
|
||||
bool isDynamic = (mass != 0.f);
|
||||
|
||||
btVector3 localInertia(0,0,0);
|
||||
btVector3 localInertia(0, 0, 0);
|
||||
if (isDynamic)
|
||||
groundShape->calculateLocalInertia(mass,localInertia);
|
||||
groundShape->calculateLocalInertia(mass, localInertia);
|
||||
|
||||
//using motionstate is optional, it provides interpolation capabilities, and only synchronizes 'active' objects
|
||||
btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform);
|
||||
btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,groundShape,localInertia);
|
||||
btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, groundShape, localInertia);
|
||||
btRigidBody* body = new btRigidBody(rbInfo);
|
||||
|
||||
//add the body to the dynamics world
|
||||
dynamicsWorld->addRigidBody(body);
|
||||
}
|
||||
|
||||
|
||||
{
|
||||
//create a dynamic rigidbody
|
||||
|
||||
@@ -92,37 +90,34 @@ int main(int argc, char** argv)
|
||||
btTransform startTransform;
|
||||
startTransform.setIdentity();
|
||||
|
||||
btScalar mass(1.f);
|
||||
btScalar mass(1.f);
|
||||
|
||||
//rigidbody is dynamic if and only if mass is non zero, otherwise static
|
||||
bool isDynamic = (mass != 0.f);
|
||||
|
||||
btVector3 localInertia(0,0,0);
|
||||
btVector3 localInertia(0, 0, 0);
|
||||
if (isDynamic)
|
||||
colShape->calculateLocalInertia(mass,localInertia);
|
||||
colShape->calculateLocalInertia(mass, localInertia);
|
||||
|
||||
startTransform.setOrigin(btVector3(2,10,0));
|
||||
|
||||
//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
|
||||
btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform);
|
||||
btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,colShape,localInertia);
|
||||
btRigidBody* body = new btRigidBody(rbInfo);
|
||||
startTransform.setOrigin(btVector3(2, 10, 0));
|
||||
|
||||
dynamicsWorld->addRigidBody(body);
|
||||
//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
|
||||
btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform);
|
||||
btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, colShape, localInertia);
|
||||
btRigidBody* body = new btRigidBody(rbInfo);
|
||||
|
||||
dynamicsWorld->addRigidBody(body);
|
||||
}
|
||||
|
||||
|
||||
|
||||
/// Do some simulation
|
||||
|
||||
/// Do some simulation
|
||||
|
||||
///-----stepsimulation_start-----
|
||||
for (i=0;i<150;i++)
|
||||
for (i = 0; i < 150; i++)
|
||||
{
|
||||
dynamicsWorld->stepSimulation(1.f/60.f,10);
|
||||
|
||||
dynamicsWorld->stepSimulation(1.f / 60.f, 10);
|
||||
|
||||
//print positions of all objects
|
||||
for (int j=dynamicsWorld->getNumCollisionObjects()-1; j>=0 ;j--)
|
||||
for (int j = dynamicsWorld->getNumCollisionObjects() - 1; j >= 0; j--)
|
||||
{
|
||||
btCollisionObject* obj = dynamicsWorld->getCollisionObjectArray()[j];
|
||||
btRigidBody* body = btRigidBody::upcast(obj);
|
||||
@@ -130,23 +125,23 @@ int main(int argc, char** argv)
|
||||
if (body && body->getMotionState())
|
||||
{
|
||||
body->getMotionState()->getWorldTransform(trans);
|
||||
|
||||
} else
|
||||
}
|
||||
else
|
||||
{
|
||||
trans = obj->getWorldTransform();
|
||||
}
|
||||
printf("world pos object %d = %f,%f,%f\n",j,float(trans.getOrigin().getX()),float(trans.getOrigin().getY()),float(trans.getOrigin().getZ()));
|
||||
printf("world pos object %d = %f,%f,%f\n", j, float(trans.getOrigin().getX()), float(trans.getOrigin().getY()), float(trans.getOrigin().getZ()));
|
||||
}
|
||||
}
|
||||
|
||||
///-----stepsimulation_end-----
|
||||
|
||||
//cleanup in the reverse order of creation/initialization
|
||||
|
||||
|
||||
///-----cleanup_start-----
|
||||
|
||||
//remove the rigidbodies from the dynamics world and delete them
|
||||
for (i=dynamicsWorld->getNumCollisionObjects()-1; i>=0 ;i--)
|
||||
for (i = dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--)
|
||||
{
|
||||
btCollisionObject* obj = dynamicsWorld->getCollisionObjectArray()[i];
|
||||
btRigidBody* body = btRigidBody::upcast(obj);
|
||||
@@ -154,12 +149,12 @@ int main(int argc, char** argv)
|
||||
{
|
||||
delete body->getMotionState();
|
||||
}
|
||||
dynamicsWorld->removeCollisionObject( obj );
|
||||
dynamicsWorld->removeCollisionObject(obj);
|
||||
delete obj;
|
||||
}
|
||||
|
||||
//delete collision shapes
|
||||
for (int j=0;j<collisionShapes.size();j++)
|
||||
for (int j = 0; j < collisionShapes.size(); j++)
|
||||
{
|
||||
btCollisionShape* shape = collisionShapes[j];
|
||||
collisionShapes[j] = 0;
|
||||
@@ -183,4 +178,3 @@ int main(int argc, char** argv)
|
||||
//next line is optional: it will be cleared by the destructor when the array goes out of scope
|
||||
collisionShapes.clear();
|
||||
}
|
||||
|
||||
|
||||
@@ -30,12 +30,6 @@
|
||||
|
||||
#include <vector>
|
||||
|
||||
enum eMJCF_FILE_TYPE_ENUMS
|
||||
{
|
||||
MJCF_FILE_STL = 1,
|
||||
MJCF_FILE_OBJ = 2
|
||||
};
|
||||
|
||||
enum ePARENT_LINK_ENUMS
|
||||
{
|
||||
BASE_LINK_INDEX=-1,
|
||||
@@ -137,9 +131,11 @@ struct MyMJCFAsset
|
||||
struct BulletMJCFImporterInternalData
|
||||
{
|
||||
GUIHelperInterface* m_guiHelper;
|
||||
struct LinkVisualShapesConverter* m_customVisualShapesConverter;
|
||||
char m_pathPrefix[1024];
|
||||
|
||||
std::string m_fileModelName;
|
||||
std::string m_sourceFileName; // with path
|
||||
std::string m_fileModelName; // without path
|
||||
btHashMap<btHashString,MyMJCFAsset> m_assets;
|
||||
|
||||
btAlignedObjectArray<UrdfModel*> m_models;
|
||||
@@ -150,22 +146,37 @@ struct BulletMJCFImporterInternalData
|
||||
|
||||
|
||||
int m_activeModel;
|
||||
int m_activeBodyUniqueId;
|
||||
//todo: for full MJCF compatibility, we would need a stack of default values
|
||||
int m_defaultCollisionGroup;
|
||||
int m_defaultCollisionMask;
|
||||
btScalar m_defaultCollisionMargin;
|
||||
|
||||
// joint defaults
|
||||
std::string m_defaultJointLimited;
|
||||
|
||||
// geom defaults
|
||||
std::string m_defaultGeomRgba;
|
||||
|
||||
//those collision shapes are deleted by caller (todo: make sure this happens!)
|
||||
btAlignedObjectArray<btCollisionShape*> m_allocatedCollisionShapes;
|
||||
|
||||
BulletMJCFImporterInternalData()
|
||||
:m_activeModel(-1),
|
||||
m_activeBodyUniqueId(-1),
|
||||
m_defaultCollisionGroup(1),
|
||||
m_defaultCollisionMask(1),
|
||||
m_defaultCollisionMargin(0.001)//assume unit meters, margin is 1mm
|
||||
{
|
||||
m_pathPrefix[0] = 0;
|
||||
}
|
||||
|
||||
std::string sourceFileLocation(TiXmlElement* e)
|
||||
{
|
||||
char buf[1024];
|
||||
snprintf(buf, sizeof(buf), "%s:%i", m_sourceFileName.c_str(), e->Row());
|
||||
return buf;
|
||||
}
|
||||
|
||||
const UrdfLink* getLink(int modelIndex, int linkIndex) const
|
||||
{
|
||||
@@ -238,6 +249,17 @@ struct BulletMJCFImporterInternalData
|
||||
{
|
||||
parseAssets(child_xml,logger);
|
||||
}
|
||||
if (n=="joint")
|
||||
{
|
||||
// Other attributes here:
|
||||
// armature="1"
|
||||
// damping="1"
|
||||
// limited="true"
|
||||
if (const char* conTypeStr = child_xml->Attribute("limited"))
|
||||
{
|
||||
m_defaultJointLimited = child_xml->Attribute("limited");
|
||||
}
|
||||
}
|
||||
if (n=="geom")
|
||||
{
|
||||
//contype, conaffinity
|
||||
@@ -251,6 +273,11 @@ struct BulletMJCFImporterInternalData
|
||||
{
|
||||
m_defaultCollisionMask = urdfLexicalCast<int>(conAffinityStr);
|
||||
}
|
||||
const char* rgba = child_xml->Attribute("rgba");
|
||||
if (rgba)
|
||||
{
|
||||
m_defaultGeomRgba = rgba;
|
||||
}
|
||||
}
|
||||
}
|
||||
handled=true;
|
||||
@@ -361,39 +388,57 @@ struct BulletMJCFImporterInternalData
|
||||
bool isLimited = false;
|
||||
double range[2] = {1,0};
|
||||
|
||||
std::string lim = m_defaultJointLimited;
|
||||
if (limitedStr)
|
||||
{
|
||||
std::string lim = limitedStr;
|
||||
if (lim=="true")
|
||||
{
|
||||
isLimited = true;
|
||||
//parse the 'range' field
|
||||
btArray<std::string> pieces;
|
||||
btArray<float> sizes;
|
||||
btAlignedObjectArray<std::string> strArray;
|
||||
urdfIsAnyOf(" ", strArray);
|
||||
urdfStringSplit(pieces, rangeStr, strArray);
|
||||
for (int i = 0; i < pieces.size(); ++i)
|
||||
{
|
||||
if (!pieces[i].empty())
|
||||
{
|
||||
sizes.push_back(urdfLexicalCast<double>(pieces[i].c_str()));
|
||||
}
|
||||
}
|
||||
if (sizes.size()==2)
|
||||
{
|
||||
range[0] = sizes[0];
|
||||
range[1] = sizes[1];
|
||||
} else
|
||||
{
|
||||
logger->reportWarning("Expected range[2] in joint with limits");
|
||||
}
|
||||
|
||||
}
|
||||
} else
|
||||
{
|
||||
// logger->reportWarning("joint without limited field");
|
||||
lim = limitedStr;
|
||||
}
|
||||
if (lim=="true")
|
||||
{
|
||||
isLimited = true;
|
||||
//parse the 'range' field
|
||||
btArray<std::string> pieces;
|
||||
btArray<float> sizes;
|
||||
btAlignedObjectArray<std::string> strArray;
|
||||
urdfIsAnyOf(" ", strArray);
|
||||
urdfStringSplit(pieces, rangeStr, strArray);
|
||||
for (int i = 0; i < pieces.size(); ++i)
|
||||
{
|
||||
if (!pieces[i].empty())
|
||||
{
|
||||
sizes.push_back(urdfLexicalCast<double>(pieces[i].c_str()));
|
||||
}
|
||||
}
|
||||
if (sizes.size()==2)
|
||||
{
|
||||
// TODO angle units are in "<compiler angle="degree" inertiafromgeom="true"/>
|
||||
range[0] = sizes[0] * B3_PI / 180;
|
||||
range[1] = sizes[1] * B3_PI / 180;
|
||||
} else
|
||||
{
|
||||
logger->reportWarning("Expected range[2] in joint with limits");
|
||||
}
|
||||
}
|
||||
|
||||
// TODO armature : real, "0" Armature inertia (or rotor inertia) of all
|
||||
// degrees of freedom created by this joint. These are constants added to the
|
||||
// diagonal of the inertia matrix in generalized coordinates. They make the
|
||||
// simulation more stable, and often increase physical realism. This is because
|
||||
// when a motor is attached to the system with a transmission that amplifies
|
||||
// the motor force by c, the inertia of the rotor (i.e. the moving part of the
|
||||
// motor) is amplified by c*c. The same holds for gears in the early stages of
|
||||
// planetary gear boxes. These extra inertias often dominate the inertias of
|
||||
// the robot parts that are represented explicitly in the model, and the
|
||||
// armature attribute is the way to model them.
|
||||
|
||||
// TODO damping : real, "0" Damping applied to all degrees of
|
||||
// freedom created by this joint. Unlike friction loss
|
||||
// which is computed by the constraint solver, damping is
|
||||
// simply a force linear in velocity. It is included in
|
||||
// the passive forces. Despite this simplicity, larger
|
||||
// damping values can make numerical integrators unstable,
|
||||
// which is why our Euler integrator handles damping
|
||||
// implicitly. See Integration in the Computation chapter.
|
||||
|
||||
bool jointHandled = false;
|
||||
const UrdfLink* linkPtr = getLink(modelIndex,linkIndex);
|
||||
@@ -443,11 +488,11 @@ struct BulletMJCFImporterInternalData
|
||||
jointPtr->m_parentLinkToJointTransform = parentLinkToJointTransform;
|
||||
jointPtr->m_type = ejtype;
|
||||
int numJoints = m_models[modelIndex]->m_joints.size();
|
||||
|
||||
|
||||
//range
|
||||
jointPtr->m_lowerLimit = range[0];
|
||||
jointPtr->m_upperLimit = range[1];
|
||||
|
||||
|
||||
if (nameStr)
|
||||
{
|
||||
jointPtr->m_name =nameStr;
|
||||
@@ -489,10 +534,23 @@ struct BulletMJCFImporterInternalData
|
||||
|
||||
|
||||
|
||||
// const char* rgba = link_xml->Attribute("rgba");
|
||||
const char* gType = link_xml->Attribute("type");
|
||||
const char* sz = link_xml->Attribute("size");
|
||||
const char* posS = link_xml->Attribute("pos");
|
||||
|
||||
std::string rgba = m_defaultGeomRgba;
|
||||
if (const char* rgbattr = link_xml->Attribute("rgba"))
|
||||
{
|
||||
rgba = rgbattr;
|
||||
}
|
||||
if (!rgba.empty())
|
||||
{
|
||||
// "0 0.7 0.7 1"
|
||||
parseVector4(geom.m_localMaterial.m_rgbaColor, rgba);
|
||||
geom.m_hasLocalMaterial = true;
|
||||
geom.m_localMaterial.m_name = rgba;
|
||||
}
|
||||
|
||||
if (posS)
|
||||
{
|
||||
btVector3 pos(0,0,0);
|
||||
@@ -560,10 +618,10 @@ struct BulletMJCFImporterInternalData
|
||||
handledGeomType = true;
|
||||
}
|
||||
|
||||
//todo: capsule, cylinder, meshes or heightfields etc
|
||||
if (geomType == "capsule")
|
||||
if (geomType == "capsule" || geomType == "cylinder")
|
||||
{
|
||||
geom.m_type = URDF_GEOM_CAPSULE;
|
||||
// <geom conaffinity="0" contype="0" fromto="0 0 0 0 0 0.02" name="root" rgba="0.9 0.4 0.6 1" size=".011" type="cylinder"/>
|
||||
geom.m_type = geomType=="cylinder" ? URDF_GEOM_CYLINDER : URDF_GEOM_CAPSULE;
|
||||
|
||||
btArray<std::string> pieces;
|
||||
btArray<float> sizes;
|
||||
@@ -621,9 +679,14 @@ struct BulletMJCFImporterInternalData
|
||||
MyMJCFAsset* assetPtr = m_assets[meshStr];
|
||||
if (assetPtr)
|
||||
{
|
||||
handledGeomType = true;
|
||||
geom.m_type = URDF_GEOM_MESH;
|
||||
geom.m_meshFileName = assetPtr->m_fileName;
|
||||
bool exists = findExistingMeshFile(
|
||||
m_sourceFileName, assetPtr->m_fileName, sourceFileLocation(link_xml),
|
||||
&geom.m_meshFileName,
|
||||
&geom.m_meshFileType);
|
||||
handledGeomType = exists;
|
||||
|
||||
geom.m_meshScale.setValue(1,1,1);
|
||||
//todo: parse mesh scale
|
||||
if (sz)
|
||||
@@ -632,13 +695,6 @@ struct BulletMJCFImporterInternalData
|
||||
}
|
||||
}
|
||||
}
|
||||
#if 0
|
||||
if (geomType == "cylinder")
|
||||
{
|
||||
geom.m_type = URDF_GEOM_CYLINDER;
|
||||
handledGeomType = true;
|
||||
}
|
||||
#endif
|
||||
if (handledGeomType)
|
||||
{
|
||||
|
||||
@@ -803,6 +859,7 @@ struct BulletMJCFImporterInternalData
|
||||
|
||||
return orgChildLinkIndex;
|
||||
}
|
||||
|
||||
bool parseBody(TiXmlElement* link_xml, int modelIndex, int orgParentLinkIndex, MJCFErrorLogger* logger)
|
||||
{
|
||||
int newParentLinkIndex = orgParentLinkIndex;
|
||||
@@ -964,10 +1021,6 @@ struct BulletMJCFImporterInternalData
|
||||
}
|
||||
|
||||
linkPtr->m_linkTransformInWorld = linkTransform;
|
||||
if (bodyN == "cart1")//front_left_leg")
|
||||
{
|
||||
printf("found!\n");
|
||||
}
|
||||
if ((newParentLinkIndex != INVALID_LINK_INDEX) && !skipFixedJoint)
|
||||
{
|
||||
//linkPtr->m_linkTransformInWorld.setIdentity();
|
||||
@@ -1113,10 +1166,11 @@ struct BulletMJCFImporterInternalData
|
||||
|
||||
};
|
||||
|
||||
BulletMJCFImporter::BulletMJCFImporter(struct GUIHelperInterface* helper)
|
||||
BulletMJCFImporter::BulletMJCFImporter(struct GUIHelperInterface* helper, LinkVisualShapesConverter* customConverter)
|
||||
{
|
||||
m_data = new BulletMJCFImporterInternalData();
|
||||
m_data->m_guiHelper = helper;
|
||||
m_data->m_customVisualShapesConverter = customConverter;
|
||||
}
|
||||
|
||||
BulletMJCFImporter::~BulletMJCFImporter()
|
||||
@@ -1135,7 +1189,8 @@ bool BulletMJCFImporter::loadMJCF(const char* fileName, MJCFErrorLogger* logger,
|
||||
b3FileUtils fu;
|
||||
|
||||
//bool fileFound = fu.findFile(fileName, relativeFileName, 1024);
|
||||
bool fileFound = (b3ResourcePath::findResourcePath(fileName,relativeFileName,1024)>0);
|
||||
bool fileFound = (b3ResourcePath::findResourcePath(fileName,relativeFileName,1024)>0);
|
||||
m_data->m_sourceFileName = relativeFileName;
|
||||
|
||||
std::string xml_string;
|
||||
m_data->m_pathPrefix[0] = 0;
|
||||
@@ -1399,21 +1454,26 @@ bool BulletMJCFImporter::getLinkContactInfo(int linkIndex, URDFLinkContactInfo&
|
||||
return false;
|
||||
}
|
||||
|
||||
|
||||
void BulletMJCFImporter::convertLinkVisualShapes2(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int objectIndex) const
|
||||
void BulletMJCFImporter::convertLinkVisualShapes2(int linkIndex, int urdfIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int objectIndex) const
|
||||
{
|
||||
if (m_data->m_customVisualShapesConverter)
|
||||
{
|
||||
const UrdfLink* link = m_data->getLink(m_data->m_activeModel, urdfIndex);
|
||||
m_data->m_customVisualShapesConverter->convertVisualShapes(linkIndex,pathPrefix,inertialFrame, link, 0, colObj, objectIndex);
|
||||
}
|
||||
}
|
||||
|
||||
void BulletMJCFImporter::setBodyUniqueId(int bodyId)
|
||||
{
|
||||
|
||||
m_data->m_activeBodyUniqueId = bodyId;
|
||||
}
|
||||
|
||||
int BulletMJCFImporter::getBodyUniqueId() const
|
||||
{
|
||||
return 0;
|
||||
b3Assert(m_data->m_activeBodyUniqueId != -1);
|
||||
return m_data->m_activeBodyUniqueId;
|
||||
}
|
||||
|
||||
|
||||
static btCollisionShape* MjcfCreateConvexHullFromShapes(std::vector<tinyobj::shape_t>& shapes, const btVector3& geomScale, btScalar collisionMargin)
|
||||
{
|
||||
btCompoundShape* compound = new btCompoundShape();
|
||||
@@ -1496,132 +1556,87 @@ class btCompoundShape* BulletMJCFImporter::convertLinkCollisionShapes(int linkIn
|
||||
}
|
||||
case URDF_GEOM_MESH:
|
||||
{
|
||||
//////////////////////
|
||||
if (1)
|
||||
{
|
||||
if (col->m_geometry.m_meshFileName.length())
|
||||
GLInstanceGraphicsShape* glmesh = 0;
|
||||
switch (col->m_geometry.m_meshFileType)
|
||||
{
|
||||
const char* filename = col->m_geometry.m_meshFileName.c_str();
|
||||
//b3Printf("mesh->filename=%s\n",filename);
|
||||
char fullPath[1024];
|
||||
int fileType = 0;
|
||||
sprintf(fullPath,"%s%s",pathPrefix,filename);
|
||||
b3FileUtils::toLower(fullPath);
|
||||
char tmpPathPrefix[1024];
|
||||
int maxPathLen = 1024;
|
||||
b3FileUtils::extractPath(filename,tmpPathPrefix,maxPathLen);
|
||||
|
||||
char collisionPathPrefix[1024];
|
||||
sprintf(collisionPathPrefix,"%s%s",pathPrefix,tmpPathPrefix);
|
||||
|
||||
if (strstr(fullPath,".stl"))
|
||||
case UrdfGeometry::FILE_OBJ:
|
||||
{
|
||||
fileType = MJCF_FILE_STL;
|
||||
if (col->m_flags & URDF_FORCE_CONCAVE_TRIMESH)
|
||||
{
|
||||
glmesh = LoadMeshFromObj(col->m_geometry.m_meshFileName.c_str(), 0);
|
||||
}
|
||||
else
|
||||
{
|
||||
std::vector<tinyobj::shape_t> shapes;
|
||||
std::string err = tinyobj::LoadObj(shapes, col->m_geometry.m_meshFileName.c_str());
|
||||
//create a convex hull for each shape, and store it in a btCompoundShape
|
||||
|
||||
childShape = MjcfCreateConvexHullFromShapes(shapes, col->m_geometry.m_meshScale, m_data->m_defaultCollisionMargin);
|
||||
|
||||
}
|
||||
break;
|
||||
}
|
||||
if (strstr(fullPath,".obj"))
|
||||
{
|
||||
fileType = MJCF_FILE_OBJ;
|
||||
}
|
||||
|
||||
sprintf(fullPath,"%s%s",pathPrefix,filename);
|
||||
FILE* f = fopen(fullPath,"rb");
|
||||
if (f)
|
||||
case UrdfGeometry::FILE_STL:
|
||||
{
|
||||
fclose(f);
|
||||
GLInstanceGraphicsShape* glmesh = 0;
|
||||
|
||||
|
||||
switch (fileType)
|
||||
{
|
||||
case MJCF_FILE_OBJ:
|
||||
{
|
||||
if (col->m_flags & URDF_FORCE_CONCAVE_TRIMESH)
|
||||
{
|
||||
glmesh = LoadMeshFromObj(fullPath, collisionPathPrefix);
|
||||
}
|
||||
else
|
||||
{
|
||||
std::vector<tinyobj::shape_t> shapes;
|
||||
std::string err = tinyobj::LoadObj(shapes, fullPath, collisionPathPrefix);
|
||||
//create a convex hull for each shape, and store it in a btCompoundShape
|
||||
glmesh = LoadMeshFromSTL(col->m_geometry.m_meshFileName.c_str());
|
||||
break;
|
||||
}
|
||||
default:
|
||||
b3Warning("%s: Unsupported file type in Collision: %s (maybe .dae?)\n", col->m_sourceFileLocation.c_str(), col->m_geometry.m_meshFileType);
|
||||
}
|
||||
|
||||
childShape = MjcfCreateConvexHullFromShapes(shapes, col->m_geometry.m_meshScale, m_data->m_defaultCollisionMargin);
|
||||
|
||||
}
|
||||
break;
|
||||
}
|
||||
case MJCF_FILE_STL:
|
||||
{
|
||||
glmesh = LoadMeshFromSTL(fullPath);
|
||||
break;
|
||||
}
|
||||
|
||||
default:
|
||||
{
|
||||
b3Warning("Unsupported file type in Collision: %s\n",fullPath);
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
if (childShape)
|
||||
{
|
||||
// okay!
|
||||
}
|
||||
else if (!glmesh || glmesh->m_numvertices<=0)
|
||||
{
|
||||
b3Warning("%s: cannot extract anything useful from mesh '%s'\n", col->m_sourceFileLocation.c_str(), col->m_geometry.m_meshFileName.c_str());
|
||||
}
|
||||
else
|
||||
{
|
||||
//b3Printf("extracted %d verticed from STL file %s\n", glmesh->m_numvertices,fullPath);
|
||||
//int shapeId = m_glApp->m_instancingRenderer->registerShape(&gvertices[0].pos[0],gvertices.size(),&indices[0],indices.size());
|
||||
//convex->setUserIndex(shapeId);
|
||||
btAlignedObjectArray<btVector3> convertedVerts;
|
||||
convertedVerts.reserve(glmesh->m_numvertices);
|
||||
for (int i=0;i<glmesh->m_numvertices;i++)
|
||||
{
|
||||
convertedVerts.push_back(btVector3(
|
||||
glmesh->m_vertices->at(i).xyzw[0]*col->m_geometry.m_meshScale[0],
|
||||
glmesh->m_vertices->at(i).xyzw[1]*col->m_geometry.m_meshScale[1],
|
||||
glmesh->m_vertices->at(i).xyzw[2]*col->m_geometry.m_meshScale[2]));
|
||||
}
|
||||
|
||||
if (!childShape && glmesh && (glmesh->m_numvertices>0))
|
||||
if (col->m_flags & URDF_FORCE_CONCAVE_TRIMESH)
|
||||
{
|
||||
|
||||
btTriangleMesh* meshInterface = new btTriangleMesh();
|
||||
for (int i=0;i<glmesh->m_numIndices/3;i++)
|
||||
{
|
||||
//b3Printf("extracted %d verticed from STL file %s\n", glmesh->m_numvertices,fullPath);
|
||||
//int shapeId = m_glApp->m_instancingRenderer->registerShape(&gvertices[0].pos[0],gvertices.size(),&indices[0],indices.size());
|
||||
//convex->setUserIndex(shapeId);
|
||||
btAlignedObjectArray<btVector3> convertedVerts;
|
||||
convertedVerts.reserve(glmesh->m_numvertices);
|
||||
for (int i=0;i<glmesh->m_numvertices;i++)
|
||||
{
|
||||
convertedVerts.push_back(btVector3(
|
||||
glmesh->m_vertices->at(i).xyzw[0]*col->m_geometry.m_meshScale[0],
|
||||
glmesh->m_vertices->at(i).xyzw[1]*col->m_geometry.m_meshScale[1],
|
||||
glmesh->m_vertices->at(i).xyzw[2]*col->m_geometry.m_meshScale[2]));
|
||||
}
|
||||
|
||||
if (col->m_flags & URDF_FORCE_CONCAVE_TRIMESH)
|
||||
{
|
||||
|
||||
btTriangleMesh* meshInterface = new btTriangleMesh();
|
||||
for (int i=0;i<glmesh->m_numIndices/3;i++)
|
||||
{
|
||||
float* v0 = glmesh->m_vertices->at(glmesh->m_indices->at(i*3)).xyzw;
|
||||
float* v1 = glmesh->m_vertices->at(glmesh->m_indices->at(i*3+1)).xyzw;
|
||||
float* v2 = glmesh->m_vertices->at(glmesh->m_indices->at(i*3+2)).xyzw;
|
||||
meshInterface->addTriangle(btVector3(v0[0],v0[1],v0[2]),
|
||||
btVector3(v1[0],v1[1],v1[2]),
|
||||
btVector3(v2[0],v2[1],v2[2]));
|
||||
}
|
||||
|
||||
btBvhTriangleMeshShape* trimesh = new btBvhTriangleMeshShape(meshInterface,true,true);
|
||||
childShape = trimesh;
|
||||
} else
|
||||
{
|
||||
btConvexHullShape* convexHull = new btConvexHullShape(&convertedVerts[0].getX(), convertedVerts.size(), sizeof(btVector3));
|
||||
convexHull->optimizeConvexHull();
|
||||
//convexHull->initializePolyhedralFeatures();
|
||||
convexHull->setMargin(m_data->m_defaultCollisionMargin);
|
||||
childShape = convexHull;
|
||||
}
|
||||
} else
|
||||
{
|
||||
b3Warning("issue extracting mesh from STL file %s\n", fullPath);
|
||||
float* v0 = glmesh->m_vertices->at(glmesh->m_indices->at(i*3)).xyzw;
|
||||
float* v1 = glmesh->m_vertices->at(glmesh->m_indices->at(i*3+1)).xyzw;
|
||||
float* v2 = glmesh->m_vertices->at(glmesh->m_indices->at(i*3+2)).xyzw;
|
||||
meshInterface->addTriangle(btVector3(v0[0],v0[1],v0[2]),
|
||||
btVector3(v1[0],v1[1],v1[2]),
|
||||
btVector3(v2[0],v2[1],v2[2]));
|
||||
}
|
||||
|
||||
delete glmesh;
|
||||
|
||||
btBvhTriangleMeshShape* trimesh = new btBvhTriangleMeshShape(meshInterface,true,true);
|
||||
childShape = trimesh;
|
||||
} else
|
||||
{
|
||||
b3Warning("mesh geometry not found %s\n",fullPath);
|
||||
btConvexHullShape* convexHull = new btConvexHullShape(&convertedVerts[0].getX(), convertedVerts.size(), sizeof(btVector3));
|
||||
convexHull->optimizeConvexHull();
|
||||
//convexHull->initializePolyhedralFeatures();
|
||||
convexHull->setMargin(m_data->m_defaultCollisionMargin);
|
||||
childShape = convexHull;
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
//////////////////////
|
||||
delete glmesh;
|
||||
break;
|
||||
}
|
||||
|
||||
case URDF_GEOM_CAPSULE:
|
||||
{
|
||||
//todo: convert fromto to btCapsuleShape + local btTransform
|
||||
@@ -1636,7 +1651,7 @@ class btCompoundShape* BulletMJCFImporter::convertLinkCollisionShapes(int linkIn
|
||||
btVector3 fromto[2] = {f,t};
|
||||
btScalar radii[2] = {btScalar(col->m_geometry.m_capsuleRadius)
|
||||
,btScalar(col->m_geometry.m_capsuleRadius)};
|
||||
|
||||
|
||||
btMultiSphereShape* ms = new btMultiSphereShape(fromto,radii,2);
|
||||
childShape = ms;
|
||||
} else
|
||||
@@ -1647,11 +1662,8 @@ class btCompoundShape* BulletMJCFImporter::convertLinkCollisionShapes(int linkIn
|
||||
}
|
||||
break;
|
||||
}
|
||||
default:
|
||||
{
|
||||
} // switch geom
|
||||
|
||||
}
|
||||
}
|
||||
if (childShape)
|
||||
{
|
||||
m_data->m_allocatedCollisionShapes.push_back(childShape);
|
||||
|
||||
@@ -18,9 +18,8 @@ class BulletMJCFImporter : public URDFImporterInterface
|
||||
{
|
||||
struct BulletMJCFImporterInternalData* m_data;
|
||||
|
||||
|
||||
public:
|
||||
BulletMJCFImporter(struct GUIHelperInterface* helper);
|
||||
BulletMJCFImporter(struct GUIHelperInterface* helper, LinkVisualShapesConverter* customConverter);
|
||||
virtual ~BulletMJCFImporter();
|
||||
|
||||
virtual bool parseMJCFString(const char* xmlString, MJCFErrorLogger* logger);
|
||||
@@ -66,7 +65,7 @@ public:
|
||||
|
||||
virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const;
|
||||
|
||||
virtual void convertLinkVisualShapes2(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int objectIndex) const;
|
||||
virtual void convertLinkVisualShapes2(int linkIndex, int urdfIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int objectIndex) const;
|
||||
virtual void setBodyUniqueId(int bodyId);
|
||||
virtual int getBodyUniqueId() const;
|
||||
|
||||
|
||||
@@ -188,12 +188,12 @@ void ImportMJCFSetup::initPhysics()
|
||||
m_filterCallback->m_filterMode = FILTER_GROUPAMASKB_OR_GROUPBMASKA2;
|
||||
|
||||
//m_dynamicsWorld->getSolverInfo().m_numIterations = 100;
|
||||
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
|
||||
m_dynamicsWorld->getDebugDrawer()->setDebugMode(
|
||||
btIDebugDraw::DBG_DrawConstraints
|
||||
+btIDebugDraw::DBG_DrawContactPoints
|
||||
+btIDebugDraw::DBG_DrawAabb
|
||||
);//+btIDebugDraw::DBG_DrawConstraintLimits);
|
||||
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
|
||||
m_dynamicsWorld->getDebugDrawer()->setDebugMode(
|
||||
btIDebugDraw::DBG_DrawConstraints
|
||||
+btIDebugDraw::DBG_DrawContactPoints
|
||||
+btIDebugDraw::DBG_DrawAabb
|
||||
);//+btIDebugDraw::DBG_DrawConstraintLimits);
|
||||
|
||||
|
||||
if (m_guiHelper->getParameterInterface())
|
||||
@@ -203,20 +203,23 @@ void ImportMJCFSetup::initPhysics()
|
||||
slider.m_maxVal = 10;
|
||||
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
|
||||
}
|
||||
|
||||
BulletMJCFImporter importer(m_guiHelper);
|
||||
|
||||
BulletMJCFImporter importer(m_guiHelper, 0);
|
||||
MyMJCFLogger logger;
|
||||
bool result = importer.loadMJCF(m_fileName,&logger);
|
||||
if (result)
|
||||
{
|
||||
btTransform rootTrans;
|
||||
rootTrans.setIdentity();
|
||||
|
||||
for (int m =0; m<importer.getNumModels();m++)
|
||||
{
|
||||
btTransform rootTrans;
|
||||
rootTrans.setIdentity();
|
||||
|
||||
for (int m =0; m<importer.getNumModels();m++)
|
||||
{
|
||||
importer.activateModel(m);
|
||||
|
||||
// normally used with PhysicsServerCommandProcessor that allocates unique ids to multibodies,
|
||||
// emulate this behavior here:
|
||||
importer.setBodyUniqueId(m);
|
||||
|
||||
importer.activateModel(m);
|
||||
|
||||
btMultiBody* mb = 0;
|
||||
|
||||
|
||||
@@ -226,18 +229,17 @@ void ImportMJCFSetup::initPhysics()
|
||||
MyMultiBodyCreator creation(m_guiHelper);
|
||||
|
||||
rootTrans.setIdentity();
|
||||
importer.getRootTransformInWorld(rootTrans);
|
||||
importer.getRootTransformInWorld(rootTrans);
|
||||
|
||||
ConvertURDF2Bullet(importer,creation, rootTrans,m_dynamicsWorld,m_useMultiBody,importer.getPathPrefix(),CUF_USE_MJCF);
|
||||
|
||||
mb = creation.getBulletMultiBody();
|
||||
if (mb)
|
||||
{
|
||||
printf("first MJCF file converted!\n");
|
||||
std::string* name =
|
||||
new std::string(importer.getLinkName(
|
||||
importer.getRootLinkIndex()));
|
||||
m_nameMemory.push_back(name);
|
||||
if (mb)
|
||||
{
|
||||
std::string* name =
|
||||
new std::string(importer.getLinkName(
|
||||
importer.getRootLinkIndex()));
|
||||
m_nameMemory.push_back(name);
|
||||
#ifdef TEST_MULTIBODY_SERIALIZATION
|
||||
s->registerNameForPointer(name->c_str(),name->c_str());
|
||||
#endif//TEST_MULTIBODY_SERIALIZATION
|
||||
@@ -287,10 +289,11 @@ void ImportMJCFSetup::initPhysics()
|
||||
m_data->m_numMotors++;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
} else
|
||||
{
|
||||
// not multibody
|
||||
if (1)
|
||||
{
|
||||
//create motors for each generic joint
|
||||
|
||||
@@ -98,8 +98,11 @@ static GLInstanceGraphicsShape* LoadMeshFromSTL(const char* relativeFileName)
|
||||
}
|
||||
fclose(file);
|
||||
}
|
||||
shape->m_numIndices = shape->m_indices->size();
|
||||
shape->m_numvertices = shape->m_vertices->size();
|
||||
if (shape)
|
||||
{
|
||||
shape->m_numIndices = shape->m_indices->size();
|
||||
shape->m_numvertices = shape->m_vertices->size();
|
||||
}
|
||||
return shape;
|
||||
}
|
||||
|
||||
|
||||
@@ -32,6 +32,7 @@ static btScalar gUrdfDefaultCollisionMargin = 0.001;
|
||||
|
||||
#include <iostream>
|
||||
#include <fstream>
|
||||
#include <list>
|
||||
#include "UrdfParser.h"
|
||||
|
||||
struct MyTexture
|
||||
@@ -47,36 +48,38 @@ ATTRIBUTE_ALIGNED16(struct) BulletURDFInternalData
|
||||
|
||||
UrdfParser m_urdfParser;
|
||||
struct GUIHelperInterface* m_guiHelper;
|
||||
std::string m_sourceFile;
|
||||
char m_pathPrefix[1024];
|
||||
int m_bodyId;
|
||||
btHashMap<btHashInt,btVector4> m_linkColors;
|
||||
btAlignedObjectArray<btCollisionShape*> m_allocatedCollisionShapes;
|
||||
|
||||
LinkVisualShapesConverter* m_customVisualShapesConverter;
|
||||
};
|
||||
|
||||
void setSourceFile(const std::string& relativeFileName, const std::string& prefix)
|
||||
{
|
||||
m_sourceFile = relativeFileName;
|
||||
m_urdfParser.setSourceFile(relativeFileName);
|
||||
strncpy(m_pathPrefix, prefix.c_str(), sizeof(m_pathPrefix));
|
||||
m_pathPrefix[sizeof(m_pathPrefix)-1] = 0; // required, strncpy doesn't write zero on overflow
|
||||
}
|
||||
|
||||
BulletURDFInternalData()
|
||||
{
|
||||
m_pathPrefix[0] = 0;
|
||||
}
|
||||
};
|
||||
|
||||
void BulletURDFImporter::printTree()
|
||||
{
|
||||
// btAssert(0);
|
||||
}
|
||||
|
||||
|
||||
enum MyFileType
|
||||
{
|
||||
FILE_STL=1,
|
||||
FILE_COLLADA=2,
|
||||
FILE_OBJ=3,
|
||||
};
|
||||
|
||||
|
||||
|
||||
BulletURDFImporter::BulletURDFImporter(struct GUIHelperInterface* helper, LinkVisualShapesConverter* customConverter)
|
||||
{
|
||||
m_data = new BulletURDFInternalData;
|
||||
|
||||
m_data->m_guiHelper = helper;
|
||||
m_data->m_pathPrefix[0]=0;
|
||||
m_data->m_customVisualShapesConverter = customConverter;
|
||||
|
||||
|
||||
@@ -111,7 +114,6 @@ struct BulletErrorLogger : public ErrorLogger
|
||||
|
||||
bool BulletURDFImporter::loadURDF(const char* fileName, bool forceFixedBase)
|
||||
{
|
||||
|
||||
if (strlen(fileName)==0)
|
||||
return false;
|
||||
|
||||
@@ -124,17 +126,16 @@ bool BulletURDFImporter::loadURDF(const char* fileName, bool forceFixedBase)
|
||||
bool fileFound = (b3ResourcePath::findResourcePath(fileName,relativeFileName,1024))>0;
|
||||
|
||||
std::string xml_string;
|
||||
m_data->m_pathPrefix[0] = 0;
|
||||
|
||||
if (!fileFound){
|
||||
std::cerr << "URDF file not found" << std::endl;
|
||||
return false;
|
||||
} else
|
||||
{
|
||||
|
||||
int maxPathLen = 1024;
|
||||
fu.extractPath(relativeFileName,m_data->m_pathPrefix,maxPathLen);
|
||||
|
||||
if (!fileFound){
|
||||
b3Warning("URDF file '%s' not found\n", fileName);
|
||||
return false;
|
||||
} else
|
||||
{
|
||||
|
||||
char path[1024];
|
||||
fu.extractPath(relativeFileName, path, sizeof(path));
|
||||
m_data->setSourceFile(relativeFileName, path);
|
||||
|
||||
std::fstream xml_file(relativeFileName, std::fstream::in);
|
||||
while ( xml_file.good())
|
||||
@@ -166,7 +167,7 @@ void BulletURDFImporter::activateModel(int modelIndex)
|
||||
|
||||
bool BulletURDFImporter::loadSDF(const char* fileName, bool forceFixedBase)
|
||||
{
|
||||
|
||||
|
||||
//int argc=0;
|
||||
char relativeFileName[1024];
|
||||
|
||||
@@ -176,17 +177,16 @@ bool BulletURDFImporter::loadSDF(const char* fileName, bool forceFixedBase)
|
||||
bool fileFound = (b3ResourcePath::findResourcePath(fileName,relativeFileName,1024))>0;
|
||||
|
||||
std::string xml_string;
|
||||
m_data->m_pathPrefix[0] = 0;
|
||||
|
||||
if (!fileFound){
|
||||
std::cerr << "SDF file not found" << std::endl;
|
||||
b3Warning("SDF file '%s' not found\n", fileName);
|
||||
return false;
|
||||
} else
|
||||
{
|
||||
|
||||
int maxPathLen = 1024;
|
||||
fu.extractPath(relativeFileName,m_data->m_pathPrefix,maxPathLen);
|
||||
|
||||
char path[1024];
|
||||
fu.extractPath(relativeFileName, path, sizeof(path));
|
||||
m_data->setSourceFile(relativeFileName, path);
|
||||
|
||||
std::fstream xml_file(relativeFileName, std::fstream::in);
|
||||
while ( xml_file.good() )
|
||||
@@ -447,6 +447,101 @@ static btCollisionShape* createConvexHullFromShapes(std::vector<tinyobj::shape_t
|
||||
return compound;
|
||||
}
|
||||
|
||||
bool findExistingMeshFile(
|
||||
const std::string& urdf_path, std::string fn,
|
||||
const std::string& error_message_prefix,
|
||||
std::string* out_found_filename, int* out_type)
|
||||
{
|
||||
if (fn.size() <= 4)
|
||||
{
|
||||
b3Warning("%s: invalid mesh filename '%s'\n", error_message_prefix.c_str(), fn.c_str());
|
||||
return false;
|
||||
}
|
||||
|
||||
std::string ext;
|
||||
std::string ext_ = fn.substr(fn.size()-4);
|
||||
for (std::string::iterator i=ext_.begin(); i!=ext_.end(); ++i)
|
||||
{
|
||||
ext += char(tolower(*i));
|
||||
}
|
||||
|
||||
if (ext==".dae")
|
||||
{
|
||||
*out_type = UrdfGeometry::FILE_COLLADA;
|
||||
}
|
||||
else if (ext==".stl")
|
||||
{
|
||||
*out_type = UrdfGeometry::FILE_STL;
|
||||
}
|
||||
else if (ext==".obj")
|
||||
{
|
||||
*out_type = UrdfGeometry::FILE_OBJ;
|
||||
}
|
||||
else
|
||||
{
|
||||
b3Warning("%s: invalid mesh filename extension '%s'\n", error_message_prefix.c_str(), ext.c_str());
|
||||
return false;
|
||||
}
|
||||
|
||||
std::string drop_it = "package://";
|
||||
if (fn.substr(0, drop_it.length())==drop_it)
|
||||
fn = fn.substr(drop_it.length());
|
||||
|
||||
std::list<std::string> shorter;
|
||||
shorter.push_back("../..");
|
||||
shorter.push_back("..");
|
||||
shorter.push_back(".");
|
||||
int cnt = urdf_path.size();
|
||||
for (int i=0; i<cnt; ++i)
|
||||
{
|
||||
if (urdf_path[i]=='/' || urdf_path[i]=='\\')
|
||||
{
|
||||
shorter.push_back(urdf_path.substr(0, i));
|
||||
}
|
||||
}
|
||||
shorter.reverse();
|
||||
|
||||
std::string existing_file;
|
||||
|
||||
{
|
||||
std::string attempt = fn;
|
||||
FILE* f = fopen(attempt.c_str(), "rb");
|
||||
if (f)
|
||||
{
|
||||
existing_file = attempt;
|
||||
fclose(f);
|
||||
}
|
||||
}
|
||||
if (existing_file.empty())
|
||||
{
|
||||
for (std::list<std::string>::iterator x=shorter.begin(); x!=shorter.end(); ++x)
|
||||
{
|
||||
std::string attempt = *x + "/" + fn;
|
||||
FILE* f = fopen(attempt.c_str(), "rb");
|
||||
if (!f)
|
||||
{
|
||||
//b3Printf("%s: tried '%s'", error_message_prefix.c_str(), attempt.c_str());
|
||||
continue;
|
||||
}
|
||||
fclose(f);
|
||||
existing_file = attempt;
|
||||
//b3Printf("%s: found '%s'", error_message_prefix.c_str(), attempt.c_str());
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (existing_file.empty())
|
||||
{
|
||||
b3Warning("%s: cannot find '%s' in any directory in urdf path\n", error_message_prefix.c_str(), fn.c_str());
|
||||
return false;
|
||||
}
|
||||
else
|
||||
{
|
||||
*out_found_filename = existing_file;
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, const char* urdfPathPrefix)
|
||||
{
|
||||
BT_PROFILE("convertURDFToCollisionShape");
|
||||
@@ -467,8 +562,8 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co
|
||||
|
||||
case URDF_GEOM_CYLINDER:
|
||||
{
|
||||
btScalar cylRadius = collision->m_geometry.m_cylinderRadius;
|
||||
btScalar cylLength = collision->m_geometry.m_cylinderLength;
|
||||
btScalar cylRadius = collision->m_geometry.m_capsuleRadius;
|
||||
btScalar cylLength = collision->m_geometry.m_capsuleHalfHeight;
|
||||
|
||||
btAlignedObjectArray<btVector3> vertices;
|
||||
//int numVerts = sizeof(barrel_vertices)/(9*sizeof(float));
|
||||
@@ -507,239 +602,165 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co
|
||||
}
|
||||
case URDF_GEOM_SPHERE:
|
||||
{
|
||||
|
||||
btScalar radius = collision->m_geometry.m_sphereRadius;
|
||||
btSphereShape* sphereShape = new btSphereShape(radius);
|
||||
shape = sphereShape;
|
||||
shape ->setMargin(gUrdfDefaultCollisionMargin);
|
||||
break;
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
case URDF_GEOM_MESH:
|
||||
{
|
||||
if (collision->m_name.length())
|
||||
case URDF_GEOM_MESH:
|
||||
{
|
||||
GLInstanceGraphicsShape* glmesh = 0;
|
||||
switch (collision->m_geometry.m_meshFileType)
|
||||
{
|
||||
case UrdfGeometry::FILE_OBJ:
|
||||
if (collision->m_flags & URDF_FORCE_CONCAVE_TRIMESH)
|
||||
{
|
||||
//b3Printf("collision->name=%s\n",collision->m_name.c_str());
|
||||
glmesh = LoadMeshFromObj(collision->m_geometry.m_meshFileName.c_str(), 0);
|
||||
}
|
||||
if (1)
|
||||
else
|
||||
{
|
||||
if (collision->m_geometry.m_meshFileName.length())
|
||||
std::vector<tinyobj::shape_t> shapes;
|
||||
std::string err = tinyobj::LoadObj(shapes, collision->m_geometry.m_meshFileName.c_str());
|
||||
//create a convex hull for each shape, and store it in a btCompoundShape
|
||||
|
||||
shape = createConvexHullFromShapes(shapes, collision->m_geometry.m_meshScale);
|
||||
return shape;
|
||||
}
|
||||
break;
|
||||
|
||||
case UrdfGeometry::FILE_STL:
|
||||
glmesh = LoadMeshFromSTL(collision->m_geometry.m_meshFileName.c_str());
|
||||
break;
|
||||
|
||||
case UrdfGeometry::FILE_COLLADA:
|
||||
{
|
||||
btAlignedObjectArray<GLInstanceGraphicsShape> visualShapes;
|
||||
btAlignedObjectArray<ColladaGraphicsInstance> visualShapeInstances;
|
||||
btTransform upAxisTrans;upAxisTrans.setIdentity();
|
||||
float unitMeterScaling = 1;
|
||||
LoadMeshFromCollada(collision->m_geometry.m_meshFileName.c_str(), visualShapes, visualShapeInstances, upAxisTrans, unitMeterScaling, 2);
|
||||
|
||||
glmesh = new GLInstanceGraphicsShape;
|
||||
glmesh->m_indices = new b3AlignedObjectArray<int>();
|
||||
glmesh->m_vertices = new b3AlignedObjectArray<GLInstanceVertex>();
|
||||
|
||||
for (int i=0;i<visualShapeInstances.size();i++)
|
||||
{
|
||||
const char* filename = collision->m_geometry.m_meshFileName.c_str();
|
||||
//b3Printf("mesh->filename=%s\n",filename);
|
||||
char fullPath[1024];
|
||||
int fileType = 0;
|
||||
sprintf(fullPath,"%s%s",urdfPathPrefix,filename);
|
||||
b3FileUtils::toLower(fullPath);
|
||||
char tmpPathPrefix[1024];
|
||||
int maxPathLen = 1024;
|
||||
b3FileUtils::extractPath(filename,tmpPathPrefix,maxPathLen);
|
||||
|
||||
char collisionPathPrefix[1024];
|
||||
sprintf(collisionPathPrefix,"%s%s",urdfPathPrefix,tmpPathPrefix);
|
||||
|
||||
|
||||
|
||||
if (strstr(fullPath,".dae"))
|
||||
ColladaGraphicsInstance* instance = &visualShapeInstances[i];
|
||||
GLInstanceGraphicsShape* gfxShape = &visualShapes[instance->m_shapeIndex];
|
||||
|
||||
b3AlignedObjectArray<GLInstanceVertex> verts;
|
||||
verts.resize(gfxShape->m_vertices->size());
|
||||
|
||||
int baseIndex = glmesh->m_vertices->size();
|
||||
|
||||
for (int i=0;i<gfxShape->m_vertices->size();i++)
|
||||
{
|
||||
fileType = FILE_COLLADA;
|
||||
verts[i].normal[0] = gfxShape->m_vertices->at(i).normal[0];
|
||||
verts[i].normal[1] = gfxShape->m_vertices->at(i).normal[1];
|
||||
verts[i].normal[2] = gfxShape->m_vertices->at(i).normal[2];
|
||||
verts[i].uv[0] = gfxShape->m_vertices->at(i).uv[0];
|
||||
verts[i].uv[1] = gfxShape->m_vertices->at(i).uv[1];
|
||||
verts[i].xyzw[0] = gfxShape->m_vertices->at(i).xyzw[0];
|
||||
verts[i].xyzw[1] = gfxShape->m_vertices->at(i).xyzw[1];
|
||||
verts[i].xyzw[2] = gfxShape->m_vertices->at(i).xyzw[2];
|
||||
verts[i].xyzw[3] = gfxShape->m_vertices->at(i).xyzw[3];
|
||||
|
||||
}
|
||||
if (strstr(fullPath,".stl"))
|
||||
|
||||
int curNumIndices = glmesh->m_indices->size();
|
||||
int additionalIndices = gfxShape->m_indices->size();
|
||||
glmesh->m_indices->resize(curNumIndices+additionalIndices);
|
||||
for (int k=0;k<additionalIndices;k++)
|
||||
{
|
||||
fileType = FILE_STL;
|
||||
glmesh->m_indices->at(curNumIndices+k)=gfxShape->m_indices->at(k)+baseIndex;
|
||||
}
|
||||
if (strstr(fullPath,".obj"))
|
||||
{
|
||||
fileType = FILE_OBJ;
|
||||
}
|
||||
|
||||
sprintf(fullPath,"%s%s",urdfPathPrefix,filename);
|
||||
FILE* f = fopen(fullPath,"rb");
|
||||
if (f)
|
||||
//compensate upAxisTrans and unitMeterScaling here
|
||||
btMatrix4x4 upAxisMat;
|
||||
upAxisMat.setIdentity();
|
||||
//upAxisMat.setPureRotation(upAxisTrans.getRotation());
|
||||
btMatrix4x4 unitMeterScalingMat;
|
||||
unitMeterScalingMat.setPureScaling(btVector3(unitMeterScaling,unitMeterScaling,unitMeterScaling));
|
||||
btMatrix4x4 worldMat = unitMeterScalingMat*instance->m_worldTransform*upAxisMat;
|
||||
//btMatrix4x4 worldMat = instance->m_worldTransform;
|
||||
int curNumVertices = glmesh->m_vertices->size();
|
||||
int additionalVertices = verts.size();
|
||||
glmesh->m_vertices->reserve(curNumVertices+additionalVertices);
|
||||
|
||||
for(int v=0;v<verts.size();v++)
|
||||
{
|
||||
fclose(f);
|
||||
GLInstanceGraphicsShape* glmesh = 0;
|
||||
|
||||
|
||||
switch (fileType)
|
||||
{
|
||||
case FILE_OBJ:
|
||||
{
|
||||
if (collision->m_flags & URDF_FORCE_CONCAVE_TRIMESH)
|
||||
{
|
||||
glmesh = LoadMeshFromObj(fullPath, collisionPathPrefix);
|
||||
}
|
||||
else
|
||||
{
|
||||
std::vector<tinyobj::shape_t> shapes;
|
||||
std::string err = tinyobj::LoadObj(shapes, fullPath, collisionPathPrefix);
|
||||
//create a convex hull for each shape, and store it in a btCompoundShape
|
||||
|
||||
shape = createConvexHullFromShapes(shapes, collision->m_geometry.m_meshScale);
|
||||
return shape;
|
||||
}
|
||||
break;
|
||||
}
|
||||
case FILE_STL:
|
||||
{
|
||||
glmesh = LoadMeshFromSTL(fullPath);
|
||||
break;
|
||||
}
|
||||
case FILE_COLLADA:
|
||||
{
|
||||
|
||||
btAlignedObjectArray<GLInstanceGraphicsShape> visualShapes;
|
||||
btAlignedObjectArray<ColladaGraphicsInstance> visualShapeInstances;
|
||||
btTransform upAxisTrans;upAxisTrans.setIdentity();
|
||||
float unitMeterScaling=1;
|
||||
int upAxis = 2;
|
||||
LoadMeshFromCollada(fullPath,
|
||||
visualShapes,
|
||||
visualShapeInstances,
|
||||
upAxisTrans,
|
||||
unitMeterScaling,
|
||||
upAxis );
|
||||
|
||||
glmesh = new GLInstanceGraphicsShape;
|
||||
// int index = 0;
|
||||
glmesh->m_indices = new b3AlignedObjectArray<int>();
|
||||
glmesh->m_vertices = new b3AlignedObjectArray<GLInstanceVertex>();
|
||||
|
||||
for (int i=0;i<visualShapeInstances.size();i++)
|
||||
{
|
||||
ColladaGraphicsInstance* instance = &visualShapeInstances[i];
|
||||
GLInstanceGraphicsShape* gfxShape = &visualShapes[instance->m_shapeIndex];
|
||||
|
||||
b3AlignedObjectArray<GLInstanceVertex> verts;
|
||||
verts.resize(gfxShape->m_vertices->size());
|
||||
|
||||
int baseIndex = glmesh->m_vertices->size();
|
||||
|
||||
for (int i=0;i<gfxShape->m_vertices->size();i++)
|
||||
{
|
||||
verts[i].normal[0] = gfxShape->m_vertices->at(i).normal[0];
|
||||
verts[i].normal[1] = gfxShape->m_vertices->at(i).normal[1];
|
||||
verts[i].normal[2] = gfxShape->m_vertices->at(i).normal[2];
|
||||
verts[i].uv[0] = gfxShape->m_vertices->at(i).uv[0];
|
||||
verts[i].uv[1] = gfxShape->m_vertices->at(i).uv[1];
|
||||
verts[i].xyzw[0] = gfxShape->m_vertices->at(i).xyzw[0];
|
||||
verts[i].xyzw[1] = gfxShape->m_vertices->at(i).xyzw[1];
|
||||
verts[i].xyzw[2] = gfxShape->m_vertices->at(i).xyzw[2];
|
||||
verts[i].xyzw[3] = gfxShape->m_vertices->at(i).xyzw[3];
|
||||
|
||||
}
|
||||
|
||||
int curNumIndices = glmesh->m_indices->size();
|
||||
int additionalIndices = gfxShape->m_indices->size();
|
||||
glmesh->m_indices->resize(curNumIndices+additionalIndices);
|
||||
for (int k=0;k<additionalIndices;k++)
|
||||
{
|
||||
glmesh->m_indices->at(curNumIndices+k)=gfxShape->m_indices->at(k)+baseIndex;
|
||||
}
|
||||
|
||||
//compensate upAxisTrans and unitMeterScaling here
|
||||
btMatrix4x4 upAxisMat;
|
||||
upAxisMat.setIdentity();
|
||||
//upAxisMat.setPureRotation(upAxisTrans.getRotation());
|
||||
btMatrix4x4 unitMeterScalingMat;
|
||||
unitMeterScalingMat.setPureScaling(btVector3(unitMeterScaling,unitMeterScaling,unitMeterScaling));
|
||||
btMatrix4x4 worldMat = unitMeterScalingMat*instance->m_worldTransform*upAxisMat;
|
||||
//btMatrix4x4 worldMat = instance->m_worldTransform;
|
||||
int curNumVertices = glmesh->m_vertices->size();
|
||||
int additionalVertices = verts.size();
|
||||
glmesh->m_vertices->reserve(curNumVertices+additionalVertices);
|
||||
|
||||
for(int v=0;v<verts.size();v++)
|
||||
{
|
||||
btVector3 pos(verts[v].xyzw[0],verts[v].xyzw[1],verts[v].xyzw[2]);
|
||||
pos = worldMat*pos;
|
||||
verts[v].xyzw[0] = float(pos[0]);
|
||||
verts[v].xyzw[1] = float(pos[1]);
|
||||
verts[v].xyzw[2] = float(pos[2]);
|
||||
glmesh->m_vertices->push_back(verts[v]);
|
||||
}
|
||||
}
|
||||
glmesh->m_numIndices = glmesh->m_indices->size();
|
||||
glmesh->m_numvertices = glmesh->m_vertices->size();
|
||||
//glmesh = LoadMeshFromCollada(fullPath);
|
||||
|
||||
break;
|
||||
}
|
||||
default:
|
||||
{
|
||||
b3Warning("Unsupported file type in Collision: %s\n",fullPath);
|
||||
btAssert(0);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
if (glmesh && (glmesh->m_numvertices>0))
|
||||
{
|
||||
//b3Printf("extracted %d verticed from STL file %s\n", glmesh->m_numvertices,fullPath);
|
||||
//int shapeId = m_glApp->m_instancingRenderer->registerShape(&gvertices[0].pos[0],gvertices.size(),&indices[0],indices.size());
|
||||
//convex->setUserIndex(shapeId);
|
||||
btAlignedObjectArray<btVector3> convertedVerts;
|
||||
convertedVerts.reserve(glmesh->m_numvertices);
|
||||
for (int i=0;i<glmesh->m_numvertices;i++)
|
||||
{
|
||||
convertedVerts.push_back(btVector3(
|
||||
glmesh->m_vertices->at(i).xyzw[0]*collision->m_geometry.m_meshScale[0],
|
||||
glmesh->m_vertices->at(i).xyzw[1]*collision->m_geometry.m_meshScale[1],
|
||||
glmesh->m_vertices->at(i).xyzw[2]*collision->m_geometry.m_meshScale[2]));
|
||||
}
|
||||
|
||||
if (collision->m_flags & URDF_FORCE_CONCAVE_TRIMESH)
|
||||
{
|
||||
BT_PROFILE("convert trimesh");
|
||||
btTriangleMesh* meshInterface = new btTriangleMesh();
|
||||
for (int i=0;i<glmesh->m_numIndices/3;i++)
|
||||
{
|
||||
float* v0 = glmesh->m_vertices->at(glmesh->m_indices->at(i*3)).xyzw;
|
||||
float* v1 = glmesh->m_vertices->at(glmesh->m_indices->at(i*3+1)).xyzw;
|
||||
float* v2 = glmesh->m_vertices->at(glmesh->m_indices->at(i*3+2)).xyzw;
|
||||
meshInterface->addTriangle(btVector3(v0[0],v0[1],v0[2]),
|
||||
btVector3(v1[0],v1[1],v1[2]),
|
||||
btVector3(v2[0],v2[1],v2[2]));
|
||||
}
|
||||
|
||||
btBvhTriangleMeshShape* trimesh = new btBvhTriangleMeshShape(meshInterface,true,true);
|
||||
trimesh->setLocalScaling(collision->m_geometry.m_meshScale);
|
||||
shape = trimesh;
|
||||
} else
|
||||
{
|
||||
BT_PROFILE("convert btConvexHullShape");
|
||||
|
||||
btConvexHullShape* convexHull = new btConvexHullShape(&convertedVerts[0].getX(), convertedVerts.size(), sizeof(btVector3));
|
||||
convexHull->optimizeConvexHull();
|
||||
//convexHull->initializePolyhedralFeatures();
|
||||
convexHull->setMargin(gUrdfDefaultCollisionMargin);
|
||||
convexHull->setLocalScaling(collision->m_geometry.m_meshScale);
|
||||
shape = convexHull;
|
||||
}
|
||||
} else
|
||||
{
|
||||
b3Warning("issue extracting mesh from STL file %s\n", fullPath);
|
||||
}
|
||||
|
||||
delete glmesh;
|
||||
|
||||
} else
|
||||
{
|
||||
b3Warning("mesh geometry not found %s\n",fullPath);
|
||||
btVector3 pos(verts[v].xyzw[0],verts[v].xyzw[1],verts[v].xyzw[2]);
|
||||
pos = worldMat*pos;
|
||||
verts[v].xyzw[0] = float(pos[0]);
|
||||
verts[v].xyzw[1] = float(pos[1]);
|
||||
verts[v].xyzw[2] = float(pos[2]);
|
||||
glmesh->m_vertices->push_back(verts[v]);
|
||||
}
|
||||
|
||||
}
|
||||
glmesh->m_numIndices = glmesh->m_indices->size();
|
||||
glmesh->m_numvertices = glmesh->m_vertices->size();
|
||||
//glmesh = LoadMeshFromCollada(success.c_str());
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (!glmesh || glmesh->m_numvertices<=0)
|
||||
{
|
||||
b3Warning("%s: cannot extract mesh from '%s'\n", urdfPathPrefix, collision->m_geometry.m_meshFileName.c_str());
|
||||
delete glmesh;
|
||||
break;
|
||||
}
|
||||
|
||||
btAlignedObjectArray<btVector3> convertedVerts;
|
||||
convertedVerts.reserve(glmesh->m_numvertices);
|
||||
for (int i=0; i<glmesh->m_numvertices; i++)
|
||||
{
|
||||
convertedVerts.push_back(btVector3(
|
||||
glmesh->m_vertices->at(i).xyzw[0]*collision->m_geometry.m_meshScale[0],
|
||||
glmesh->m_vertices->at(i).xyzw[1]*collision->m_geometry.m_meshScale[1],
|
||||
glmesh->m_vertices->at(i).xyzw[2]*collision->m_geometry.m_meshScale[2]));
|
||||
}
|
||||
|
||||
if (collision->m_flags & URDF_FORCE_CONCAVE_TRIMESH)
|
||||
{
|
||||
BT_PROFILE("convert trimesh");
|
||||
btTriangleMesh* meshInterface = new btTriangleMesh();
|
||||
for (int i=0; i<glmesh->m_numIndices/3; i++)
|
||||
{
|
||||
float* v0 = glmesh->m_vertices->at(glmesh->m_indices->at(i*3)).xyzw;
|
||||
float* v1 = glmesh->m_vertices->at(glmesh->m_indices->at(i*3+1)).xyzw;
|
||||
float* v2 = glmesh->m_vertices->at(glmesh->m_indices->at(i*3+2)).xyzw;
|
||||
meshInterface->addTriangle(
|
||||
btVector3(v0[0],v0[1],v0[2]),
|
||||
btVector3(v1[0],v1[1],v1[2]),
|
||||
btVector3(v2[0],v2[1],v2[2]));
|
||||
}
|
||||
btBvhTriangleMeshShape* trimesh = new btBvhTriangleMeshShape(meshInterface,true,true);
|
||||
trimesh->setLocalScaling(collision->m_geometry.m_meshScale);
|
||||
shape = trimesh;
|
||||
|
||||
} else
|
||||
{
|
||||
BT_PROFILE("convert btConvexHullShape");
|
||||
btConvexHullShape* convexHull = new btConvexHullShape(&convertedVerts[0].getX(), convertedVerts.size(), sizeof(btVector3));
|
||||
convexHull->optimizeConvexHull();
|
||||
//convexHull->initializePolyhedralFeatures();
|
||||
convexHull->setMargin(gUrdfDefaultCollisionMargin);
|
||||
convexHull->setLocalScaling(collision->m_geometry.m_meshScale);
|
||||
shape = convexHull;
|
||||
}
|
||||
|
||||
delete glmesh;
|
||||
break;
|
||||
} // mesh case
|
||||
|
||||
|
||||
break;
|
||||
}
|
||||
default:
|
||||
{
|
||||
b3Warning("Error: unknown visual geometry type\n");
|
||||
}
|
||||
}
|
||||
b3Warning("Error: unknown collision geometry type %i\n", collision->m_geometry.m_type);
|
||||
// for example, URDF_GEOM_PLANE
|
||||
}
|
||||
return shape;
|
||||
}
|
||||
|
||||
@@ -764,8 +785,8 @@ static void convertURDFToVisualShapeInternal(const UrdfVisual* visual, const cha
|
||||
for (int i = 0; i<numSteps; i++)
|
||||
{
|
||||
|
||||
btScalar cylRadius = visual->m_geometry.m_cylinderRadius;
|
||||
btScalar cylLength = visual->m_geometry.m_cylinderLength;
|
||||
btScalar cylRadius = visual->m_geometry.m_capsuleRadius;
|
||||
btScalar cylLength = visual->m_geometry.m_capsuleHalfHeight;
|
||||
|
||||
btVector3 vert(cylRadius*btSin(SIMD_2_PI*(float(i) / numSteps)), cylRadius*btCos(SIMD_2_PI*(float(i) / numSteps)), cylLength / 2.);
|
||||
vertices.push_back(vert);
|
||||
@@ -778,17 +799,17 @@ static void convertURDFToVisualShapeInternal(const UrdfVisual* visual, const cha
|
||||
convexColShape = cylZShape;
|
||||
break;
|
||||
}
|
||||
|
||||
case URDF_GEOM_BOX:
|
||||
{
|
||||
|
||||
btVector3 extents = visual->m_geometry.m_boxSize;
|
||||
|
||||
btBoxShape* boxShape = new btBoxShape(extents*0.5f);
|
||||
//btConvexShape* boxShape = new btConeShapeX(extents[2]*0.5,extents[0]*0.5);
|
||||
convexColShape = boxShape;
|
||||
convexColShape->setMargin(gUrdfDefaultCollisionMargin);
|
||||
break;
|
||||
}
|
||||
|
||||
case URDF_GEOM_SPHERE:
|
||||
{
|
||||
btScalar radius = visual->m_geometry.m_sphereRadius;
|
||||
@@ -796,206 +817,137 @@ static void convertURDFToVisualShapeInternal(const UrdfVisual* visual, const cha
|
||||
convexColShape = sphereShape;
|
||||
convexColShape->setMargin(gUrdfDefaultCollisionMargin);
|
||||
break;
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
case URDF_GEOM_MESH:
|
||||
{
|
||||
if (visual->m_name.length())
|
||||
switch (visual->m_geometry.m_meshFileType)
|
||||
{
|
||||
//b3Printf("visual->name=%s\n", visual->m_name.c_str());
|
||||
}
|
||||
if (1)//visual->m_geometry)
|
||||
{
|
||||
if (visual->m_geometry.m_meshFileName.length())
|
||||
case UrdfGeometry::FILE_OBJ:
|
||||
{
|
||||
const char* filename = visual->m_geometry.m_meshFileName.c_str();
|
||||
//b3Printf("mesh->filename=%s\n", filename);
|
||||
char fullPath[1024];
|
||||
int fileType = 0;
|
||||
|
||||
char tmpPathPrefix[1024];
|
||||
std::string xml_string;
|
||||
int maxPathLen = 1024;
|
||||
b3FileUtils::extractPath(filename,tmpPathPrefix,maxPathLen);
|
||||
|
||||
char visualPathPrefix[1024];
|
||||
sprintf(visualPathPrefix,"%s%s",urdfPathPrefix,tmpPathPrefix);
|
||||
|
||||
|
||||
sprintf(fullPath, "%s%s", urdfPathPrefix, filename);
|
||||
b3FileUtils::toLower(fullPath);
|
||||
if (strstr(fullPath, ".dae"))
|
||||
b3ImportMeshData meshData;
|
||||
if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(visual->m_geometry.m_meshFileName, meshData))
|
||||
{
|
||||
fileType = FILE_COLLADA;
|
||||
|
||||
if (meshData.m_textureImage)
|
||||
{
|
||||
MyTexture texData;
|
||||
texData.m_width = meshData.m_textureWidth;
|
||||
texData.m_height = meshData.m_textureHeight;
|
||||
texData.textureData = meshData.m_textureImage;
|
||||
texturesOut.push_back(texData);
|
||||
}
|
||||
glmesh = meshData.m_gfxShape;
|
||||
}
|
||||
if (strstr(fullPath, ".stl"))
|
||||
{
|
||||
fileType = FILE_STL;
|
||||
}
|
||||
if (strstr(fullPath,".obj"))
|
||||
{
|
||||
fileType = FILE_OBJ;
|
||||
}
|
||||
|
||||
|
||||
sprintf(fullPath, "%s%s", urdfPathPrefix, filename);
|
||||
FILE* f = fopen(fullPath, "rb");
|
||||
if (f)
|
||||
{
|
||||
fclose(f);
|
||||
|
||||
switch (fileType)
|
||||
{
|
||||
case FILE_OBJ:
|
||||
{
|
||||
// glmesh = LoadMeshFromObj(fullPath,visualPathPrefix);
|
||||
|
||||
b3ImportMeshData meshData;
|
||||
if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(fullPath, meshData))
|
||||
{
|
||||
|
||||
if (meshData.m_textureImage)
|
||||
{
|
||||
MyTexture texData;
|
||||
texData.m_width = meshData.m_textureWidth;
|
||||
texData.m_height = meshData.m_textureHeight;
|
||||
texData.textureData = meshData.m_textureImage;
|
||||
texturesOut.push_back(texData);
|
||||
}
|
||||
glmesh = meshData.m_gfxShape;
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
case FILE_STL:
|
||||
{
|
||||
glmesh = LoadMeshFromSTL(fullPath);
|
||||
break;
|
||||
}
|
||||
case FILE_COLLADA:
|
||||
{
|
||||
|
||||
btAlignedObjectArray<GLInstanceGraphicsShape> visualShapes;
|
||||
btAlignedObjectArray<ColladaGraphicsInstance> visualShapeInstances;
|
||||
btTransform upAxisTrans; upAxisTrans.setIdentity();
|
||||
float unitMeterScaling = 1;
|
||||
int upAxis = 2;
|
||||
|
||||
LoadMeshFromCollada(fullPath,
|
||||
visualShapes,
|
||||
visualShapeInstances,
|
||||
upAxisTrans,
|
||||
unitMeterScaling,
|
||||
upAxis);
|
||||
|
||||
glmesh = new GLInstanceGraphicsShape;
|
||||
// int index = 0;
|
||||
glmesh->m_indices = new b3AlignedObjectArray<int>();
|
||||
glmesh->m_vertices = new b3AlignedObjectArray<GLInstanceVertex>();
|
||||
|
||||
for (int i = 0; i<visualShapeInstances.size(); i++)
|
||||
{
|
||||
ColladaGraphicsInstance* instance = &visualShapeInstances[i];
|
||||
GLInstanceGraphicsShape* gfxShape = &visualShapes[instance->m_shapeIndex];
|
||||
|
||||
b3AlignedObjectArray<GLInstanceVertex> verts;
|
||||
verts.resize(gfxShape->m_vertices->size());
|
||||
|
||||
int baseIndex = glmesh->m_vertices->size();
|
||||
|
||||
for (int i = 0; i<gfxShape->m_vertices->size(); i++)
|
||||
{
|
||||
verts[i].normal[0] = gfxShape->m_vertices->at(i).normal[0];
|
||||
verts[i].normal[1] = gfxShape->m_vertices->at(i).normal[1];
|
||||
verts[i].normal[2] = gfxShape->m_vertices->at(i).normal[2];
|
||||
verts[i].uv[0] = gfxShape->m_vertices->at(i).uv[0];
|
||||
verts[i].uv[1] = gfxShape->m_vertices->at(i).uv[1];
|
||||
verts[i].xyzw[0] = gfxShape->m_vertices->at(i).xyzw[0];
|
||||
verts[i].xyzw[1] = gfxShape->m_vertices->at(i).xyzw[1];
|
||||
verts[i].xyzw[2] = gfxShape->m_vertices->at(i).xyzw[2];
|
||||
verts[i].xyzw[3] = gfxShape->m_vertices->at(i).xyzw[3];
|
||||
|
||||
}
|
||||
|
||||
int curNumIndices = glmesh->m_indices->size();
|
||||
int additionalIndices = gfxShape->m_indices->size();
|
||||
glmesh->m_indices->resize(curNumIndices + additionalIndices);
|
||||
for (int k = 0; k<additionalIndices; k++)
|
||||
{
|
||||
glmesh->m_indices->at(curNumIndices + k) = gfxShape->m_indices->at(k) + baseIndex;
|
||||
}
|
||||
|
||||
//compensate upAxisTrans and unitMeterScaling here
|
||||
btMatrix4x4 upAxisMat;
|
||||
upAxisMat.setIdentity();
|
||||
// upAxisMat.setPureRotation(upAxisTrans.getRotation());
|
||||
btMatrix4x4 unitMeterScalingMat;
|
||||
unitMeterScalingMat.setPureScaling(btVector3(unitMeterScaling, unitMeterScaling, unitMeterScaling));
|
||||
btMatrix4x4 worldMat = unitMeterScalingMat*upAxisMat*instance->m_worldTransform;
|
||||
//btMatrix4x4 worldMat = instance->m_worldTransform;
|
||||
int curNumVertices = glmesh->m_vertices->size();
|
||||
int additionalVertices = verts.size();
|
||||
glmesh->m_vertices->reserve(curNumVertices + additionalVertices);
|
||||
|
||||
for (int v = 0; v<verts.size(); v++)
|
||||
{
|
||||
btVector3 pos(verts[v].xyzw[0], verts[v].xyzw[1], verts[v].xyzw[2]);
|
||||
pos = worldMat*pos;
|
||||
verts[v].xyzw[0] = float(pos[0]);
|
||||
verts[v].xyzw[1] = float(pos[1]);
|
||||
verts[v].xyzw[2] = float(pos[2]);
|
||||
glmesh->m_vertices->push_back(verts[v]);
|
||||
}
|
||||
}
|
||||
glmesh->m_numIndices = glmesh->m_indices->size();
|
||||
glmesh->m_numvertices = glmesh->m_vertices->size();
|
||||
//glmesh = LoadMeshFromCollada(fullPath);
|
||||
|
||||
break;
|
||||
}
|
||||
default:
|
||||
{
|
||||
b3Warning("Error: unsupported file type for Visual mesh: %s\n", fullPath);
|
||||
btAssert(0);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
if (glmesh && glmesh->m_vertices && (glmesh->m_numvertices>0))
|
||||
{
|
||||
//apply the geometry scaling
|
||||
for (int i=0;i<glmesh->m_vertices->size();i++)
|
||||
{
|
||||
glmesh->m_vertices->at(i).xyzw[0] *= visual->m_geometry.m_meshScale[0];
|
||||
glmesh->m_vertices->at(i).xyzw[1] *= visual->m_geometry.m_meshScale[1];
|
||||
glmesh->m_vertices->at(i).xyzw[2] *= visual->m_geometry.m_meshScale[2];
|
||||
}
|
||||
|
||||
}
|
||||
else
|
||||
{
|
||||
b3Warning("issue extracting mesh from COLLADA/STL file %s\n", fullPath);
|
||||
}
|
||||
|
||||
}
|
||||
else
|
||||
{
|
||||
b3Warning("mesh geometry not found %s\n", fullPath);
|
||||
}
|
||||
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
case UrdfGeometry::FILE_STL:
|
||||
{
|
||||
glmesh = LoadMeshFromSTL(visual->m_geometry.m_meshFileName.c_str());
|
||||
break;
|
||||
}
|
||||
|
||||
case UrdfGeometry::FILE_COLLADA:
|
||||
{
|
||||
btAlignedObjectArray<GLInstanceGraphicsShape> visualShapes;
|
||||
btAlignedObjectArray<ColladaGraphicsInstance> visualShapeInstances;
|
||||
btTransform upAxisTrans; upAxisTrans.setIdentity();
|
||||
float unitMeterScaling = 1;
|
||||
int upAxis = 2;
|
||||
|
||||
LoadMeshFromCollada(visual->m_geometry.m_meshFileName.c_str(),
|
||||
visualShapes,
|
||||
visualShapeInstances,
|
||||
upAxisTrans,
|
||||
unitMeterScaling,
|
||||
upAxis);
|
||||
|
||||
glmesh = new GLInstanceGraphicsShape;
|
||||
// int index = 0;
|
||||
glmesh->m_indices = new b3AlignedObjectArray<int>();
|
||||
glmesh->m_vertices = new b3AlignedObjectArray<GLInstanceVertex>();
|
||||
|
||||
for (int i = 0; i<visualShapeInstances.size(); i++)
|
||||
{
|
||||
ColladaGraphicsInstance* instance = &visualShapeInstances[i];
|
||||
GLInstanceGraphicsShape* gfxShape = &visualShapes[instance->m_shapeIndex];
|
||||
|
||||
b3AlignedObjectArray<GLInstanceVertex> verts;
|
||||
verts.resize(gfxShape->m_vertices->size());
|
||||
|
||||
int baseIndex = glmesh->m_vertices->size();
|
||||
|
||||
for (int i = 0; i<gfxShape->m_vertices->size(); i++)
|
||||
{
|
||||
verts[i].normal[0] = gfxShape->m_vertices->at(i).normal[0];
|
||||
verts[i].normal[1] = gfxShape->m_vertices->at(i).normal[1];
|
||||
verts[i].normal[2] = gfxShape->m_vertices->at(i).normal[2];
|
||||
verts[i].uv[0] = gfxShape->m_vertices->at(i).uv[0];
|
||||
verts[i].uv[1] = gfxShape->m_vertices->at(i).uv[1];
|
||||
verts[i].xyzw[0] = gfxShape->m_vertices->at(i).xyzw[0];
|
||||
verts[i].xyzw[1] = gfxShape->m_vertices->at(i).xyzw[1];
|
||||
verts[i].xyzw[2] = gfxShape->m_vertices->at(i).xyzw[2];
|
||||
verts[i].xyzw[3] = gfxShape->m_vertices->at(i).xyzw[3];
|
||||
|
||||
}
|
||||
|
||||
int curNumIndices = glmesh->m_indices->size();
|
||||
int additionalIndices = gfxShape->m_indices->size();
|
||||
glmesh->m_indices->resize(curNumIndices + additionalIndices);
|
||||
for (int k = 0; k<additionalIndices; k++)
|
||||
{
|
||||
glmesh->m_indices->at(curNumIndices + k) = gfxShape->m_indices->at(k) + baseIndex;
|
||||
}
|
||||
|
||||
//compensate upAxisTrans and unitMeterScaling here
|
||||
btMatrix4x4 upAxisMat;
|
||||
upAxisMat.setIdentity();
|
||||
// upAxisMat.setPureRotation(upAxisTrans.getRotation());
|
||||
btMatrix4x4 unitMeterScalingMat;
|
||||
unitMeterScalingMat.setPureScaling(btVector3(unitMeterScaling, unitMeterScaling, unitMeterScaling));
|
||||
btMatrix4x4 worldMat = unitMeterScalingMat*upAxisMat*instance->m_worldTransform;
|
||||
//btMatrix4x4 worldMat = instance->m_worldTransform;
|
||||
int curNumVertices = glmesh->m_vertices->size();
|
||||
int additionalVertices = verts.size();
|
||||
glmesh->m_vertices->reserve(curNumVertices + additionalVertices);
|
||||
|
||||
for (int v = 0; v<verts.size(); v++)
|
||||
{
|
||||
btVector3 pos(verts[v].xyzw[0], verts[v].xyzw[1], verts[v].xyzw[2]);
|
||||
pos = worldMat*pos;
|
||||
verts[v].xyzw[0] = float(pos[0]);
|
||||
verts[v].xyzw[1] = float(pos[1]);
|
||||
verts[v].xyzw[2] = float(pos[2]);
|
||||
glmesh->m_vertices->push_back(verts[v]);
|
||||
}
|
||||
}
|
||||
glmesh->m_numIndices = glmesh->m_indices->size();
|
||||
glmesh->m_numvertices = glmesh->m_vertices->size();
|
||||
//glmesh = LoadMeshFromCollada(visual->m_geometry.m_meshFileName);
|
||||
|
||||
break;
|
||||
}
|
||||
} // switch file type
|
||||
|
||||
if (!glmesh || !glmesh->m_vertices || glmesh->m_numvertices<=0)
|
||||
{
|
||||
b3Warning("%s: cannot extract anything useful from mesh '%s'\n", urdfPathPrefix, visual->m_geometry.m_meshFileName.c_str());
|
||||
break;
|
||||
}
|
||||
|
||||
|
||||
//apply the geometry scaling
|
||||
for (int i=0;i<glmesh->m_vertices->size();i++)
|
||||
{
|
||||
glmesh->m_vertices->at(i).xyzw[0] *= visual->m_geometry.m_meshScale[0];
|
||||
glmesh->m_vertices->at(i).xyzw[1] *= visual->m_geometry.m_meshScale[1];
|
||||
glmesh->m_vertices->at(i).xyzw[2] *= visual->m_geometry.m_meshScale[2];
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
default:
|
||||
{
|
||||
b3Warning("Error: unknown visual geometry type\n");
|
||||
}
|
||||
b3Warning("Error: unknown visual geometry type %i\n", visual->m_geometry.m_type);
|
||||
}
|
||||
|
||||
//if we have a convex, tesselate into localVertices/localIndices
|
||||
@@ -1166,15 +1118,17 @@ bool BulletURDFImporter::getLinkContactInfo(int linkIndex, URDFLinkContactInfo&
|
||||
return false;
|
||||
}
|
||||
|
||||
void BulletURDFImporter::convertLinkVisualShapes2(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, class btCollisionObject* colObj, int bodyUniqueId) const
|
||||
void BulletURDFImporter::convertLinkVisualShapes2(int linkIndex, int urdfIndex, const char* pathPrefix, const btTransform& localInertiaFrame, class btCollisionObject* colObj, int bodyUniqueId) const
|
||||
{
|
||||
|
||||
if (m_data->m_customVisualShapesConverter)
|
||||
{
|
||||
const UrdfModel& model = m_data->m_urdfParser.getModel();
|
||||
m_data->m_customVisualShapesConverter->convertVisualShapes(linkIndex,pathPrefix,localInertiaFrame, model, colObj, bodyUniqueId);
|
||||
UrdfLink*const* linkPtr = model.m_links.getAtIndex(urdfIndex);
|
||||
if (linkPtr)
|
||||
{
|
||||
m_data->m_customVisualShapesConverter->convertVisualShapes(linkIndex,pathPrefix,localInertiaFrame, *linkPtr, &model, colObj, bodyUniqueId);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
int BulletURDFImporter::getNumAllocatedCollisionShapes() const
|
||||
|
||||
@@ -51,7 +51,7 @@ public:
|
||||
|
||||
virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const;
|
||||
|
||||
virtual void convertLinkVisualShapes2(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int bodyUniqueId) const;
|
||||
virtual void convertLinkVisualShapes2(int linkIndex, int urdfIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int bodyUniqueId) const;
|
||||
|
||||
///todo(erwincoumans) refactor this convertLinkCollisionShapes/memory allocation
|
||||
|
||||
|
||||
@@ -1,9 +1,14 @@
|
||||
#ifndef LINK_VISUAL_SHAPES_CONVERTER_H
|
||||
#define LINK_VISUAL_SHAPES_CONVERTER_H
|
||||
|
||||
struct UrdfLink;
|
||||
struct UrdfModel;
|
||||
class btTransform;
|
||||
class btCollisionObject;
|
||||
|
||||
struct LinkVisualShapesConverter
|
||||
{
|
||||
virtual void convertVisualShapes(int linkIndex, const char* pathPrefix, const class btTransform& localInertiaFrame, const struct UrdfModel& model, class btCollisionObject* colObj, int objectIndex)=0;
|
||||
virtual void convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfLink* linkPtr, const UrdfModel* model, class btCollisionObject* colShape, int objectIndex) =0;
|
||||
};
|
||||
|
||||
#endif //LINK_VISUAL_SHAPES_CONVERTER_H
|
||||
|
||||
@@ -166,6 +166,10 @@ void processContactParameters(const URDFLinkContactInfo& contactInfo, btCollisio
|
||||
{
|
||||
col->setContactStiffnessAndDamping(contactInfo.m_contactStiffness, contactInfo.m_contactDamping);
|
||||
}
|
||||
if ((contactInfo.m_flags & URDF_CONTACT_HAS_FRICTION_ANCHOR) != 0)
|
||||
{
|
||||
col->setCollisionFlags(col->getCollisionFlags() | btCollisionObject::CF_HAS_FRICTION_ANCHOR);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -311,7 +315,7 @@ void ConvertURDF2BulletInternal(
|
||||
|
||||
|
||||
|
||||
//untested: u2b.convertLinkVisualShapes2(urdfLinkIndex,pathPrefix,localInertialFrame,body);
|
||||
//untested: u2b.convertLinkVisualShapes2(linkIndex,urdfLinkIndex,pathPrefix,localInertialFrame,body);
|
||||
} else
|
||||
{
|
||||
if (cache.m_bulletMultiBody==0)
|
||||
@@ -469,8 +473,8 @@ void ConvertURDF2BulletInternal(
|
||||
btVector4 color = selectColor2();//(0.0,0.0,0.5);
|
||||
u2b.getLinkColor(urdfLinkIndex,color);
|
||||
creation.createCollisionObjectGraphicsInstance(urdfLinkIndex,col,color);
|
||||
|
||||
u2b.convertLinkVisualShapes2(urdfLinkIndex,pathPrefix,localInertialFrame,col, u2b.getBodyUniqueId());
|
||||
|
||||
u2b.convertLinkVisualShapes2(mbLinkIndex, urdfLinkIndex, pathPrefix, localInertialFrame,col, u2b.getBodyUniqueId());
|
||||
|
||||
URDFLinkContactInfo contactInfo;
|
||||
u2b.getLinkContactInfo(urdfLinkIndex,contactInfo);
|
||||
@@ -487,7 +491,7 @@ void ConvertURDF2BulletInternal(
|
||||
}
|
||||
} else
|
||||
{
|
||||
//u2b.convertLinkVisualShapes2(urdfLinkIndex,pathPrefix,localInertialFrame,compoundShape);
|
||||
//u2b.convertLinkVisualShapes2(urdfLinkIndex,urdfIndex,pathPrefix,localInertialFrame,compoundShape);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -49,7 +49,7 @@ public:
|
||||
///quick hack: need to rethink the API/dependencies of this
|
||||
virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const { return -1;}
|
||||
|
||||
virtual void convertLinkVisualShapes2(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int objectIndex) const { }
|
||||
virtual void convertLinkVisualShapes2(int linkIndex, int urdfIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int objectIndex) const { }
|
||||
virtual void setBodyUniqueId(int bodyId) {}
|
||||
virtual int getBodyUniqueId() const { return 0;}
|
||||
|
||||
|
||||
@@ -23,7 +23,8 @@ enum URDF_LinkContactFlags
|
||||
URDF_CONTACT_HAS_ROLLING_FRICTION=32,
|
||||
URDF_CONTACT_HAS_SPINNING_FRICTION=64,
|
||||
URDF_CONTACT_HAS_RESTITUTION=128,
|
||||
|
||||
URDF_CONTACT_HAS_FRICTION_ANCHOR=256,
|
||||
|
||||
};
|
||||
|
||||
struct URDFLinkContactInfo
|
||||
|
||||
@@ -8,9 +8,9 @@ UrdfParser::UrdfParser()
|
||||
:m_parseSDF(false),
|
||||
m_activeSdfModel(-1)
|
||||
{
|
||||
m_urdf2Model.m_sourceFile = "IN_MEMORY_STRING"; // if loadUrdf() called later, source file name will be replaced with real
|
||||
}
|
||||
|
||||
|
||||
UrdfParser::~UrdfParser()
|
||||
{
|
||||
cleanModel(&m_urdf2Model);
|
||||
@@ -401,8 +401,9 @@ bool UrdfParser::parseGeometry(UrdfGeometry& geom, TiXmlElement* g, ErrorLogger*
|
||||
logger->reportError("Cylinder shape must have both length and radius attributes");
|
||||
return false;
|
||||
}
|
||||
geom.m_cylinderRadius = urdfLexicalCast<double>(shape->Attribute("radius"));
|
||||
geom.m_cylinderLength = urdfLexicalCast<double>(shape->Attribute("length"));
|
||||
geom.m_hasFromTo = false;
|
||||
geom.m_capsuleRadius = urdfLexicalCast<double>(shape->Attribute("radius"));
|
||||
geom.m_capsuleHalfHeight = urdfLexicalCast<double>(shape->Attribute("length"));
|
||||
|
||||
}
|
||||
else if (type_name == "capsule")
|
||||
@@ -410,59 +411,68 @@ bool UrdfParser::parseGeometry(UrdfGeometry& geom, TiXmlElement* g, ErrorLogger*
|
||||
geom.m_type = URDF_GEOM_CAPSULE;
|
||||
if (!shape->Attribute("length") ||
|
||||
!shape->Attribute("radius"))
|
||||
{
|
||||
logger->reportError("Capsule shape must have both length and radius attributes");
|
||||
return false;
|
||||
}
|
||||
{
|
||||
logger->reportError("Capsule shape must have both length and radius attributes");
|
||||
return false;
|
||||
}
|
||||
geom.m_hasFromTo = false;
|
||||
geom.m_capsuleRadius = urdfLexicalCast<double>(shape->Attribute("radius"));
|
||||
geom.m_capsuleHalfHeight = btScalar(0.5)*urdfLexicalCast<double>(shape->Attribute("length"));
|
||||
|
||||
}
|
||||
else if (type_name == "mesh")
|
||||
{
|
||||
geom.m_type = URDF_GEOM_MESH;
|
||||
if (m_parseSDF)
|
||||
{
|
||||
TiXmlElement* scale = shape->FirstChildElement("scale");
|
||||
if (0==scale)
|
||||
{
|
||||
geom.m_meshScale.setValue(1,1,1);
|
||||
}
|
||||
else
|
||||
{
|
||||
parseVector3(geom.m_meshScale,scale->GetText(),logger);
|
||||
}
|
||||
|
||||
TiXmlElement* filename = shape->FirstChildElement("uri");
|
||||
geom.m_meshFileName = filename->GetText();
|
||||
}
|
||||
else
|
||||
{
|
||||
if (!shape->Attribute("filename")) {
|
||||
logger->reportError("Mesh must contain a filename attribute");
|
||||
return false;
|
||||
}
|
||||
|
||||
geom.m_meshFileName = shape->Attribute("filename");
|
||||
geom.m_meshScale.setValue(1,1,1);
|
||||
else if (type_name == "mesh")
|
||||
{
|
||||
geom.m_type = URDF_GEOM_MESH;
|
||||
geom.m_meshScale.setValue(1,1,1);
|
||||
std::string fn;
|
||||
|
||||
if (shape->Attribute("scale"))
|
||||
{
|
||||
if (!parseVector3(geom.m_meshScale,shape->Attribute("scale"),logger))
|
||||
{
|
||||
logger->reportWarning("scale should be a vector3, not single scalar. Workaround activated.\n");
|
||||
std::string scalar_str = shape->Attribute("scale");
|
||||
double scaleFactor = urdfLexicalCast<double>(scalar_str.c_str());
|
||||
if (scaleFactor)
|
||||
{
|
||||
geom.m_meshScale.setValue(scaleFactor,scaleFactor,scaleFactor);
|
||||
}
|
||||
}
|
||||
} else
|
||||
{
|
||||
}
|
||||
}
|
||||
if (m_parseSDF)
|
||||
{
|
||||
if (TiXmlElement* scale = shape->FirstChildElement("scale"))
|
||||
{
|
||||
parseVector3(geom.m_meshScale,scale->GetText(),logger);
|
||||
}
|
||||
if (TiXmlElement* filename = shape->FirstChildElement("uri"))
|
||||
{
|
||||
fn = filename->GetText();
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
// URDF
|
||||
if (shape->Attribute("filename"))
|
||||
{
|
||||
fn = shape->Attribute("filename");
|
||||
}
|
||||
if (shape->Attribute("scale"))
|
||||
{
|
||||
if (!parseVector3(geom.m_meshScale, shape->Attribute("scale"), logger))
|
||||
{
|
||||
logger->reportWarning("Scale should be a vector3, not single scalar. Workaround activated.\n");
|
||||
std::string scalar_str = shape->Attribute("scale");
|
||||
double scaleFactor = urdfLexicalCast<double>(scalar_str.c_str());
|
||||
if (scaleFactor)
|
||||
{
|
||||
geom.m_meshScale.setValue(scaleFactor, scaleFactor, scaleFactor);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (fn.empty())
|
||||
{
|
||||
logger->reportError("Mesh filename is empty");
|
||||
return false;
|
||||
}
|
||||
|
||||
geom.m_meshFileName = fn;
|
||||
bool success = findExistingMeshFile(
|
||||
m_urdf2Model.m_sourceFile, fn, sourceFileLocation(shape),
|
||||
&geom.m_meshFileName, &geom.m_meshFileType);
|
||||
if (!success)
|
||||
{
|
||||
// warning already printed
|
||||
return false;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
@@ -566,7 +576,7 @@ bool UrdfParser::parseVisual(UrdfModel& model, UrdfVisual& visual, TiXmlElement*
|
||||
if (name_char)
|
||||
visual.m_name = name_char;
|
||||
|
||||
visual.m_hasLocalMaterial = false;
|
||||
visual.m_geometry.m_hasLocalMaterial = false;
|
||||
|
||||
// Material
|
||||
TiXmlElement *mat = config->FirstChildElement("material");
|
||||
@@ -588,7 +598,7 @@ bool UrdfParser::parseVisual(UrdfModel& model, UrdfVisual& visual, TiXmlElement*
|
||||
matPtr->m_rgbaColor = rgba;
|
||||
|
||||
visual.m_materialName = matPtr->m_name;
|
||||
visual.m_hasLocalMaterial = true;
|
||||
visual.m_geometry.m_hasLocalMaterial = true;
|
||||
}
|
||||
}
|
||||
else
|
||||
@@ -607,11 +617,11 @@ bool UrdfParser::parseVisual(UrdfModel& model, UrdfVisual& visual, TiXmlElement*
|
||||
TiXmlElement *c = mat->FirstChildElement("color");
|
||||
if (t||c)
|
||||
{
|
||||
if (parseMaterial(visual.m_localMaterial, mat,logger))
|
||||
if (parseMaterial(visual.m_geometry.m_localMaterial, mat,logger))
|
||||
{
|
||||
UrdfMaterial* matPtr = new UrdfMaterial(visual.m_localMaterial);
|
||||
UrdfMaterial* matPtr = new UrdfMaterial(visual.m_geometry.m_localMaterial);
|
||||
model.m_materials.insert(matPtr->m_name.c_str(),matPtr);
|
||||
visual.m_hasLocalMaterial = true;
|
||||
visual.m_geometry.m_hasLocalMaterial = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -758,6 +768,13 @@ bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *confi
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
{
|
||||
TiXmlElement *friction_anchor = ci->FirstChildElement("friction_anchor");
|
||||
if (friction_anchor)
|
||||
{
|
||||
link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_FRICTION_ANCHOR;
|
||||
}
|
||||
}
|
||||
{
|
||||
|
||||
@@ -845,7 +862,8 @@ bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *confi
|
||||
for (TiXmlElement* vis_xml = config->FirstChildElement("visual"); vis_xml; vis_xml = vis_xml->NextSiblingElement("visual"))
|
||||
{
|
||||
UrdfVisual visual;
|
||||
|
||||
visual.m_sourceFileLocation = sourceFileLocation(vis_xml);
|
||||
|
||||
if (parseVisual(model, visual, vis_xml,logger))
|
||||
{
|
||||
link.m_visualArray.push_back(visual);
|
||||
@@ -864,6 +882,8 @@ bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *confi
|
||||
for (TiXmlElement* col_xml = config->FirstChildElement("collision"); col_xml; col_xml = col_xml->NextSiblingElement("collision"))
|
||||
{
|
||||
UrdfCollision col;
|
||||
col.m_sourceFileLocation = sourceFileLocation(col_xml);
|
||||
|
||||
if (parseCollision(col, col_xml,logger))
|
||||
{
|
||||
link.m_collisionArray.push_back(col);
|
||||
@@ -1396,12 +1416,12 @@ bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceF
|
||||
for (int i=0;i<link->m_visualArray.size();i++)
|
||||
{
|
||||
UrdfVisual& vis = link->m_visualArray.at(i);
|
||||
if (!vis.m_hasLocalMaterial && vis.m_materialName.c_str())
|
||||
if (!vis.m_geometry.m_hasLocalMaterial && vis.m_materialName.c_str())
|
||||
{
|
||||
UrdfMaterial** mat = m_urdf2Model.m_materials.find(vis.m_materialName.c_str());
|
||||
if (mat && *mat)
|
||||
{
|
||||
vis.m_localMaterial = **mat;
|
||||
vis.m_geometry.m_localMaterial = **mat;
|
||||
} else
|
||||
{
|
||||
//logger->reportError("Cannot find material with name:");
|
||||
@@ -1591,12 +1611,12 @@ bool UrdfParser::loadSDF(const char* sdfText, ErrorLogger* logger)
|
||||
for (int i=0;i<link->m_visualArray.size();i++)
|
||||
{
|
||||
UrdfVisual& vis = link->m_visualArray.at(i);
|
||||
if (!vis.m_hasLocalMaterial && vis.m_materialName.c_str())
|
||||
if (!vis.m_geometry.m_hasLocalMaterial && vis.m_materialName.c_str())
|
||||
{
|
||||
UrdfMaterial** mat = localModel->m_materials.find(vis.m_materialName.c_str());
|
||||
if (mat && *mat)
|
||||
{
|
||||
vis.m_localMaterial = **mat;
|
||||
vis.m_geometry.m_localMaterial = **mat;
|
||||
} else
|
||||
{
|
||||
//logger->reportError("Cannot find material with name:");
|
||||
@@ -1657,3 +1677,9 @@ bool UrdfParser::loadSDF(const char* sdfText, ErrorLogger* logger)
|
||||
return true;
|
||||
}
|
||||
|
||||
std::string UrdfParser::sourceFileLocation(TiXmlElement* e)
|
||||
{
|
||||
char buf[1024];
|
||||
snprintf(buf, sizeof(buf), "%s:%i", m_urdf2Model.m_sourceFile.c_str(), e->Row());
|
||||
return buf;
|
||||
}
|
||||
|
||||
@@ -18,9 +18,13 @@ struct ErrorLogger
|
||||
|
||||
struct UrdfMaterial
|
||||
{
|
||||
std::string m_name;
|
||||
std::string m_name;
|
||||
std::string m_textureFilename;
|
||||
btVector4 m_rgbaColor;
|
||||
btVector4 m_rgbaColor; // [0]==r [1]==g [2]==b [3]==a
|
||||
UrdfMaterial():
|
||||
m_rgbaColor(0.8, 0.8, 0.8, 1)
|
||||
{
|
||||
}
|
||||
};
|
||||
|
||||
struct UrdfInertia
|
||||
@@ -66,33 +70,40 @@ struct UrdfGeometry
|
||||
btVector3 m_capsuleFrom;
|
||||
btVector3 m_capsuleTo;
|
||||
|
||||
double m_cylinderRadius;
|
||||
double m_cylinderLength;
|
||||
|
||||
btVector3 m_planeNormal;
|
||||
btVector3 m_planeNormal;
|
||||
|
||||
enum {
|
||||
FILE_STL =1,
|
||||
FILE_COLLADA =2,
|
||||
FILE_OBJ =3,
|
||||
};
|
||||
int m_meshFileType;
|
||||
std::string m_meshFileName;
|
||||
btVector3 m_meshScale;
|
||||
};
|
||||
btVector3 m_meshScale;
|
||||
|
||||
struct UrdfVisual
|
||||
{
|
||||
btTransform m_linkLocalFrame;
|
||||
UrdfGeometry m_geometry;
|
||||
std::string m_name;
|
||||
std::string m_materialName;
|
||||
bool m_hasLocalMaterial;
|
||||
UrdfMaterial m_localMaterial;
|
||||
bool m_hasLocalMaterial;
|
||||
};
|
||||
|
||||
bool findExistingMeshFile(const std::string& urdf_path, std::string fn,
|
||||
const std::string& error_message_prefix,
|
||||
std::string* out_found_filename, int* out_type); // intended to fill UrdfGeometry::m_meshFileName and Type, but can be used elsewhere
|
||||
|
||||
|
||||
|
||||
struct UrdfCollision
|
||||
struct UrdfShape
|
||||
{
|
||||
std::string m_sourceFileLocation;
|
||||
btTransform m_linkLocalFrame;
|
||||
UrdfGeometry m_geometry;
|
||||
std::string m_name;
|
||||
};
|
||||
|
||||
struct UrdfVisual: UrdfShape
|
||||
{
|
||||
std::string m_materialName;
|
||||
};
|
||||
|
||||
struct UrdfCollision: UrdfShape
|
||||
{
|
||||
int m_flags;
|
||||
int m_collisionGroup;
|
||||
int m_collisionMask;
|
||||
@@ -159,6 +170,7 @@ struct UrdfJoint
|
||||
struct UrdfModel
|
||||
{
|
||||
std::string m_name;
|
||||
std::string m_sourceFile;
|
||||
btTransform m_rootTransformInWorld;
|
||||
btHashMap<btHashString, UrdfMaterial*> m_materials;
|
||||
btHashMap<btHashString, UrdfLink*> m_links;
|
||||
@@ -204,7 +216,7 @@ public:
|
||||
|
||||
UrdfParser();
|
||||
virtual ~UrdfParser();
|
||||
|
||||
|
||||
void setParseSDF(bool useSDF)
|
||||
{
|
||||
m_parseSDF = useSDF;
|
||||
@@ -263,6 +275,13 @@ public:
|
||||
}
|
||||
return m_urdf2Model;
|
||||
}
|
||||
|
||||
std::string sourceFileLocation(TiXmlElement* e);
|
||||
|
||||
void setSourceFile(const std::string& sourceFile)
|
||||
{
|
||||
m_urdf2Model.m_sourceFile = sourceFile;
|
||||
}
|
||||
};
|
||||
|
||||
#endif
|
||||
|
||||
@@ -774,16 +774,18 @@ void SimpleOpenGL3App::swapBuffer()
|
||||
m_data->m_frameDumpPngFileName = 0;
|
||||
}
|
||||
}
|
||||
m_window->endRendering();
|
||||
m_window->startRendering();
|
||||
m_window->endRendering();
|
||||
m_window->startRendering();
|
||||
}
|
||||
|
||||
// see also http://blog.mmacklin.com/2013/06/11/real-time-video-capture-with-ffmpeg/
|
||||
void SimpleOpenGL3App::dumpFramesToVideo(const char* mp4FileName)
|
||||
{
|
||||
int width = (int)m_window->getRetinaScale()*m_instancingRenderer->getScreenWidth();
|
||||
int height = (int)m_window->getRetinaScale()*m_instancingRenderer->getScreenHeight();
|
||||
char cmd[8192];
|
||||
if (mp4FileName)
|
||||
{
|
||||
int width = (int)m_window->getRetinaScale()*m_instancingRenderer->getScreenWidth();
|
||||
int height = (int)m_window->getRetinaScale()*m_instancingRenderer->getScreenHeight();
|
||||
char cmd[8192];
|
||||
|
||||
#ifdef _WIN32
|
||||
sprintf(cmd, "ffmpeg -r 60 -f rawvideo -pix_fmt rgba -s %dx%d -i - "
|
||||
@@ -803,15 +805,25 @@ void SimpleOpenGL3App::dumpFramesToVideo(const char* mp4FileName)
|
||||
// sprintf(cmd,"ffmpeg -r 60 -f rawvideo -pix_fmt rgba -s %dx%d -i - "
|
||||
// "-threads 0 -preset fast -y -crf 21 -vf vflip %s",width,height,mp4FileName);
|
||||
|
||||
if (m_data->m_ffmpegFile)
|
||||
{
|
||||
pclose(m_data->m_ffmpegFile);
|
||||
}
|
||||
if (mp4FileName)
|
||||
{
|
||||
m_data->m_ffmpegFile = popen(cmd, "w");
|
||||
if (m_data->m_ffmpegFile)
|
||||
{
|
||||
pclose(m_data->m_ffmpegFile);
|
||||
}
|
||||
if (mp4FileName)
|
||||
{
|
||||
m_data->m_ffmpegFile = popen(cmd, "w");
|
||||
|
||||
m_data->m_frameDumpPngFileName = mp4FileName;
|
||||
m_data->m_frameDumpPngFileName = mp4FileName;
|
||||
}
|
||||
} else
|
||||
{
|
||||
if (m_data->m_ffmpegFile)
|
||||
{
|
||||
fflush(m_data->m_ffmpegFile);
|
||||
pclose(m_data->m_ffmpegFile);
|
||||
m_data->m_frameDumpPngFileName = 0;
|
||||
}
|
||||
m_data->m_ffmpegFile = 0;
|
||||
}
|
||||
}
|
||||
void SimpleOpenGL3App::dumpNextFrameToPng(const char* filename)
|
||||
|
||||
@@ -138,8 +138,34 @@ IF(BUILD_CLSOCKET)
|
||||
)
|
||||
ENDIF()
|
||||
|
||||
ADD_EXECUTABLE(App_RobotSimulator ${RobotSimulator_SRCS})
|
||||
|
||||
#some code to support OpenGL and Glew cross platform
|
||||
IF (WIN32)
|
||||
INCLUDE_DIRECTORIES(
|
||||
${BULLET_PHYSICS_SOURCE_DIR}/btgui/OpenGLWindow/GlewWindows
|
||||
)
|
||||
ADD_DEFINITIONS(-DGLEW_STATIC)
|
||||
LINK_LIBRARIES( ${OPENGL_gl_LIBRARY} ${OPENGL_glu_LIBRARY} )
|
||||
ELSE(WIN32)
|
||||
IF(APPLE)
|
||||
find_library(COCOA NAMES Cocoa)
|
||||
MESSAGE(${COCOA})
|
||||
link_libraries(${COCOA} ${OPENGL_gl_LIBRARY} ${OPENGL_glu_LIBRARY})
|
||||
|
||||
ELSE(APPLE)
|
||||
INCLUDE_DIRECTORIES(
|
||||
${BULLET_PHYSICS_SOURCE_DIR}/btgui/OpenGLWindow/GlewWindows
|
||||
)
|
||||
ADD_DEFINITIONS("-DGLEW_INIT_OPENGL11_FUNCTIONS=1")
|
||||
ADD_DEFINITIONS("-DGLEW_STATIC")
|
||||
ADD_DEFINITIONS("-DGLEW_DYNAMIC_LOAD_ALL_GLX_FUNCTIONS=1")
|
||||
|
||||
LINK_LIBRARIES( pthread dl )
|
||||
ENDIF(APPLE)
|
||||
ENDIF(WIN32)
|
||||
|
||||
|
||||
ADD_EXECUTABLE(App_RobotSimulator ${RobotSimulator_SRCS})
|
||||
|
||||
SET_TARGET_PROPERTIES(App_RobotSimulator PROPERTIES VERSION ${BULLET_VERSION})
|
||||
SET_TARGET_PROPERTIES(App_RobotSimulator PROPERTIES DEBUG_POSTFIX "_d")
|
||||
@@ -153,7 +179,6 @@ ENDIF(WIN32)
|
||||
|
||||
|
||||
|
||||
|
||||
TARGET_LINK_LIBRARIES(App_RobotSimulator BulletExampleBrowserLib BulletFileLoader BulletWorldImporter BulletSoftBody BulletDynamics BulletCollision BulletInverseDynamicsUtils BulletInverseDynamics LinearMath OpenGLWindow gwen BussIK Bullet3Common)
|
||||
|
||||
|
||||
|
||||
@@ -49,57 +49,69 @@ void MinitaurSetup::resetPose(class b3RobotSimulatorClientAPI* sim)
|
||||
sim->setJointMotorControl(m_data->m_quadrupedUniqueId,i,controlArgs);
|
||||
}
|
||||
|
||||
//right front leg
|
||||
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["motor_front_rightR_joint"],1.57);
|
||||
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_front_rightR_link"],-2.2);
|
||||
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["motor_front_rightL_joint"],-1.57);
|
||||
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_front_rightL_link"],2.2);
|
||||
b3Scalar startAngle = B3_HALF_PI;
|
||||
b3Scalar upperLegLength = 11.5;
|
||||
b3Scalar lowerLegLength = 20;
|
||||
b3Scalar kneeAngle = B3_PI+b3Acos(upperLegLength/lowerLegLength);
|
||||
|
||||
b3Scalar motorDirs[8] = {-1,-1,-1,-1,1,1,1,1};
|
||||
b3JointInfo jointInfo;
|
||||
jointInfo.m_jointType = ePoint2PointType;
|
||||
jointInfo.m_parentFrame[0] = 0; jointInfo.m_parentFrame[1] = 0.01; jointInfo.m_parentFrame[2] = 0.2;
|
||||
jointInfo.m_childFrame[0] = 0; jointInfo.m_childFrame[1] = -0.015; jointInfo.m_childFrame[2] = 0.2;
|
||||
sim->createConstraint(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_front_rightR_link"],
|
||||
m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_front_rightL_link"],&jointInfo);
|
||||
setDesiredMotorAngle(sim,"motor_front_rightR_joint",1.57);
|
||||
setDesiredMotorAngle(sim,"motor_front_rightL_joint",-1.57);
|
||||
|
||||
//left front leg
|
||||
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["motor_front_leftR_joint"],1.57);
|
||||
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_front_leftR_link"],-2.2);
|
||||
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["motor_front_leftL_joint"],-1.57);
|
||||
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_front_leftL_link"],2.2);
|
||||
jointInfo.m_parentFrame[0] = 0; jointInfo.m_parentFrame[1] = -0.01; jointInfo.m_parentFrame[2] = 0.2;
|
||||
jointInfo.m_childFrame[0] = 0; jointInfo.m_childFrame[1] = 0.015; jointInfo.m_childFrame[2] = 0.2;
|
||||
//left front leg
|
||||
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["motor_front_leftL_joint"],motorDirs[0] * startAngle);
|
||||
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_front_leftL_link"],motorDirs[0]*kneeAngle);
|
||||
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["motor_front_leftR_joint"],motorDirs[1] * startAngle);
|
||||
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_front_leftR_link"],motorDirs[1]*kneeAngle);
|
||||
|
||||
jointInfo.m_parentFrame[0] = 0; jointInfo.m_parentFrame[1] = 0.005; jointInfo.m_parentFrame[2] = 0.2;
|
||||
jointInfo.m_childFrame[0] = 0; jointInfo.m_childFrame[1] = 0.01; jointInfo.m_childFrame[2] = 0.2;
|
||||
sim->createConstraint(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_front_leftR_link"],
|
||||
m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_front_leftL_link"],&jointInfo);
|
||||
setDesiredMotorAngle(sim,"motor_front_leftR_joint", 1.57);
|
||||
setDesiredMotorAngle(sim,"motor_front_leftL_joint", -1.57);
|
||||
|
||||
//right back leg
|
||||
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["motor_back_rightR_joint"],1.57);
|
||||
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_back_rightR_link"],-2.2);
|
||||
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["motor_back_rightL_joint"],-1.57);
|
||||
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_back_rightL_link"],2.2);
|
||||
jointInfo.m_parentFrame[0] = 0; jointInfo.m_parentFrame[1] = 0.01; jointInfo.m_parentFrame[2] = 0.2;
|
||||
jointInfo.m_childFrame[0] = 0; jointInfo.m_childFrame[1] = -0.015; jointInfo.m_childFrame[2] = 0.2;
|
||||
sim->createConstraint(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_back_rightR_link"],
|
||||
m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_back_rightL_link"],&jointInfo);
|
||||
setDesiredMotorAngle(sim,"motor_back_rightR_joint", 1.57);
|
||||
setDesiredMotorAngle(sim,"motor_back_rightL_joint", -1.57);
|
||||
|
||||
setDesiredMotorAngle(sim,"motor_front_leftL_joint", motorDirs[0] * startAngle);
|
||||
setDesiredMotorAngle(sim,"motor_front_leftR_joint", motorDirs[1] * startAngle);
|
||||
|
||||
//left back leg
|
||||
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["motor_back_leftR_joint"],1.57);
|
||||
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_back_leftR_link"],-2.2);
|
||||
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["motor_back_leftL_joint"],-1.57);
|
||||
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_back_leftL_link"],2.2);
|
||||
jointInfo.m_parentFrame[0] = 0; jointInfo.m_parentFrame[1] = -0.01; jointInfo.m_parentFrame[2] = 0.2;
|
||||
jointInfo.m_childFrame[0] = 0; jointInfo.m_childFrame[1] = 0.015; jointInfo.m_childFrame[2] = 0.2;
|
||||
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["motor_back_leftL_joint"],motorDirs[2] * startAngle);
|
||||
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_back_leftL_link"],motorDirs[2] * kneeAngle);
|
||||
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["motor_back_leftR_joint"],motorDirs[3] * startAngle);
|
||||
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_back_leftR_link"],motorDirs[3] * kneeAngle);
|
||||
jointInfo.m_parentFrame[0] = 0; jointInfo.m_parentFrame[1] = 0.005; jointInfo.m_parentFrame[2] = 0.2;
|
||||
jointInfo.m_childFrame[0] = 0; jointInfo.m_childFrame[1] = 0.01; jointInfo.m_childFrame[2] = 0.2;
|
||||
sim->createConstraint(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_back_leftR_link"],
|
||||
m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_back_leftL_link"],&jointInfo);
|
||||
setDesiredMotorAngle(sim,"motor_back_leftR_joint", 1.57);
|
||||
setDesiredMotorAngle(sim,"motor_back_leftL_joint", -1.57);
|
||||
setDesiredMotorAngle(sim,"motor_back_leftL_joint", motorDirs[2] * startAngle);
|
||||
setDesiredMotorAngle(sim,"motor_back_leftR_joint", motorDirs[3] * startAngle);
|
||||
|
||||
|
||||
//right front leg
|
||||
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["motor_front_rightL_joint"],motorDirs[4] * startAngle);
|
||||
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_front_rightL_link"],motorDirs[4] * kneeAngle);
|
||||
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["motor_front_rightR_joint"],motorDirs[5]*startAngle);
|
||||
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_front_rightR_link"],motorDirs[5] * kneeAngle);
|
||||
|
||||
|
||||
|
||||
jointInfo.m_parentFrame[0] = 0; jointInfo.m_parentFrame[1] = 0.005; jointInfo.m_parentFrame[2] = 0.2;
|
||||
jointInfo.m_childFrame[0] = 0; jointInfo.m_childFrame[1] = 0.01; jointInfo.m_childFrame[2] = 0.2;
|
||||
sim->createConstraint(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_front_rightR_link"],
|
||||
m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_front_rightL_link"],&jointInfo);
|
||||
setDesiredMotorAngle(sim,"motor_front_rightL_joint",motorDirs[4] * startAngle);
|
||||
setDesiredMotorAngle(sim,"motor_front_rightR_joint",motorDirs[5] * startAngle);
|
||||
|
||||
|
||||
//right back leg
|
||||
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["motor_back_rightL_joint"],motorDirs[6] * startAngle);
|
||||
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_back_rightL_link"],motorDirs[6] * kneeAngle);
|
||||
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["motor_back_rightR_joint"],motorDirs[7] * startAngle);
|
||||
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_back_rightR_link"],motorDirs[7] * kneeAngle);
|
||||
|
||||
jointInfo.m_parentFrame[0] = 0; jointInfo.m_parentFrame[1] = 0.005; jointInfo.m_parentFrame[2] = 0.2;
|
||||
jointInfo.m_childFrame[0] = 0; jointInfo.m_childFrame[1] = 0.01; jointInfo.m_childFrame[2] = 0.2;
|
||||
sim->createConstraint(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_back_rightR_link"],
|
||||
m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_back_rightL_link"],&jointInfo);
|
||||
setDesiredMotorAngle(sim,"motor_back_rightL_joint", motorDirs[6] * startAngle);
|
||||
setDesiredMotorAngle(sim,"motor_back_rightR_joint", motorDirs[7] * startAngle);
|
||||
|
||||
}
|
||||
|
||||
int MinitaurSetup::setupMinitaur(class b3RobotSimulatorClientAPI* sim, const b3Vector3& startPos, const b3Quaternion& startOrn)
|
||||
@@ -109,7 +121,7 @@ int MinitaurSetup::setupMinitaur(class b3RobotSimulatorClientAPI* sim, const b3V
|
||||
args.m_startPosition = startPos;
|
||||
args.m_startOrientation = startOrn;
|
||||
|
||||
m_data->m_quadrupedUniqueId = sim->loadURDF("quadruped/quadruped.urdf",args);
|
||||
m_data->m_quadrupedUniqueId = sim->loadURDF("quadruped/minitaur.urdf",args);
|
||||
|
||||
int numJoints = sim->getNumJoints(m_data->m_quadrupedUniqueId);
|
||||
for (int i=0;i<numJoints;i++)
|
||||
|
||||
@@ -16,17 +16,22 @@ int main(int argc, char* argv[])
|
||||
//sim->connect(eCONNECT_UDP, "localhost", 1234);
|
||||
sim->configureDebugVisualizer( COV_ENABLE_GUI, 0);
|
||||
// sim->configureDebugVisualizer( COV_ENABLE_SHADOWS, 0);//COV_ENABLE_WIREFRAME
|
||||
|
||||
sim->setTimeOut(10);
|
||||
//syncBodies is only needed when connecting to an existing physics server that has already some bodies
|
||||
sim->syncBodies();
|
||||
|
||||
sim->setTimeStep(1./240.);
|
||||
b3Scalar fixedTimeStep = 1./240.;
|
||||
|
||||
sim->setGravity(b3MakeVector3(0,0,-10));
|
||||
sim->setTimeStep(fixedTimeStep);
|
||||
|
||||
int blockId = sim->loadURDF("cube.urdf");
|
||||
b3BodyInfo bodyInfo;
|
||||
sim->getBodyInfo(blockId,&bodyInfo);
|
||||
b3Quaternion q = sim->getQuaternionFromEuler(b3MakeVector3(0.1,0.2,0.3));
|
||||
b3Vector3 rpy;
|
||||
rpy = sim->getEulerFromQuaternion(q);
|
||||
|
||||
sim->setGravity(b3MakeVector3(0,0,-9.8));
|
||||
|
||||
//int blockId = sim->loadURDF("cube.urdf");
|
||||
//b3BodyInfo bodyInfo;
|
||||
//sim->getBodyInfo(blockId,&bodyInfo);
|
||||
|
||||
sim->loadURDF("plane.urdf");
|
||||
|
||||
@@ -34,15 +39,15 @@ int main(int argc, char* argv[])
|
||||
int minitaurUid = minitaur.setupMinitaur(sim, b3MakeVector3(0,0,.3));
|
||||
|
||||
|
||||
b3RobotSimulatorLoadUrdfFileArgs args;
|
||||
args.m_startPosition.setValue(2,0,1);
|
||||
int r2d2 = sim->loadURDF("r2d2.urdf",args);
|
||||
//b3RobotSimulatorLoadUrdfFileArgs args;
|
||||
//args.m_startPosition.setValue(2,0,1);
|
||||
//int r2d2 = sim->loadURDF("r2d2.urdf",args);
|
||||
|
||||
b3RobotSimulatorLoadFileResults sdfResults;
|
||||
if (!sim->loadSDF("two_cubes.sdf",sdfResults))
|
||||
{
|
||||
b3Warning("Can't load SDF!\n");
|
||||
}
|
||||
//b3RobotSimulatorLoadFileResults sdfResults;
|
||||
//if (!sim->loadSDF("two_cubes.sdf",sdfResults))
|
||||
//{
|
||||
// b3Warning("Can't load SDF!\n");
|
||||
//}
|
||||
|
||||
b3Clock clock;
|
||||
double startTime = clock.getTimeInSeconds();
|
||||
@@ -54,7 +59,10 @@ int main(int argc, char* argv[])
|
||||
}
|
||||
#endif
|
||||
sim->setRealTimeSimulation(false);
|
||||
|
||||
int vidLogId = -1;
|
||||
int minitaurLogId = -1;
|
||||
int rotateCamera = 0;
|
||||
|
||||
while (sim->canSubmitCommand())
|
||||
{
|
||||
b3KeyboardEventsData keyEvents;
|
||||
@@ -62,14 +70,63 @@ int main(int argc, char* argv[])
|
||||
if (keyEvents.m_numKeyboardEvents)
|
||||
{
|
||||
|
||||
printf("num key events = %d]\n", keyEvents.m_numKeyboardEvents);
|
||||
//printf("num key events = %d]\n", keyEvents.m_numKeyboardEvents);
|
||||
//m_keyState is a flag combination of eButtonIsDown,eButtonTriggered, eButtonReleased
|
||||
for (int i=0;i<keyEvents.m_numKeyboardEvents;i++)
|
||||
{
|
||||
printf("keyEvent[%d].m_keyCode = %d, state = %d\n", i,keyEvents.m_keyboardEvents[i].m_keyCode,keyEvents.m_keyboardEvents[i].m_keyState);
|
||||
b3KeyboardEvent& e = keyEvents.m_keyboardEvents[i];
|
||||
|
||||
if (e.m_keyCode=='0')
|
||||
{
|
||||
if ( e.m_keyState&eButtonTriggered)
|
||||
{
|
||||
if (vidLogId < 0)
|
||||
{
|
||||
vidLogId = sim->startStateLogging(STATE_LOGGING_VIDEO_MP4,"video.mp4");
|
||||
}
|
||||
else
|
||||
{
|
||||
sim->stopStateLogging(vidLogId);
|
||||
vidLogId=-1;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (e.m_keyCode=='m')
|
||||
{
|
||||
if ( minitaurLogId<0 && e.m_keyState&eButtonTriggered)
|
||||
{
|
||||
minitaurLogId = sim->startStateLogging(STATE_LOGGING_MINITAUR,"simlog.bin");
|
||||
}
|
||||
if (minitaurLogId>=0 && e.m_keyState&eButtonReleased)
|
||||
{
|
||||
sim->stopStateLogging(minitaurLogId);
|
||||
minitaurLogId=-1;
|
||||
}
|
||||
}
|
||||
|
||||
if (e.m_keyCode == 'r' && e.m_keyState&eButtonTriggered)
|
||||
{
|
||||
rotateCamera = 1-rotateCamera;
|
||||
}
|
||||
|
||||
|
||||
//printf("keyEvent[%d].m_keyCode = %d, state = %d\n", i,keyEvents.m_keyboardEvents[i].m_keyCode,keyEvents.m_keyboardEvents[i].m_keyState);
|
||||
}
|
||||
}
|
||||
b3Clock::usleep(1000*1000);
|
||||
sim->stepSimulation();
|
||||
|
||||
if (rotateCamera)
|
||||
{
|
||||
static double yaw=0;
|
||||
double distance = 1;
|
||||
yaw+=0.1;
|
||||
b3Vector3 basePos;
|
||||
b3Quaternion baseOrn;
|
||||
sim->getBasePositionAndOrientation(minitaurUid,basePos,baseOrn);
|
||||
sim->resetDebugVisualizerCamera(distance,yaw,20,basePos);
|
||||
}
|
||||
b3Clock::usleep(1000.*1000.*fixedTimeStep);
|
||||
}
|
||||
|
||||
printf("sim->disconnect\n");
|
||||
|
||||
@@ -1,17 +1,16 @@
|
||||
#include "b3RobotSimulatorClientAPI.h"
|
||||
|
||||
|
||||
//#include "SharedMemoryCommands.h"
|
||||
|
||||
#include "SharedMemory/PhysicsClientC_API.h"
|
||||
|
||||
#ifdef BT_ENABLE_ENET
|
||||
#include "SharedMemory/PhysicsClientUDP_C_API.h"
|
||||
#endif//PHYSICS_UDP
|
||||
#endif //PHYSICS_UDP
|
||||
|
||||
#ifdef BT_ENABLE_CLSOCKET
|
||||
#include "SharedMemory/PhysicsClientTCP_C_API.h"
|
||||
#endif//PHYSICS_TCP
|
||||
#endif //PHYSICS_TCP
|
||||
|
||||
#include "SharedMemory/PhysicsDirectC_API.h"
|
||||
|
||||
@@ -25,7 +24,7 @@ struct b3RobotSimulatorClientAPI_InternalData
|
||||
b3PhysicsClientHandle m_physicsClientHandle;
|
||||
|
||||
b3RobotSimulatorClientAPI_InternalData()
|
||||
:m_physicsClientHandle(0)
|
||||
: m_physicsClientHandle(0)
|
||||
{
|
||||
}
|
||||
};
|
||||
@@ -35,17 +34,16 @@ b3RobotSimulatorClientAPI::b3RobotSimulatorClientAPI()
|
||||
m_data = new b3RobotSimulatorClientAPI_InternalData();
|
||||
}
|
||||
|
||||
b3RobotSimulatorClientAPI::~b3RobotSimulatorClientAPI()
|
||||
b3RobotSimulatorClientAPI::~b3RobotSimulatorClientAPI()
|
||||
{
|
||||
delete m_data;
|
||||
delete m_data;
|
||||
}
|
||||
|
||||
|
||||
bool b3RobotSimulatorClientAPI::connect(int mode, const std::string& hostName, int portOrKey)
|
||||
{
|
||||
if (m_data->m_physicsClientHandle)
|
||||
{
|
||||
b3Warning ("Already connected, disconnect first.");
|
||||
b3Warning("Already connected, disconnect first.");
|
||||
return false;
|
||||
}
|
||||
b3PhysicsClientHandle sm = 0;
|
||||
@@ -54,86 +52,97 @@ bool b3RobotSimulatorClientAPI::connect(int mode, const std::string& hostName, i
|
||||
int tcpPort = 6667;
|
||||
int key = SHARED_MEMORY_KEY;
|
||||
bool connected = false;
|
||||
|
||||
|
||||
switch (mode)
|
||||
switch (mode)
|
||||
{
|
||||
case eCONNECT_GUI:
|
||||
case eCONNECT_GUI:
|
||||
{
|
||||
int argc = 0;
|
||||
char* argv[1] = {0};
|
||||
#ifdef __APPLE__
|
||||
sm = b3CreateInProcessPhysicsServerAndConnectMainThread(argc, argv);
|
||||
#else
|
||||
sm = b3CreateInProcessPhysicsServerAndConnect(argc, argv);
|
||||
#endif
|
||||
break;
|
||||
int argc = 0;
|
||||
char* argv[1] = {0};
|
||||
#ifdef __APPLE__
|
||||
sm = b3CreateInProcessPhysicsServerAndConnectMainThread(argc, argv);
|
||||
#else
|
||||
sm = b3CreateInProcessPhysicsServerAndConnect(argc, argv);
|
||||
#endif
|
||||
break;
|
||||
}
|
||||
case eCONNECT_DIRECT: {
|
||||
sm = b3ConnectPhysicsDirect();
|
||||
break;
|
||||
case eCONNECT_DIRECT:
|
||||
{
|
||||
sm = b3ConnectPhysicsDirect();
|
||||
break;
|
||||
}
|
||||
case eCONNECT_SHARED_MEMORY: {
|
||||
if (portOrKey>=0)
|
||||
case eCONNECT_SHARED_MEMORY:
|
||||
{
|
||||
if (portOrKey >= 0)
|
||||
{
|
||||
key = portOrKey;
|
||||
}
|
||||
sm = b3ConnectSharedMemory(key);
|
||||
break;
|
||||
sm = b3ConnectSharedMemory(key);
|
||||
break;
|
||||
}
|
||||
case eCONNECT_UDP:
|
||||
{
|
||||
if (portOrKey>=0)
|
||||
case eCONNECT_UDP:
|
||||
{
|
||||
if (portOrKey >= 0)
|
||||
{
|
||||
udpPort = portOrKey;
|
||||
}
|
||||
#ifdef BT_ENABLE_ENET
|
||||
#ifdef BT_ENABLE_ENET
|
||||
|
||||
sm = b3ConnectPhysicsUDP(hostName.c_str(), udpPort);
|
||||
#else
|
||||
b3Warning("UDP is not enabled in this build");
|
||||
#endif //BT_ENABLE_ENET
|
||||
#else
|
||||
b3Warning("UDP is not enabled in this build");
|
||||
#endif //BT_ENABLE_ENET
|
||||
|
||||
break;
|
||||
}
|
||||
case eCONNECT_TCP:
|
||||
}
|
||||
case eCONNECT_TCP:
|
||||
{
|
||||
if (portOrKey>=0)
|
||||
if (portOrKey >= 0)
|
||||
{
|
||||
tcpPort = portOrKey;
|
||||
}
|
||||
#ifdef BT_ENABLE_CLSOCKET
|
||||
|
||||
#ifdef BT_ENABLE_CLSOCKET
|
||||
|
||||
sm = b3ConnectPhysicsTCP(hostName.c_str(), tcpPort);
|
||||
#else
|
||||
#else
|
||||
b3Warning("TCP is not enabled in this pybullet build");
|
||||
#endif //BT_ENABLE_CLSOCKET
|
||||
#endif //BT_ENABLE_CLSOCKET
|
||||
break;
|
||||
}
|
||||
|
||||
|
||||
|
||||
default: {
|
||||
b3Warning("connectPhysicsServer unexpected argument");
|
||||
default:
|
||||
{
|
||||
b3Warning("connectPhysicsServer unexpected argument");
|
||||
}
|
||||
};
|
||||
|
||||
if (sm)
|
||||
{
|
||||
m_data->m_physicsClientHandle = sm;
|
||||
if (!b3CanSubmitCommand(m_data->m_physicsClientHandle))
|
||||
{
|
||||
disconnect();
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
if (sm)
|
||||
{
|
||||
m_data->m_physicsClientHandle = sm;
|
||||
if (!b3CanSubmitCommand(m_data->m_physicsClientHandle))
|
||||
{
|
||||
disconnect();
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
bool b3RobotSimulatorClientAPI::isConnected() const
|
||||
{
|
||||
return (m_data->m_physicsClientHandle!=0);
|
||||
return (m_data->m_physicsClientHandle != 0);
|
||||
}
|
||||
|
||||
void b3RobotSimulatorClientAPI::setTimeOut(double timeOutInSec)
|
||||
{
|
||||
if (!isConnected())
|
||||
{
|
||||
b3Warning("Not connected");
|
||||
return;
|
||||
}
|
||||
b3SetTimeOut(m_data->m_physicsClientHandle,timeOutInSec);
|
||||
|
||||
}
|
||||
|
||||
|
||||
@@ -157,15 +166,13 @@ void b3RobotSimulatorClientAPI::syncBodies()
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
b3SharedMemoryCommandHandle command;
|
||||
b3SharedMemoryCommandHandle command;
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
int statusType;
|
||||
|
||||
command = b3InitSyncBodyInfoCommand(m_data->m_physicsClientHandle);
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
|
||||
statusType = b3GetStatusType(statusHandle);
|
||||
|
||||
}
|
||||
|
||||
void b3RobotSimulatorClientAPI::resetSimulation()
|
||||
@@ -175,9 +182,9 @@ void b3RobotSimulatorClientAPI::resetSimulation()
|
||||
b3Warning("Not connected");
|
||||
return;
|
||||
}
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(
|
||||
m_data->m_physicsClientHandle, b3InitResetSimulationCommand(m_data->m_physicsClientHandle));
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(
|
||||
m_data->m_physicsClientHandle, b3InitResetSimulationCommand(m_data->m_physicsClientHandle));
|
||||
}
|
||||
|
||||
bool b3RobotSimulatorClientAPI::canSubmitCommand() const
|
||||
@@ -186,7 +193,7 @@ bool b3RobotSimulatorClientAPI::canSubmitCommand() const
|
||||
{
|
||||
return false;
|
||||
}
|
||||
return (b3CanSubmitCommand(m_data->m_physicsClientHandle));
|
||||
return (b3CanSubmitCommand(m_data->m_physicsClientHandle) != 0);
|
||||
}
|
||||
|
||||
void b3RobotSimulatorClientAPI::stepSimulation()
|
||||
@@ -200,11 +207,11 @@ void b3RobotSimulatorClientAPI::stepSimulation()
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
int statusType;
|
||||
if (b3CanSubmitCommand(m_data->m_physicsClientHandle))
|
||||
{
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, b3InitStepSimulationCommand(m_data->m_physicsClientHandle));
|
||||
statusType = b3GetStatusType(statusHandle);
|
||||
b3Assert(statusType==CMD_STEP_FORWARD_SIMULATION_COMPLETED);
|
||||
}
|
||||
{
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, b3InitStepSimulationCommand(m_data->m_physicsClientHandle));
|
||||
statusType = b3GetStatusType(statusHandle);
|
||||
//b3Assert(statusType == CMD_STEP_FORWARD_SIMULATION_COMPLETED);
|
||||
}
|
||||
}
|
||||
|
||||
void b3RobotSimulatorClientAPI::setGravity(const b3Vector3& gravityAcceleration)
|
||||
@@ -215,57 +222,26 @@ void b3RobotSimulatorClientAPI::setGravity(const b3Vector3& gravityAcceleration)
|
||||
return;
|
||||
}
|
||||
|
||||
b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle);
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
b3PhysicsParamSetGravity(command, gravityAcceleration[0],gravityAcceleration[1],gravityAcceleration[2]);
|
||||
b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle);
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
b3PhysicsParamSetGravity(command, gravityAcceleration[0], gravityAcceleration[1], gravityAcceleration[2]);
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
|
||||
b3Assert(b3GetStatusType(statusHandle)==CMD_CLIENT_COMMAND_COMPLETED);
|
||||
// b3Assert(b3GetStatusType(statusHandle) == CMD_CLIENT_COMMAND_COMPLETED);
|
||||
}
|
||||
|
||||
b3Quaternion b3RobotSimulatorClientAPI::getQuaternionFromEuler(const b3Vector3& rollPitchYaw)
|
||||
{
|
||||
double phi, the, psi;
|
||||
phi = rollPitchYaw[0] / 2.0;
|
||||
the = rollPitchYaw[1] / 2.0;
|
||||
psi = rollPitchYaw[2] / 2.0;
|
||||
double quat[4] = {
|
||||
sin(phi) * cos(the) * cos(psi) - cos(phi) * sin(the) * sin(psi),
|
||||
cos(phi) * sin(the) * cos(psi) + sin(phi) * cos(the) * sin(psi),
|
||||
cos(phi) * cos(the) * sin(psi) - sin(phi) * sin(the) * cos(psi),
|
||||
cos(phi) * cos(the) * cos(psi) + sin(phi) * sin(the) * sin(psi)};
|
||||
|
||||
// normalize the quaternion
|
||||
double len = sqrt(quat[0] * quat[0] + quat[1] * quat[1] +
|
||||
quat[2] * quat[2] + quat[3] * quat[3]);
|
||||
quat[0] /= len;
|
||||
quat[1] /= len;
|
||||
quat[2] /= len;
|
||||
quat[3] /= len;
|
||||
b3Quaternion q(quat[0],quat[1],quat[2],quat[3]);
|
||||
b3Quaternion q;
|
||||
q.setEulerZYX(rollPitchYaw[2],rollPitchYaw[1],rollPitchYaw[0]);
|
||||
return q;
|
||||
}
|
||||
|
||||
b3Vector3 b3RobotSimulatorClientAPI::getEulerFromQuaternion(const b3Quaternion& quat)
|
||||
{
|
||||
double squ;
|
||||
double sqx;
|
||||
double sqy;
|
||||
double sqz;
|
||||
double sarg;
|
||||
|
||||
b3Vector3 rpy;
|
||||
sqx = quat[0] * quat[0];
|
||||
sqy = quat[1] * quat[1];
|
||||
sqz = quat[2] * quat[2];
|
||||
squ = quat[3] * quat[3];
|
||||
rpy[0] = atan2(2 * (quat[1] * quat[2] + quat[3] * quat[0]),
|
||||
squ - sqx - sqy + sqz);
|
||||
sarg = -2 * (quat[0] * quat[2] - quat[3] * quat[1]);
|
||||
rpy[1] = sarg <= -1.0 ? -0.5 * 3.141592538
|
||||
: (sarg >= 1.0 ? 0.5 * 3.141592538 : asin(sarg));
|
||||
rpy[2] = atan2(2 * (quat[0] * quat[1] + quat[3] * quat[2]),
|
||||
squ + sqx - sqy - sqz);
|
||||
return rpy;
|
||||
b3Scalar roll,pitch,yaw;
|
||||
quat.getEulerZYX(yaw,pitch,roll);
|
||||
b3Vector3 rpy2 = b3MakeVector3(roll,pitch,yaw);
|
||||
return rpy2;
|
||||
}
|
||||
|
||||
int b3RobotSimulatorClientAPI::loadURDF(const std::string& fileName, const struct b3RobotSimulatorLoadUrdfFileArgs& args)
|
||||
@@ -278,31 +254,26 @@ int b3RobotSimulatorClientAPI::loadURDF(const std::string& fileName, const struc
|
||||
return robotUniqueId;
|
||||
}
|
||||
|
||||
|
||||
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
int statusType;
|
||||
b3SharedMemoryCommandHandle command = b3LoadUrdfCommandInit(m_data->m_physicsClientHandle, fileName.c_str());
|
||||
|
||||
|
||||
//setting the initial position, orientation and other arguments are optional
|
||||
|
||||
|
||||
b3LoadUrdfCommandSetStartPosition(command, args.m_startPosition[0],
|
||||
args.m_startPosition[1],
|
||||
args.m_startPosition[2]);
|
||||
b3LoadUrdfCommandSetStartOrientation(command,args.m_startOrientation[0]
|
||||
,args.m_startOrientation[1]
|
||||
,args.m_startOrientation[2]
|
||||
,args.m_startOrientation[3]);
|
||||
args.m_startPosition[1],
|
||||
args.m_startPosition[2]);
|
||||
b3LoadUrdfCommandSetStartOrientation(command, args.m_startOrientation[0], args.m_startOrientation[1], args.m_startOrientation[2], args.m_startOrientation[3]);
|
||||
if (args.m_forceOverrideFixedBase)
|
||||
{
|
||||
b3LoadUrdfCommandSetUseFixedBase(command,true);
|
||||
b3LoadUrdfCommandSetUseFixedBase(command, true);
|
||||
}
|
||||
b3LoadUrdfCommandSetUseMultiBody(command, args.m_useMultiBody);
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
|
||||
statusType = b3GetStatusType(statusHandle);
|
||||
|
||||
b3Assert(statusType==CMD_URDF_LOADING_COMPLETED);
|
||||
if (statusType==CMD_URDF_LOADING_COMPLETED)
|
||||
b3Assert(statusType == CMD_URDF_LOADING_COMPLETED);
|
||||
if (statusType == CMD_URDF_LOADING_COMPLETED)
|
||||
{
|
||||
robotUniqueId = b3GetStatusBodyIndex(statusHandle);
|
||||
}
|
||||
@@ -329,12 +300,12 @@ bool b3RobotSimulatorClientAPI::loadMJCF(const std::string& fileName, b3RobotSim
|
||||
b3Warning("Couldn't load .mjcf file.");
|
||||
return false;
|
||||
}
|
||||
int numBodies = b3GetStatusBodyIndices(statusHandle, 0,0);
|
||||
int numBodies = b3GetStatusBodyIndices(statusHandle, 0, 0);
|
||||
if (numBodies)
|
||||
{
|
||||
results.m_uniqueObjectIds.resize(numBodies);
|
||||
int numBodies;
|
||||
numBodies = b3GetStatusBodyIndices(statusHandle, &results.m_uniqueObjectIds[0],results.m_uniqueObjectIds.size());
|
||||
numBodies = b3GetStatusBodyIndices(statusHandle, &results.m_uniqueObjectIds[0], results.m_uniqueObjectIds.size());
|
||||
}
|
||||
|
||||
return true;
|
||||
@@ -359,12 +330,12 @@ bool b3RobotSimulatorClientAPI::loadBullet(const std::string& fileName, b3RobotS
|
||||
{
|
||||
return false;
|
||||
}
|
||||
int numBodies = b3GetStatusBodyIndices(statusHandle, 0,0);
|
||||
int numBodies = b3GetStatusBodyIndices(statusHandle, 0, 0);
|
||||
if (numBodies)
|
||||
{
|
||||
results.m_uniqueObjectIds.resize(numBodies);
|
||||
int numBodies;
|
||||
numBodies = b3GetStatusBodyIndices(statusHandle, &results.m_uniqueObjectIds[0],results.m_uniqueObjectIds.size());
|
||||
numBodies = b3GetStatusBodyIndices(statusHandle, &results.m_uniqueObjectIds[0], results.m_uniqueObjectIds.size());
|
||||
}
|
||||
|
||||
return true;
|
||||
@@ -383,19 +354,18 @@ bool b3RobotSimulatorClientAPI::loadSDF(const std::string& fileName, b3RobotSimu
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
int statusType;
|
||||
b3SharedMemoryCommandHandle command = b3LoadSdfCommandInit(m_data->m_physicsClientHandle, fileName.c_str());
|
||||
b3LoadSdfCommandSetUseMultiBody(command, args.m_useMultiBody);
|
||||
b3LoadSdfCommandSetUseMultiBody(command, args.m_useMultiBody);
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
|
||||
statusType = b3GetStatusType(statusHandle);
|
||||
b3Assert(statusType == CMD_SDF_LOADING_COMPLETED);
|
||||
if (statusType == CMD_SDF_LOADING_COMPLETED)
|
||||
{
|
||||
int numBodies = b3GetStatusBodyIndices(statusHandle, 0,0);
|
||||
int numBodies = b3GetStatusBodyIndices(statusHandle, 0, 0);
|
||||
if (numBodies)
|
||||
{
|
||||
results.m_uniqueObjectIds.resize(numBodies);
|
||||
int numBodies;
|
||||
numBodies = b3GetStatusBodyIndices(statusHandle, &results.m_uniqueObjectIds[0],results.m_uniqueObjectIds.size());
|
||||
|
||||
numBodies = b3GetStatusBodyIndices(statusHandle, &results.m_uniqueObjectIds[0], results.m_uniqueObjectIds.size());
|
||||
}
|
||||
statusOk = true;
|
||||
}
|
||||
@@ -411,7 +381,7 @@ bool b3RobotSimulatorClientAPI::getBodyInfo(int bodyUniqueId, struct b3BodyInfo*
|
||||
return false;
|
||||
}
|
||||
|
||||
int result = b3GetBodyInfo(m_data->m_physicsClientHandle, bodyUniqueId, bodyInfo);
|
||||
int result = b3GetBodyInfo(m_data->m_physicsClientHandle, bodyUniqueId, bodyInfo);
|
||||
return (result != 0);
|
||||
}
|
||||
|
||||
@@ -431,7 +401,8 @@ bool b3RobotSimulatorClientAPI::getBasePositionAndOrientation(int bodyUniqueId,
|
||||
const int status_type = b3GetStatusType(status_handle);
|
||||
const double* actualStateQ;
|
||||
|
||||
if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED) {
|
||||
if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
@@ -466,14 +437,72 @@ bool b3RobotSimulatorClientAPI::resetBasePositionAndOrientation(int bodyUniqueId
|
||||
|
||||
b3CreatePoseCommandSetBasePosition(commandHandle, basePosition[0], basePosition[1], basePosition[2]);
|
||||
b3CreatePoseCommandSetBaseOrientation(commandHandle, baseOrientation[0], baseOrientation[1],
|
||||
baseOrientation[2], baseOrientation[3]);
|
||||
baseOrientation[2], baseOrientation[3]);
|
||||
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
|
||||
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool b3RobotSimulatorClientAPI::getBaseVelocity(int bodyUniqueId, b3Vector3& baseLinearVelocity, b3Vector3& baseAngularVelocity) const
|
||||
{
|
||||
if (!isConnected())
|
||||
{
|
||||
b3Warning("Not connected");
|
||||
return false;
|
||||
}
|
||||
|
||||
b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(m_data->m_physicsClientHandle, bodyUniqueId);
|
||||
b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
|
||||
|
||||
const int status_type = b3GetStatusType(statusHandle);
|
||||
const double* actualStateQdot;
|
||||
|
||||
if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
b3GetStatusActualState(statusHandle, 0 /* body_unique_id */,
|
||||
0 /* num_degree_of_freedom_q */, 0 /* num_degree_of_freedom_u */,
|
||||
0 /*root_local_inertial_frame*/, 0,
|
||||
&actualStateQdot, 0 /* joint_reaction_forces */);
|
||||
|
||||
baseLinearVelocity[0] = actualStateQdot[0];
|
||||
baseLinearVelocity[1] = actualStateQdot[1];
|
||||
baseLinearVelocity[2] = actualStateQdot[2];
|
||||
|
||||
baseAngularVelocity[0] = actualStateQdot[3];
|
||||
baseAngularVelocity[1] = actualStateQdot[4];
|
||||
baseAngularVelocity[2] = actualStateQdot[5];
|
||||
return true;
|
||||
}
|
||||
|
||||
bool b3RobotSimulatorClientAPI::resetBaseVelocity(int bodyUniqueId, const b3Vector3& linearVelocity, const b3Vector3& angularVelocity) const
|
||||
{
|
||||
if (!isConnected())
|
||||
{
|
||||
b3Warning("Not connected");
|
||||
return false;
|
||||
}
|
||||
|
||||
b3SharedMemoryCommandHandle commandHandle;
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
|
||||
commandHandle = b3CreatePoseCommandInit(m_data->m_physicsClientHandle, bodyUniqueId);
|
||||
|
||||
b3Vector3DoubleData linVelDouble;
|
||||
linearVelocity.serializeDouble(linVelDouble);
|
||||
b3CreatePoseCommandSetBaseLinearVelocity(commandHandle, linVelDouble.m_floats);
|
||||
|
||||
b3Vector3DoubleData angVelDouble;
|
||||
angularVelocity.serializeDouble(angVelDouble);
|
||||
b3CreatePoseCommandSetBaseAngularVelocity(commandHandle, angVelDouble.m_floats);
|
||||
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
|
||||
return true;
|
||||
}
|
||||
|
||||
void b3RobotSimulatorClientAPI::setRealTimeSimulation(bool enableRealTimeSimulation)
|
||||
{
|
||||
@@ -500,10 +529,9 @@ int b3RobotSimulatorClientAPI::getNumJoints(int bodyUniqueId) const
|
||||
b3Warning("Not connected");
|
||||
return false;
|
||||
}
|
||||
return b3GetNumJoints(m_data->m_physicsClientHandle,bodyUniqueId);
|
||||
return b3GetNumJoints(m_data->m_physicsClientHandle, bodyUniqueId);
|
||||
}
|
||||
|
||||
|
||||
bool b3RobotSimulatorClientAPI::getJointInfo(int bodyUniqueId, int jointIndex, b3JointInfo* jointInfo)
|
||||
{
|
||||
if (!isConnected())
|
||||
@@ -511,7 +539,7 @@ bool b3RobotSimulatorClientAPI::getJointInfo(int bodyUniqueId, int jointIndex, b
|
||||
b3Warning("Not connected");
|
||||
return false;
|
||||
}
|
||||
return (b3GetJointInfo(m_data->m_physicsClientHandle,bodyUniqueId, jointIndex,jointInfo)!=0);
|
||||
return (b3GetJointInfo(m_data->m_physicsClientHandle, bodyUniqueId, jointIndex, jointInfo) != 0);
|
||||
}
|
||||
|
||||
void b3RobotSimulatorClientAPI::createConstraint(int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, b3JointInfo* jointInfo)
|
||||
@@ -521,55 +549,55 @@ void b3RobotSimulatorClientAPI::createConstraint(int parentBodyIndex, int parent
|
||||
b3Warning("Not connected");
|
||||
return;
|
||||
}
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
b3Assert(b3CanSubmitCommand(m_data->m_physicsClientHandle));
|
||||
if (b3CanSubmitCommand(m_data->m_physicsClientHandle))
|
||||
{
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, b3InitCreateUserConstraintCommand(m_data->m_physicsClientHandle, parentBodyIndex, parentJointIndex, childBodyIndex, childJointIndex, jointInfo));
|
||||
}
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
b3Assert(b3CanSubmitCommand(m_data->m_physicsClientHandle));
|
||||
if (b3CanSubmitCommand(m_data->m_physicsClientHandle))
|
||||
{
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, b3InitCreateUserConstraintCommand(m_data->m_physicsClientHandle, parentBodyIndex, parentJointIndex, childBodyIndex, childJointIndex, jointInfo));
|
||||
}
|
||||
}
|
||||
|
||||
bool b3RobotSimulatorClientAPI::getJointState(int bodyUniqueId, int jointIndex, struct b3JointSensorState *state)
|
||||
bool b3RobotSimulatorClientAPI::getJointState(int bodyUniqueId, int jointIndex, struct b3JointSensorState* state)
|
||||
{
|
||||
if (!isConnected())
|
||||
{
|
||||
b3Warning("Not connected");
|
||||
return false;
|
||||
}
|
||||
b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(m_data->m_physicsClientHandle,bodyUniqueId);
|
||||
b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
|
||||
int statusType = b3GetStatusType(statusHandle);
|
||||
if (statusType != CMD_ACTUAL_STATE_UPDATE_COMPLETED)
|
||||
b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(m_data->m_physicsClientHandle, bodyUniqueId);
|
||||
b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
|
||||
int statusType = b3GetStatusType(statusHandle);
|
||||
if (statusType == CMD_ACTUAL_STATE_UPDATE_COMPLETED)
|
||||
{
|
||||
if (b3GetJointState(m_data->m_physicsClientHandle, statusHandle, jointIndex, state))
|
||||
{
|
||||
return true;
|
||||
}
|
||||
if (b3GetJointState(m_data->m_physicsClientHandle, statusHandle, jointIndex, state))
|
||||
{
|
||||
return true;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
return false;
|
||||
}
|
||||
|
||||
bool b3RobotSimulatorClientAPI::resetJointState(int bodyUniqueId, int jointIndex, double targetValue)
|
||||
{
|
||||
b3SharedMemoryCommandHandle commandHandle;
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
int numJoints;
|
||||
b3SharedMemoryCommandHandle commandHandle;
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
int numJoints;
|
||||
|
||||
numJoints = b3GetNumJoints(m_data->m_physicsClientHandle, bodyUniqueId);
|
||||
if ((jointIndex >= numJoints) || (jointIndex < 0)) {
|
||||
return false;
|
||||
}
|
||||
numJoints = b3GetNumJoints(m_data->m_physicsClientHandle, bodyUniqueId);
|
||||
if ((jointIndex >= numJoints) || (jointIndex < 0))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
commandHandle = b3CreatePoseCommandInit(m_data->m_physicsClientHandle, bodyUniqueId);
|
||||
commandHandle = b3CreatePoseCommandInit(m_data->m_physicsClientHandle, bodyUniqueId);
|
||||
|
||||
b3CreatePoseCommandSetJointPosition(m_data->m_physicsClientHandle, commandHandle, jointIndex,
|
||||
targetValue);
|
||||
b3CreatePoseCommandSetJointPosition(m_data->m_physicsClientHandle, commandHandle, jointIndex,
|
||||
targetValue);
|
||||
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
|
||||
return false;
|
||||
}
|
||||
|
||||
|
||||
void b3RobotSimulatorClientAPI::setJointMotorControl(int bodyUniqueId, int jointIndex, const b3RobotSimulatorJointMotorArgs& args)
|
||||
{
|
||||
if (!isConnected())
|
||||
@@ -582,39 +610,39 @@ void b3RobotSimulatorClientAPI::setJointMotorControl(int bodyUniqueId, int joint
|
||||
{
|
||||
case CONTROL_MODE_VELOCITY:
|
||||
{
|
||||
b3SharedMemoryCommandHandle command = b3JointControlCommandInit2( m_data->m_physicsClientHandle, bodyUniqueId, CONTROL_MODE_VELOCITY);
|
||||
b3SharedMemoryCommandHandle command = b3JointControlCommandInit2(m_data->m_physicsClientHandle, bodyUniqueId, CONTROL_MODE_VELOCITY);
|
||||
b3JointInfo jointInfo;
|
||||
b3GetJointInfo(m_data->m_physicsClientHandle, bodyUniqueId, jointIndex, &jointInfo);
|
||||
int uIndex = jointInfo.m_uIndex;
|
||||
b3JointControlSetKd(command,uIndex,args.m_kd);
|
||||
b3JointControlSetDesiredVelocity(command,uIndex,args.m_targetVelocity);
|
||||
b3JointControlSetMaximumForce(command,uIndex,args.m_maxTorqueValue);
|
||||
b3JointControlSetKd(command, uIndex, args.m_kd);
|
||||
b3JointControlSetDesiredVelocity(command, uIndex, args.m_targetVelocity);
|
||||
b3JointControlSetMaximumForce(command, uIndex, args.m_maxTorqueValue);
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
|
||||
break;
|
||||
}
|
||||
case CONTROL_MODE_POSITION_VELOCITY_PD:
|
||||
{
|
||||
b3SharedMemoryCommandHandle command = b3JointControlCommandInit2( m_data->m_physicsClientHandle, bodyUniqueId, CONTROL_MODE_POSITION_VELOCITY_PD);
|
||||
b3SharedMemoryCommandHandle command = b3JointControlCommandInit2(m_data->m_physicsClientHandle, bodyUniqueId, CONTROL_MODE_POSITION_VELOCITY_PD);
|
||||
b3JointInfo jointInfo;
|
||||
b3GetJointInfo(m_data->m_physicsClientHandle, bodyUniqueId, jointIndex, &jointInfo);
|
||||
int uIndex = jointInfo.m_uIndex;
|
||||
int qIndex = jointInfo.m_qIndex;
|
||||
|
||||
b3JointControlSetDesiredPosition(command,qIndex,args.m_targetPosition);
|
||||
b3JointControlSetKp(command,uIndex,args.m_kp);
|
||||
b3JointControlSetDesiredVelocity(command,uIndex,args.m_targetVelocity);
|
||||
b3JointControlSetKd(command,uIndex,args.m_kd);
|
||||
b3JointControlSetMaximumForce(command,uIndex,args.m_maxTorqueValue);
|
||||
b3JointControlSetDesiredPosition(command, qIndex, args.m_targetPosition);
|
||||
b3JointControlSetKp(command, uIndex, args.m_kp);
|
||||
b3JointControlSetDesiredVelocity(command, uIndex, args.m_targetVelocity);
|
||||
b3JointControlSetKd(command, uIndex, args.m_kd);
|
||||
b3JointControlSetMaximumForce(command, uIndex, args.m_maxTorqueValue);
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
|
||||
break;
|
||||
}
|
||||
case CONTROL_MODE_TORQUE:
|
||||
{
|
||||
b3SharedMemoryCommandHandle command = b3JointControlCommandInit2( m_data->m_physicsClientHandle, bodyUniqueId, CONTROL_MODE_TORQUE);
|
||||
b3SharedMemoryCommandHandle command = b3JointControlCommandInit2(m_data->m_physicsClientHandle, bodyUniqueId, CONTROL_MODE_TORQUE);
|
||||
b3JointInfo jointInfo;
|
||||
b3GetJointInfo(m_data->m_physicsClientHandle, bodyUniqueId, jointIndex, &jointInfo);
|
||||
int uIndex = jointInfo.m_uIndex;
|
||||
b3JointControlSetDesiredForceTorque(command,uIndex,args.m_maxTorqueValue);
|
||||
b3JointControlSetDesiredForceTorque(command, uIndex, args.m_maxTorqueValue);
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
|
||||
break;
|
||||
}
|
||||
@@ -625,7 +653,6 @@ void b3RobotSimulatorClientAPI::setJointMotorControl(int bodyUniqueId, int joint
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void b3RobotSimulatorClientAPI::setNumSolverIterations(int numIterations)
|
||||
{
|
||||
if (!isConnected())
|
||||
@@ -633,12 +660,11 @@ void b3RobotSimulatorClientAPI::setNumSolverIterations(int numIterations)
|
||||
b3Warning("Not connected");
|
||||
return;
|
||||
}
|
||||
b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle);
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
b3PhysicsParamSetNumSolverIterations(command, numIterations);
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
|
||||
b3Assert(b3GetStatusType(statusHandle)==CMD_CLIENT_COMMAND_COMPLETED);
|
||||
|
||||
b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle);
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
b3PhysicsParamSetNumSolverIterations(command, numIterations);
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
|
||||
b3Assert(b3GetStatusType(statusHandle) == CMD_CLIENT_COMMAND_COMPLETED);
|
||||
}
|
||||
|
||||
void b3RobotSimulatorClientAPI::setTimeStep(double timeStepInSeconds)
|
||||
@@ -654,10 +680,8 @@ void b3RobotSimulatorClientAPI::setTimeStep(double timeStepInSeconds)
|
||||
int ret;
|
||||
ret = b3PhysicsParamSetTimeStep(command, timeStepInSeconds);
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
|
||||
|
||||
}
|
||||
|
||||
|
||||
void b3RobotSimulatorClientAPI::setNumSimulationSubSteps(int numSubSteps)
|
||||
{
|
||||
if (!isConnected())
|
||||
@@ -665,14 +689,13 @@ void b3RobotSimulatorClientAPI::setNumSimulationSubSteps(int numSubSteps)
|
||||
b3Warning("Not connected");
|
||||
return;
|
||||
}
|
||||
b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle);
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
b3PhysicsParamSetNumSubSteps(command, numSubSteps);
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
|
||||
b3Assert(b3GetStatusType(statusHandle)==CMD_CLIENT_COMMAND_COMPLETED);
|
||||
b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle);
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
b3PhysicsParamSetNumSubSteps(command, numSubSteps);
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
|
||||
b3Assert(b3GetStatusType(statusHandle) == CMD_CLIENT_COMMAND_COMPLETED);
|
||||
}
|
||||
|
||||
|
||||
bool b3RobotSimulatorClientAPI::calculateInverseKinematics(const struct b3RobotSimulatorInverseKinematicArgs& args, struct b3RobotSimulatorInverseKinematicsResults& results)
|
||||
{
|
||||
if (!isConnected())
|
||||
@@ -680,55 +703,52 @@ bool b3RobotSimulatorClientAPI::calculateInverseKinematics(const struct b3RobotS
|
||||
b3Warning("Not connected");
|
||||
return false;
|
||||
}
|
||||
b3Assert(args.m_endEffectorLinkIndex>=0);
|
||||
b3Assert(args.m_bodyUniqueId>=0);
|
||||
|
||||
b3Assert(args.m_endEffectorLinkIndex >= 0);
|
||||
b3Assert(args.m_bodyUniqueId >= 0);
|
||||
|
||||
b3SharedMemoryCommandHandle command = b3CalculateInverseKinematicsCommandInit(m_data->m_physicsClientHandle,args.m_bodyUniqueId);
|
||||
b3SharedMemoryCommandHandle command = b3CalculateInverseKinematicsCommandInit(m_data->m_physicsClientHandle, args.m_bodyUniqueId);
|
||||
if ((args.m_flags & B3_HAS_IK_TARGET_ORIENTATION) && (args.m_flags & B3_HAS_NULL_SPACE_VELOCITY))
|
||||
{
|
||||
b3CalculateInverseKinematicsPosOrnWithNullSpaceVel(command, args.m_numDegreeOfFreedom, args.m_endEffectorLinkIndex, args.m_endEffectorTargetPosition, args.m_endEffectorTargetOrientation, &args.m_lowerLimits[0], &args.m_upperLimits[0], &args.m_jointRanges[0], &args.m_restPoses[0]);
|
||||
} else if (args.m_flags & B3_HAS_IK_TARGET_ORIENTATION)
|
||||
{
|
||||
b3CalculateInverseKinematicsAddTargetPositionWithOrientation(command,args.m_endEffectorLinkIndex,args.m_endEffectorTargetPosition,args.m_endEffectorTargetOrientation);
|
||||
} else if (args.m_flags & B3_HAS_NULL_SPACE_VELOCITY)
|
||||
{
|
||||
b3CalculateInverseKinematicsPosWithNullSpaceVel(command, args.m_numDegreeOfFreedom, args.m_endEffectorLinkIndex, args.m_endEffectorTargetPosition, &args.m_lowerLimits[0], &args.m_upperLimits[0], &args.m_jointRanges[0], &args.m_restPoses[0]);
|
||||
} else
|
||||
{
|
||||
b3CalculateInverseKinematicsAddTargetPurePosition(command,args.m_endEffectorLinkIndex,args.m_endEffectorTargetPosition);
|
||||
b3CalculateInverseKinematicsPosOrnWithNullSpaceVel(command, args.m_numDegreeOfFreedom, args.m_endEffectorLinkIndex, args.m_endEffectorTargetPosition, args.m_endEffectorTargetOrientation, &args.m_lowerLimits[0], &args.m_upperLimits[0], &args.m_jointRanges[0], &args.m_restPoses[0]);
|
||||
}
|
||||
else if (args.m_flags & B3_HAS_IK_TARGET_ORIENTATION)
|
||||
{
|
||||
b3CalculateInverseKinematicsAddTargetPositionWithOrientation(command, args.m_endEffectorLinkIndex, args.m_endEffectorTargetPosition, args.m_endEffectorTargetOrientation);
|
||||
}
|
||||
else if (args.m_flags & B3_HAS_NULL_SPACE_VELOCITY)
|
||||
{
|
||||
b3CalculateInverseKinematicsPosWithNullSpaceVel(command, args.m_numDegreeOfFreedom, args.m_endEffectorLinkIndex, args.m_endEffectorTargetPosition, &args.m_lowerLimits[0], &args.m_upperLimits[0], &args.m_jointRanges[0], &args.m_restPoses[0]);
|
||||
}
|
||||
else
|
||||
{
|
||||
b3CalculateInverseKinematicsAddTargetPurePosition(command, args.m_endEffectorLinkIndex, args.m_endEffectorTargetPosition);
|
||||
}
|
||||
|
||||
if (args.m_flags & B3_HAS_JOINT_DAMPING)
|
||||
{
|
||||
b3CalculateInverseKinematicsSetJointDamping(command, args.m_numDegreeOfFreedom, &args.m_jointDamping[0]);
|
||||
}
|
||||
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
if (args.m_flags & B3_HAS_JOINT_DAMPING)
|
||||
{
|
||||
b3CalculateInverseKinematicsSetJointDamping(command, args.m_numDegreeOfFreedom, &args.m_jointDamping[0]);
|
||||
}
|
||||
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
|
||||
|
||||
|
||||
int numPos = 0;
|
||||
|
||||
int numPos=0;
|
||||
|
||||
bool result = b3GetStatusInverseKinematicsJointPositions(statusHandle,
|
||||
&results.m_bodyUniqueId,
|
||||
&numPos,
|
||||
0)!=0;
|
||||
if (result && numPos)
|
||||
{
|
||||
results.m_calculatedJointPositions.resize(numPos);
|
||||
result = b3GetStatusInverseKinematicsJointPositions(statusHandle,
|
||||
&results.m_bodyUniqueId,
|
||||
&numPos,
|
||||
&results.m_calculatedJointPositions[0])!=0;
|
||||
|
||||
}
|
||||
&results.m_bodyUniqueId,
|
||||
&numPos,
|
||||
0) != 0;
|
||||
if (result && numPos)
|
||||
{
|
||||
results.m_calculatedJointPositions.resize(numPos);
|
||||
result = b3GetStatusInverseKinematicsJointPositions(statusHandle,
|
||||
&results.m_bodyUniqueId,
|
||||
&numPos,
|
||||
&results.m_calculatedJointPositions[0]) != 0;
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
|
||||
|
||||
bool b3RobotSimulatorClientAPI::getBodyJacobian(int bodyUniqueId, int linkIndex, const double* localPosition, const double* jointPositions, const double* jointVelocities, const double* jointAccelerations, double* linearJacobian, double* angularJacobian)
|
||||
{
|
||||
if (!isConnected())
|
||||
@@ -736,12 +756,12 @@ bool b3RobotSimulatorClientAPI::getBodyJacobian(int bodyUniqueId, int linkIndex,
|
||||
b3Warning("Not connected");
|
||||
return false;
|
||||
}
|
||||
b3SharedMemoryCommandHandle command = b3CalculateJacobianCommandInit(m_data->m_physicsClientHandle, bodyUniqueId, linkIndex, localPosition, jointPositions, jointVelocities, jointAccelerations);
|
||||
b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
|
||||
|
||||
if (b3GetStatusType(statusHandle) == CMD_CALCULATED_JACOBIAN_COMPLETED)
|
||||
{
|
||||
b3GetStatusJacobian(statusHandle, linearJacobian, angularJacobian);
|
||||
b3SharedMemoryCommandHandle command = b3CalculateJacobianCommandInit(m_data->m_physicsClientHandle, bodyUniqueId, linkIndex, localPosition, jointPositions, jointVelocities, jointAccelerations);
|
||||
b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
|
||||
|
||||
if (b3GetStatusType(statusHandle) == CMD_CALCULATED_JACOBIAN_COMPLETED)
|
||||
{
|
||||
b3GetStatusJacobian(statusHandle, linearJacobian, angularJacobian);
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
@@ -754,14 +774,14 @@ bool b3RobotSimulatorClientAPI::getLinkState(int bodyUniqueId, int linkIndex, b3
|
||||
b3Warning("Not connected");
|
||||
return false;
|
||||
}
|
||||
b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(m_data->m_physicsClientHandle,bodyUniqueId);
|
||||
b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
|
||||
|
||||
if (b3GetStatusType(statusHandle) == CMD_ACTUAL_STATE_UPDATE_COMPLETED)
|
||||
{
|
||||
b3GetLinkState(m_data->m_physicsClientHandle, statusHandle, linkIndex, linkState);
|
||||
b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(m_data->m_physicsClientHandle, bodyUniqueId);
|
||||
b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
|
||||
|
||||
if (b3GetStatusType(statusHandle) == CMD_ACTUAL_STATE_UPDATE_COMPLETED)
|
||||
{
|
||||
b3GetLinkState(m_data->m_physicsClientHandle, statusHandle, linkIndex, linkState);
|
||||
return true;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
@@ -787,9 +807,9 @@ void b3RobotSimulatorClientAPI::getVREvents(b3VREventsData* vrEventsData)
|
||||
return;
|
||||
}
|
||||
|
||||
b3SharedMemoryCommandHandle commandHandle = b3RequestVREventsCommandInit(m_data->m_physicsClientHandle);
|
||||
b3SharedMemoryCommandHandle commandHandle = b3RequestVREventsCommandInit(m_data->m_physicsClientHandle);
|
||||
b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
|
||||
b3GetVREventsData(m_data->m_physicsClientHandle, vrEventsData);
|
||||
b3GetVREventsData(m_data->m_physicsClientHandle, vrEventsData);
|
||||
}
|
||||
|
||||
void b3RobotSimulatorClientAPI::getKeyboardEvents(b3KeyboardEventsData* keyboardEventsData)
|
||||
@@ -802,32 +822,35 @@ void b3RobotSimulatorClientAPI::getKeyboardEvents(b3KeyboardEventsData* keyboard
|
||||
return;
|
||||
}
|
||||
|
||||
b3SharedMemoryCommandHandle commandHandle = b3RequestKeyboardEventsCommandInit(m_data->m_physicsClientHandle);
|
||||
b3SharedMemoryCommandHandle commandHandle = b3RequestKeyboardEventsCommandInit(m_data->m_physicsClientHandle);
|
||||
b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
|
||||
b3GetKeyboardEventsData(m_data->m_physicsClientHandle,keyboardEventsData);
|
||||
|
||||
b3GetKeyboardEventsData(m_data->m_physicsClientHandle, keyboardEventsData);
|
||||
}
|
||||
|
||||
|
||||
int b3RobotSimulatorClientAPI::startStateLogging(b3StateLoggingType loggingType, const std::string& fileName, const b3AlignedObjectArray<int>& objectUniqueIds)
|
||||
int b3RobotSimulatorClientAPI::startStateLogging(b3StateLoggingType loggingType, const std::string& fileName, const b3AlignedObjectArray<int>& objectUniqueIds, int maxLogDof)
|
||||
{
|
||||
int loggingUniqueId = -1;
|
||||
b3SharedMemoryCommandHandle commandHandle;
|
||||
commandHandle = b3StateLoggingCommandInit(m_data->m_physicsClientHandle);
|
||||
|
||||
b3StateLoggingStart(commandHandle,loggingType,fileName.c_str());
|
||||
|
||||
for ( int i=0;i<objectUniqueIds.size();i++)
|
||||
b3StateLoggingStart(commandHandle, loggingType, fileName.c_str());
|
||||
|
||||
for (int i = 0; i < objectUniqueIds.size(); i++)
|
||||
{
|
||||
int objectUid = objectUniqueIds[i];
|
||||
b3StateLoggingAddLoggingObjectUniqueId(commandHandle,objectUid);
|
||||
b3StateLoggingAddLoggingObjectUniqueId(commandHandle, objectUid);
|
||||
}
|
||||
|
||||
if (maxLogDof > 0)
|
||||
{
|
||||
b3StateLoggingSetMaxLogDof(commandHandle, maxLogDof);
|
||||
}
|
||||
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
int statusType;
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
|
||||
statusType = b3GetStatusType(statusHandle);
|
||||
if (statusType==CMD_STATE_LOGGING_START_COMPLETED)
|
||||
if (statusType == CMD_STATE_LOGGING_START_COMPLETED)
|
||||
{
|
||||
loggingUniqueId = b3GetStatusLoggingUniqueId(statusHandle);
|
||||
}
|
||||
@@ -843,3 +866,17 @@ void b3RobotSimulatorClientAPI::stopStateLogging(int stateLoggerUniqueId)
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
|
||||
}
|
||||
|
||||
void b3RobotSimulatorClientAPI::resetDebugVisualizerCamera(double cameraDistance, double cameraPitch, double cameraYaw, const b3Vector3& targetPos)
|
||||
{
|
||||
b3SharedMemoryCommandHandle commandHandle = b3InitConfigureOpenGLVisualizer(m_data->m_physicsClientHandle);
|
||||
if (commandHandle)
|
||||
{
|
||||
if ((cameraDistance >= 0))
|
||||
{
|
||||
b3Vector3FloatData camTargetPos;
|
||||
targetPos.serializeFloat(camTargetPos);
|
||||
b3ConfigureOpenGLVisualizerSetViewMatrix(commandHandle, cameraDistance, cameraPitch, cameraYaw, camTargetPos.m_floats);
|
||||
}
|
||||
b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -9,23 +9,18 @@
|
||||
|
||||
#include <string>
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
struct b3RobotSimulatorLoadUrdfFileArgs
|
||||
{
|
||||
b3Vector3 m_startPosition;
|
||||
b3Quaternion m_startOrientation;
|
||||
bool m_forceOverrideFixedBase;
|
||||
bool m_useMultiBody;
|
||||
bool m_useMultiBody;
|
||||
|
||||
b3RobotSimulatorLoadUrdfFileArgs()
|
||||
:
|
||||
m_startPosition(b3MakeVector3(0,0,0)),
|
||||
m_startOrientation(b3Quaternion(0,0,0,1)),
|
||||
m_forceOverrideFixedBase(false),
|
||||
m_useMultiBody(true)
|
||||
: m_startPosition(b3MakeVector3(0, 0, 0)),
|
||||
m_startOrientation(b3Quaternion(0, 0, 0, 1)),
|
||||
m_forceOverrideFixedBase(false),
|
||||
m_useMultiBody(true)
|
||||
{
|
||||
}
|
||||
};
|
||||
@@ -33,17 +28,15 @@ struct b3RobotSimulatorLoadUrdfFileArgs
|
||||
struct b3RobotSimulatorLoadSdfFileArgs
|
||||
{
|
||||
bool m_forceOverrideFixedBase;
|
||||
bool m_useMultiBody;
|
||||
bool m_useMultiBody;
|
||||
|
||||
b3RobotSimulatorLoadSdfFileArgs()
|
||||
:
|
||||
m_forceOverrideFixedBase(false),
|
||||
m_useMultiBody(true)
|
||||
: m_forceOverrideFixedBase(false),
|
||||
m_useMultiBody(true)
|
||||
{
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
struct b3RobotSimulatorLoadFileResults
|
||||
{
|
||||
b3AlignedObjectArray<int> m_uniqueObjectIds;
|
||||
@@ -55,7 +48,7 @@ struct b3RobotSimulatorLoadFileResults
|
||||
struct b3RobotSimulatorJointMotorArgs
|
||||
{
|
||||
int m_controlMode;
|
||||
|
||||
|
||||
double m_targetPosition;
|
||||
double m_kp;
|
||||
|
||||
@@ -65,52 +58,52 @@ struct b3RobotSimulatorJointMotorArgs
|
||||
double m_maxTorqueValue;
|
||||
|
||||
b3RobotSimulatorJointMotorArgs(int controlMode)
|
||||
:m_controlMode(controlMode),
|
||||
m_targetPosition(0),
|
||||
m_kp(0.1),
|
||||
m_targetVelocity(0),
|
||||
m_kd(0.9),
|
||||
m_maxTorqueValue(1000)
|
||||
: m_controlMode(controlMode),
|
||||
m_targetPosition(0),
|
||||
m_kp(0.1),
|
||||
m_targetVelocity(0),
|
||||
m_kd(0.9),
|
||||
m_maxTorqueValue(1000)
|
||||
{
|
||||
}
|
||||
};
|
||||
|
||||
enum b3RobotSimulatorInverseKinematicsFlags
|
||||
{
|
||||
B3_HAS_IK_TARGET_ORIENTATION=1,
|
||||
B3_HAS_NULL_SPACE_VELOCITY=2,
|
||||
B3_HAS_JOINT_DAMPING=4,
|
||||
B3_HAS_IK_TARGET_ORIENTATION = 1,
|
||||
B3_HAS_NULL_SPACE_VELOCITY = 2,
|
||||
B3_HAS_JOINT_DAMPING = 4,
|
||||
};
|
||||
|
||||
struct b3RobotSimulatorInverseKinematicArgs
|
||||
{
|
||||
int m_bodyUniqueId;
|
||||
// double* m_currentJointPositions;
|
||||
// int m_numPositions;
|
||||
// double* m_currentJointPositions;
|
||||
// int m_numPositions;
|
||||
double m_endEffectorTargetPosition[3];
|
||||
double m_endEffectorTargetOrientation[4];
|
||||
int m_endEffectorLinkIndex;
|
||||
int m_endEffectorLinkIndex;
|
||||
int m_flags;
|
||||
int m_numDegreeOfFreedom;
|
||||
b3AlignedObjectArray<double> m_lowerLimits;
|
||||
b3AlignedObjectArray<double> m_upperLimits;
|
||||
b3AlignedObjectArray<double> m_jointRanges;
|
||||
b3AlignedObjectArray<double> m_restPoses;
|
||||
b3AlignedObjectArray<double> m_jointDamping;
|
||||
int m_numDegreeOfFreedom;
|
||||
b3AlignedObjectArray<double> m_lowerLimits;
|
||||
b3AlignedObjectArray<double> m_upperLimits;
|
||||
b3AlignedObjectArray<double> m_jointRanges;
|
||||
b3AlignedObjectArray<double> m_restPoses;
|
||||
b3AlignedObjectArray<double> m_jointDamping;
|
||||
|
||||
b3RobotSimulatorInverseKinematicArgs()
|
||||
:m_bodyUniqueId(-1),
|
||||
m_endEffectorLinkIndex(-1),
|
||||
m_flags(0)
|
||||
: m_bodyUniqueId(-1),
|
||||
m_endEffectorLinkIndex(-1),
|
||||
m_flags(0)
|
||||
{
|
||||
m_endEffectorTargetPosition[0]=0;
|
||||
m_endEffectorTargetPosition[1]=0;
|
||||
m_endEffectorTargetPosition[2]=0;
|
||||
m_endEffectorTargetPosition[0] = 0;
|
||||
m_endEffectorTargetPosition[1] = 0;
|
||||
m_endEffectorTargetPosition[2] = 0;
|
||||
|
||||
m_endEffectorTargetOrientation[0]=0;
|
||||
m_endEffectorTargetOrientation[1]=0;
|
||||
m_endEffectorTargetOrientation[2]=0;
|
||||
m_endEffectorTargetOrientation[3]=1;
|
||||
m_endEffectorTargetOrientation[0] = 0;
|
||||
m_endEffectorTargetOrientation[1] = 0;
|
||||
m_endEffectorTargetOrientation[2] = 0;
|
||||
m_endEffectorTargetOrientation[3] = 1;
|
||||
}
|
||||
};
|
||||
|
||||
@@ -120,24 +113,25 @@ struct b3RobotSimulatorInverseKinematicsResults
|
||||
b3AlignedObjectArray<double> m_calculatedJointPositions;
|
||||
};
|
||||
|
||||
|
||||
///The b3RobotSimulatorClientAPI is pretty much the C++ version of pybullet
|
||||
///as documented in the pybullet Quickstart Guide
|
||||
///https://docs.google.com/document/d/10sXEhzFRSnvFcl3XxNGhnD4N2SedqwdAvK3dsihxVUA
|
||||
class b3RobotSimulatorClientAPI
|
||||
{
|
||||
struct b3RobotSimulatorClientAPI_InternalData* m_data;
|
||||
|
||||
|
||||
public:
|
||||
b3RobotSimulatorClientAPI();
|
||||
virtual ~b3RobotSimulatorClientAPI();
|
||||
|
||||
bool connect(int mode, const std::string& hostName="localhost", int portOrKey=-1);
|
||||
|
||||
bool connect(int mode, const std::string& hostName = "localhost", int portOrKey = -1);
|
||||
|
||||
void disconnect();
|
||||
|
||||
bool isConnected() const;
|
||||
|
||||
void setTimeOut(double timeOutInSec);
|
||||
|
||||
void syncBodies();
|
||||
|
||||
void resetSimulation();
|
||||
@@ -145,8 +139,8 @@ public:
|
||||
b3Quaternion getQuaternionFromEuler(const b3Vector3& rollPitchYaw);
|
||||
b3Vector3 getEulerFromQuaternion(const b3Quaternion& quat);
|
||||
|
||||
int loadURDF(const std::string& fileName, const struct b3RobotSimulatorLoadUrdfFileArgs& args=b3RobotSimulatorLoadUrdfFileArgs());
|
||||
bool loadSDF(const std::string& fileName, b3RobotSimulatorLoadFileResults& results, const struct b3RobotSimulatorLoadSdfFileArgs& args=b3RobotSimulatorLoadSdfFileArgs());
|
||||
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 loadMJCF(const std::string& fileName, b3RobotSimulatorLoadFileResults& results);
|
||||
bool loadBullet(const std::string& fileName, b3RobotSimulatorLoadFileResults& results);
|
||||
|
||||
@@ -155,14 +149,17 @@ public:
|
||||
bool getBasePositionAndOrientation(int bodyUniqueId, b3Vector3& basePosition, b3Quaternion& baseOrientation) const;
|
||||
bool resetBasePositionAndOrientation(int bodyUniqueId, b3Vector3& basePosition, b3Quaternion& baseOrientation);
|
||||
|
||||
bool getBaseVelocity(int bodyUniqueId, b3Vector3& baseLinearVelocity, b3Vector3& baseAngularVelocity) const;
|
||||
bool resetBaseVelocity(int bodyUniqueId, const b3Vector3& linearVelocity, const b3Vector3& angularVelocity) const;
|
||||
|
||||
int getNumJoints(int bodyUniqueId) const;
|
||||
|
||||
bool getJointInfo(int bodyUniqueId, int jointIndex, b3JointInfo* jointInfo);
|
||||
|
||||
void createConstraint(int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, b3JointInfo* jointInfo);
|
||||
|
||||
bool getJointState(int bodyUniqueId, int jointIndex, struct b3JointSensorState *state);
|
||||
|
||||
void createConstraint(int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, b3JointInfo* jointInfo);
|
||||
|
||||
bool getJointState(int bodyUniqueId, int jointIndex, struct b3JointSensorState* state);
|
||||
|
||||
bool resetJointState(int bodyUniqueId, int jointIndex, double targetValue);
|
||||
|
||||
void setJointMotorControl(int bodyUniqueId, int jointIndex, const struct b3RobotSimulatorJointMotorArgs& args);
|
||||
@@ -170,29 +167,29 @@ public:
|
||||
void stepSimulation();
|
||||
|
||||
bool canSubmitCommand() const;
|
||||
|
||||
|
||||
void setRealTimeSimulation(bool enableRealTimeSimulation);
|
||||
|
||||
void setGravity(const b3Vector3& gravityAcceleration);
|
||||
|
||||
|
||||
void setTimeStep(double timeStepInSeconds);
|
||||
void setNumSimulationSubSteps(int numSubSteps);
|
||||
void setNumSimulationSubSteps(int numSubSteps);
|
||||
void setNumSolverIterations(int numIterations);
|
||||
|
||||
bool calculateInverseKinematics(const struct b3RobotSimulatorInverseKinematicArgs& args, struct b3RobotSimulatorInverseKinematicsResults& results);
|
||||
|
||||
bool getBodyJacobian(int bodyUniqueId, int linkIndex, const double* localPosition, const double* jointPositions, const double* jointVelocities, const double* jointAccelerations, double* linearJacobian, double* angularJacobian);
|
||||
|
||||
bool getBodyJacobian(int bodyUniqueId, int linkIndex, const double* localPosition, const double* jointPositions, const double* jointVelocities, const double* jointAccelerations, double* linearJacobian, double* angularJacobian);
|
||||
|
||||
bool getLinkState(int bodyUniqueId, int linkIndex, b3LinkState* linkState);
|
||||
|
||||
void configureDebugVisualizer(enum b3ConfigureDebugVisualizerEnum flag, int enable);
|
||||
void resetDebugVisualizerCamera(double cameraDistance, double cameraPitch, double cameraYaw, const b3Vector3& targetPos);
|
||||
|
||||
int startStateLogging(b3StateLoggingType loggingType, const std::string& fileName, const b3AlignedObjectArray<int>& objectUniqueIds);
|
||||
int startStateLogging(b3StateLoggingType loggingType, const std::string& fileName, const b3AlignedObjectArray<int>& objectUniqueIds=b3AlignedObjectArray<int>(), int maxLogDof = -1);
|
||||
void stopStateLogging(int stateLoggerUniqueId);
|
||||
|
||||
void getVREvents(b3VREventsData* vrEventsData);
|
||||
void getKeyboardEvents(b3KeyboardEventsData* keyboardEventsData);
|
||||
|
||||
};
|
||||
|
||||
#endif //B3_ROBOT_SIMULATOR_CLIENT_API_H
|
||||
#endif //B3_ROBOT_SIMULATOR_CLIENT_API_H
|
||||
|
||||
@@ -38,6 +38,8 @@ SET(SharedMemory_SRCS
|
||||
PhysicsServerCommandProcessor.h
|
||||
TinyRendererVisualShapeConverter.cpp
|
||||
TinyRendererVisualShapeConverter.h
|
||||
SharedMemoryCommands.h
|
||||
SharedMemoryPublic.h
|
||||
../TinyRenderer/geometry.cpp
|
||||
../TinyRenderer/model.cpp
|
||||
../TinyRenderer/tgaimage.cpp
|
||||
|
||||
@@ -914,7 +914,7 @@ b3SharedMemoryStatusHandle b3ProcessServerStatus(b3PhysicsClientHandle physClien
|
||||
int b3GetStatusType(b3SharedMemoryStatusHandle statusHandle)
|
||||
{
|
||||
const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle;
|
||||
b3Assert(status);
|
||||
//b3Assert(status);
|
||||
if (status)
|
||||
{
|
||||
return status->m_type;
|
||||
@@ -1063,7 +1063,7 @@ b3SharedMemoryStatusHandle b3SubmitClientCommandAndWaitStatus(b3PhysicsClientHan
|
||||
|
||||
b3SubmitClientCommand(physClient, commandHandle);
|
||||
|
||||
while ((statusHandle == 0) && (clock.getTimeInSeconds()-startTime < timeOutInSeconds))
|
||||
while (cl->isConnected() && (statusHandle == 0) && (clock.getTimeInSeconds()-startTime < timeOutInSeconds))
|
||||
{
|
||||
statusHandle = b3ProcessServerStatus(physClient);
|
||||
}
|
||||
@@ -2585,6 +2585,20 @@ int b3StateLoggingAddLoggingObjectUniqueId(b3SharedMemoryCommandHandle commandHa
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
int b3StateLoggingSetMaxLogDof(b3SharedMemoryCommandHandle commandHandle, int maxLogDof)
|
||||
{
|
||||
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
|
||||
b3Assert(command);
|
||||
b3Assert(command->m_type == CMD_STATE_LOGGING);
|
||||
if (command->m_type == CMD_STATE_LOGGING)
|
||||
{
|
||||
command->m_updateFlags |= STATE_LOGGING_MAX_LOG_DOF;
|
||||
command->m_stateLoggingArguments.m_maxLogDof = maxLogDof;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
int b3StateLoggingStop(b3SharedMemoryCommandHandle commandHandle, int loggingUid)
|
||||
{
|
||||
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
|
||||
|
||||
@@ -356,9 +356,13 @@ void b3GetKeyboardEventsData(b3PhysicsClientHandle physClient, struct b3Keyboard
|
||||
b3SharedMemoryCommandHandle b3StateLoggingCommandInit(b3PhysicsClientHandle physClient);
|
||||
int b3StateLoggingStart(b3SharedMemoryCommandHandle commandHandle, int loggingType, const char* fileName);
|
||||
int b3StateLoggingAddLoggingObjectUniqueId(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId);
|
||||
int b3StateLoggingSetMaxLogDof(b3SharedMemoryCommandHandle commandHandle, int maxLogDof);
|
||||
|
||||
int b3GetStatusLoggingUniqueId(b3SharedMemoryStatusHandle statusHandle);
|
||||
int b3StateLoggingStop(b3SharedMemoryCommandHandle commandHandle, int loggingUniqueId);
|
||||
|
||||
|
||||
|
||||
void b3SetTimeOut(b3PhysicsClientHandle physClient, double timeOutInSeconds);
|
||||
double b3GetTimeOut(b3PhysicsClientHandle physClient);
|
||||
|
||||
|
||||
@@ -1067,4 +1067,4 @@ void PhysicsDirect::setTimeOut(double timeOutInSeconds)
|
||||
double PhysicsDirect::getTimeOut() const
|
||||
{
|
||||
return m_data->m_timeOutInSeconds;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -48,7 +48,7 @@ bool gResetSimulation = 0;
|
||||
int gVRTrackingObjectUniqueId = -1;
|
||||
btTransform gVRTrackingObjectTr = btTransform::getIdentity();
|
||||
|
||||
int gMaxNumCmdPer1ms = 10;//experiment: add some delay to avoid threads starving other threads
|
||||
int gMaxNumCmdPer1ms = -1;//experiment: add some delay to avoid threads starving other threads
|
||||
int gCreateObjectSimVR = -1;
|
||||
int gEnableKukaControl = 0;
|
||||
btVector3 gVRTeleportPos1(0,0,0);
|
||||
@@ -438,6 +438,31 @@ struct InternalStateLogger
|
||||
|
||||
};
|
||||
|
||||
struct VideoMP4Loggger : public InternalStateLogger
|
||||
{
|
||||
|
||||
struct GUIHelperInterface* m_guiHelper;
|
||||
std::string m_fileName;
|
||||
VideoMP4Loggger(int loggerUid,const char* fileName,GUIHelperInterface* guiHelper)
|
||||
:m_guiHelper(guiHelper)
|
||||
{
|
||||
m_fileName = fileName;
|
||||
m_loggingUniqueId = loggerUid;
|
||||
m_loggingType = STATE_LOGGING_VIDEO_MP4;
|
||||
m_guiHelper->dumpFramesToVideo(fileName);
|
||||
}
|
||||
|
||||
virtual void stop()
|
||||
{
|
||||
m_guiHelper->dumpFramesToVideo(0);
|
||||
}
|
||||
virtual void logState(btScalar timeStamp)
|
||||
{
|
||||
//dumping video frames happens in another thread
|
||||
//we could add some overlay of timestamp here, if needed/wanted
|
||||
}
|
||||
};
|
||||
|
||||
struct MinitaurStateLogger : public InternalStateLogger
|
||||
{
|
||||
int m_loggingTimeStamp;
|
||||
@@ -454,6 +479,7 @@ struct MinitaurStateLogger : public InternalStateLogger
|
||||
m_logFileHandle(0),
|
||||
m_minitaurMultiBody(minitaurMultiBody)
|
||||
{
|
||||
m_loggingUniqueId = loggingUniqueId;
|
||||
m_loggingType = STATE_LOGGING_MINITAUR;
|
||||
m_motorIdList.resize(motorIdList.size());
|
||||
for (int m=0;m<motorIdList.size();m++)
|
||||
@@ -512,7 +538,8 @@ struct MinitaurStateLogger : public InternalStateLogger
|
||||
|
||||
MinitaurLogRecord logData;
|
||||
//'t', 'r', 'p', 'y', 'q0', 'q1', 'q2', 'q3', 'q4', 'q5', 'q6', 'q7', 'u0', 'u1', 'u2', 'u3', 'u4', 'u5', 'u6', 'u7', 'xd', 'mo'
|
||||
btScalar motorDir[8] = {1, -1, 1, -1, -1, 1, -1, 1};
|
||||
btScalar motorDir[8] = {1, 1, 1, 1, 1, 1, 1, 1};
|
||||
|
||||
|
||||
btQuaternion orn = m_minitaurMultiBody->getBaseWorldTransform().getRotation();
|
||||
btMatrix3x3 mat(orn);
|
||||
@@ -568,13 +595,16 @@ struct GenericRobotStateLogger : public InternalStateLogger
|
||||
btMultiBodyDynamicsWorld* m_dynamicsWorld;
|
||||
btAlignedObjectArray<int> m_bodyIdList;
|
||||
bool m_filterObjectUniqueId;
|
||||
|
||||
GenericRobotStateLogger(int loggingUniqueId, const std::string& fileName, btMultiBodyDynamicsWorld* dynamicsWorld)
|
||||
int m_maxLogDof;
|
||||
|
||||
GenericRobotStateLogger(int loggingUniqueId, const std::string& fileName, btMultiBodyDynamicsWorld* dynamicsWorld, int maxLogDof)
|
||||
:m_loggingTimeStamp(0),
|
||||
m_logFileHandle(0),
|
||||
m_dynamicsWorld(dynamicsWorld),
|
||||
m_filterObjectUniqueId(false)
|
||||
m_filterObjectUniqueId(false),
|
||||
m_maxLogDof(maxLogDof)
|
||||
{
|
||||
m_loggingUniqueId = loggingUniqueId;
|
||||
m_loggingType = STATE_LOGGING_GENERIC_ROBOT;
|
||||
|
||||
btAlignedObjectArray<std::string> structNames;
|
||||
@@ -595,32 +625,24 @@ struct GenericRobotStateLogger : public InternalStateLogger
|
||||
structNames.push_back("omegaY");
|
||||
structNames.push_back("omegaZ");
|
||||
structNames.push_back("qNum");
|
||||
structNames.push_back("q0");
|
||||
structNames.push_back("q1");
|
||||
structNames.push_back("q2");
|
||||
structNames.push_back("q3");
|
||||
structNames.push_back("q4");
|
||||
structNames.push_back("q5");
|
||||
structNames.push_back("q6");
|
||||
structNames.push_back("q7");
|
||||
structNames.push_back("q8");
|
||||
structNames.push_back("q9");
|
||||
structNames.push_back("q10");
|
||||
structNames.push_back("q11");
|
||||
structNames.push_back("u0");
|
||||
structNames.push_back("u1");
|
||||
structNames.push_back("u2");
|
||||
structNames.push_back("u3");
|
||||
structNames.push_back("u4");
|
||||
structNames.push_back("u5");
|
||||
structNames.push_back("u6");
|
||||
structNames.push_back("u7");
|
||||
structNames.push_back("u8");
|
||||
structNames.push_back("u9");
|
||||
structNames.push_back("u10");
|
||||
structNames.push_back("u11");
|
||||
|
||||
m_structTypes = "IfIfffffffffffffI";
|
||||
|
||||
for (int i=0;i<m_maxLogDof;i++)
|
||||
{
|
||||
m_structTypes.append("f");
|
||||
char jointName[256];
|
||||
sprintf(jointName,"q%d",i);
|
||||
structNames.push_back(jointName);
|
||||
}
|
||||
for (int i=0;i<m_maxLogDof;i++)
|
||||
{
|
||||
m_structTypes.append("f");
|
||||
char jointName[256];
|
||||
sprintf(jointName,"u%d",i);
|
||||
structNames.push_back(jointName);
|
||||
}
|
||||
|
||||
m_structTypes = "IfIfffffffffffffIffffffffffffffffffffffff";
|
||||
const char* fileNameC = fileName.c_str();
|
||||
|
||||
m_logFileHandle = createMinitaurLogFile(fileNameC, structNames, m_structTypes);
|
||||
@@ -699,7 +721,7 @@ struct GenericRobotStateLogger : public InternalStateLogger
|
||||
logData.m_values.push_back(q);
|
||||
}
|
||||
}
|
||||
for (int j = numDofs; j < 12; ++j)
|
||||
for (int j = numDofs; j < m_maxLogDof; ++j)
|
||||
{
|
||||
float q = 0.0;
|
||||
logData.m_values.push_back(q);
|
||||
@@ -713,7 +735,7 @@ struct GenericRobotStateLogger : public InternalStateLogger
|
||||
logData.m_values.push_back(u);
|
||||
}
|
||||
}
|
||||
for (int j = numDofs; j < 12; ++j)
|
||||
for (int j = numDofs; j < m_maxLogDof; ++j)
|
||||
{
|
||||
float u = 0.0;
|
||||
logData.m_values.push_back(u);
|
||||
@@ -897,6 +919,7 @@ struct PhysicsServerCommandProcessorInternalData
|
||||
|
||||
//data for picking objects
|
||||
class btRigidBody* m_pickedBody;
|
||||
int m_savedActivationState;
|
||||
class btTypedConstraint* m_pickedConstraint;
|
||||
class btMultiBodyPoint2Point* m_pickingMultiBodyPoint2Point;
|
||||
btVector3 m_oldPickingPos;
|
||||
@@ -1124,7 +1147,8 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld()
|
||||
|
||||
m_data->m_dynamicsWorld->setGravity(btVector3(0, 0, 0));
|
||||
m_data->m_dynamicsWorld->getSolverInfo().m_erp2 = 0.08;
|
||||
|
||||
|
||||
m_data->m_dynamicsWorld->getSolverInfo().m_frictionERP = 0.2;//need to check if there are artifacts with frictionERP
|
||||
m_data->m_dynamicsWorld->getSolverInfo().m_linearSlop = 0.00001;
|
||||
m_data->m_dynamicsWorld->getSolverInfo().m_numIterations = 50;
|
||||
m_data->m_dynamicsWorld->getSolverInfo().m_leastSquaresResidualThreshold = 1e-7;
|
||||
@@ -1475,7 +1499,7 @@ bool PhysicsServerCommandProcessor::loadMjcf(const char* fileName, char* bufferS
|
||||
|
||||
m_data->m_sdfRecentLoadedBodies.clear();
|
||||
|
||||
BulletMJCFImporter u2b(m_data->m_guiHelper); //, &m_data->m_visualConverter
|
||||
BulletMJCFImporter u2b(m_data->m_guiHelper, &m_data->m_visualConverter);
|
||||
|
||||
bool useFixedBase = false;
|
||||
MyMJCFLogger2 logger;
|
||||
@@ -1800,6 +1824,17 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
|
||||
if (clientCmd.m_updateFlags & STATE_LOGGING_START_LOG)
|
||||
{
|
||||
|
||||
if (clientCmd.m_stateLoggingArguments.m_logType == STATE_LOGGING_VIDEO_MP4)
|
||||
{
|
||||
//if (clientCmd.m_stateLoggingArguments.m_fileName)
|
||||
{
|
||||
int loggerUid = m_data->m_stateLoggersUniqueId++;
|
||||
VideoMP4Loggger* logger = new VideoMP4Loggger(loggerUid,clientCmd.m_stateLoggingArguments.m_fileName,this->m_data->m_guiHelper);
|
||||
m_data->m_stateLoggers.push_back(logger);
|
||||
serverStatusOut.m_type = CMD_STATE_LOGGING_START_COMPLETED;
|
||||
serverStatusOut.m_stateLoggingResultArgs.m_loggingUniqueId = loggerUid;
|
||||
}
|
||||
}
|
||||
|
||||
if (clientCmd.m_stateLoggingArguments.m_logType == STATE_LOGGING_MINITAUR)
|
||||
{
|
||||
@@ -1860,14 +1895,20 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
|
||||
std::string fileName = clientCmd.m_stateLoggingArguments.m_fileName;
|
||||
|
||||
int loggerUid = m_data->m_stateLoggersUniqueId++;
|
||||
GenericRobotStateLogger* logger = new GenericRobotStateLogger(loggerUid,fileName,m_data->m_dynamicsWorld);
|
||||
int maxLogDof = 12;
|
||||
if ((clientCmd.m_updateFlags & STATE_LOGGING_MAX_LOG_DOF))
|
||||
{
|
||||
maxLogDof = clientCmd.m_stateLoggingArguments.m_maxLogDof;
|
||||
}
|
||||
GenericRobotStateLogger* logger = new GenericRobotStateLogger(loggerUid,fileName,m_data->m_dynamicsWorld,maxLogDof);
|
||||
|
||||
if ((clientCmd.m_updateFlags & STATE_LOGGING_FILTER_OBJECT_UNIQUE_ID) && (clientCmd.m_stateLoggingArguments.m_numBodyUniqueIds>0))
|
||||
{
|
||||
logger->m_filterObjectUniqueId = true;
|
||||
for (int i = 0; i < clientCmd.m_stateLoggingArguments.m_numBodyUniqueIds; ++i)
|
||||
{
|
||||
logger->m_bodyIdList.push_back(clientCmd.m_stateLoggingArguments.m_bodyUniqueIds[i]);
|
||||
int objectUniqueId = clientCmd.m_stateLoggingArguments.m_bodyUniqueIds[i];
|
||||
logger->m_bodyIdList.push_back(objectUniqueId);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -4571,20 +4612,19 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
|
||||
int remain = totalNumVisualShapes - clientCmd.m_requestVisualShapeDataArguments.m_startingVisualShapeIndex;
|
||||
int shapeIndex = clientCmd.m_requestVisualShapeDataArguments.m_startingVisualShapeIndex;
|
||||
|
||||
m_data->m_visualConverter.getVisualShapesData(clientCmd.m_requestVisualShapeDataArguments.m_bodyUniqueId,
|
||||
int success = m_data->m_visualConverter.getVisualShapesData(clientCmd.m_requestVisualShapeDataArguments.m_bodyUniqueId,
|
||||
shapeIndex,
|
||||
visualShapeStoragePtr);
|
||||
|
||||
|
||||
//m_visualConverter
|
||||
serverCmd.m_sendVisualShapeArgs.m_numRemainingVisualShapes = remain-1;
|
||||
serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied = 1;
|
||||
serverCmd.m_sendVisualShapeArgs.m_startingVisualShapeIndex = clientCmd.m_requestVisualShapeDataArguments.m_startingVisualShapeIndex;
|
||||
serverCmd.m_sendVisualShapeArgs.m_bodyUniqueId = clientCmd.m_requestVisualShapeDataArguments.m_bodyUniqueId;
|
||||
serverCmd.m_numDataStreamBytes = sizeof(b3VisualShapeData)*serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied;
|
||||
serverCmd.m_type =CMD_VISUAL_SHAPE_INFO_COMPLETED;
|
||||
hasStatus = true;
|
||||
break;
|
||||
if (success) {
|
||||
serverCmd.m_sendVisualShapeArgs.m_numRemainingVisualShapes = remain-1;
|
||||
serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied = 1;
|
||||
serverCmd.m_sendVisualShapeArgs.m_startingVisualShapeIndex = clientCmd.m_requestVisualShapeDataArguments.m_startingVisualShapeIndex;
|
||||
serverCmd.m_sendVisualShapeArgs.m_bodyUniqueId = clientCmd.m_requestVisualShapeDataArguments.m_bodyUniqueId;
|
||||
serverCmd.m_numDataStreamBytes = sizeof(b3VisualShapeData)*serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied;
|
||||
serverCmd.m_type = CMD_VISUAL_SHAPE_INFO_COMPLETED;
|
||||
}
|
||||
hasStatus = true;
|
||||
break;
|
||||
}
|
||||
case CMD_UPDATE_VISUAL_SHAPE:
|
||||
{
|
||||
@@ -4939,6 +4979,7 @@ bool PhysicsServerCommandProcessor::pickBody(const btVector3& rayFromWorld, cons
|
||||
if (!(body->isStaticObject() || body->isKinematicObject()))
|
||||
{
|
||||
m_data->m_pickedBody = body;
|
||||
m_data->m_savedActivationState = body->getActivationState();
|
||||
m_data->m_pickedBody->setActivationState(DISABLE_DEACTIVATION);
|
||||
//printf("pickPos=%f,%f,%f\n",pickPos.getX(),pickPos.getY(),pickPos.getZ());
|
||||
btVector3 localPivot = body->getCenterOfMassTransform().inverse() * pickPos;
|
||||
@@ -5031,7 +5072,7 @@ void PhysicsServerCommandProcessor::removePickingConstraint()
|
||||
m_data->m_dynamicsWorld->removeConstraint(m_data->m_pickedConstraint);
|
||||
delete m_data->m_pickedConstraint;
|
||||
m_data->m_pickedConstraint = 0;
|
||||
m_data->m_pickedBody->forceActivationState(ACTIVE_TAG);
|
||||
m_data->m_pickedBody->forceActivationState(m_data->m_savedActivationState);
|
||||
m_data->m_pickedBody = 0;
|
||||
}
|
||||
if (m_data->m_pickingMultiBodyPoint2Point)
|
||||
|
||||
@@ -179,8 +179,10 @@ enum MultiThreadedGUIHelperCommunicationEnums
|
||||
eGUIUserDebugAddParameter,
|
||||
eGUIUserDebugRemoveItem,
|
||||
eGUIUserDebugRemoveAllItems,
|
||||
eGUIDumpFramesToVideo,
|
||||
};
|
||||
|
||||
|
||||
#include <stdio.h>
|
||||
//#include "BulletMultiThreaded/PlatformDefinitions.h"
|
||||
|
||||
@@ -313,12 +315,15 @@ void MotionThreadFunc(void* userPtr,void* lsMemory)
|
||||
b3Clock::usleep(0);
|
||||
}
|
||||
|
||||
if (numCmdSinceSleep1ms>gMaxNumCmdPer1ms)
|
||||
if (gMaxNumCmdPer1ms>0)
|
||||
{
|
||||
BT_PROFILE("usleep(1000)");
|
||||
b3Clock::usleep(1000);
|
||||
numCmdSinceSleep1ms = 0;
|
||||
sleepClock.reset();
|
||||
if (numCmdSinceSleep1ms>gMaxNumCmdPer1ms)
|
||||
{
|
||||
BT_PROFILE("usleep(10)");
|
||||
b3Clock::usleep(10);
|
||||
numCmdSinceSleep1ms = 0;
|
||||
sleepClock.reset();
|
||||
}
|
||||
}
|
||||
if (sleepClock.getTimeMilliseconds()>1)
|
||||
{
|
||||
@@ -486,7 +491,7 @@ void MotionThreadFunc(void* userPtr,void* lsMemory)
|
||||
args->m_cs->unlock();
|
||||
}
|
||||
|
||||
|
||||
args->m_physicsServerPtr->disconnectSharedMemory(true);
|
||||
//do nothing
|
||||
|
||||
}
|
||||
@@ -956,6 +961,16 @@ public:
|
||||
|
||||
}
|
||||
|
||||
const char* m_mp4FileName;
|
||||
virtual void dumpFramesToVideo(const char* mp4FileName)
|
||||
{
|
||||
m_cs->lock();
|
||||
m_mp4FileName = mp4FileName;
|
||||
m_cs->setSharedParam(1, eGUIDumpFramesToVideo);
|
||||
workerThreadWait();
|
||||
m_mp4FileName = 0;
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
|
||||
@@ -1611,6 +1626,14 @@ void PhysicsServerExample::updateGraphics()
|
||||
m_multiThreadedHelper->mainThreadRelease();
|
||||
break;
|
||||
}
|
||||
|
||||
case eGUIDumpFramesToVideo:
|
||||
{
|
||||
m_multiThreadedHelper->m_childGuiHelper->dumpFramesToVideo(m_multiThreadedHelper->m_mp4FileName);
|
||||
m_multiThreadedHelper->mainThreadRelease();
|
||||
break;
|
||||
}
|
||||
|
||||
case eGUIHelperIdle:
|
||||
{
|
||||
break;
|
||||
|
||||
@@ -188,6 +188,8 @@ bool PhysicsServerSharedMemory::connectSharedMemory( struct GUIHelperInterface*
|
||||
|
||||
void PhysicsServerSharedMemory::disconnectSharedMemory(bool deInitializeSharedMemory)
|
||||
{
|
||||
m_data->m_commandProcessor->deleteDynamicsWorld();
|
||||
|
||||
m_data->m_commandProcessor->setGuiHelper(0);
|
||||
|
||||
if (m_data->m_verboseOutput)
|
||||
|
||||
@@ -626,6 +626,7 @@ enum eStateLoggingEnums
|
||||
STATE_LOGGING_START_LOG=1,
|
||||
STATE_LOGGING_STOP_LOG=2,
|
||||
STATE_LOGGING_FILTER_OBJECT_UNIQUE_ID=4,
|
||||
STATE_LOGGING_MAX_LOG_DOF=8
|
||||
};
|
||||
|
||||
struct VRCameraState
|
||||
@@ -644,6 +645,7 @@ struct StateLoggingRequest
|
||||
int m_numBodyUniqueIds;////only if ROBOT_LOGGING_FILTER_OBJECT_UNIQUE_ID flag is set
|
||||
int m_bodyUniqueIds[MAX_SDF_BODIES];
|
||||
int m_loggingUniqueId;
|
||||
int m_maxLogDof;
|
||||
};
|
||||
|
||||
struct StateLoggingResultArgs
|
||||
|
||||
@@ -330,7 +330,8 @@ enum b3StateLoggingType
|
||||
STATE_LOGGING_MINITAUR = 0,
|
||||
STATE_LOGGING_GENERIC_ROBOT = 1,
|
||||
STATE_LOGGING_VR_CONTROLLERS = 2,
|
||||
STATE_LOGGING_COMMANDS = 3,
|
||||
STATE_LOGGING_VIDEO_MP4 = 3,
|
||||
STATE_LOGGING_COMMANDS = 4,
|
||||
};
|
||||
|
||||
|
||||
|
||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user