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>
|
</visual>
|
||||||
|
|
||||||
<asset>
|
<asset>
|
||||||
<mesh name="index0" file="index0.stl"/>
|
<mesh name="index0" file="index0_collision.stl"/>
|
||||||
<mesh name="index1" file="index1.stl"/>
|
<mesh name="index1" file="index1_collision.stl"/>
|
||||||
<mesh name="index2" file="index2.stl"/>
|
<mesh name="index2" file="index2_collision.stl"/>
|
||||||
<mesh name="index3" file="index3.stl"/>
|
<mesh name="index3" file="index3_collision.stl"/>
|
||||||
<mesh name="middle0" file="middle0.stl"/>
|
<mesh name="middle0" file="middle0_collision.stl"/>
|
||||||
<mesh name="middle1" file="middle1.stl"/>
|
<mesh name="middle1" file="middle1_collision.stl"/>
|
||||||
<mesh name="middle2" file="middle2.stl"/>
|
<mesh name="middle2" file="middle2_collision.stl"/>
|
||||||
<mesh name="middle3" file="middle3.stl"/>
|
<mesh name="middle3" file="middle3_collision.stl"/>
|
||||||
<mesh name="palm" file="palm.stl"/>
|
<mesh name="palm" file="palm.stl"/>
|
||||||
<mesh name="pinky0" file="pinky0.stl"/>
|
<mesh name="pinky0" file="pinky0_collision.stl"/>
|
||||||
<mesh name="pinky1" file="pinky1.stl"/>
|
<mesh name="pinky1" file="pinky1_collision.stl"/>
|
||||||
<mesh name="pinky2" file="pinky2.stl"/>
|
<mesh name="pinky2" file="pinky2_collision.stl"/>
|
||||||
<mesh name="pinky3" file="pinky3.stl"/>
|
<mesh name="pinky3" file="pinky3_collision.stl"/>
|
||||||
<mesh name="ring0" file="ring0.stl"/>
|
<mesh name="ring0" file="ring0_collision.stl"/>
|
||||||
<mesh name="ring1" file="ring1.stl"/>
|
<mesh name="ring1" file="ring1_collision.stl"/>
|
||||||
<mesh name="ring2" file="ring2.stl"/>
|
<mesh name="ring2" file="ring2_collision.stl"/>
|
||||||
<mesh name="ring3" file="ring3.stl"/>
|
<mesh name="ring3" file="ring3_collision.stl"/>
|
||||||
<mesh name="thumb0" file="thumb0.stl"/>
|
<mesh name="thumb0" file="thumb0_collision.stl"/>
|
||||||
<mesh name="thumb1" file="thumb1.stl"/>
|
<mesh name="thumb1" file="thumb1_collision.stl"/>
|
||||||
<mesh name="thumb2" file="thumb2.stl"/>
|
<mesh name="thumb2" file="thumb2_collision.stl"/>
|
||||||
<mesh name="thumb3" file="thumb3.stl"/>
|
<mesh name="thumb3" file="thumb3_collision.stl"/>
|
||||||
<mesh name="wristx" file="wristx.stl"/>
|
<mesh name="wristx" file="wristx_collision.stl"/>
|
||||||
<mesh name="wristy" file="wristy.stl"/>
|
<mesh name="wristy" file="wristy_collision.stl"/>
|
||||||
<mesh name="wristz" file="wristz.stl"/>
|
<mesh name="wristz" file="wristz_collision.stl"/>
|
||||||
|
|
||||||
<texture type="skybox" builtin="gradient" rgb1=".4 .6 .8" rgb2="0 0 0"
|
<texture type="skybox" builtin="gradient" rgb1=".4 .6 .8" rgb2="0 0 0"
|
||||||
width="100" height="100"/>
|
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>
|
</visual>
|
||||||
|
|
||||||
<asset>
|
<asset>
|
||||||
<mesh name="index0" file="index0.stl"/>
|
<mesh name="index0" file="index0_collision.stl"/>
|
||||||
<mesh name="index1" file="index1.stl"/>
|
<mesh name="index1" file="index1_collision.stl"/>
|
||||||
<mesh name="index2" file="index2.stl"/>
|
<mesh name="index2" file="index2_collision.stl"/>
|
||||||
<mesh name="index3" file="index3.stl"/>
|
<mesh name="index3" file="index3_collision.stl"/>
|
||||||
<mesh name="middle0" file="middle0.stl"/>
|
<mesh name="middle0" file="middle0_collision.stl"/>
|
||||||
<mesh name="middle1" file="middle1.stl"/>
|
<mesh name="middle1" file="middle1_collision.stl"/>
|
||||||
<mesh name="middle2" file="middle2.stl"/>
|
<mesh name="middle2" file="middle2_collision.stl"/>
|
||||||
<mesh name="middle3" file="middle3.stl"/>
|
<mesh name="middle3" file="middle3_collision.stl"/>
|
||||||
<mesh name="palm" file="palm.stl"/>
|
<mesh name="palm" file="palm_collision.stl"/>
|
||||||
<mesh name="pinky0" file="pinky0.stl"/>
|
<mesh name="pinky0" file="pinky0_collision.stl"/>
|
||||||
<mesh name="pinky1" file="pinky1.stl"/>
|
<mesh name="pinky1" file="pinky1_collision.stl"/>
|
||||||
<mesh name="pinky2" file="pinky2.stl"/>
|
<mesh name="pinky2" file="pinky2_collision.stl"/>
|
||||||
<mesh name="pinky3" file="pinky3.stl"/>
|
<mesh name="pinky3" file="pinky3_collision.stl"/>
|
||||||
<mesh name="ring0" file="ring0.stl"/>
|
<mesh name="ring0" file="ring0_collision.stl"/>
|
||||||
<mesh name="ring1" file="ring1.stl"/>
|
<mesh name="ring1" file="ring1_collision.stl"/>
|
||||||
<mesh name="ring2" file="ring2.stl"/>
|
<mesh name="ring2" file="ring2_collision.stl"/>
|
||||||
<mesh name="ring3" file="ring3.stl"/>
|
<mesh name="ring3" file="ring3_collision.stl"/>
|
||||||
<mesh name="thumb0" file="thumb0.stl"/>
|
<mesh name="thumb0" file="thumb0_collision.stl"/>
|
||||||
<mesh name="thumb1" file="thumb1.stl"/>
|
<mesh name="thumb1" file="thumb1_collision.stl"/>
|
||||||
<mesh name="thumb2" file="thumb2.stl"/>
|
<mesh name="thumb2" file="thumb2_collision.stl"/>
|
||||||
<mesh name="thumb3" file="thumb3.stl"/>
|
<mesh name="thumb3" file="thumb3_collision.stl"/>
|
||||||
<mesh name="wristx" file="wristx.stl"/>
|
<mesh name="wristx" file="wristx_collision.stl"/>
|
||||||
<mesh name="wristy" file="wristy.stl"/>
|
<mesh name="wristy" file="wristy_collision.stl"/>
|
||||||
<mesh name="wristz" file="wristz.stl"/>
|
<mesh name="wristz" file="wristz_collision.stl"/>
|
||||||
|
|
||||||
<texture type="skybox" builtin="gradient" rgb1=".4 .6 .8" rgb2="0 0 0"
|
<texture type="skybox" builtin="gradient" rgb1=".4 .6 .8" rgb2="0 0 0"
|
||||||
width="100" height="100"/>
|
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">
|
<robot name="cube.urdf">
|
||||||
<link name="baseLink">
|
<link name="baseLink">
|
||||||
<contact>
|
<contact>
|
||||||
<lateral_friction value="1.0"/>
|
<friction_anchor/>
|
||||||
|
<lateral_friction value="1.3"/>
|
||||||
<rolling_friction value="0.0"/>
|
<rolling_friction value="0.0"/>
|
||||||
<stiffness value="300"/>
|
<stiffness value="300"/>
|
||||||
<damping value="10"/>
|
<damping value="10"/>
|
||||||
|
|||||||
@@ -215,6 +215,10 @@
|
|||||||
</joint>
|
</joint>
|
||||||
|
|
||||||
<link name='finger_right'>
|
<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>
|
<pose frame=''>0.062 0 0.145 0 0 1.5708</pose>
|
||||||
<inertial>
|
<inertial>
|
||||||
<mass>0.2</mass>
|
<mass>0.2</mass>
|
||||||
@@ -255,6 +259,10 @@
|
|||||||
</joint>
|
</joint>
|
||||||
|
|
||||||
<link name='finger_left'>
|
<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>
|
<pose frame=''>-0.062 0 0.145 0 0 4.71239</pose>
|
||||||
<inertial>
|
<inertial>
|
||||||
<mass>0.2</mass>
|
<mass>0.2</mass>
|
||||||
|
|||||||
File diff suppressed because it is too large
Load Diff
@@ -2,7 +2,7 @@
|
|||||||
<robot name="cube.urdf">
|
<robot name="cube.urdf">
|
||||||
<link name="planeLink">
|
<link name="planeLink">
|
||||||
<contact>
|
<contact>
|
||||||
<lateral_friction value="2"/>
|
<lateral_friction value="1"/>
|
||||||
</contact>
|
</contact>
|
||||||
<inertial>
|
<inertial>
|
||||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
<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 removeAllUserDebugItems( ){};
|
||||||
virtual void setVisualizerFlagCallback(VisualizerFlagCallback callback){}
|
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/PhysicsLoopBackC_API.h
|
||||||
../SharedMemory/PhysicsServerCommandProcessor.cpp
|
../SharedMemory/PhysicsServerCommandProcessor.cpp
|
||||||
../SharedMemory/PhysicsServerCommandProcessor.h
|
../SharedMemory/PhysicsServerCommandProcessor.h
|
||||||
|
../SharedMemory/SharedMemoryCommands.h
|
||||||
|
../SharedMemory/SharedMemoryPublic.h
|
||||||
../BasicDemo/BasicExample.cpp
|
../BasicDemo/BasicExample.cpp
|
||||||
../BasicDemo/BasicExample.h
|
../BasicDemo/BasicExample.h
|
||||||
../InverseDynamics/InverseDynamicsExample.cpp
|
../InverseDynamics/InverseDynamicsExample.cpp
|
||||||
|
|||||||
@@ -371,6 +371,7 @@ void OpenGLExampleBrowserVisualizerFlagCallback(int flag, bool enable)
|
|||||||
if (flag == COV_ENABLE_GUI)
|
if (flag == COV_ENABLE_GUI)
|
||||||
{
|
{
|
||||||
renderGui = enable;
|
renderGui = enable;
|
||||||
|
renderGrid = enable;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (flag == COV_ENABLE_WIREFRAME)
|
if (flag == COV_ENABLE_WIREFRAME)
|
||||||
|
|||||||
@@ -427,47 +427,67 @@ void OpenGLGuiHelper::copyCameraImageData(const float viewMatrix[16], const floa
|
|||||||
SimpleCamera tempCam;
|
SimpleCamera tempCam;
|
||||||
getRenderInterface()->setActiveCamera(&tempCam);
|
getRenderInterface()->setActiveCamera(&tempCam);
|
||||||
getRenderInterface()->getActiveCamera()->setVRCamera(viewMatrix,projectionMatrix);
|
getRenderInterface()->getActiveCamera()->setVRCamera(viewMatrix,projectionMatrix);
|
||||||
getRenderInterface()->renderScene();
|
{
|
||||||
|
BT_PROFILE("renderScene");
|
||||||
|
getRenderInterface()->renderScene();
|
||||||
|
}
|
||||||
getRenderInterface()->setActiveCamera(oldCam);
|
getRenderInterface()->setActiveCamera(oldCam);
|
||||||
|
|
||||||
{
|
{
|
||||||
|
BT_PROFILE("copy pixels");
|
||||||
btAlignedObjectArray<unsigned char> sourceRgbaPixelBuffer;
|
btAlignedObjectArray<unsigned char> sourceRgbaPixelBuffer;
|
||||||
btAlignedObjectArray<float> sourceDepthBuffer;
|
btAlignedObjectArray<float> sourceDepthBuffer;
|
||||||
//copy the image into our local cache
|
//copy the image into our local cache
|
||||||
sourceRgbaPixelBuffer.resize(sourceWidth*sourceHeight*numBytesPerPixel);
|
sourceRgbaPixelBuffer.resize(sourceWidth*sourceHeight*numBytesPerPixel);
|
||||||
sourceDepthBuffer.resize(sourceWidth*sourceHeight);
|
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_rgbaPixelBuffer1.resize(destinationWidth*destinationHeight*numBytesPerPixel);
|
||||||
m_data->m_depthBuffer1.resize(destinationWidth*destinationHeight);
|
m_data->m_depthBuffer1.resize(destinationWidth*destinationHeight);
|
||||||
//rescale and flip
|
//rescale and flip
|
||||||
|
{
|
||||||
|
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
|
||||||
|
|
||||||
for (int i=0;i<destinationWidth;i++)
|
int sourcePixelIndex = (xIndex+yIndex*sourceWidth)*bytesPerPixel;
|
||||||
{
|
int sourceDepthIndex = xIndex+yIndex*sourceWidth;
|
||||||
for (int j=0;j<destinationHeight;j++)
|
#define COPY4PIXELS 1
|
||||||
{
|
#ifdef COPY4PIXELS
|
||||||
int xIndex = int(float(i)*(float(sourceWidth)/float(destinationWidth)));
|
int* dst = (int*)&m_data->m_rgbaPixelBuffer1[(i+j*destinationWidth)*4+0];
|
||||||
int yIndex = int(float(destinationHeight-1-j)*(float(sourceHeight)/float(destinationHeight)));
|
int* src = (int*)&sourceRgbaPixelBuffer[sourcePixelIndex+0];
|
||||||
btClamp(xIndex,0,sourceWidth);
|
*dst = *src;
|
||||||
btClamp(yIndex,0,sourceHeight);
|
|
||||||
int bytesPerPixel = 4; //RGBA
|
|
||||||
|
|
||||||
int sourcePixelIndex = (xIndex+yIndex*sourceWidth)*bytesPerPixel;
|
#else
|
||||||
int sourceDepthIndex = xIndex+yIndex*sourceWidth;
|
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)
|
if (pixelsRGBA)
|
||||||
{
|
{
|
||||||
|
BT_PROFILE("copy rgba pixels");
|
||||||
|
|
||||||
for (int i=0;i<numRequestedPixels*numBytesPerPixel;i++)
|
for (int i=0;i<numRequestedPixels*numBytesPerPixel;i++)
|
||||||
{
|
{
|
||||||
pixelsRGBA[i] = m_data->m_rgbaPixelBuffer1[i+startPixelIndex*numBytesPerPixel];
|
pixelsRGBA[i] = m_data->m_rgbaPixelBuffer1[i+startPixelIndex*numBytesPerPixel];
|
||||||
@@ -475,6 +495,8 @@ void OpenGLGuiHelper::copyCameraImageData(const float viewMatrix[16], const floa
|
|||||||
}
|
}
|
||||||
if (depthBuffer)
|
if (depthBuffer)
|
||||||
{
|
{
|
||||||
|
BT_PROFILE("copy depth buffer pixels");
|
||||||
|
|
||||||
for (int i=0;i<numRequestedPixels;i++)
|
for (int i=0;i<numRequestedPixels;i++)
|
||||||
{
|
{
|
||||||
depthBuffer[i] = m_data->m_depthBuffer1[i+startPixelIndex];
|
depthBuffer[i] = m_data->m_depthBuffer1[i+startPixelIndex];
|
||||||
@@ -546,3 +568,10 @@ struct CommonGraphicsApp* OpenGLGuiHelper::getAppInterface()
|
|||||||
return m_data->m_glApp;
|
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 setVisualizerFlagCallback(VisualizerFlagCallback callback);
|
||||||
|
|
||||||
|
virtual void dumpFramesToVideo(const char* mp4FileName);
|
||||||
|
|
||||||
};
|
};
|
||||||
|
|
||||||
#endif //OPENGL_GUI_HELPER_H
|
#endif //OPENGL_GUI_HELPER_H
|
||||||
|
|||||||
@@ -88,6 +88,8 @@ project "App_BulletExampleBrowser"
|
|||||||
"../SharedMemory/PhysicsServerCommandProcessor.h",
|
"../SharedMemory/PhysicsServerCommandProcessor.h",
|
||||||
"../SharedMemory/TinyRendererVisualShapeConverter.cpp",
|
"../SharedMemory/TinyRendererVisualShapeConverter.cpp",
|
||||||
"../SharedMemory/TinyRendererVisualShapeConverter.h",
|
"../SharedMemory/TinyRendererVisualShapeConverter.h",
|
||||||
|
"../SharedMemory/SharedMemoryCommands.h",
|
||||||
|
"../SharedMemory/SharedMemoryPublic.h",
|
||||||
"../MultiThreading/MultiThreadingExample.cpp",
|
"../MultiThreading/MultiThreadingExample.cpp",
|
||||||
"../MultiThreading/b3PosixThreadSupport.cpp",
|
"../MultiThreading/b3PosixThreadSupport.cpp",
|
||||||
"../MultiThreading/b3Win32ThreadSupport.cpp",
|
"../MultiThreading/b3Win32ThreadSupport.cpp",
|
||||||
|
|||||||
@@ -30,7 +30,7 @@ int main(int argc, char** argv)
|
|||||||
btDefaultCollisionConfiguration* collisionConfiguration = new btDefaultCollisionConfiguration();
|
btDefaultCollisionConfiguration* collisionConfiguration = new btDefaultCollisionConfiguration();
|
||||||
|
|
||||||
///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
|
///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.
|
///btDbvtBroadphase is a good general purpose broadphase. You can also try out btAxis3Sweep.
|
||||||
btBroadphaseInterface* overlappingPairCache = new btDbvtBroadphase();
|
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)
|
///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded)
|
||||||
btSequentialImpulseConstraintSolver* solver = new btSequentialImpulseConstraintSolver;
|
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-----
|
///-----initialization_end-----
|
||||||
|
|
||||||
@@ -48,39 +48,37 @@ int main(int argc, char** argv)
|
|||||||
//make sure to re-use collision shapes among rigid bodies whenever possible!
|
//make sure to re-use collision shapes among rigid bodies whenever possible!
|
||||||
btAlignedObjectArray<btCollisionShape*> collisionShapes;
|
btAlignedObjectArray<btCollisionShape*> collisionShapes;
|
||||||
|
|
||||||
|
|
||||||
///create a few basic rigid bodies
|
///create a few basic rigid bodies
|
||||||
|
|
||||||
//the ground is a cube of side 100 at position y = -56.
|
//the ground is a cube of side 100 at position y = -56.
|
||||||
//the sphere will hit it at y = -6, with center at -5
|
//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);
|
collisionShapes.push_back(groundShape);
|
||||||
|
|
||||||
btTransform groundTransform;
|
btTransform groundTransform;
|
||||||
groundTransform.setIdentity();
|
groundTransform.setIdentity();
|
||||||
groundTransform.setOrigin(btVector3(0,-56,0));
|
groundTransform.setOrigin(btVector3(0, -56, 0));
|
||||||
|
|
||||||
btScalar mass(0.);
|
btScalar mass(0.);
|
||||||
|
|
||||||
//rigidbody is dynamic if and only if mass is non zero, otherwise static
|
//rigidbody is dynamic if and only if mass is non zero, otherwise static
|
||||||
bool isDynamic = (mass != 0.f);
|
bool isDynamic = (mass != 0.f);
|
||||||
|
|
||||||
btVector3 localInertia(0,0,0);
|
btVector3 localInertia(0, 0, 0);
|
||||||
if (isDynamic)
|
if (isDynamic)
|
||||||
groundShape->calculateLocalInertia(mass,localInertia);
|
groundShape->calculateLocalInertia(mass, localInertia);
|
||||||
|
|
||||||
//using motionstate is optional, it provides interpolation capabilities, and only synchronizes 'active' objects
|
//using motionstate is optional, it provides interpolation capabilities, and only synchronizes 'active' objects
|
||||||
btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform);
|
btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform);
|
||||||
btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,groundShape,localInertia);
|
btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, groundShape, localInertia);
|
||||||
btRigidBody* body = new btRigidBody(rbInfo);
|
btRigidBody* body = new btRigidBody(rbInfo);
|
||||||
|
|
||||||
//add the body to the dynamics world
|
//add the body to the dynamics world
|
||||||
dynamicsWorld->addRigidBody(body);
|
dynamicsWorld->addRigidBody(body);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
{
|
{
|
||||||
//create a dynamic rigidbody
|
//create a dynamic rigidbody
|
||||||
|
|
||||||
@@ -92,37 +90,34 @@ int main(int argc, char** argv)
|
|||||||
btTransform startTransform;
|
btTransform startTransform;
|
||||||
startTransform.setIdentity();
|
startTransform.setIdentity();
|
||||||
|
|
||||||
btScalar mass(1.f);
|
btScalar mass(1.f);
|
||||||
|
|
||||||
//rigidbody is dynamic if and only if mass is non zero, otherwise static
|
//rigidbody is dynamic if and only if mass is non zero, otherwise static
|
||||||
bool isDynamic = (mass != 0.f);
|
bool isDynamic = (mass != 0.f);
|
||||||
|
|
||||||
btVector3 localInertia(0,0,0);
|
btVector3 localInertia(0, 0, 0);
|
||||||
if (isDynamic)
|
if (isDynamic)
|
||||||
colShape->calculateLocalInertia(mass,localInertia);
|
colShape->calculateLocalInertia(mass, localInertia);
|
||||||
|
|
||||||
startTransform.setOrigin(btVector3(2,10,0));
|
startTransform.setOrigin(btVector3(2, 10, 0));
|
||||||
|
|
||||||
//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
|
//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
|
||||||
btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform);
|
btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform);
|
||||||
btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,colShape,localInertia);
|
btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, colShape, localInertia);
|
||||||
btRigidBody* body = new btRigidBody(rbInfo);
|
btRigidBody* body = new btRigidBody(rbInfo);
|
||||||
|
|
||||||
dynamicsWorld->addRigidBody(body);
|
dynamicsWorld->addRigidBody(body);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// Do some simulation
|
||||||
|
|
||||||
/// Do some simulation
|
|
||||||
|
|
||||||
|
|
||||||
///-----stepsimulation_start-----
|
///-----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
|
//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];
|
btCollisionObject* obj = dynamicsWorld->getCollisionObjectArray()[j];
|
||||||
btRigidBody* body = btRigidBody::upcast(obj);
|
btRigidBody* body = btRigidBody::upcast(obj);
|
||||||
@@ -130,12 +125,12 @@ int main(int argc, char** argv)
|
|||||||
if (body && body->getMotionState())
|
if (body && body->getMotionState())
|
||||||
{
|
{
|
||||||
body->getMotionState()->getWorldTransform(trans);
|
body->getMotionState()->getWorldTransform(trans);
|
||||||
|
}
|
||||||
} else
|
else
|
||||||
{
|
{
|
||||||
trans = obj->getWorldTransform();
|
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()));
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -146,7 +141,7 @@ int main(int argc, char** argv)
|
|||||||
///-----cleanup_start-----
|
///-----cleanup_start-----
|
||||||
|
|
||||||
//remove the rigidbodies from the dynamics world and delete them
|
//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];
|
btCollisionObject* obj = dynamicsWorld->getCollisionObjectArray()[i];
|
||||||
btRigidBody* body = btRigidBody::upcast(obj);
|
btRigidBody* body = btRigidBody::upcast(obj);
|
||||||
@@ -154,12 +149,12 @@ int main(int argc, char** argv)
|
|||||||
{
|
{
|
||||||
delete body->getMotionState();
|
delete body->getMotionState();
|
||||||
}
|
}
|
||||||
dynamicsWorld->removeCollisionObject( obj );
|
dynamicsWorld->removeCollisionObject(obj);
|
||||||
delete obj;
|
delete obj;
|
||||||
}
|
}
|
||||||
|
|
||||||
//delete collision shapes
|
//delete collision shapes
|
||||||
for (int j=0;j<collisionShapes.size();j++)
|
for (int j = 0; j < collisionShapes.size(); j++)
|
||||||
{
|
{
|
||||||
btCollisionShape* shape = collisionShapes[j];
|
btCollisionShape* shape = collisionShapes[j];
|
||||||
collisionShapes[j] = 0;
|
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
|
//next line is optional: it will be cleared by the destructor when the array goes out of scope
|
||||||
collisionShapes.clear();
|
collisionShapes.clear();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@@ -30,12 +30,6 @@
|
|||||||
|
|
||||||
#include <vector>
|
#include <vector>
|
||||||
|
|
||||||
enum eMJCF_FILE_TYPE_ENUMS
|
|
||||||
{
|
|
||||||
MJCF_FILE_STL = 1,
|
|
||||||
MJCF_FILE_OBJ = 2
|
|
||||||
};
|
|
||||||
|
|
||||||
enum ePARENT_LINK_ENUMS
|
enum ePARENT_LINK_ENUMS
|
||||||
{
|
{
|
||||||
BASE_LINK_INDEX=-1,
|
BASE_LINK_INDEX=-1,
|
||||||
@@ -137,9 +131,11 @@ struct MyMJCFAsset
|
|||||||
struct BulletMJCFImporterInternalData
|
struct BulletMJCFImporterInternalData
|
||||||
{
|
{
|
||||||
GUIHelperInterface* m_guiHelper;
|
GUIHelperInterface* m_guiHelper;
|
||||||
|
struct LinkVisualShapesConverter* m_customVisualShapesConverter;
|
||||||
char m_pathPrefix[1024];
|
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;
|
btHashMap<btHashString,MyMJCFAsset> m_assets;
|
||||||
|
|
||||||
btAlignedObjectArray<UrdfModel*> m_models;
|
btAlignedObjectArray<UrdfModel*> m_models;
|
||||||
@@ -150,16 +146,24 @@ struct BulletMJCFImporterInternalData
|
|||||||
|
|
||||||
|
|
||||||
int m_activeModel;
|
int m_activeModel;
|
||||||
|
int m_activeBodyUniqueId;
|
||||||
//todo: for full MJCF compatibility, we would need a stack of default values
|
//todo: for full MJCF compatibility, we would need a stack of default values
|
||||||
int m_defaultCollisionGroup;
|
int m_defaultCollisionGroup;
|
||||||
int m_defaultCollisionMask;
|
int m_defaultCollisionMask;
|
||||||
btScalar m_defaultCollisionMargin;
|
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!)
|
//those collision shapes are deleted by caller (todo: make sure this happens!)
|
||||||
btAlignedObjectArray<btCollisionShape*> m_allocatedCollisionShapes;
|
btAlignedObjectArray<btCollisionShape*> m_allocatedCollisionShapes;
|
||||||
|
|
||||||
BulletMJCFImporterInternalData()
|
BulletMJCFImporterInternalData()
|
||||||
:m_activeModel(-1),
|
:m_activeModel(-1),
|
||||||
|
m_activeBodyUniqueId(-1),
|
||||||
m_defaultCollisionGroup(1),
|
m_defaultCollisionGroup(1),
|
||||||
m_defaultCollisionMask(1),
|
m_defaultCollisionMask(1),
|
||||||
m_defaultCollisionMargin(0.001)//assume unit meters, margin is 1mm
|
m_defaultCollisionMargin(0.001)//assume unit meters, margin is 1mm
|
||||||
@@ -167,6 +171,13 @@ struct BulletMJCFImporterInternalData
|
|||||||
m_pathPrefix[0] = 0;
|
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
|
const UrdfLink* getLink(int modelIndex, int linkIndex) const
|
||||||
{
|
{
|
||||||
if (modelIndex>=0 && modelIndex<m_models.size())
|
if (modelIndex>=0 && modelIndex<m_models.size())
|
||||||
@@ -238,6 +249,17 @@ struct BulletMJCFImporterInternalData
|
|||||||
{
|
{
|
||||||
parseAssets(child_xml,logger);
|
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")
|
if (n=="geom")
|
||||||
{
|
{
|
||||||
//contype, conaffinity
|
//contype, conaffinity
|
||||||
@@ -251,6 +273,11 @@ struct BulletMJCFImporterInternalData
|
|||||||
{
|
{
|
||||||
m_defaultCollisionMask = urdfLexicalCast<int>(conAffinityStr);
|
m_defaultCollisionMask = urdfLexicalCast<int>(conAffinityStr);
|
||||||
}
|
}
|
||||||
|
const char* rgba = child_xml->Attribute("rgba");
|
||||||
|
if (rgba)
|
||||||
|
{
|
||||||
|
m_defaultGeomRgba = rgba;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
handled=true;
|
handled=true;
|
||||||
@@ -361,39 +388,57 @@ struct BulletMJCFImporterInternalData
|
|||||||
bool isLimited = false;
|
bool isLimited = false;
|
||||||
double range[2] = {1,0};
|
double range[2] = {1,0};
|
||||||
|
|
||||||
|
std::string lim = m_defaultJointLimited;
|
||||||
if (limitedStr)
|
if (limitedStr)
|
||||||
{
|
{
|
||||||
std::string lim = limitedStr;
|
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");
|
|
||||||
}
|
}
|
||||||
|
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;
|
bool jointHandled = false;
|
||||||
const UrdfLink* linkPtr = getLink(modelIndex,linkIndex);
|
const UrdfLink* linkPtr = getLink(modelIndex,linkIndex);
|
||||||
@@ -489,10 +534,23 @@ struct BulletMJCFImporterInternalData
|
|||||||
|
|
||||||
|
|
||||||
|
|
||||||
// const char* rgba = link_xml->Attribute("rgba");
|
|
||||||
const char* gType = link_xml->Attribute("type");
|
const char* gType = link_xml->Attribute("type");
|
||||||
const char* sz = link_xml->Attribute("size");
|
const char* sz = link_xml->Attribute("size");
|
||||||
const char* posS = link_xml->Attribute("pos");
|
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)
|
if (posS)
|
||||||
{
|
{
|
||||||
btVector3 pos(0,0,0);
|
btVector3 pos(0,0,0);
|
||||||
@@ -560,10 +618,10 @@ struct BulletMJCFImporterInternalData
|
|||||||
handledGeomType = true;
|
handledGeomType = true;
|
||||||
}
|
}
|
||||||
|
|
||||||
//todo: capsule, cylinder, meshes or heightfields etc
|
if (geomType == "capsule" || geomType == "cylinder")
|
||||||
if (geomType == "capsule")
|
|
||||||
{
|
{
|
||||||
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<std::string> pieces;
|
||||||
btArray<float> sizes;
|
btArray<float> sizes;
|
||||||
@@ -621,9 +679,14 @@ struct BulletMJCFImporterInternalData
|
|||||||
MyMJCFAsset* assetPtr = m_assets[meshStr];
|
MyMJCFAsset* assetPtr = m_assets[meshStr];
|
||||||
if (assetPtr)
|
if (assetPtr)
|
||||||
{
|
{
|
||||||
handledGeomType = true;
|
|
||||||
geom.m_type = URDF_GEOM_MESH;
|
geom.m_type = URDF_GEOM_MESH;
|
||||||
geom.m_meshFileName = assetPtr->m_fileName;
|
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);
|
geom.m_meshScale.setValue(1,1,1);
|
||||||
//todo: parse mesh scale
|
//todo: parse mesh scale
|
||||||
if (sz)
|
if (sz)
|
||||||
@@ -632,13 +695,6 @@ struct BulletMJCFImporterInternalData
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
#if 0
|
|
||||||
if (geomType == "cylinder")
|
|
||||||
{
|
|
||||||
geom.m_type = URDF_GEOM_CYLINDER;
|
|
||||||
handledGeomType = true;
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
if (handledGeomType)
|
if (handledGeomType)
|
||||||
{
|
{
|
||||||
|
|
||||||
@@ -803,6 +859,7 @@ struct BulletMJCFImporterInternalData
|
|||||||
|
|
||||||
return orgChildLinkIndex;
|
return orgChildLinkIndex;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool parseBody(TiXmlElement* link_xml, int modelIndex, int orgParentLinkIndex, MJCFErrorLogger* logger)
|
bool parseBody(TiXmlElement* link_xml, int modelIndex, int orgParentLinkIndex, MJCFErrorLogger* logger)
|
||||||
{
|
{
|
||||||
int newParentLinkIndex = orgParentLinkIndex;
|
int newParentLinkIndex = orgParentLinkIndex;
|
||||||
@@ -964,10 +1021,6 @@ struct BulletMJCFImporterInternalData
|
|||||||
}
|
}
|
||||||
|
|
||||||
linkPtr->m_linkTransformInWorld = linkTransform;
|
linkPtr->m_linkTransformInWorld = linkTransform;
|
||||||
if (bodyN == "cart1")//front_left_leg")
|
|
||||||
{
|
|
||||||
printf("found!\n");
|
|
||||||
}
|
|
||||||
if ((newParentLinkIndex != INVALID_LINK_INDEX) && !skipFixedJoint)
|
if ((newParentLinkIndex != INVALID_LINK_INDEX) && !skipFixedJoint)
|
||||||
{
|
{
|
||||||
//linkPtr->m_linkTransformInWorld.setIdentity();
|
//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 = new BulletMJCFImporterInternalData();
|
||||||
m_data->m_guiHelper = helper;
|
m_data->m_guiHelper = helper;
|
||||||
|
m_data->m_customVisualShapesConverter = customConverter;
|
||||||
}
|
}
|
||||||
|
|
||||||
BulletMJCFImporter::~BulletMJCFImporter()
|
BulletMJCFImporter::~BulletMJCFImporter()
|
||||||
@@ -1135,7 +1189,8 @@ bool BulletMJCFImporter::loadMJCF(const char* fileName, MJCFErrorLogger* logger,
|
|||||||
b3FileUtils fu;
|
b3FileUtils fu;
|
||||||
|
|
||||||
//bool fileFound = fu.findFile(fileName, relativeFileName, 1024);
|
//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;
|
std::string xml_string;
|
||||||
m_data->m_pathPrefix[0] = 0;
|
m_data->m_pathPrefix[0] = 0;
|
||||||
@@ -1399,21 +1454,26 @@ bool BulletMJCFImporter::getLinkContactInfo(int linkIndex, URDFLinkContactInfo&
|
|||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void BulletMJCFImporter::convertLinkVisualShapes2(int linkIndex, int urdfIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int objectIndex) const
|
||||||
void BulletMJCFImporter::convertLinkVisualShapes2(int linkIndex, 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)
|
void BulletMJCFImporter::setBodyUniqueId(int bodyId)
|
||||||
{
|
{
|
||||||
|
m_data->m_activeBodyUniqueId = bodyId;
|
||||||
}
|
}
|
||||||
|
|
||||||
int BulletMJCFImporter::getBodyUniqueId() const
|
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)
|
static btCollisionShape* MjcfCreateConvexHullFromShapes(std::vector<tinyobj::shape_t>& shapes, const btVector3& geomScale, btScalar collisionMargin)
|
||||||
{
|
{
|
||||||
btCompoundShape* compound = new btCompoundShape();
|
btCompoundShape* compound = new btCompoundShape();
|
||||||
@@ -1496,132 +1556,87 @@ class btCompoundShape* BulletMJCFImporter::convertLinkCollisionShapes(int linkIn
|
|||||||
}
|
}
|
||||||
case URDF_GEOM_MESH:
|
case URDF_GEOM_MESH:
|
||||||
{
|
{
|
||||||
//////////////////////
|
GLInstanceGraphicsShape* glmesh = 0;
|
||||||
if (1)
|
switch (col->m_geometry.m_meshFileType)
|
||||||
{
|
|
||||||
if (col->m_geometry.m_meshFileName.length())
|
|
||||||
{
|
{
|
||||||
const char* filename = col->m_geometry.m_meshFileName.c_str();
|
case UrdfGeometry::FILE_OBJ:
|
||||||
//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"))
|
|
||||||
{
|
{
|
||||||
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"))
|
case UrdfGeometry::FILE_STL:
|
||||||
{
|
|
||||||
fileType = MJCF_FILE_OBJ;
|
|
||||||
}
|
|
||||||
|
|
||||||
sprintf(fullPath,"%s%s",pathPrefix,filename);
|
|
||||||
FILE* f = fopen(fullPath,"rb");
|
|
||||||
if (f)
|
|
||||||
{
|
{
|
||||||
fclose(f);
|
glmesh = LoadMeshFromSTL(col->m_geometry.m_meshFileName.c_str());
|
||||||
GLInstanceGraphicsShape* glmesh = 0;
|
break;
|
||||||
|
}
|
||||||
|
default:
|
||||||
|
b3Warning("%s: Unsupported file type in Collision: %s (maybe .dae?)\n", col->m_sourceFileLocation.c_str(), col->m_geometry.m_meshFileType);
|
||||||
|
}
|
||||||
|
|
||||||
|
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]));
|
||||||
|
}
|
||||||
|
|
||||||
switch (fileType)
|
if (col->m_flags & URDF_FORCE_CONCAVE_TRIMESH)
|
||||||
|
{
|
||||||
|
|
||||||
|
btTriangleMesh* meshInterface = new btTriangleMesh();
|
||||||
|
for (int i=0;i<glmesh->m_numIndices/3;i++)
|
||||||
{
|
{
|
||||||
case MJCF_FILE_OBJ:
|
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;
|
||||||
if (col->m_flags & URDF_FORCE_CONCAVE_TRIMESH)
|
float* v2 = glmesh->m_vertices->at(glmesh->m_indices->at(i*3+2)).xyzw;
|
||||||
{
|
meshInterface->addTriangle(btVector3(v0[0],v0[1],v0[2]),
|
||||||
glmesh = LoadMeshFromObj(fullPath, collisionPathPrefix);
|
btVector3(v1[0],v1[1],v1[2]),
|
||||||
}
|
btVector3(v2[0],v2[1],v2[2]));
|
||||||
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
|
|
||||||
|
|
||||||
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);
|
|
||||||
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
btBvhTriangleMeshShape* trimesh = new btBvhTriangleMeshShape(meshInterface,true,true);
|
||||||
if (!childShape && glmesh && (glmesh->m_numvertices>0))
|
childShape = trimesh;
|
||||||
{
|
|
||||||
//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);
|
|
||||||
}
|
|
||||||
|
|
||||||
delete glmesh;
|
|
||||||
|
|
||||||
} else
|
} 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;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
case URDF_GEOM_CAPSULE:
|
case URDF_GEOM_CAPSULE:
|
||||||
{
|
{
|
||||||
//todo: convert fromto to btCapsuleShape + local btTransform
|
//todo: convert fromto to btCapsuleShape + local btTransform
|
||||||
@@ -1647,11 +1662,8 @@ class btCompoundShape* BulletMJCFImporter::convertLinkCollisionShapes(int linkIn
|
|||||||
}
|
}
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
default:
|
} // switch geom
|
||||||
{
|
|
||||||
|
|
||||||
}
|
|
||||||
}
|
|
||||||
if (childShape)
|
if (childShape)
|
||||||
{
|
{
|
||||||
m_data->m_allocatedCollisionShapes.push_back(childShape);
|
m_data->m_allocatedCollisionShapes.push_back(childShape);
|
||||||
|
|||||||
@@ -18,9 +18,8 @@ class BulletMJCFImporter : public URDFImporterInterface
|
|||||||
{
|
{
|
||||||
struct BulletMJCFImporterInternalData* m_data;
|
struct BulletMJCFImporterInternalData* m_data;
|
||||||
|
|
||||||
|
|
||||||
public:
|
public:
|
||||||
BulletMJCFImporter(struct GUIHelperInterface* helper);
|
BulletMJCFImporter(struct GUIHelperInterface* helper, LinkVisualShapesConverter* customConverter);
|
||||||
virtual ~BulletMJCFImporter();
|
virtual ~BulletMJCFImporter();
|
||||||
|
|
||||||
virtual bool parseMJCFString(const char* xmlString, MJCFErrorLogger* logger);
|
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 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 void setBodyUniqueId(int bodyId);
|
||||||
virtual int getBodyUniqueId() const;
|
virtual int getBodyUniqueId() const;
|
||||||
|
|
||||||
|
|||||||
@@ -188,12 +188,12 @@ void ImportMJCFSetup::initPhysics()
|
|||||||
m_filterCallback->m_filterMode = FILTER_GROUPAMASKB_OR_GROUPBMASKA2;
|
m_filterCallback->m_filterMode = FILTER_GROUPAMASKB_OR_GROUPBMASKA2;
|
||||||
|
|
||||||
//m_dynamicsWorld->getSolverInfo().m_numIterations = 100;
|
//m_dynamicsWorld->getSolverInfo().m_numIterations = 100;
|
||||||
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
|
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
|
||||||
m_dynamicsWorld->getDebugDrawer()->setDebugMode(
|
m_dynamicsWorld->getDebugDrawer()->setDebugMode(
|
||||||
btIDebugDraw::DBG_DrawConstraints
|
btIDebugDraw::DBG_DrawConstraints
|
||||||
+btIDebugDraw::DBG_DrawContactPoints
|
+btIDebugDraw::DBG_DrawContactPoints
|
||||||
+btIDebugDraw::DBG_DrawAabb
|
+btIDebugDraw::DBG_DrawAabb
|
||||||
);//+btIDebugDraw::DBG_DrawConstraintLimits);
|
);//+btIDebugDraw::DBG_DrawConstraintLimits);
|
||||||
|
|
||||||
|
|
||||||
if (m_guiHelper->getParameterInterface())
|
if (m_guiHelper->getParameterInterface())
|
||||||
@@ -204,18 +204,21 @@ void ImportMJCFSetup::initPhysics()
|
|||||||
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
|
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
|
||||||
}
|
}
|
||||||
|
|
||||||
BulletMJCFImporter importer(m_guiHelper);
|
BulletMJCFImporter importer(m_guiHelper, 0);
|
||||||
MyMJCFLogger logger;
|
MyMJCFLogger logger;
|
||||||
bool result = importer.loadMJCF(m_fileName,&logger);
|
bool result = importer.loadMJCF(m_fileName,&logger);
|
||||||
if (result)
|
if (result)
|
||||||
{
|
{
|
||||||
btTransform rootTrans;
|
btTransform rootTrans;
|
||||||
rootTrans.setIdentity();
|
rootTrans.setIdentity();
|
||||||
|
|
||||||
for (int m =0; m<importer.getNumModels();m++)
|
for (int m =0; m<importer.getNumModels();m++)
|
||||||
{
|
{
|
||||||
|
importer.activateModel(m);
|
||||||
|
|
||||||
importer.activateModel(m);
|
// normally used with PhysicsServerCommandProcessor that allocates unique ids to multibodies,
|
||||||
|
// emulate this behavior here:
|
||||||
|
importer.setBodyUniqueId(m);
|
||||||
|
|
||||||
btMultiBody* mb = 0;
|
btMultiBody* mb = 0;
|
||||||
|
|
||||||
@@ -226,18 +229,17 @@ void ImportMJCFSetup::initPhysics()
|
|||||||
MyMultiBodyCreator creation(m_guiHelper);
|
MyMultiBodyCreator creation(m_guiHelper);
|
||||||
|
|
||||||
rootTrans.setIdentity();
|
rootTrans.setIdentity();
|
||||||
importer.getRootTransformInWorld(rootTrans);
|
importer.getRootTransformInWorld(rootTrans);
|
||||||
|
|
||||||
ConvertURDF2Bullet(importer,creation, rootTrans,m_dynamicsWorld,m_useMultiBody,importer.getPathPrefix(),CUF_USE_MJCF);
|
ConvertURDF2Bullet(importer,creation, rootTrans,m_dynamicsWorld,m_useMultiBody,importer.getPathPrefix(),CUF_USE_MJCF);
|
||||||
|
|
||||||
mb = creation.getBulletMultiBody();
|
mb = creation.getBulletMultiBody();
|
||||||
if (mb)
|
if (mb)
|
||||||
{
|
{
|
||||||
printf("first MJCF file converted!\n");
|
std::string* name =
|
||||||
std::string* name =
|
new std::string(importer.getLinkName(
|
||||||
new std::string(importer.getLinkName(
|
importer.getRootLinkIndex()));
|
||||||
importer.getRootLinkIndex()));
|
m_nameMemory.push_back(name);
|
||||||
m_nameMemory.push_back(name);
|
|
||||||
#ifdef TEST_MULTIBODY_SERIALIZATION
|
#ifdef TEST_MULTIBODY_SERIALIZATION
|
||||||
s->registerNameForPointer(name->c_str(),name->c_str());
|
s->registerNameForPointer(name->c_str(),name->c_str());
|
||||||
#endif//TEST_MULTIBODY_SERIALIZATION
|
#endif//TEST_MULTIBODY_SERIALIZATION
|
||||||
@@ -287,10 +289,11 @@ void ImportMJCFSetup::initPhysics()
|
|||||||
m_data->m_numMotors++;
|
m_data->m_numMotors++;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
} else
|
} else
|
||||||
{
|
{
|
||||||
|
// not multibody
|
||||||
if (1)
|
if (1)
|
||||||
{
|
{
|
||||||
//create motors for each generic joint
|
//create motors for each generic joint
|
||||||
|
|||||||
@@ -98,8 +98,11 @@ static GLInstanceGraphicsShape* LoadMeshFromSTL(const char* relativeFileName)
|
|||||||
}
|
}
|
||||||
fclose(file);
|
fclose(file);
|
||||||
}
|
}
|
||||||
shape->m_numIndices = shape->m_indices->size();
|
if (shape)
|
||||||
shape->m_numvertices = shape->m_vertices->size();
|
{
|
||||||
|
shape->m_numIndices = shape->m_indices->size();
|
||||||
|
shape->m_numvertices = shape->m_vertices->size();
|
||||||
|
}
|
||||||
return shape;
|
return shape;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@@ -32,6 +32,7 @@ static btScalar gUrdfDefaultCollisionMargin = 0.001;
|
|||||||
|
|
||||||
#include <iostream>
|
#include <iostream>
|
||||||
#include <fstream>
|
#include <fstream>
|
||||||
|
#include <list>
|
||||||
#include "UrdfParser.h"
|
#include "UrdfParser.h"
|
||||||
|
|
||||||
struct MyTexture
|
struct MyTexture
|
||||||
@@ -47,36 +48,38 @@ ATTRIBUTE_ALIGNED16(struct) BulletURDFInternalData
|
|||||||
|
|
||||||
UrdfParser m_urdfParser;
|
UrdfParser m_urdfParser;
|
||||||
struct GUIHelperInterface* m_guiHelper;
|
struct GUIHelperInterface* m_guiHelper;
|
||||||
|
std::string m_sourceFile;
|
||||||
char m_pathPrefix[1024];
|
char m_pathPrefix[1024];
|
||||||
int m_bodyId;
|
int m_bodyId;
|
||||||
btHashMap<btHashInt,btVector4> m_linkColors;
|
btHashMap<btHashInt,btVector4> m_linkColors;
|
||||||
btAlignedObjectArray<btCollisionShape*> m_allocatedCollisionShapes;
|
btAlignedObjectArray<btCollisionShape*> m_allocatedCollisionShapes;
|
||||||
|
|
||||||
LinkVisualShapesConverter* m_customVisualShapesConverter;
|
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()
|
void BulletURDFImporter::printTree()
|
||||||
{
|
{
|
||||||
// btAssert(0);
|
// btAssert(0);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
enum MyFileType
|
|
||||||
{
|
|
||||||
FILE_STL=1,
|
|
||||||
FILE_COLLADA=2,
|
|
||||||
FILE_OBJ=3,
|
|
||||||
};
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
BulletURDFImporter::BulletURDFImporter(struct GUIHelperInterface* helper, LinkVisualShapesConverter* customConverter)
|
BulletURDFImporter::BulletURDFImporter(struct GUIHelperInterface* helper, LinkVisualShapesConverter* customConverter)
|
||||||
{
|
{
|
||||||
m_data = new BulletURDFInternalData;
|
m_data = new BulletURDFInternalData;
|
||||||
|
|
||||||
m_data->m_guiHelper = helper;
|
m_data->m_guiHelper = helper;
|
||||||
m_data->m_pathPrefix[0]=0;
|
|
||||||
m_data->m_customVisualShapesConverter = customConverter;
|
m_data->m_customVisualShapesConverter = customConverter;
|
||||||
|
|
||||||
|
|
||||||
@@ -111,7 +114,6 @@ struct BulletErrorLogger : public ErrorLogger
|
|||||||
|
|
||||||
bool BulletURDFImporter::loadURDF(const char* fileName, bool forceFixedBase)
|
bool BulletURDFImporter::loadURDF(const char* fileName, bool forceFixedBase)
|
||||||
{
|
{
|
||||||
|
|
||||||
if (strlen(fileName)==0)
|
if (strlen(fileName)==0)
|
||||||
return false;
|
return false;
|
||||||
|
|
||||||
@@ -124,17 +126,16 @@ bool BulletURDFImporter::loadURDF(const char* fileName, bool forceFixedBase)
|
|||||||
bool fileFound = (b3ResourcePath::findResourcePath(fileName,relativeFileName,1024))>0;
|
bool fileFound = (b3ResourcePath::findResourcePath(fileName,relativeFileName,1024))>0;
|
||||||
|
|
||||||
std::string xml_string;
|
std::string xml_string;
|
||||||
m_data->m_pathPrefix[0] = 0;
|
|
||||||
|
|
||||||
if (!fileFound){
|
if (!fileFound){
|
||||||
std::cerr << "URDF file not found" << std::endl;
|
b3Warning("URDF file '%s' not found\n", fileName);
|
||||||
return false;
|
return false;
|
||||||
} else
|
} 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);
|
std::fstream xml_file(relativeFileName, std::fstream::in);
|
||||||
while ( xml_file.good())
|
while ( xml_file.good())
|
||||||
@@ -176,17 +177,16 @@ bool BulletURDFImporter::loadSDF(const char* fileName, bool forceFixedBase)
|
|||||||
bool fileFound = (b3ResourcePath::findResourcePath(fileName,relativeFileName,1024))>0;
|
bool fileFound = (b3ResourcePath::findResourcePath(fileName,relativeFileName,1024))>0;
|
||||||
|
|
||||||
std::string xml_string;
|
std::string xml_string;
|
||||||
m_data->m_pathPrefix[0] = 0;
|
|
||||||
|
|
||||||
if (!fileFound){
|
if (!fileFound){
|
||||||
std::cerr << "SDF file not found" << std::endl;
|
b3Warning("SDF file '%s' not found\n", fileName);
|
||||||
return false;
|
return false;
|
||||||
} else
|
} else
|
||||||
{
|
{
|
||||||
|
|
||||||
int maxPathLen = 1024;
|
char path[1024];
|
||||||
fu.extractPath(relativeFileName,m_data->m_pathPrefix,maxPathLen);
|
fu.extractPath(relativeFileName, path, sizeof(path));
|
||||||
|
m_data->setSourceFile(relativeFileName, path);
|
||||||
|
|
||||||
std::fstream xml_file(relativeFileName, std::fstream::in);
|
std::fstream xml_file(relativeFileName, std::fstream::in);
|
||||||
while ( xml_file.good() )
|
while ( xml_file.good() )
|
||||||
@@ -447,6 +447,101 @@ static btCollisionShape* createConvexHullFromShapes(std::vector<tinyobj::shape_t
|
|||||||
return compound;
|
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)
|
btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, const char* urdfPathPrefix)
|
||||||
{
|
{
|
||||||
BT_PROFILE("convertURDFToCollisionShape");
|
BT_PROFILE("convertURDFToCollisionShape");
|
||||||
@@ -467,8 +562,8 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co
|
|||||||
|
|
||||||
case URDF_GEOM_CYLINDER:
|
case URDF_GEOM_CYLINDER:
|
||||||
{
|
{
|
||||||
btScalar cylRadius = collision->m_geometry.m_cylinderRadius;
|
btScalar cylRadius = collision->m_geometry.m_capsuleRadius;
|
||||||
btScalar cylLength = collision->m_geometry.m_cylinderLength;
|
btScalar cylLength = collision->m_geometry.m_capsuleHalfHeight;
|
||||||
|
|
||||||
btAlignedObjectArray<btVector3> vertices;
|
btAlignedObjectArray<btVector3> vertices;
|
||||||
//int numVerts = sizeof(barrel_vertices)/(9*sizeof(float));
|
//int numVerts = sizeof(barrel_vertices)/(9*sizeof(float));
|
||||||
@@ -507,239 +602,165 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co
|
|||||||
}
|
}
|
||||||
case URDF_GEOM_SPHERE:
|
case URDF_GEOM_SPHERE:
|
||||||
{
|
{
|
||||||
|
|
||||||
btScalar radius = collision->m_geometry.m_sphereRadius;
|
btScalar radius = collision->m_geometry.m_sphereRadius;
|
||||||
btSphereShape* sphereShape = new btSphereShape(radius);
|
btSphereShape* sphereShape = new btSphereShape(radius);
|
||||||
shape = sphereShape;
|
shape = sphereShape;
|
||||||
shape ->setMargin(gUrdfDefaultCollisionMargin);
|
shape ->setMargin(gUrdfDefaultCollisionMargin);
|
||||||
break;
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
break;
|
case URDF_GEOM_MESH:
|
||||||
}
|
{
|
||||||
case URDF_GEOM_MESH:
|
GLInstanceGraphicsShape* glmesh = 0;
|
||||||
{
|
switch (collision->m_geometry.m_meshFileType)
|
||||||
if (collision->m_name.length())
|
{
|
||||||
|
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();
|
ColladaGraphicsInstance* instance = &visualShapeInstances[i];
|
||||||
//b3Printf("mesh->filename=%s\n",filename);
|
GLInstanceGraphicsShape* gfxShape = &visualShapes[instance->m_shapeIndex];
|
||||||
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];
|
b3AlignedObjectArray<GLInstanceVertex> verts;
|
||||||
sprintf(collisionPathPrefix,"%s%s",urdfPathPrefix,tmpPathPrefix);
|
verts.resize(gfxShape->m_vertices->size());
|
||||||
|
|
||||||
|
int baseIndex = glmesh->m_vertices->size();
|
||||||
|
|
||||||
|
for (int i=0;i<gfxShape->m_vertices->size();i++)
|
||||||
if (strstr(fullPath,".dae"))
|
|
||||||
{
|
{
|
||||||
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];
|
||||||
if (strstr(fullPath,".stl"))
|
verts[i].normal[2] = gfxShape->m_vertices->at(i).normal[2];
|
||||||
{
|
verts[i].uv[0] = gfxShape->m_vertices->at(i).uv[0];
|
||||||
fileType = FILE_STL;
|
verts[i].uv[1] = gfxShape->m_vertices->at(i).uv[1];
|
||||||
}
|
verts[i].xyzw[0] = gfxShape->m_vertices->at(i).xyzw[0];
|
||||||
if (strstr(fullPath,".obj"))
|
verts[i].xyzw[1] = gfxShape->m_vertices->at(i).xyzw[1];
|
||||||
{
|
verts[i].xyzw[2] = gfxShape->m_vertices->at(i).xyzw[2];
|
||||||
fileType = FILE_OBJ;
|
verts[i].xyzw[3] = gfxShape->m_vertices->at(i).xyzw[3];
|
||||||
}
|
|
||||||
|
|
||||||
sprintf(fullPath,"%s%s",urdfPathPrefix,filename);
|
|
||||||
FILE* f = fopen(fullPath,"rb");
|
|
||||||
if (f)
|
|
||||||
{
|
|
||||||
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);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
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(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:
|
default:
|
||||||
{
|
b3Warning("Error: unknown collision geometry type %i\n", collision->m_geometry.m_type);
|
||||||
b3Warning("Error: unknown visual geometry type\n");
|
// for example, URDF_GEOM_PLANE
|
||||||
}
|
}
|
||||||
}
|
|
||||||
return shape;
|
return shape;
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -764,8 +785,8 @@ static void convertURDFToVisualShapeInternal(const UrdfVisual* visual, const cha
|
|||||||
for (int i = 0; i<numSteps; i++)
|
for (int i = 0; i<numSteps; i++)
|
||||||
{
|
{
|
||||||
|
|
||||||
btScalar cylRadius = visual->m_geometry.m_cylinderRadius;
|
btScalar cylRadius = visual->m_geometry.m_capsuleRadius;
|
||||||
btScalar cylLength = visual->m_geometry.m_cylinderLength;
|
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.);
|
btVector3 vert(cylRadius*btSin(SIMD_2_PI*(float(i) / numSteps)), cylRadius*btCos(SIMD_2_PI*(float(i) / numSteps)), cylLength / 2.);
|
||||||
vertices.push_back(vert);
|
vertices.push_back(vert);
|
||||||
@@ -778,17 +799,17 @@ static void convertURDFToVisualShapeInternal(const UrdfVisual* visual, const cha
|
|||||||
convexColShape = cylZShape;
|
convexColShape = cylZShape;
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
case URDF_GEOM_BOX:
|
case URDF_GEOM_BOX:
|
||||||
{
|
{
|
||||||
|
|
||||||
btVector3 extents = visual->m_geometry.m_boxSize;
|
btVector3 extents = visual->m_geometry.m_boxSize;
|
||||||
|
|
||||||
btBoxShape* boxShape = new btBoxShape(extents*0.5f);
|
btBoxShape* boxShape = new btBoxShape(extents*0.5f);
|
||||||
//btConvexShape* boxShape = new btConeShapeX(extents[2]*0.5,extents[0]*0.5);
|
//btConvexShape* boxShape = new btConeShapeX(extents[2]*0.5,extents[0]*0.5);
|
||||||
convexColShape = boxShape;
|
convexColShape = boxShape;
|
||||||
convexColShape->setMargin(gUrdfDefaultCollisionMargin);
|
convexColShape->setMargin(gUrdfDefaultCollisionMargin);
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
case URDF_GEOM_SPHERE:
|
case URDF_GEOM_SPHERE:
|
||||||
{
|
{
|
||||||
btScalar radius = visual->m_geometry.m_sphereRadius;
|
btScalar radius = visual->m_geometry.m_sphereRadius;
|
||||||
@@ -796,206 +817,137 @@ static void convertURDFToVisualShapeInternal(const UrdfVisual* visual, const cha
|
|||||||
convexColShape = sphereShape;
|
convexColShape = sphereShape;
|
||||||
convexColShape->setMargin(gUrdfDefaultCollisionMargin);
|
convexColShape->setMargin(gUrdfDefaultCollisionMargin);
|
||||||
break;
|
break;
|
||||||
|
|
||||||
break;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
case URDF_GEOM_MESH:
|
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());
|
case UrdfGeometry::FILE_OBJ:
|
||||||
}
|
|
||||||
if (1)//visual->m_geometry)
|
|
||||||
{
|
|
||||||
if (visual->m_geometry.m_meshFileName.length())
|
|
||||||
{
|
{
|
||||||
const char* filename = visual->m_geometry.m_meshFileName.c_str();
|
b3ImportMeshData meshData;
|
||||||
//b3Printf("mesh->filename=%s\n", filename);
|
if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(visual->m_geometry.m_meshFileName, meshData))
|
||||||
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"))
|
|
||||||
{
|
{
|
||||||
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"))
|
break;
|
||||||
{
|
|
||||||
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);
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
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;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
default:
|
default:
|
||||||
{
|
b3Warning("Error: unknown visual geometry type %i\n", visual->m_geometry.m_type);
|
||||||
b3Warning("Error: unknown visual geometry type\n");
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
//if we have a convex, tesselate into localVertices/localIndices
|
//if we have a convex, tesselate into localVertices/localIndices
|
||||||
@@ -1166,15 +1118,17 @@ bool BulletURDFImporter::getLinkContactInfo(int linkIndex, URDFLinkContactInfo&
|
|||||||
return false;
|
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)
|
if (m_data->m_customVisualShapesConverter)
|
||||||
{
|
{
|
||||||
const UrdfModel& model = m_data->m_urdfParser.getModel();
|
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
|
int BulletURDFImporter::getNumAllocatedCollisionShapes() const
|
||||||
|
|||||||
@@ -51,7 +51,7 @@ public:
|
|||||||
|
|
||||||
virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const;
|
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
|
///todo(erwincoumans) refactor this convertLinkCollisionShapes/memory allocation
|
||||||
|
|
||||||
|
|||||||
@@ -1,9 +1,14 @@
|
|||||||
#ifndef LINK_VISUAL_SHAPES_CONVERTER_H
|
#ifndef LINK_VISUAL_SHAPES_CONVERTER_H
|
||||||
#define LINK_VISUAL_SHAPES_CONVERTER_H
|
#define LINK_VISUAL_SHAPES_CONVERTER_H
|
||||||
|
|
||||||
|
struct UrdfLink;
|
||||||
|
struct UrdfModel;
|
||||||
|
class btTransform;
|
||||||
|
class btCollisionObject;
|
||||||
|
|
||||||
struct LinkVisualShapesConverter
|
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
|
#endif //LINK_VISUAL_SHAPES_CONVERTER_H
|
||||||
|
|||||||
@@ -166,6 +166,10 @@ void processContactParameters(const URDFLinkContactInfo& contactInfo, btCollisio
|
|||||||
{
|
{
|
||||||
col->setContactStiffnessAndDamping(contactInfo.m_contactStiffness, contactInfo.m_contactDamping);
|
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
|
} else
|
||||||
{
|
{
|
||||||
if (cache.m_bulletMultiBody==0)
|
if (cache.m_bulletMultiBody==0)
|
||||||
@@ -470,7 +474,7 @@ void ConvertURDF2BulletInternal(
|
|||||||
u2b.getLinkColor(urdfLinkIndex,color);
|
u2b.getLinkColor(urdfLinkIndex,color);
|
||||||
creation.createCollisionObjectGraphicsInstance(urdfLinkIndex,col,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;
|
URDFLinkContactInfo contactInfo;
|
||||||
u2b.getLinkContactInfo(urdfLinkIndex,contactInfo);
|
u2b.getLinkContactInfo(urdfLinkIndex,contactInfo);
|
||||||
@@ -487,7 +491,7 @@ void ConvertURDF2BulletInternal(
|
|||||||
}
|
}
|
||||||
} else
|
} 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
|
///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 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 void setBodyUniqueId(int bodyId) {}
|
||||||
virtual int getBodyUniqueId() const { return 0;}
|
virtual int getBodyUniqueId() const { return 0;}
|
||||||
|
|
||||||
|
|||||||
@@ -23,6 +23,7 @@ enum URDF_LinkContactFlags
|
|||||||
URDF_CONTACT_HAS_ROLLING_FRICTION=32,
|
URDF_CONTACT_HAS_ROLLING_FRICTION=32,
|
||||||
URDF_CONTACT_HAS_SPINNING_FRICTION=64,
|
URDF_CONTACT_HAS_SPINNING_FRICTION=64,
|
||||||
URDF_CONTACT_HAS_RESTITUTION=128,
|
URDF_CONTACT_HAS_RESTITUTION=128,
|
||||||
|
URDF_CONTACT_HAS_FRICTION_ANCHOR=256,
|
||||||
|
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|||||||
@@ -8,9 +8,9 @@ UrdfParser::UrdfParser()
|
|||||||
:m_parseSDF(false),
|
:m_parseSDF(false),
|
||||||
m_activeSdfModel(-1)
|
m_activeSdfModel(-1)
|
||||||
{
|
{
|
||||||
|
m_urdf2Model.m_sourceFile = "IN_MEMORY_STRING"; // if loadUrdf() called later, source file name will be replaced with real
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
UrdfParser::~UrdfParser()
|
UrdfParser::~UrdfParser()
|
||||||
{
|
{
|
||||||
cleanModel(&m_urdf2Model);
|
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");
|
logger->reportError("Cylinder shape must have both length and radius attributes");
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
geom.m_cylinderRadius = urdfLexicalCast<double>(shape->Attribute("radius"));
|
geom.m_hasFromTo = false;
|
||||||
geom.m_cylinderLength = urdfLexicalCast<double>(shape->Attribute("length"));
|
geom.m_capsuleRadius = urdfLexicalCast<double>(shape->Attribute("radius"));
|
||||||
|
geom.m_capsuleHalfHeight = urdfLexicalCast<double>(shape->Attribute("length"));
|
||||||
|
|
||||||
}
|
}
|
||||||
else if (type_name == "capsule")
|
else if (type_name == "capsule")
|
||||||
@@ -410,59 +411,68 @@ bool UrdfParser::parseGeometry(UrdfGeometry& geom, TiXmlElement* g, ErrorLogger*
|
|||||||
geom.m_type = URDF_GEOM_CAPSULE;
|
geom.m_type = URDF_GEOM_CAPSULE;
|
||||||
if (!shape->Attribute("length") ||
|
if (!shape->Attribute("length") ||
|
||||||
!shape->Attribute("radius"))
|
!shape->Attribute("radius"))
|
||||||
{
|
{
|
||||||
logger->reportError("Capsule shape must have both length and radius attributes");
|
logger->reportError("Capsule shape must have both length and radius attributes");
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
geom.m_hasFromTo = false;
|
geom.m_hasFromTo = false;
|
||||||
geom.m_capsuleRadius = urdfLexicalCast<double>(shape->Attribute("radius"));
|
geom.m_capsuleRadius = urdfLexicalCast<double>(shape->Attribute("radius"));
|
||||||
geom.m_capsuleHalfHeight = btScalar(0.5)*urdfLexicalCast<double>(shape->Attribute("length"));
|
geom.m_capsuleHalfHeight = btScalar(0.5)*urdfLexicalCast<double>(shape->Attribute("length"));
|
||||||
|
|
||||||
}
|
}
|
||||||
else if (type_name == "mesh")
|
else if (type_name == "mesh")
|
||||||
{
|
{
|
||||||
geom.m_type = URDF_GEOM_MESH;
|
geom.m_type = URDF_GEOM_MESH;
|
||||||
if (m_parseSDF)
|
geom.m_meshScale.setValue(1,1,1);
|
||||||
{
|
std::string fn;
|
||||||
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");
|
if (m_parseSDF)
|
||||||
geom.m_meshFileName = filename->GetText();
|
{
|
||||||
}
|
if (TiXmlElement* scale = shape->FirstChildElement("scale"))
|
||||||
else
|
{
|
||||||
{
|
parseVector3(geom.m_meshScale,scale->GetText(),logger);
|
||||||
if (!shape->Attribute("filename")) {
|
}
|
||||||
logger->reportError("Mesh must contain a filename attribute");
|
if (TiXmlElement* filename = shape->FirstChildElement("uri"))
|
||||||
return false;
|
{
|
||||||
}
|
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);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
geom.m_meshFileName = shape->Attribute("filename");
|
if (fn.empty())
|
||||||
geom.m_meshScale.setValue(1,1,1);
|
{
|
||||||
|
logger->reportError("Mesh filename is empty");
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
if (shape->Attribute("scale"))
|
geom.m_meshFileName = fn;
|
||||||
{
|
bool success = findExistingMeshFile(
|
||||||
if (!parseVector3(geom.m_meshScale,shape->Attribute("scale"),logger))
|
m_urdf2Model.m_sourceFile, fn, sourceFileLocation(shape),
|
||||||
{
|
&geom.m_meshFileName, &geom.m_meshFileType);
|
||||||
logger->reportWarning("scale should be a vector3, not single scalar. Workaround activated.\n");
|
if (!success)
|
||||||
std::string scalar_str = shape->Attribute("scale");
|
{
|
||||||
double scaleFactor = urdfLexicalCast<double>(scalar_str.c_str());
|
// warning already printed
|
||||||
if (scaleFactor)
|
return false;
|
||||||
{
|
}
|
||||||
geom.m_meshScale.setValue(scaleFactor,scaleFactor,scaleFactor);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
} else
|
|
||||||
{
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
@@ -566,7 +576,7 @@ bool UrdfParser::parseVisual(UrdfModel& model, UrdfVisual& visual, TiXmlElement*
|
|||||||
if (name_char)
|
if (name_char)
|
||||||
visual.m_name = name_char;
|
visual.m_name = name_char;
|
||||||
|
|
||||||
visual.m_hasLocalMaterial = false;
|
visual.m_geometry.m_hasLocalMaterial = false;
|
||||||
|
|
||||||
// Material
|
// Material
|
||||||
TiXmlElement *mat = config->FirstChildElement("material");
|
TiXmlElement *mat = config->FirstChildElement("material");
|
||||||
@@ -588,7 +598,7 @@ bool UrdfParser::parseVisual(UrdfModel& model, UrdfVisual& visual, TiXmlElement*
|
|||||||
matPtr->m_rgbaColor = rgba;
|
matPtr->m_rgbaColor = rgba;
|
||||||
|
|
||||||
visual.m_materialName = matPtr->m_name;
|
visual.m_materialName = matPtr->m_name;
|
||||||
visual.m_hasLocalMaterial = true;
|
visual.m_geometry.m_hasLocalMaterial = true;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
@@ -607,11 +617,11 @@ bool UrdfParser::parseVisual(UrdfModel& model, UrdfVisual& visual, TiXmlElement*
|
|||||||
TiXmlElement *c = mat->FirstChildElement("color");
|
TiXmlElement *c = mat->FirstChildElement("color");
|
||||||
if (t||c)
|
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);
|
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,6 +862,7 @@ bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *confi
|
|||||||
for (TiXmlElement* vis_xml = config->FirstChildElement("visual"); vis_xml; vis_xml = vis_xml->NextSiblingElement("visual"))
|
for (TiXmlElement* vis_xml = config->FirstChildElement("visual"); vis_xml; vis_xml = vis_xml->NextSiblingElement("visual"))
|
||||||
{
|
{
|
||||||
UrdfVisual visual;
|
UrdfVisual visual;
|
||||||
|
visual.m_sourceFileLocation = sourceFileLocation(vis_xml);
|
||||||
|
|
||||||
if (parseVisual(model, visual, vis_xml,logger))
|
if (parseVisual(model, visual, vis_xml,logger))
|
||||||
{
|
{
|
||||||
@@ -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"))
|
for (TiXmlElement* col_xml = config->FirstChildElement("collision"); col_xml; col_xml = col_xml->NextSiblingElement("collision"))
|
||||||
{
|
{
|
||||||
UrdfCollision col;
|
UrdfCollision col;
|
||||||
|
col.m_sourceFileLocation = sourceFileLocation(col_xml);
|
||||||
|
|
||||||
if (parseCollision(col, col_xml,logger))
|
if (parseCollision(col, col_xml,logger))
|
||||||
{
|
{
|
||||||
link.m_collisionArray.push_back(col);
|
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++)
|
for (int i=0;i<link->m_visualArray.size();i++)
|
||||||
{
|
{
|
||||||
UrdfVisual& vis = link->m_visualArray.at(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());
|
UrdfMaterial** mat = m_urdf2Model.m_materials.find(vis.m_materialName.c_str());
|
||||||
if (mat && *mat)
|
if (mat && *mat)
|
||||||
{
|
{
|
||||||
vis.m_localMaterial = **mat;
|
vis.m_geometry.m_localMaterial = **mat;
|
||||||
} else
|
} else
|
||||||
{
|
{
|
||||||
//logger->reportError("Cannot find material with name:");
|
//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++)
|
for (int i=0;i<link->m_visualArray.size();i++)
|
||||||
{
|
{
|
||||||
UrdfVisual& vis = link->m_visualArray.at(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());
|
UrdfMaterial** mat = localModel->m_materials.find(vis.m_materialName.c_str());
|
||||||
if (mat && *mat)
|
if (mat && *mat)
|
||||||
{
|
{
|
||||||
vis.m_localMaterial = **mat;
|
vis.m_geometry.m_localMaterial = **mat;
|
||||||
} else
|
} else
|
||||||
{
|
{
|
||||||
//logger->reportError("Cannot find material with name:");
|
//logger->reportError("Cannot find material with name:");
|
||||||
@@ -1657,3 +1677,9 @@ bool UrdfParser::loadSDF(const char* sdfText, ErrorLogger* logger)
|
|||||||
return true;
|
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;
|
||||||
|
}
|
||||||
|
|||||||
@@ -20,7 +20,11 @@ struct UrdfMaterial
|
|||||||
{
|
{
|
||||||
std::string m_name;
|
std::string m_name;
|
||||||
std::string m_textureFilename;
|
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
|
struct UrdfInertia
|
||||||
@@ -66,33 +70,40 @@ struct UrdfGeometry
|
|||||||
btVector3 m_capsuleFrom;
|
btVector3 m_capsuleFrom;
|
||||||
btVector3 m_capsuleTo;
|
btVector3 m_capsuleTo;
|
||||||
|
|
||||||
double m_cylinderRadius;
|
btVector3 m_planeNormal;
|
||||||
double m_cylinderLength;
|
|
||||||
|
|
||||||
btVector3 m_planeNormal;
|
|
||||||
|
|
||||||
|
enum {
|
||||||
|
FILE_STL =1,
|
||||||
|
FILE_COLLADA =2,
|
||||||
|
FILE_OBJ =3,
|
||||||
|
};
|
||||||
|
int m_meshFileType;
|
||||||
std::string m_meshFileName;
|
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;
|
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 UrdfShape
|
||||||
|
|
||||||
struct UrdfCollision
|
|
||||||
{
|
{
|
||||||
|
std::string m_sourceFileLocation;
|
||||||
btTransform m_linkLocalFrame;
|
btTransform m_linkLocalFrame;
|
||||||
UrdfGeometry m_geometry;
|
UrdfGeometry m_geometry;
|
||||||
std::string m_name;
|
std::string m_name;
|
||||||
|
};
|
||||||
|
|
||||||
|
struct UrdfVisual: UrdfShape
|
||||||
|
{
|
||||||
|
std::string m_materialName;
|
||||||
|
};
|
||||||
|
|
||||||
|
struct UrdfCollision: UrdfShape
|
||||||
|
{
|
||||||
int m_flags;
|
int m_flags;
|
||||||
int m_collisionGroup;
|
int m_collisionGroup;
|
||||||
int m_collisionMask;
|
int m_collisionMask;
|
||||||
@@ -159,6 +170,7 @@ struct UrdfJoint
|
|||||||
struct UrdfModel
|
struct UrdfModel
|
||||||
{
|
{
|
||||||
std::string m_name;
|
std::string m_name;
|
||||||
|
std::string m_sourceFile;
|
||||||
btTransform m_rootTransformInWorld;
|
btTransform m_rootTransformInWorld;
|
||||||
btHashMap<btHashString, UrdfMaterial*> m_materials;
|
btHashMap<btHashString, UrdfMaterial*> m_materials;
|
||||||
btHashMap<btHashString, UrdfLink*> m_links;
|
btHashMap<btHashString, UrdfLink*> m_links;
|
||||||
@@ -263,6 +275,13 @@ public:
|
|||||||
}
|
}
|
||||||
return m_urdf2Model;
|
return m_urdf2Model;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
std::string sourceFileLocation(TiXmlElement* e);
|
||||||
|
|
||||||
|
void setSourceFile(const std::string& sourceFile)
|
||||||
|
{
|
||||||
|
m_urdf2Model.m_sourceFile = sourceFile;
|
||||||
|
}
|
||||||
};
|
};
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|||||||
@@ -774,16 +774,18 @@ void SimpleOpenGL3App::swapBuffer()
|
|||||||
m_data->m_frameDumpPngFileName = 0;
|
m_data->m_frameDumpPngFileName = 0;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
m_window->endRendering();
|
m_window->endRendering();
|
||||||
m_window->startRendering();
|
m_window->startRendering();
|
||||||
}
|
}
|
||||||
|
|
||||||
// see also http://blog.mmacklin.com/2013/06/11/real-time-video-capture-with-ffmpeg/
|
// see also http://blog.mmacklin.com/2013/06/11/real-time-video-capture-with-ffmpeg/
|
||||||
void SimpleOpenGL3App::dumpFramesToVideo(const char* mp4FileName)
|
void SimpleOpenGL3App::dumpFramesToVideo(const char* mp4FileName)
|
||||||
{
|
{
|
||||||
int width = (int)m_window->getRetinaScale()*m_instancingRenderer->getScreenWidth();
|
if (mp4FileName)
|
||||||
int height = (int)m_window->getRetinaScale()*m_instancingRenderer->getScreenHeight();
|
{
|
||||||
char cmd[8192];
|
int width = (int)m_window->getRetinaScale()*m_instancingRenderer->getScreenWidth();
|
||||||
|
int height = (int)m_window->getRetinaScale()*m_instancingRenderer->getScreenHeight();
|
||||||
|
char cmd[8192];
|
||||||
|
|
||||||
#ifdef _WIN32
|
#ifdef _WIN32
|
||||||
sprintf(cmd, "ffmpeg -r 60 -f rawvideo -pix_fmt rgba -s %dx%d -i - "
|
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 - "
|
// 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);
|
// "-threads 0 -preset fast -y -crf 21 -vf vflip %s",width,height,mp4FileName);
|
||||||
|
|
||||||
if (m_data->m_ffmpegFile)
|
if (m_data->m_ffmpegFile)
|
||||||
{
|
{
|
||||||
pclose(m_data->m_ffmpegFile);
|
pclose(m_data->m_ffmpegFile);
|
||||||
}
|
}
|
||||||
if (mp4FileName)
|
if (mp4FileName)
|
||||||
{
|
{
|
||||||
m_data->m_ffmpegFile = popen(cmd, "w");
|
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)
|
void SimpleOpenGL3App::dumpNextFrameToPng(const char* filename)
|
||||||
|
|||||||
@@ -138,8 +138,34 @@ IF(BUILD_CLSOCKET)
|
|||||||
)
|
)
|
||||||
ENDIF()
|
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 VERSION ${BULLET_VERSION})
|
||||||
SET_TARGET_PROPERTIES(App_RobotSimulator PROPERTIES DEBUG_POSTFIX "_d")
|
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)
|
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);
|
sim->setJointMotorControl(m_data->m_quadrupedUniqueId,i,controlArgs);
|
||||||
}
|
}
|
||||||
|
|
||||||
//right front leg
|
b3Scalar startAngle = B3_HALF_PI;
|
||||||
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["motor_front_rightR_joint"],1.57);
|
b3Scalar upperLegLength = 11.5;
|
||||||
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_front_rightR_link"],-2.2);
|
b3Scalar lowerLegLength = 20;
|
||||||
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["motor_front_rightL_joint"],-1.57);
|
b3Scalar kneeAngle = B3_PI+b3Acos(upperLegLength/lowerLegLength);
|
||||||
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_front_rightL_link"],2.2);
|
|
||||||
|
b3Scalar motorDirs[8] = {-1,-1,-1,-1,1,1,1,1};
|
||||||
b3JointInfo jointInfo;
|
b3JointInfo jointInfo;
|
||||||
jointInfo.m_jointType = ePoint2PointType;
|
jointInfo.m_jointType = ePoint2PointType;
|
||||||
jointInfo.m_parentFrame[0] = 0; jointInfo.m_parentFrame[1] = 0.01; jointInfo.m_parentFrame[2] = 0.2;
|
//left front leg
|
||||||
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_front_leftL_joint"],motorDirs[0] * startAngle);
|
||||||
sim->createConstraint(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_front_rightR_link"],
|
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_front_leftL_link"],motorDirs[0]*kneeAngle);
|
||||||
m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_front_rightL_link"],&jointInfo);
|
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["motor_front_leftR_joint"],motorDirs[1] * startAngle);
|
||||||
setDesiredMotorAngle(sim,"motor_front_rightR_joint",1.57);
|
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_front_leftR_link"],motorDirs[1]*kneeAngle);
|
||||||
setDesiredMotorAngle(sim,"motor_front_rightL_joint",-1.57);
|
|
||||||
|
|
||||||
//left front leg
|
jointInfo.m_parentFrame[0] = 0; jointInfo.m_parentFrame[1] = 0.005; jointInfo.m_parentFrame[2] = 0.2;
|
||||||
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["motor_front_leftR_joint"],1.57);
|
jointInfo.m_childFrame[0] = 0; jointInfo.m_childFrame[1] = 0.01; jointInfo.m_childFrame[2] = 0.2;
|
||||||
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;
|
|
||||||
sim->createConstraint(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_front_leftR_link"],
|
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);
|
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", motorDirs[0] * startAngle);
|
||||||
setDesiredMotorAngle(sim,"motor_front_leftL_joint", -1.57);
|
setDesiredMotorAngle(sim,"motor_front_leftR_joint", motorDirs[1] * startAngle);
|
||||||
|
|
||||||
//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);
|
|
||||||
|
|
||||||
//left back leg
|
//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["motor_back_leftL_joint"],motorDirs[2] * startAngle);
|
||||||
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["knee_back_leftL_link"],motorDirs[2] * kneeAngle);
|
||||||
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["motor_back_leftR_joint"],motorDirs[3] * startAngle);
|
||||||
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_back_leftL_link"],2.2);
|
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.01; jointInfo.m_parentFrame[2] = 0.2;
|
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.015; jointInfo.m_childFrame[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"],
|
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);
|
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", motorDirs[2] * startAngle);
|
||||||
setDesiredMotorAngle(sim,"motor_back_leftL_joint", -1.57);
|
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)
|
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_startPosition = startPos;
|
||||||
args.m_startOrientation = startOrn;
|
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);
|
int numJoints = sim->getNumJoints(m_data->m_quadrupedUniqueId);
|
||||||
for (int i=0;i<numJoints;i++)
|
for (int i=0;i<numJoints;i++)
|
||||||
|
|||||||
@@ -16,17 +16,22 @@ int main(int argc, char* argv[])
|
|||||||
//sim->connect(eCONNECT_UDP, "localhost", 1234);
|
//sim->connect(eCONNECT_UDP, "localhost", 1234);
|
||||||
sim->configureDebugVisualizer( COV_ENABLE_GUI, 0);
|
sim->configureDebugVisualizer( COV_ENABLE_GUI, 0);
|
||||||
// sim->configureDebugVisualizer( COV_ENABLE_SHADOWS, 0);//COV_ENABLE_WIREFRAME
|
// 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
|
//syncBodies is only needed when connecting to an existing physics server that has already some bodies
|
||||||
sim->syncBodies();
|
sim->syncBodies();
|
||||||
|
b3Scalar fixedTimeStep = 1./240.;
|
||||||
|
|
||||||
sim->setTimeStep(1./240.);
|
sim->setTimeStep(fixedTimeStep);
|
||||||
|
|
||||||
sim->setGravity(b3MakeVector3(0,0,-10));
|
b3Quaternion q = sim->getQuaternionFromEuler(b3MakeVector3(0.1,0.2,0.3));
|
||||||
|
b3Vector3 rpy;
|
||||||
|
rpy = sim->getEulerFromQuaternion(q);
|
||||||
|
|
||||||
int blockId = sim->loadURDF("cube.urdf");
|
sim->setGravity(b3MakeVector3(0,0,-9.8));
|
||||||
b3BodyInfo bodyInfo;
|
|
||||||
sim->getBodyInfo(blockId,&bodyInfo);
|
//int blockId = sim->loadURDF("cube.urdf");
|
||||||
|
//b3BodyInfo bodyInfo;
|
||||||
|
//sim->getBodyInfo(blockId,&bodyInfo);
|
||||||
|
|
||||||
sim->loadURDF("plane.urdf");
|
sim->loadURDF("plane.urdf");
|
||||||
|
|
||||||
@@ -34,15 +39,15 @@ int main(int argc, char* argv[])
|
|||||||
int minitaurUid = minitaur.setupMinitaur(sim, b3MakeVector3(0,0,.3));
|
int minitaurUid = minitaur.setupMinitaur(sim, b3MakeVector3(0,0,.3));
|
||||||
|
|
||||||
|
|
||||||
b3RobotSimulatorLoadUrdfFileArgs args;
|
//b3RobotSimulatorLoadUrdfFileArgs args;
|
||||||
args.m_startPosition.setValue(2,0,1);
|
//args.m_startPosition.setValue(2,0,1);
|
||||||
int r2d2 = sim->loadURDF("r2d2.urdf",args);
|
//int r2d2 = sim->loadURDF("r2d2.urdf",args);
|
||||||
|
|
||||||
b3RobotSimulatorLoadFileResults sdfResults;
|
//b3RobotSimulatorLoadFileResults sdfResults;
|
||||||
if (!sim->loadSDF("two_cubes.sdf",sdfResults))
|
//if (!sim->loadSDF("two_cubes.sdf",sdfResults))
|
||||||
{
|
//{
|
||||||
b3Warning("Can't load SDF!\n");
|
// b3Warning("Can't load SDF!\n");
|
||||||
}
|
//}
|
||||||
|
|
||||||
b3Clock clock;
|
b3Clock clock;
|
||||||
double startTime = clock.getTimeInSeconds();
|
double startTime = clock.getTimeInSeconds();
|
||||||
@@ -54,6 +59,9 @@ int main(int argc, char* argv[])
|
|||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
sim->setRealTimeSimulation(false);
|
sim->setRealTimeSimulation(false);
|
||||||
|
int vidLogId = -1;
|
||||||
|
int minitaurLogId = -1;
|
||||||
|
int rotateCamera = 0;
|
||||||
|
|
||||||
while (sim->canSubmitCommand())
|
while (sim->canSubmitCommand())
|
||||||
{
|
{
|
||||||
@@ -62,14 +70,63 @@ int main(int argc, char* argv[])
|
|||||||
if (keyEvents.m_numKeyboardEvents)
|
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
|
//m_keyState is a flag combination of eButtonIsDown,eButtonTriggered, eButtonReleased
|
||||||
for (int i=0;i<keyEvents.m_numKeyboardEvents;i++)
|
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");
|
printf("sim->disconnect\n");
|
||||||
|
|||||||
@@ -1,17 +1,16 @@
|
|||||||
#include "b3RobotSimulatorClientAPI.h"
|
#include "b3RobotSimulatorClientAPI.h"
|
||||||
|
|
||||||
|
|
||||||
//#include "SharedMemoryCommands.h"
|
//#include "SharedMemoryCommands.h"
|
||||||
|
|
||||||
#include "SharedMemory/PhysicsClientC_API.h"
|
#include "SharedMemory/PhysicsClientC_API.h"
|
||||||
|
|
||||||
#ifdef BT_ENABLE_ENET
|
#ifdef BT_ENABLE_ENET
|
||||||
#include "SharedMemory/PhysicsClientUDP_C_API.h"
|
#include "SharedMemory/PhysicsClientUDP_C_API.h"
|
||||||
#endif//PHYSICS_UDP
|
#endif //PHYSICS_UDP
|
||||||
|
|
||||||
#ifdef BT_ENABLE_CLSOCKET
|
#ifdef BT_ENABLE_CLSOCKET
|
||||||
#include "SharedMemory/PhysicsClientTCP_C_API.h"
|
#include "SharedMemory/PhysicsClientTCP_C_API.h"
|
||||||
#endif//PHYSICS_TCP
|
#endif //PHYSICS_TCP
|
||||||
|
|
||||||
#include "SharedMemory/PhysicsDirectC_API.h"
|
#include "SharedMemory/PhysicsDirectC_API.h"
|
||||||
|
|
||||||
@@ -25,7 +24,7 @@ struct b3RobotSimulatorClientAPI_InternalData
|
|||||||
b3PhysicsClientHandle m_physicsClientHandle;
|
b3PhysicsClientHandle m_physicsClientHandle;
|
||||||
|
|
||||||
b3RobotSimulatorClientAPI_InternalData()
|
b3RobotSimulatorClientAPI_InternalData()
|
||||||
:m_physicsClientHandle(0)
|
: m_physicsClientHandle(0)
|
||||||
{
|
{
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
@@ -35,17 +34,16 @@ b3RobotSimulatorClientAPI::b3RobotSimulatorClientAPI()
|
|||||||
m_data = new b3RobotSimulatorClientAPI_InternalData();
|
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)
|
bool b3RobotSimulatorClientAPI::connect(int mode, const std::string& hostName, int portOrKey)
|
||||||
{
|
{
|
||||||
if (m_data->m_physicsClientHandle)
|
if (m_data->m_physicsClientHandle)
|
||||||
{
|
{
|
||||||
b3Warning ("Already connected, disconnect first.");
|
b3Warning("Already connected, disconnect first.");
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
b3PhysicsClientHandle sm = 0;
|
b3PhysicsClientHandle sm = 0;
|
||||||
@@ -55,85 +53,96 @@ bool b3RobotSimulatorClientAPI::connect(int mode, const std::string& hostName, i
|
|||||||
int key = SHARED_MEMORY_KEY;
|
int key = SHARED_MEMORY_KEY;
|
||||||
bool connected = false;
|
bool connected = false;
|
||||||
|
|
||||||
|
|
||||||
switch (mode)
|
switch (mode)
|
||||||
{
|
{
|
||||||
case eCONNECT_GUI:
|
case eCONNECT_GUI:
|
||||||
{
|
{
|
||||||
int argc = 0;
|
int argc = 0;
|
||||||
char* argv[1] = {0};
|
char* argv[1] = {0};
|
||||||
#ifdef __APPLE__
|
#ifdef __APPLE__
|
||||||
sm = b3CreateInProcessPhysicsServerAndConnectMainThread(argc, argv);
|
sm = b3CreateInProcessPhysicsServerAndConnectMainThread(argc, argv);
|
||||||
#else
|
#else
|
||||||
sm = b3CreateInProcessPhysicsServerAndConnect(argc, argv);
|
sm = b3CreateInProcessPhysicsServerAndConnect(argc, argv);
|
||||||
#endif
|
#endif
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
case eCONNECT_DIRECT: {
|
case eCONNECT_DIRECT:
|
||||||
sm = b3ConnectPhysicsDirect();
|
{
|
||||||
break;
|
sm = b3ConnectPhysicsDirect();
|
||||||
|
break;
|
||||||
}
|
}
|
||||||
case eCONNECT_SHARED_MEMORY: {
|
case eCONNECT_SHARED_MEMORY:
|
||||||
if (portOrKey>=0)
|
{
|
||||||
|
if (portOrKey >= 0)
|
||||||
{
|
{
|
||||||
key = portOrKey;
|
key = portOrKey;
|
||||||
}
|
}
|
||||||
sm = b3ConnectSharedMemory(key);
|
sm = b3ConnectSharedMemory(key);
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
case eCONNECT_UDP:
|
case eCONNECT_UDP:
|
||||||
{
|
{
|
||||||
if (portOrKey>=0)
|
if (portOrKey >= 0)
|
||||||
{
|
{
|
||||||
udpPort = portOrKey;
|
udpPort = portOrKey;
|
||||||
}
|
}
|
||||||
#ifdef BT_ENABLE_ENET
|
#ifdef BT_ENABLE_ENET
|
||||||
|
|
||||||
sm = b3ConnectPhysicsUDP(hostName.c_str(), udpPort);
|
sm = b3ConnectPhysicsUDP(hostName.c_str(), udpPort);
|
||||||
#else
|
#else
|
||||||
b3Warning("UDP is not enabled in this build");
|
b3Warning("UDP is not enabled in this build");
|
||||||
#endif //BT_ENABLE_ENET
|
#endif //BT_ENABLE_ENET
|
||||||
|
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
case eCONNECT_TCP:
|
case eCONNECT_TCP:
|
||||||
{
|
{
|
||||||
if (portOrKey>=0)
|
if (portOrKey >= 0)
|
||||||
{
|
{
|
||||||
tcpPort = portOrKey;
|
tcpPort = portOrKey;
|
||||||
}
|
}
|
||||||
#ifdef BT_ENABLE_CLSOCKET
|
#ifdef BT_ENABLE_CLSOCKET
|
||||||
|
|
||||||
sm = b3ConnectPhysicsTCP(hostName.c_str(), tcpPort);
|
sm = b3ConnectPhysicsTCP(hostName.c_str(), tcpPort);
|
||||||
#else
|
#else
|
||||||
b3Warning("TCP is not enabled in this pybullet build");
|
b3Warning("TCP is not enabled in this pybullet build");
|
||||||
#endif //BT_ENABLE_CLSOCKET
|
#endif //BT_ENABLE_CLSOCKET
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
default:
|
||||||
|
{
|
||||||
default: {
|
b3Warning("connectPhysicsServer unexpected argument");
|
||||||
b3Warning("connectPhysicsServer unexpected argument");
|
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|
||||||
if (sm)
|
if (sm)
|
||||||
{
|
{
|
||||||
m_data->m_physicsClientHandle = sm;
|
m_data->m_physicsClientHandle = sm;
|
||||||
if (!b3CanSubmitCommand(m_data->m_physicsClientHandle))
|
if (!b3CanSubmitCommand(m_data->m_physicsClientHandle))
|
||||||
{
|
{
|
||||||
disconnect();
|
disconnect();
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool b3RobotSimulatorClientAPI::isConnected() const
|
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;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
b3SharedMemoryCommandHandle command;
|
||||||
b3SharedMemoryCommandHandle command;
|
|
||||||
b3SharedMemoryStatusHandle statusHandle;
|
b3SharedMemoryStatusHandle statusHandle;
|
||||||
int statusType;
|
int statusType;
|
||||||
|
|
||||||
command = b3InitSyncBodyInfoCommand(m_data->m_physicsClientHandle);
|
command = b3InitSyncBodyInfoCommand(m_data->m_physicsClientHandle);
|
||||||
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
|
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
|
||||||
statusType = b3GetStatusType(statusHandle);
|
statusType = b3GetStatusType(statusHandle);
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void b3RobotSimulatorClientAPI::resetSimulation()
|
void b3RobotSimulatorClientAPI::resetSimulation()
|
||||||
@@ -175,9 +182,9 @@ void b3RobotSimulatorClientAPI::resetSimulation()
|
|||||||
b3Warning("Not connected");
|
b3Warning("Not connected");
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
b3SharedMemoryStatusHandle statusHandle;
|
b3SharedMemoryStatusHandle statusHandle;
|
||||||
statusHandle = b3SubmitClientCommandAndWaitStatus(
|
statusHandle = b3SubmitClientCommandAndWaitStatus(
|
||||||
m_data->m_physicsClientHandle, b3InitResetSimulationCommand(m_data->m_physicsClientHandle));
|
m_data->m_physicsClientHandle, b3InitResetSimulationCommand(m_data->m_physicsClientHandle));
|
||||||
}
|
}
|
||||||
|
|
||||||
bool b3RobotSimulatorClientAPI::canSubmitCommand() const
|
bool b3RobotSimulatorClientAPI::canSubmitCommand() const
|
||||||
@@ -186,7 +193,7 @@ bool b3RobotSimulatorClientAPI::canSubmitCommand() const
|
|||||||
{
|
{
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
return (b3CanSubmitCommand(m_data->m_physicsClientHandle));
|
return (b3CanSubmitCommand(m_data->m_physicsClientHandle) != 0);
|
||||||
}
|
}
|
||||||
|
|
||||||
void b3RobotSimulatorClientAPI::stepSimulation()
|
void b3RobotSimulatorClientAPI::stepSimulation()
|
||||||
@@ -200,11 +207,11 @@ void b3RobotSimulatorClientAPI::stepSimulation()
|
|||||||
b3SharedMemoryStatusHandle statusHandle;
|
b3SharedMemoryStatusHandle statusHandle;
|
||||||
int statusType;
|
int statusType;
|
||||||
if (b3CanSubmitCommand(m_data->m_physicsClientHandle))
|
if (b3CanSubmitCommand(m_data->m_physicsClientHandle))
|
||||||
{
|
{
|
||||||
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, b3InitStepSimulationCommand(m_data->m_physicsClientHandle));
|
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, b3InitStepSimulationCommand(m_data->m_physicsClientHandle));
|
||||||
statusType = b3GetStatusType(statusHandle);
|
statusType = b3GetStatusType(statusHandle);
|
||||||
b3Assert(statusType==CMD_STEP_FORWARD_SIMULATION_COMPLETED);
|
//b3Assert(statusType == CMD_STEP_FORWARD_SIMULATION_COMPLETED);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void b3RobotSimulatorClientAPI::setGravity(const b3Vector3& gravityAcceleration)
|
void b3RobotSimulatorClientAPI::setGravity(const b3Vector3& gravityAcceleration)
|
||||||
@@ -215,57 +222,26 @@ void b3RobotSimulatorClientAPI::setGravity(const b3Vector3& gravityAcceleration)
|
|||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle);
|
b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle);
|
||||||
b3SharedMemoryStatusHandle statusHandle;
|
b3SharedMemoryStatusHandle statusHandle;
|
||||||
b3PhysicsParamSetGravity(command, gravityAcceleration[0],gravityAcceleration[1],gravityAcceleration[2]);
|
b3PhysicsParamSetGravity(command, gravityAcceleration[0], gravityAcceleration[1], gravityAcceleration[2]);
|
||||||
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
|
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)
|
b3Quaternion b3RobotSimulatorClientAPI::getQuaternionFromEuler(const b3Vector3& rollPitchYaw)
|
||||||
{
|
{
|
||||||
double phi, the, psi;
|
b3Quaternion q;
|
||||||
phi = rollPitchYaw[0] / 2.0;
|
q.setEulerZYX(rollPitchYaw[2],rollPitchYaw[1],rollPitchYaw[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]);
|
|
||||||
return q;
|
return q;
|
||||||
}
|
}
|
||||||
|
|
||||||
b3Vector3 b3RobotSimulatorClientAPI::getEulerFromQuaternion(const b3Quaternion& quat)
|
b3Vector3 b3RobotSimulatorClientAPI::getEulerFromQuaternion(const b3Quaternion& quat)
|
||||||
{
|
{
|
||||||
double squ;
|
b3Scalar roll,pitch,yaw;
|
||||||
double sqx;
|
quat.getEulerZYX(yaw,pitch,roll);
|
||||||
double sqy;
|
b3Vector3 rpy2 = b3MakeVector3(roll,pitch,yaw);
|
||||||
double sqz;
|
return rpy2;
|
||||||
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;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
int b3RobotSimulatorClientAPI::loadURDF(const std::string& fileName, const struct b3RobotSimulatorLoadUrdfFileArgs& args)
|
int b3RobotSimulatorClientAPI::loadURDF(const std::string& fileName, const struct b3RobotSimulatorLoadUrdfFileArgs& args)
|
||||||
@@ -278,8 +254,6 @@ int b3RobotSimulatorClientAPI::loadURDF(const std::string& fileName, const struc
|
|||||||
return robotUniqueId;
|
return robotUniqueId;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
b3SharedMemoryStatusHandle statusHandle;
|
b3SharedMemoryStatusHandle statusHandle;
|
||||||
int statusType;
|
int statusType;
|
||||||
b3SharedMemoryCommandHandle command = b3LoadUrdfCommandInit(m_data->m_physicsClientHandle, fileName.c_str());
|
b3SharedMemoryCommandHandle command = b3LoadUrdfCommandInit(m_data->m_physicsClientHandle, fileName.c_str());
|
||||||
@@ -287,22 +261,19 @@ int b3RobotSimulatorClientAPI::loadURDF(const std::string& fileName, const struc
|
|||||||
//setting the initial position, orientation and other arguments are optional
|
//setting the initial position, orientation and other arguments are optional
|
||||||
|
|
||||||
b3LoadUrdfCommandSetStartPosition(command, args.m_startPosition[0],
|
b3LoadUrdfCommandSetStartPosition(command, args.m_startPosition[0],
|
||||||
args.m_startPosition[1],
|
args.m_startPosition[1],
|
||||||
args.m_startPosition[2]);
|
args.m_startPosition[2]);
|
||||||
b3LoadUrdfCommandSetStartOrientation(command,args.m_startOrientation[0]
|
b3LoadUrdfCommandSetStartOrientation(command, args.m_startOrientation[0], args.m_startOrientation[1], args.m_startOrientation[2], args.m_startOrientation[3]);
|
||||||
,args.m_startOrientation[1]
|
|
||||||
,args.m_startOrientation[2]
|
|
||||||
,args.m_startOrientation[3]);
|
|
||||||
if (args.m_forceOverrideFixedBase)
|
if (args.m_forceOverrideFixedBase)
|
||||||
{
|
{
|
||||||
b3LoadUrdfCommandSetUseFixedBase(command,true);
|
b3LoadUrdfCommandSetUseFixedBase(command, true);
|
||||||
}
|
}
|
||||||
b3LoadUrdfCommandSetUseMultiBody(command, args.m_useMultiBody);
|
b3LoadUrdfCommandSetUseMultiBody(command, args.m_useMultiBody);
|
||||||
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
|
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
|
||||||
statusType = b3GetStatusType(statusHandle);
|
statusType = b3GetStatusType(statusHandle);
|
||||||
|
|
||||||
b3Assert(statusType==CMD_URDF_LOADING_COMPLETED);
|
b3Assert(statusType == CMD_URDF_LOADING_COMPLETED);
|
||||||
if (statusType==CMD_URDF_LOADING_COMPLETED)
|
if (statusType == CMD_URDF_LOADING_COMPLETED)
|
||||||
{
|
{
|
||||||
robotUniqueId = b3GetStatusBodyIndex(statusHandle);
|
robotUniqueId = b3GetStatusBodyIndex(statusHandle);
|
||||||
}
|
}
|
||||||
@@ -329,12 +300,12 @@ bool b3RobotSimulatorClientAPI::loadMJCF(const std::string& fileName, b3RobotSim
|
|||||||
b3Warning("Couldn't load .mjcf file.");
|
b3Warning("Couldn't load .mjcf file.");
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
int numBodies = b3GetStatusBodyIndices(statusHandle, 0,0);
|
int numBodies = b3GetStatusBodyIndices(statusHandle, 0, 0);
|
||||||
if (numBodies)
|
if (numBodies)
|
||||||
{
|
{
|
||||||
results.m_uniqueObjectIds.resize(numBodies);
|
results.m_uniqueObjectIds.resize(numBodies);
|
||||||
int 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;
|
return true;
|
||||||
@@ -359,12 +330,12 @@ bool b3RobotSimulatorClientAPI::loadBullet(const std::string& fileName, b3RobotS
|
|||||||
{
|
{
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
int numBodies = b3GetStatusBodyIndices(statusHandle, 0,0);
|
int numBodies = b3GetStatusBodyIndices(statusHandle, 0, 0);
|
||||||
if (numBodies)
|
if (numBodies)
|
||||||
{
|
{
|
||||||
results.m_uniqueObjectIds.resize(numBodies);
|
results.m_uniqueObjectIds.resize(numBodies);
|
||||||
int 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;
|
return true;
|
||||||
@@ -383,19 +354,18 @@ bool b3RobotSimulatorClientAPI::loadSDF(const std::string& fileName, b3RobotSimu
|
|||||||
b3SharedMemoryStatusHandle statusHandle;
|
b3SharedMemoryStatusHandle statusHandle;
|
||||||
int statusType;
|
int statusType;
|
||||||
b3SharedMemoryCommandHandle command = b3LoadSdfCommandInit(m_data->m_physicsClientHandle, fileName.c_str());
|
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);
|
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
|
||||||
statusType = b3GetStatusType(statusHandle);
|
statusType = b3GetStatusType(statusHandle);
|
||||||
b3Assert(statusType == CMD_SDF_LOADING_COMPLETED);
|
b3Assert(statusType == CMD_SDF_LOADING_COMPLETED);
|
||||||
if (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)
|
if (numBodies)
|
||||||
{
|
{
|
||||||
results.m_uniqueObjectIds.resize(numBodies);
|
results.m_uniqueObjectIds.resize(numBodies);
|
||||||
int 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;
|
statusOk = true;
|
||||||
}
|
}
|
||||||
@@ -411,7 +381,7 @@ bool b3RobotSimulatorClientAPI::getBodyInfo(int bodyUniqueId, struct b3BodyInfo*
|
|||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
int result = b3GetBodyInfo(m_data->m_physicsClientHandle, bodyUniqueId, bodyInfo);
|
int result = b3GetBodyInfo(m_data->m_physicsClientHandle, bodyUniqueId, bodyInfo);
|
||||||
return (result != 0);
|
return (result != 0);
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -431,7 +401,8 @@ bool b3RobotSimulatorClientAPI::getBasePositionAndOrientation(int bodyUniqueId,
|
|||||||
const int status_type = b3GetStatusType(status_handle);
|
const int status_type = b3GetStatusType(status_handle);
|
||||||
const double* actualStateQ;
|
const double* actualStateQ;
|
||||||
|
|
||||||
if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED) {
|
if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED)
|
||||||
|
{
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -466,7 +437,7 @@ bool b3RobotSimulatorClientAPI::resetBasePositionAndOrientation(int bodyUniqueId
|
|||||||
|
|
||||||
b3CreatePoseCommandSetBasePosition(commandHandle, basePosition[0], basePosition[1], basePosition[2]);
|
b3CreatePoseCommandSetBasePosition(commandHandle, basePosition[0], basePosition[1], basePosition[2]);
|
||||||
b3CreatePoseCommandSetBaseOrientation(commandHandle, baseOrientation[0], baseOrientation[1],
|
b3CreatePoseCommandSetBaseOrientation(commandHandle, baseOrientation[0], baseOrientation[1],
|
||||||
baseOrientation[2], baseOrientation[3]);
|
baseOrientation[2], baseOrientation[3]);
|
||||||
|
|
||||||
b3SharedMemoryStatusHandle statusHandle;
|
b3SharedMemoryStatusHandle statusHandle;
|
||||||
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
|
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
|
||||||
@@ -474,6 +445,64 @@ bool b3RobotSimulatorClientAPI::resetBasePositionAndOrientation(int bodyUniqueId
|
|||||||
return true;
|
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)
|
void b3RobotSimulatorClientAPI::setRealTimeSimulation(bool enableRealTimeSimulation)
|
||||||
{
|
{
|
||||||
@@ -500,10 +529,9 @@ int b3RobotSimulatorClientAPI::getNumJoints(int bodyUniqueId) const
|
|||||||
b3Warning("Not connected");
|
b3Warning("Not connected");
|
||||||
return false;
|
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)
|
bool b3RobotSimulatorClientAPI::getJointInfo(int bodyUniqueId, int jointIndex, b3JointInfo* jointInfo)
|
||||||
{
|
{
|
||||||
if (!isConnected())
|
if (!isConnected())
|
||||||
@@ -511,7 +539,7 @@ bool b3RobotSimulatorClientAPI::getJointInfo(int bodyUniqueId, int jointIndex, b
|
|||||||
b3Warning("Not connected");
|
b3Warning("Not connected");
|
||||||
return false;
|
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)
|
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");
|
b3Warning("Not connected");
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
b3SharedMemoryStatusHandle statusHandle;
|
b3SharedMemoryStatusHandle statusHandle;
|
||||||
b3Assert(b3CanSubmitCommand(m_data->m_physicsClientHandle));
|
b3Assert(b3CanSubmitCommand(m_data->m_physicsClientHandle));
|
||||||
if (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));
|
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())
|
if (!isConnected())
|
||||||
{
|
{
|
||||||
b3Warning("Not connected");
|
b3Warning("Not connected");
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(m_data->m_physicsClientHandle,bodyUniqueId);
|
b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(m_data->m_physicsClientHandle, bodyUniqueId);
|
||||||
b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
|
b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
|
||||||
int statusType = b3GetStatusType(statusHandle);
|
int statusType = b3GetStatusType(statusHandle);
|
||||||
if (statusType != CMD_ACTUAL_STATE_UPDATE_COMPLETED)
|
if (statusType == CMD_ACTUAL_STATE_UPDATE_COMPLETED)
|
||||||
{
|
{
|
||||||
if (b3GetJointState(m_data->m_physicsClientHandle, statusHandle, jointIndex, state))
|
if (b3GetJointState(m_data->m_physicsClientHandle, statusHandle, jointIndex, state))
|
||||||
{
|
{
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool b3RobotSimulatorClientAPI::resetJointState(int bodyUniqueId, int jointIndex, double targetValue)
|
bool b3RobotSimulatorClientAPI::resetJointState(int bodyUniqueId, int jointIndex, double targetValue)
|
||||||
{
|
{
|
||||||
b3SharedMemoryCommandHandle commandHandle;
|
b3SharedMemoryCommandHandle commandHandle;
|
||||||
b3SharedMemoryStatusHandle statusHandle;
|
b3SharedMemoryStatusHandle statusHandle;
|
||||||
int numJoints;
|
int numJoints;
|
||||||
|
|
||||||
numJoints = b3GetNumJoints(m_data->m_physicsClientHandle, bodyUniqueId);
|
numJoints = b3GetNumJoints(m_data->m_physicsClientHandle, bodyUniqueId);
|
||||||
if ((jointIndex >= numJoints) || (jointIndex < 0)) {
|
if ((jointIndex >= numJoints) || (jointIndex < 0))
|
||||||
return false;
|
{
|
||||||
}
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
commandHandle = b3CreatePoseCommandInit(m_data->m_physicsClientHandle, bodyUniqueId);
|
commandHandle = b3CreatePoseCommandInit(m_data->m_physicsClientHandle, bodyUniqueId);
|
||||||
|
|
||||||
b3CreatePoseCommandSetJointPosition(m_data->m_physicsClientHandle, commandHandle, jointIndex,
|
b3CreatePoseCommandSetJointPosition(m_data->m_physicsClientHandle, commandHandle, jointIndex,
|
||||||
targetValue);
|
targetValue);
|
||||||
|
|
||||||
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
|
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
void b3RobotSimulatorClientAPI::setJointMotorControl(int bodyUniqueId, int jointIndex, const b3RobotSimulatorJointMotorArgs& args)
|
void b3RobotSimulatorClientAPI::setJointMotorControl(int bodyUniqueId, int jointIndex, const b3RobotSimulatorJointMotorArgs& args)
|
||||||
{
|
{
|
||||||
if (!isConnected())
|
if (!isConnected())
|
||||||
@@ -582,39 +610,39 @@ void b3RobotSimulatorClientAPI::setJointMotorControl(int bodyUniqueId, int joint
|
|||||||
{
|
{
|
||||||
case CONTROL_MODE_VELOCITY:
|
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;
|
b3JointInfo jointInfo;
|
||||||
b3GetJointInfo(m_data->m_physicsClientHandle, bodyUniqueId, jointIndex, &jointInfo);
|
b3GetJointInfo(m_data->m_physicsClientHandle, bodyUniqueId, jointIndex, &jointInfo);
|
||||||
int uIndex = jointInfo.m_uIndex;
|
int uIndex = jointInfo.m_uIndex;
|
||||||
b3JointControlSetKd(command,uIndex,args.m_kd);
|
b3JointControlSetKd(command, uIndex, args.m_kd);
|
||||||
b3JointControlSetDesiredVelocity(command,uIndex,args.m_targetVelocity);
|
b3JointControlSetDesiredVelocity(command, uIndex, args.m_targetVelocity);
|
||||||
b3JointControlSetMaximumForce(command,uIndex,args.m_maxTorqueValue);
|
b3JointControlSetMaximumForce(command, uIndex, args.m_maxTorqueValue);
|
||||||
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
|
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
case CONTROL_MODE_POSITION_VELOCITY_PD:
|
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;
|
b3JointInfo jointInfo;
|
||||||
b3GetJointInfo(m_data->m_physicsClientHandle, bodyUniqueId, jointIndex, &jointInfo);
|
b3GetJointInfo(m_data->m_physicsClientHandle, bodyUniqueId, jointIndex, &jointInfo);
|
||||||
int uIndex = jointInfo.m_uIndex;
|
int uIndex = jointInfo.m_uIndex;
|
||||||
int qIndex = jointInfo.m_qIndex;
|
int qIndex = jointInfo.m_qIndex;
|
||||||
|
|
||||||
b3JointControlSetDesiredPosition(command,qIndex,args.m_targetPosition);
|
b3JointControlSetDesiredPosition(command, qIndex, args.m_targetPosition);
|
||||||
b3JointControlSetKp(command,uIndex,args.m_kp);
|
b3JointControlSetKp(command, uIndex, args.m_kp);
|
||||||
b3JointControlSetDesiredVelocity(command,uIndex,args.m_targetVelocity);
|
b3JointControlSetDesiredVelocity(command, uIndex, args.m_targetVelocity);
|
||||||
b3JointControlSetKd(command,uIndex,args.m_kd);
|
b3JointControlSetKd(command, uIndex, args.m_kd);
|
||||||
b3JointControlSetMaximumForce(command,uIndex,args.m_maxTorqueValue);
|
b3JointControlSetMaximumForce(command, uIndex, args.m_maxTorqueValue);
|
||||||
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
|
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
case CONTROL_MODE_TORQUE:
|
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;
|
b3JointInfo jointInfo;
|
||||||
b3GetJointInfo(m_data->m_physicsClientHandle, bodyUniqueId, jointIndex, &jointInfo);
|
b3GetJointInfo(m_data->m_physicsClientHandle, bodyUniqueId, jointIndex, &jointInfo);
|
||||||
int uIndex = jointInfo.m_uIndex;
|
int uIndex = jointInfo.m_uIndex;
|
||||||
b3JointControlSetDesiredForceTorque(command,uIndex,args.m_maxTorqueValue);
|
b3JointControlSetDesiredForceTorque(command, uIndex, args.m_maxTorqueValue);
|
||||||
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
|
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
@@ -625,7 +653,6 @@ void b3RobotSimulatorClientAPI::setJointMotorControl(int bodyUniqueId, int joint
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
void b3RobotSimulatorClientAPI::setNumSolverIterations(int numIterations)
|
void b3RobotSimulatorClientAPI::setNumSolverIterations(int numIterations)
|
||||||
{
|
{
|
||||||
if (!isConnected())
|
if (!isConnected())
|
||||||
@@ -633,12 +660,11 @@ void b3RobotSimulatorClientAPI::setNumSolverIterations(int numIterations)
|
|||||||
b3Warning("Not connected");
|
b3Warning("Not connected");
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle);
|
b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle);
|
||||||
b3SharedMemoryStatusHandle statusHandle;
|
b3SharedMemoryStatusHandle statusHandle;
|
||||||
b3PhysicsParamSetNumSolverIterations(command, numIterations);
|
b3PhysicsParamSetNumSolverIterations(command, numIterations);
|
||||||
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
|
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
|
||||||
b3Assert(b3GetStatusType(statusHandle)==CMD_CLIENT_COMMAND_COMPLETED);
|
b3Assert(b3GetStatusType(statusHandle) == CMD_CLIENT_COMMAND_COMPLETED);
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void b3RobotSimulatorClientAPI::setTimeStep(double timeStepInSeconds)
|
void b3RobotSimulatorClientAPI::setTimeStep(double timeStepInSeconds)
|
||||||
@@ -654,10 +680,8 @@ void b3RobotSimulatorClientAPI::setTimeStep(double timeStepInSeconds)
|
|||||||
int ret;
|
int ret;
|
||||||
ret = b3PhysicsParamSetTimeStep(command, timeStepInSeconds);
|
ret = b3PhysicsParamSetTimeStep(command, timeStepInSeconds);
|
||||||
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
|
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
void b3RobotSimulatorClientAPI::setNumSimulationSubSteps(int numSubSteps)
|
void b3RobotSimulatorClientAPI::setNumSimulationSubSteps(int numSubSteps)
|
||||||
{
|
{
|
||||||
if (!isConnected())
|
if (!isConnected())
|
||||||
@@ -665,14 +689,13 @@ void b3RobotSimulatorClientAPI::setNumSimulationSubSteps(int numSubSteps)
|
|||||||
b3Warning("Not connected");
|
b3Warning("Not connected");
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle);
|
b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle);
|
||||||
b3SharedMemoryStatusHandle statusHandle;
|
b3SharedMemoryStatusHandle statusHandle;
|
||||||
b3PhysicsParamSetNumSubSteps(command, numSubSteps);
|
b3PhysicsParamSetNumSubSteps(command, numSubSteps);
|
||||||
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
|
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
|
||||||
b3Assert(b3GetStatusType(statusHandle)==CMD_CLIENT_COMMAND_COMPLETED);
|
b3Assert(b3GetStatusType(statusHandle) == CMD_CLIENT_COMMAND_COMPLETED);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
bool b3RobotSimulatorClientAPI::calculateInverseKinematics(const struct b3RobotSimulatorInverseKinematicArgs& args, struct b3RobotSimulatorInverseKinematicsResults& results)
|
bool b3RobotSimulatorClientAPI::calculateInverseKinematics(const struct b3RobotSimulatorInverseKinematicArgs& args, struct b3RobotSimulatorInverseKinematicsResults& results)
|
||||||
{
|
{
|
||||||
if (!isConnected())
|
if (!isConnected())
|
||||||
@@ -680,55 +703,52 @@ bool b3RobotSimulatorClientAPI::calculateInverseKinematics(const struct b3RobotS
|
|||||||
b3Warning("Not connected");
|
b3Warning("Not connected");
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
b3Assert(args.m_endEffectorLinkIndex>=0);
|
b3Assert(args.m_endEffectorLinkIndex >= 0);
|
||||||
b3Assert(args.m_bodyUniqueId>=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))
|
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);
|
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_NULL_SPACE_VELOCITY)
|
}
|
||||||
{
|
else if (args.m_flags & B3_HAS_IK_TARGET_ORIENTATION)
|
||||||
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);
|
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)
|
if (args.m_flags & B3_HAS_JOINT_DAMPING)
|
||||||
{
|
{
|
||||||
b3CalculateInverseKinematicsSetJointDamping(command, args.m_numDegreeOfFreedom, &args.m_jointDamping[0]);
|
b3CalculateInverseKinematicsSetJointDamping(command, args.m_numDegreeOfFreedom, &args.m_jointDamping[0]);
|
||||||
}
|
}
|
||||||
|
|
||||||
b3SharedMemoryStatusHandle statusHandle;
|
b3SharedMemoryStatusHandle statusHandle;
|
||||||
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
|
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
|
||||||
|
|
||||||
|
int numPos = 0;
|
||||||
|
|
||||||
int numPos=0;
|
|
||||||
|
|
||||||
bool result = b3GetStatusInverseKinematicsJointPositions(statusHandle,
|
bool result = b3GetStatusInverseKinematicsJointPositions(statusHandle,
|
||||||
&results.m_bodyUniqueId,
|
&results.m_bodyUniqueId,
|
||||||
&numPos,
|
&numPos,
|
||||||
0)!=0;
|
0) != 0;
|
||||||
if (result && numPos)
|
if (result && numPos)
|
||||||
{
|
{
|
||||||
results.m_calculatedJointPositions.resize(numPos);
|
results.m_calculatedJointPositions.resize(numPos);
|
||||||
result = b3GetStatusInverseKinematicsJointPositions(statusHandle,
|
result = b3GetStatusInverseKinematicsJointPositions(statusHandle,
|
||||||
&results.m_bodyUniqueId,
|
&results.m_bodyUniqueId,
|
||||||
&numPos,
|
&numPos,
|
||||||
&results.m_calculatedJointPositions[0])!=0;
|
&results.m_calculatedJointPositions[0]) != 0;
|
||||||
|
}
|
||||||
}
|
|
||||||
return result;
|
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)
|
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())
|
if (!isConnected())
|
||||||
@@ -736,12 +756,12 @@ bool b3RobotSimulatorClientAPI::getBodyJacobian(int bodyUniqueId, int linkIndex,
|
|||||||
b3Warning("Not connected");
|
b3Warning("Not connected");
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
b3SharedMemoryCommandHandle command = b3CalculateJacobianCommandInit(m_data->m_physicsClientHandle, bodyUniqueId, linkIndex, localPosition, jointPositions, jointVelocities, jointAccelerations);
|
b3SharedMemoryCommandHandle command = b3CalculateJacobianCommandInit(m_data->m_physicsClientHandle, bodyUniqueId, linkIndex, localPosition, jointPositions, jointVelocities, jointAccelerations);
|
||||||
b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
|
b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
|
||||||
|
|
||||||
if (b3GetStatusType(statusHandle) == CMD_CALCULATED_JACOBIAN_COMPLETED)
|
if (b3GetStatusType(statusHandle) == CMD_CALCULATED_JACOBIAN_COMPLETED)
|
||||||
{
|
{
|
||||||
b3GetStatusJacobian(statusHandle, linearJacobian, angularJacobian);
|
b3GetStatusJacobian(statusHandle, linearJacobian, angularJacobian);
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
return false;
|
return false;
|
||||||
@@ -754,14 +774,14 @@ bool b3RobotSimulatorClientAPI::getLinkState(int bodyUniqueId, int linkIndex, b3
|
|||||||
b3Warning("Not connected");
|
b3Warning("Not connected");
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(m_data->m_physicsClientHandle,bodyUniqueId);
|
b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(m_data->m_physicsClientHandle, bodyUniqueId);
|
||||||
b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
|
b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
|
||||||
|
|
||||||
if (b3GetStatusType(statusHandle) == CMD_ACTUAL_STATE_UPDATE_COMPLETED)
|
if (b3GetStatusType(statusHandle) == CMD_ACTUAL_STATE_UPDATE_COMPLETED)
|
||||||
{
|
{
|
||||||
b3GetLinkState(m_data->m_physicsClientHandle, statusHandle, linkIndex, linkState);
|
b3GetLinkState(m_data->m_physicsClientHandle, statusHandle, linkIndex, linkState);
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -787,7 +807,7 @@ void b3RobotSimulatorClientAPI::getVREvents(b3VREventsData* vrEventsData)
|
|||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
b3SharedMemoryCommandHandle commandHandle = b3RequestVREventsCommandInit(m_data->m_physicsClientHandle);
|
b3SharedMemoryCommandHandle commandHandle = b3RequestVREventsCommandInit(m_data->m_physicsClientHandle);
|
||||||
b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
|
b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
|
||||||
b3GetVREventsData(m_data->m_physicsClientHandle, vrEventsData);
|
b3GetVREventsData(m_data->m_physicsClientHandle, vrEventsData);
|
||||||
}
|
}
|
||||||
@@ -802,32 +822,35 @@ void b3RobotSimulatorClientAPI::getKeyboardEvents(b3KeyboardEventsData* keyboard
|
|||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
b3SharedMemoryCommandHandle commandHandle = b3RequestKeyboardEventsCommandInit(m_data->m_physicsClientHandle);
|
b3SharedMemoryCommandHandle commandHandle = b3RequestKeyboardEventsCommandInit(m_data->m_physicsClientHandle);
|
||||||
b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
|
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 maxLogDof)
|
||||||
int b3RobotSimulatorClientAPI::startStateLogging(b3StateLoggingType loggingType, const std::string& fileName, const b3AlignedObjectArray<int>& objectUniqueIds)
|
|
||||||
{
|
{
|
||||||
int loggingUniqueId = -1;
|
int loggingUniqueId = -1;
|
||||||
b3SharedMemoryCommandHandle commandHandle;
|
b3SharedMemoryCommandHandle commandHandle;
|
||||||
commandHandle = b3StateLoggingCommandInit(m_data->m_physicsClientHandle);
|
commandHandle = b3StateLoggingCommandInit(m_data->m_physicsClientHandle);
|
||||||
|
|
||||||
b3StateLoggingStart(commandHandle,loggingType,fileName.c_str());
|
b3StateLoggingStart(commandHandle, loggingType, fileName.c_str());
|
||||||
|
|
||||||
for ( int i=0;i<objectUniqueIds.size();i++)
|
for (int i = 0; i < objectUniqueIds.size(); i++)
|
||||||
{
|
{
|
||||||
int objectUid = objectUniqueIds[i];
|
int objectUid = objectUniqueIds[i];
|
||||||
b3StateLoggingAddLoggingObjectUniqueId(commandHandle,objectUid);
|
b3StateLoggingAddLoggingObjectUniqueId(commandHandle, objectUid);
|
||||||
|
}
|
||||||
|
|
||||||
|
if (maxLogDof > 0)
|
||||||
|
{
|
||||||
|
b3StateLoggingSetMaxLogDof(commandHandle, maxLogDof);
|
||||||
}
|
}
|
||||||
|
|
||||||
b3SharedMemoryStatusHandle statusHandle;
|
b3SharedMemoryStatusHandle statusHandle;
|
||||||
int statusType;
|
int statusType;
|
||||||
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
|
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
|
||||||
statusType = b3GetStatusType(statusHandle);
|
statusType = b3GetStatusType(statusHandle);
|
||||||
if (statusType==CMD_STATE_LOGGING_START_COMPLETED)
|
if (statusType == CMD_STATE_LOGGING_START_COMPLETED)
|
||||||
{
|
{
|
||||||
loggingUniqueId = b3GetStatusLoggingUniqueId(statusHandle);
|
loggingUniqueId = b3GetStatusLoggingUniqueId(statusHandle);
|
||||||
}
|
}
|
||||||
@@ -843,3 +866,17 @@ void b3RobotSimulatorClientAPI::stopStateLogging(int stateLoggerUniqueId)
|
|||||||
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
|
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>
|
#include <string>
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
struct b3RobotSimulatorLoadUrdfFileArgs
|
struct b3RobotSimulatorLoadUrdfFileArgs
|
||||||
{
|
{
|
||||||
b3Vector3 m_startPosition;
|
b3Vector3 m_startPosition;
|
||||||
b3Quaternion m_startOrientation;
|
b3Quaternion m_startOrientation;
|
||||||
bool m_forceOverrideFixedBase;
|
bool m_forceOverrideFixedBase;
|
||||||
bool m_useMultiBody;
|
bool m_useMultiBody;
|
||||||
|
|
||||||
b3RobotSimulatorLoadUrdfFileArgs()
|
b3RobotSimulatorLoadUrdfFileArgs()
|
||||||
:
|
: m_startPosition(b3MakeVector3(0, 0, 0)),
|
||||||
m_startPosition(b3MakeVector3(0,0,0)),
|
m_startOrientation(b3Quaternion(0, 0, 0, 1)),
|
||||||
m_startOrientation(b3Quaternion(0,0,0,1)),
|
m_forceOverrideFixedBase(false),
|
||||||
m_forceOverrideFixedBase(false),
|
m_useMultiBody(true)
|
||||||
m_useMultiBody(true)
|
|
||||||
{
|
{
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
@@ -33,17 +28,15 @@ struct b3RobotSimulatorLoadUrdfFileArgs
|
|||||||
struct b3RobotSimulatorLoadSdfFileArgs
|
struct b3RobotSimulatorLoadSdfFileArgs
|
||||||
{
|
{
|
||||||
bool m_forceOverrideFixedBase;
|
bool m_forceOverrideFixedBase;
|
||||||
bool m_useMultiBody;
|
bool m_useMultiBody;
|
||||||
|
|
||||||
b3RobotSimulatorLoadSdfFileArgs()
|
b3RobotSimulatorLoadSdfFileArgs()
|
||||||
:
|
: m_forceOverrideFixedBase(false),
|
||||||
m_forceOverrideFixedBase(false),
|
m_useMultiBody(true)
|
||||||
m_useMultiBody(true)
|
|
||||||
{
|
{
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
struct b3RobotSimulatorLoadFileResults
|
struct b3RobotSimulatorLoadFileResults
|
||||||
{
|
{
|
||||||
b3AlignedObjectArray<int> m_uniqueObjectIds;
|
b3AlignedObjectArray<int> m_uniqueObjectIds;
|
||||||
@@ -65,52 +58,52 @@ struct b3RobotSimulatorJointMotorArgs
|
|||||||
double m_maxTorqueValue;
|
double m_maxTorqueValue;
|
||||||
|
|
||||||
b3RobotSimulatorJointMotorArgs(int controlMode)
|
b3RobotSimulatorJointMotorArgs(int controlMode)
|
||||||
:m_controlMode(controlMode),
|
: m_controlMode(controlMode),
|
||||||
m_targetPosition(0),
|
m_targetPosition(0),
|
||||||
m_kp(0.1),
|
m_kp(0.1),
|
||||||
m_targetVelocity(0),
|
m_targetVelocity(0),
|
||||||
m_kd(0.9),
|
m_kd(0.9),
|
||||||
m_maxTorqueValue(1000)
|
m_maxTorqueValue(1000)
|
||||||
{
|
{
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|
||||||
enum b3RobotSimulatorInverseKinematicsFlags
|
enum b3RobotSimulatorInverseKinematicsFlags
|
||||||
{
|
{
|
||||||
B3_HAS_IK_TARGET_ORIENTATION=1,
|
B3_HAS_IK_TARGET_ORIENTATION = 1,
|
||||||
B3_HAS_NULL_SPACE_VELOCITY=2,
|
B3_HAS_NULL_SPACE_VELOCITY = 2,
|
||||||
B3_HAS_JOINT_DAMPING=4,
|
B3_HAS_JOINT_DAMPING = 4,
|
||||||
};
|
};
|
||||||
|
|
||||||
struct b3RobotSimulatorInverseKinematicArgs
|
struct b3RobotSimulatorInverseKinematicArgs
|
||||||
{
|
{
|
||||||
int m_bodyUniqueId;
|
int m_bodyUniqueId;
|
||||||
// double* m_currentJointPositions;
|
// double* m_currentJointPositions;
|
||||||
// int m_numPositions;
|
// int m_numPositions;
|
||||||
double m_endEffectorTargetPosition[3];
|
double m_endEffectorTargetPosition[3];
|
||||||
double m_endEffectorTargetOrientation[4];
|
double m_endEffectorTargetOrientation[4];
|
||||||
int m_endEffectorLinkIndex;
|
int m_endEffectorLinkIndex;
|
||||||
int m_flags;
|
int m_flags;
|
||||||
int m_numDegreeOfFreedom;
|
int m_numDegreeOfFreedom;
|
||||||
b3AlignedObjectArray<double> m_lowerLimits;
|
b3AlignedObjectArray<double> m_lowerLimits;
|
||||||
b3AlignedObjectArray<double> m_upperLimits;
|
b3AlignedObjectArray<double> m_upperLimits;
|
||||||
b3AlignedObjectArray<double> m_jointRanges;
|
b3AlignedObjectArray<double> m_jointRanges;
|
||||||
b3AlignedObjectArray<double> m_restPoses;
|
b3AlignedObjectArray<double> m_restPoses;
|
||||||
b3AlignedObjectArray<double> m_jointDamping;
|
b3AlignedObjectArray<double> m_jointDamping;
|
||||||
|
|
||||||
b3RobotSimulatorInverseKinematicArgs()
|
b3RobotSimulatorInverseKinematicArgs()
|
||||||
:m_bodyUniqueId(-1),
|
: m_bodyUniqueId(-1),
|
||||||
m_endEffectorLinkIndex(-1),
|
m_endEffectorLinkIndex(-1),
|
||||||
m_flags(0)
|
m_flags(0)
|
||||||
{
|
{
|
||||||
m_endEffectorTargetPosition[0]=0;
|
m_endEffectorTargetPosition[0] = 0;
|
||||||
m_endEffectorTargetPosition[1]=0;
|
m_endEffectorTargetPosition[1] = 0;
|
||||||
m_endEffectorTargetPosition[2]=0;
|
m_endEffectorTargetPosition[2] = 0;
|
||||||
|
|
||||||
m_endEffectorTargetOrientation[0]=0;
|
m_endEffectorTargetOrientation[0] = 0;
|
||||||
m_endEffectorTargetOrientation[1]=0;
|
m_endEffectorTargetOrientation[1] = 0;
|
||||||
m_endEffectorTargetOrientation[2]=0;
|
m_endEffectorTargetOrientation[2] = 0;
|
||||||
m_endEffectorTargetOrientation[3]=1;
|
m_endEffectorTargetOrientation[3] = 1;
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|
||||||
@@ -120,7 +113,6 @@ struct b3RobotSimulatorInverseKinematicsResults
|
|||||||
b3AlignedObjectArray<double> m_calculatedJointPositions;
|
b3AlignedObjectArray<double> m_calculatedJointPositions;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
///The b3RobotSimulatorClientAPI is pretty much the C++ version of pybullet
|
///The b3RobotSimulatorClientAPI is pretty much the C++ version of pybullet
|
||||||
///as documented in the pybullet Quickstart Guide
|
///as documented in the pybullet Quickstart Guide
|
||||||
///https://docs.google.com/document/d/10sXEhzFRSnvFcl3XxNGhnD4N2SedqwdAvK3dsihxVUA
|
///https://docs.google.com/document/d/10sXEhzFRSnvFcl3XxNGhnD4N2SedqwdAvK3dsihxVUA
|
||||||
@@ -132,12 +124,14 @@ public:
|
|||||||
b3RobotSimulatorClientAPI();
|
b3RobotSimulatorClientAPI();
|
||||||
virtual ~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();
|
void disconnect();
|
||||||
|
|
||||||
bool isConnected() const;
|
bool isConnected() const;
|
||||||
|
|
||||||
|
void setTimeOut(double timeOutInSec);
|
||||||
|
|
||||||
void syncBodies();
|
void syncBodies();
|
||||||
|
|
||||||
void resetSimulation();
|
void resetSimulation();
|
||||||
@@ -145,8 +139,8 @@ public:
|
|||||||
b3Quaternion getQuaternionFromEuler(const b3Vector3& rollPitchYaw);
|
b3Quaternion getQuaternionFromEuler(const b3Vector3& rollPitchYaw);
|
||||||
b3Vector3 getEulerFromQuaternion(const b3Quaternion& quat);
|
b3Vector3 getEulerFromQuaternion(const b3Quaternion& quat);
|
||||||
|
|
||||||
int loadURDF(const std::string& fileName, const struct b3RobotSimulatorLoadUrdfFileArgs& args=b3RobotSimulatorLoadUrdfFileArgs());
|
int loadURDF(const std::string& fileName, const struct b3RobotSimulatorLoadUrdfFileArgs& args = b3RobotSimulatorLoadUrdfFileArgs());
|
||||||
bool loadSDF(const std::string& fileName, b3RobotSimulatorLoadFileResults& results, const struct b3RobotSimulatorLoadSdfFileArgs& args=b3RobotSimulatorLoadSdfFileArgs());
|
bool loadSDF(const std::string& fileName, b3RobotSimulatorLoadFileResults& results, const struct b3RobotSimulatorLoadSdfFileArgs& args = b3RobotSimulatorLoadSdfFileArgs());
|
||||||
bool loadMJCF(const std::string& fileName, b3RobotSimulatorLoadFileResults& results);
|
bool loadMJCF(const std::string& fileName, b3RobotSimulatorLoadFileResults& results);
|
||||||
bool loadBullet(const std::string& fileName, b3RobotSimulatorLoadFileResults& results);
|
bool loadBullet(const std::string& fileName, b3RobotSimulatorLoadFileResults& results);
|
||||||
|
|
||||||
@@ -155,13 +149,16 @@ public:
|
|||||||
bool getBasePositionAndOrientation(int bodyUniqueId, b3Vector3& basePosition, b3Quaternion& baseOrientation) const;
|
bool getBasePositionAndOrientation(int bodyUniqueId, b3Vector3& basePosition, b3Quaternion& baseOrientation) const;
|
||||||
bool resetBasePositionAndOrientation(int bodyUniqueId, b3Vector3& basePosition, b3Quaternion& baseOrientation);
|
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;
|
int getNumJoints(int bodyUniqueId) const;
|
||||||
|
|
||||||
bool getJointInfo(int bodyUniqueId, int jointIndex, b3JointInfo* jointInfo);
|
bool getJointInfo(int bodyUniqueId, int jointIndex, b3JointInfo* jointInfo);
|
||||||
|
|
||||||
void createConstraint(int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, b3JointInfo* jointInfo);
|
void createConstraint(int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, b3JointInfo* jointInfo);
|
||||||
|
|
||||||
bool getJointState(int bodyUniqueId, int jointIndex, struct b3JointSensorState *state);
|
bool getJointState(int bodyUniqueId, int jointIndex, struct b3JointSensorState* state);
|
||||||
|
|
||||||
bool resetJointState(int bodyUniqueId, int jointIndex, double targetValue);
|
bool resetJointState(int bodyUniqueId, int jointIndex, double targetValue);
|
||||||
|
|
||||||
@@ -176,23 +173,23 @@ public:
|
|||||||
void setGravity(const b3Vector3& gravityAcceleration);
|
void setGravity(const b3Vector3& gravityAcceleration);
|
||||||
|
|
||||||
void setTimeStep(double timeStepInSeconds);
|
void setTimeStep(double timeStepInSeconds);
|
||||||
void setNumSimulationSubSteps(int numSubSteps);
|
void setNumSimulationSubSteps(int numSubSteps);
|
||||||
void setNumSolverIterations(int numIterations);
|
void setNumSolverIterations(int numIterations);
|
||||||
|
|
||||||
bool calculateInverseKinematics(const struct b3RobotSimulatorInverseKinematicArgs& args, struct b3RobotSimulatorInverseKinematicsResults& results);
|
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);
|
bool getLinkState(int bodyUniqueId, int linkIndex, b3LinkState* linkState);
|
||||||
|
|
||||||
void configureDebugVisualizer(enum b3ConfigureDebugVisualizerEnum flag, int enable);
|
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 stopStateLogging(int stateLoggerUniqueId);
|
||||||
|
|
||||||
void getVREvents(b3VREventsData* vrEventsData);
|
void getVREvents(b3VREventsData* vrEventsData);
|
||||||
void getKeyboardEvents(b3KeyboardEventsData* keyboardEventsData);
|
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
|
PhysicsServerCommandProcessor.h
|
||||||
TinyRendererVisualShapeConverter.cpp
|
TinyRendererVisualShapeConverter.cpp
|
||||||
TinyRendererVisualShapeConverter.h
|
TinyRendererVisualShapeConverter.h
|
||||||
|
SharedMemoryCommands.h
|
||||||
|
SharedMemoryPublic.h
|
||||||
../TinyRenderer/geometry.cpp
|
../TinyRenderer/geometry.cpp
|
||||||
../TinyRenderer/model.cpp
|
../TinyRenderer/model.cpp
|
||||||
../TinyRenderer/tgaimage.cpp
|
../TinyRenderer/tgaimage.cpp
|
||||||
|
|||||||
@@ -914,7 +914,7 @@ b3SharedMemoryStatusHandle b3ProcessServerStatus(b3PhysicsClientHandle physClien
|
|||||||
int b3GetStatusType(b3SharedMemoryStatusHandle statusHandle)
|
int b3GetStatusType(b3SharedMemoryStatusHandle statusHandle)
|
||||||
{
|
{
|
||||||
const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle;
|
const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle;
|
||||||
b3Assert(status);
|
//b3Assert(status);
|
||||||
if (status)
|
if (status)
|
||||||
{
|
{
|
||||||
return status->m_type;
|
return status->m_type;
|
||||||
@@ -1063,7 +1063,7 @@ b3SharedMemoryStatusHandle b3SubmitClientCommandAndWaitStatus(b3PhysicsClientHan
|
|||||||
|
|
||||||
b3SubmitClientCommand(physClient, commandHandle);
|
b3SubmitClientCommand(physClient, commandHandle);
|
||||||
|
|
||||||
while ((statusHandle == 0) && (clock.getTimeInSeconds()-startTime < timeOutInSeconds))
|
while (cl->isConnected() && (statusHandle == 0) && (clock.getTimeInSeconds()-startTime < timeOutInSeconds))
|
||||||
{
|
{
|
||||||
statusHandle = b3ProcessServerStatus(physClient);
|
statusHandle = b3ProcessServerStatus(physClient);
|
||||||
}
|
}
|
||||||
@@ -2585,6 +2585,20 @@ int b3StateLoggingAddLoggingObjectUniqueId(b3SharedMemoryCommandHandle commandHa
|
|||||||
}
|
}
|
||||||
return 0;
|
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)
|
int b3StateLoggingStop(b3SharedMemoryCommandHandle commandHandle, int loggingUid)
|
||||||
{
|
{
|
||||||
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
|
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
|
||||||
|
|||||||
@@ -356,9 +356,13 @@ void b3GetKeyboardEventsData(b3PhysicsClientHandle physClient, struct b3Keyboard
|
|||||||
b3SharedMemoryCommandHandle b3StateLoggingCommandInit(b3PhysicsClientHandle physClient);
|
b3SharedMemoryCommandHandle b3StateLoggingCommandInit(b3PhysicsClientHandle physClient);
|
||||||
int b3StateLoggingStart(b3SharedMemoryCommandHandle commandHandle, int loggingType, const char* fileName);
|
int b3StateLoggingStart(b3SharedMemoryCommandHandle commandHandle, int loggingType, const char* fileName);
|
||||||
int b3StateLoggingAddLoggingObjectUniqueId(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId);
|
int b3StateLoggingAddLoggingObjectUniqueId(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId);
|
||||||
|
int b3StateLoggingSetMaxLogDof(b3SharedMemoryCommandHandle commandHandle, int maxLogDof);
|
||||||
|
|
||||||
int b3GetStatusLoggingUniqueId(b3SharedMemoryStatusHandle statusHandle);
|
int b3GetStatusLoggingUniqueId(b3SharedMemoryStatusHandle statusHandle);
|
||||||
int b3StateLoggingStop(b3SharedMemoryCommandHandle commandHandle, int loggingUniqueId);
|
int b3StateLoggingStop(b3SharedMemoryCommandHandle commandHandle, int loggingUniqueId);
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
void b3SetTimeOut(b3PhysicsClientHandle physClient, double timeOutInSeconds);
|
void b3SetTimeOut(b3PhysicsClientHandle physClient, double timeOutInSeconds);
|
||||||
double b3GetTimeOut(b3PhysicsClientHandle physClient);
|
double b3GetTimeOut(b3PhysicsClientHandle physClient);
|
||||||
|
|
||||||
|
|||||||
@@ -48,7 +48,7 @@ bool gResetSimulation = 0;
|
|||||||
int gVRTrackingObjectUniqueId = -1;
|
int gVRTrackingObjectUniqueId = -1;
|
||||||
btTransform gVRTrackingObjectTr = btTransform::getIdentity();
|
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 gCreateObjectSimVR = -1;
|
||||||
int gEnableKukaControl = 0;
|
int gEnableKukaControl = 0;
|
||||||
btVector3 gVRTeleportPos1(0,0,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
|
struct MinitaurStateLogger : public InternalStateLogger
|
||||||
{
|
{
|
||||||
int m_loggingTimeStamp;
|
int m_loggingTimeStamp;
|
||||||
@@ -454,6 +479,7 @@ struct MinitaurStateLogger : public InternalStateLogger
|
|||||||
m_logFileHandle(0),
|
m_logFileHandle(0),
|
||||||
m_minitaurMultiBody(minitaurMultiBody)
|
m_minitaurMultiBody(minitaurMultiBody)
|
||||||
{
|
{
|
||||||
|
m_loggingUniqueId = loggingUniqueId;
|
||||||
m_loggingType = STATE_LOGGING_MINITAUR;
|
m_loggingType = STATE_LOGGING_MINITAUR;
|
||||||
m_motorIdList.resize(motorIdList.size());
|
m_motorIdList.resize(motorIdList.size());
|
||||||
for (int m=0;m<motorIdList.size();m++)
|
for (int m=0;m<motorIdList.size();m++)
|
||||||
@@ -512,7 +538,8 @@ struct MinitaurStateLogger : public InternalStateLogger
|
|||||||
|
|
||||||
MinitaurLogRecord logData;
|
MinitaurLogRecord logData;
|
||||||
//'t', 'r', 'p', 'y', 'q0', 'q1', 'q2', 'q3', 'q4', 'q5', 'q6', 'q7', 'u0', 'u1', 'u2', 'u3', 'u4', 'u5', 'u6', 'u7', 'xd', 'mo'
|
//'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();
|
btQuaternion orn = m_minitaurMultiBody->getBaseWorldTransform().getRotation();
|
||||||
btMatrix3x3 mat(orn);
|
btMatrix3x3 mat(orn);
|
||||||
@@ -568,13 +595,16 @@ struct GenericRobotStateLogger : public InternalStateLogger
|
|||||||
btMultiBodyDynamicsWorld* m_dynamicsWorld;
|
btMultiBodyDynamicsWorld* m_dynamicsWorld;
|
||||||
btAlignedObjectArray<int> m_bodyIdList;
|
btAlignedObjectArray<int> m_bodyIdList;
|
||||||
bool m_filterObjectUniqueId;
|
bool m_filterObjectUniqueId;
|
||||||
|
int m_maxLogDof;
|
||||||
|
|
||||||
GenericRobotStateLogger(int loggingUniqueId, const std::string& fileName, btMultiBodyDynamicsWorld* dynamicsWorld)
|
GenericRobotStateLogger(int loggingUniqueId, const std::string& fileName, btMultiBodyDynamicsWorld* dynamicsWorld, int maxLogDof)
|
||||||
:m_loggingTimeStamp(0),
|
:m_loggingTimeStamp(0),
|
||||||
m_logFileHandle(0),
|
m_logFileHandle(0),
|
||||||
m_dynamicsWorld(dynamicsWorld),
|
m_dynamicsWorld(dynamicsWorld),
|
||||||
m_filterObjectUniqueId(false)
|
m_filterObjectUniqueId(false),
|
||||||
|
m_maxLogDof(maxLogDof)
|
||||||
{
|
{
|
||||||
|
m_loggingUniqueId = loggingUniqueId;
|
||||||
m_loggingType = STATE_LOGGING_GENERIC_ROBOT;
|
m_loggingType = STATE_LOGGING_GENERIC_ROBOT;
|
||||||
|
|
||||||
btAlignedObjectArray<std::string> structNames;
|
btAlignedObjectArray<std::string> structNames;
|
||||||
@@ -595,32 +625,24 @@ struct GenericRobotStateLogger : public InternalStateLogger
|
|||||||
structNames.push_back("omegaY");
|
structNames.push_back("omegaY");
|
||||||
structNames.push_back("omegaZ");
|
structNames.push_back("omegaZ");
|
||||||
structNames.push_back("qNum");
|
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 = "IfIfffffffffffffIffffffffffffffffffffffff";
|
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);
|
||||||
|
}
|
||||||
|
|
||||||
const char* fileNameC = fileName.c_str();
|
const char* fileNameC = fileName.c_str();
|
||||||
|
|
||||||
m_logFileHandle = createMinitaurLogFile(fileNameC, structNames, m_structTypes);
|
m_logFileHandle = createMinitaurLogFile(fileNameC, structNames, m_structTypes);
|
||||||
@@ -699,7 +721,7 @@ struct GenericRobotStateLogger : public InternalStateLogger
|
|||||||
logData.m_values.push_back(q);
|
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;
|
float q = 0.0;
|
||||||
logData.m_values.push_back(q);
|
logData.m_values.push_back(q);
|
||||||
@@ -713,7 +735,7 @@ struct GenericRobotStateLogger : public InternalStateLogger
|
|||||||
logData.m_values.push_back(u);
|
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;
|
float u = 0.0;
|
||||||
logData.m_values.push_back(u);
|
logData.m_values.push_back(u);
|
||||||
@@ -897,6 +919,7 @@ struct PhysicsServerCommandProcessorInternalData
|
|||||||
|
|
||||||
//data for picking objects
|
//data for picking objects
|
||||||
class btRigidBody* m_pickedBody;
|
class btRigidBody* m_pickedBody;
|
||||||
|
int m_savedActivationState;
|
||||||
class btTypedConstraint* m_pickedConstraint;
|
class btTypedConstraint* m_pickedConstraint;
|
||||||
class btMultiBodyPoint2Point* m_pickingMultiBodyPoint2Point;
|
class btMultiBodyPoint2Point* m_pickingMultiBodyPoint2Point;
|
||||||
btVector3 m_oldPickingPos;
|
btVector3 m_oldPickingPos;
|
||||||
@@ -1125,6 +1148,7 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld()
|
|||||||
m_data->m_dynamicsWorld->setGravity(btVector3(0, 0, 0));
|
m_data->m_dynamicsWorld->setGravity(btVector3(0, 0, 0));
|
||||||
m_data->m_dynamicsWorld->getSolverInfo().m_erp2 = 0.08;
|
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_linearSlop = 0.00001;
|
||||||
m_data->m_dynamicsWorld->getSolverInfo().m_numIterations = 50;
|
m_data->m_dynamicsWorld->getSolverInfo().m_numIterations = 50;
|
||||||
m_data->m_dynamicsWorld->getSolverInfo().m_leastSquaresResidualThreshold = 1e-7;
|
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();
|
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;
|
bool useFixedBase = false;
|
||||||
MyMJCFLogger2 logger;
|
MyMJCFLogger2 logger;
|
||||||
@@ -1800,6 +1824,17 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
|
|||||||
if (clientCmd.m_updateFlags & STATE_LOGGING_START_LOG)
|
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)
|
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;
|
std::string fileName = clientCmd.m_stateLoggingArguments.m_fileName;
|
||||||
|
|
||||||
int loggerUid = m_data->m_stateLoggersUniqueId++;
|
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))
|
if ((clientCmd.m_updateFlags & STATE_LOGGING_FILTER_OBJECT_UNIQUE_ID) && (clientCmd.m_stateLoggingArguments.m_numBodyUniqueIds>0))
|
||||||
{
|
{
|
||||||
logger->m_filterObjectUniqueId = true;
|
logger->m_filterObjectUniqueId = true;
|
||||||
for (int i = 0; i < clientCmd.m_stateLoggingArguments.m_numBodyUniqueIds; ++i)
|
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 remain = totalNumVisualShapes - clientCmd.m_requestVisualShapeDataArguments.m_startingVisualShapeIndex;
|
||||||
int shapeIndex = 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,
|
shapeIndex,
|
||||||
visualShapeStoragePtr);
|
visualShapeStoragePtr);
|
||||||
|
if (success) {
|
||||||
|
serverCmd.m_sendVisualShapeArgs.m_numRemainingVisualShapes = remain-1;
|
||||||
//m_visualConverter
|
serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied = 1;
|
||||||
serverCmd.m_sendVisualShapeArgs.m_numRemainingVisualShapes = remain-1;
|
serverCmd.m_sendVisualShapeArgs.m_startingVisualShapeIndex = clientCmd.m_requestVisualShapeDataArguments.m_startingVisualShapeIndex;
|
||||||
serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied = 1;
|
serverCmd.m_sendVisualShapeArgs.m_bodyUniqueId = clientCmd.m_requestVisualShapeDataArguments.m_bodyUniqueId;
|
||||||
serverCmd.m_sendVisualShapeArgs.m_startingVisualShapeIndex = clientCmd.m_requestVisualShapeDataArguments.m_startingVisualShapeIndex;
|
serverCmd.m_numDataStreamBytes = sizeof(b3VisualShapeData)*serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied;
|
||||||
serverCmd.m_sendVisualShapeArgs.m_bodyUniqueId = clientCmd.m_requestVisualShapeDataArguments.m_bodyUniqueId;
|
serverCmd.m_type = CMD_VISUAL_SHAPE_INFO_COMPLETED;
|
||||||
serverCmd.m_numDataStreamBytes = sizeof(b3VisualShapeData)*serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied;
|
}
|
||||||
serverCmd.m_type =CMD_VISUAL_SHAPE_INFO_COMPLETED;
|
hasStatus = true;
|
||||||
hasStatus = true;
|
break;
|
||||||
break;
|
|
||||||
}
|
}
|
||||||
case CMD_UPDATE_VISUAL_SHAPE:
|
case CMD_UPDATE_VISUAL_SHAPE:
|
||||||
{
|
{
|
||||||
@@ -4939,6 +4979,7 @@ bool PhysicsServerCommandProcessor::pickBody(const btVector3& rayFromWorld, cons
|
|||||||
if (!(body->isStaticObject() || body->isKinematicObject()))
|
if (!(body->isStaticObject() || body->isKinematicObject()))
|
||||||
{
|
{
|
||||||
m_data->m_pickedBody = body;
|
m_data->m_pickedBody = body;
|
||||||
|
m_data->m_savedActivationState = body->getActivationState();
|
||||||
m_data->m_pickedBody->setActivationState(DISABLE_DEACTIVATION);
|
m_data->m_pickedBody->setActivationState(DISABLE_DEACTIVATION);
|
||||||
//printf("pickPos=%f,%f,%f\n",pickPos.getX(),pickPos.getY(),pickPos.getZ());
|
//printf("pickPos=%f,%f,%f\n",pickPos.getX(),pickPos.getY(),pickPos.getZ());
|
||||||
btVector3 localPivot = body->getCenterOfMassTransform().inverse() * pickPos;
|
btVector3 localPivot = body->getCenterOfMassTransform().inverse() * pickPos;
|
||||||
@@ -5031,7 +5072,7 @@ void PhysicsServerCommandProcessor::removePickingConstraint()
|
|||||||
m_data->m_dynamicsWorld->removeConstraint(m_data->m_pickedConstraint);
|
m_data->m_dynamicsWorld->removeConstraint(m_data->m_pickedConstraint);
|
||||||
delete m_data->m_pickedConstraint;
|
delete m_data->m_pickedConstraint;
|
||||||
m_data->m_pickedConstraint = 0;
|
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;
|
m_data->m_pickedBody = 0;
|
||||||
}
|
}
|
||||||
if (m_data->m_pickingMultiBodyPoint2Point)
|
if (m_data->m_pickingMultiBodyPoint2Point)
|
||||||
|
|||||||
@@ -179,8 +179,10 @@ enum MultiThreadedGUIHelperCommunicationEnums
|
|||||||
eGUIUserDebugAddParameter,
|
eGUIUserDebugAddParameter,
|
||||||
eGUIUserDebugRemoveItem,
|
eGUIUserDebugRemoveItem,
|
||||||
eGUIUserDebugRemoveAllItems,
|
eGUIUserDebugRemoveAllItems,
|
||||||
|
eGUIDumpFramesToVideo,
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
#include <stdio.h>
|
#include <stdio.h>
|
||||||
//#include "BulletMultiThreaded/PlatformDefinitions.h"
|
//#include "BulletMultiThreaded/PlatformDefinitions.h"
|
||||||
|
|
||||||
@@ -313,12 +315,15 @@ void MotionThreadFunc(void* userPtr,void* lsMemory)
|
|||||||
b3Clock::usleep(0);
|
b3Clock::usleep(0);
|
||||||
}
|
}
|
||||||
|
|
||||||
if (numCmdSinceSleep1ms>gMaxNumCmdPer1ms)
|
if (gMaxNumCmdPer1ms>0)
|
||||||
{
|
{
|
||||||
BT_PROFILE("usleep(1000)");
|
if (numCmdSinceSleep1ms>gMaxNumCmdPer1ms)
|
||||||
b3Clock::usleep(1000);
|
{
|
||||||
numCmdSinceSleep1ms = 0;
|
BT_PROFILE("usleep(10)");
|
||||||
sleepClock.reset();
|
b3Clock::usleep(10);
|
||||||
|
numCmdSinceSleep1ms = 0;
|
||||||
|
sleepClock.reset();
|
||||||
|
}
|
||||||
}
|
}
|
||||||
if (sleepClock.getTimeMilliseconds()>1)
|
if (sleepClock.getTimeMilliseconds()>1)
|
||||||
{
|
{
|
||||||
@@ -486,7 +491,7 @@ void MotionThreadFunc(void* userPtr,void* lsMemory)
|
|||||||
args->m_cs->unlock();
|
args->m_cs->unlock();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
args->m_physicsServerPtr->disconnectSharedMemory(true);
|
||||||
//do nothing
|
//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();
|
m_multiThreadedHelper->mainThreadRelease();
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
case eGUIDumpFramesToVideo:
|
||||||
|
{
|
||||||
|
m_multiThreadedHelper->m_childGuiHelper->dumpFramesToVideo(m_multiThreadedHelper->m_mp4FileName);
|
||||||
|
m_multiThreadedHelper->mainThreadRelease();
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
case eGUIHelperIdle:
|
case eGUIHelperIdle:
|
||||||
{
|
{
|
||||||
break;
|
break;
|
||||||
|
|||||||
@@ -188,6 +188,8 @@ bool PhysicsServerSharedMemory::connectSharedMemory( struct GUIHelperInterface*
|
|||||||
|
|
||||||
void PhysicsServerSharedMemory::disconnectSharedMemory(bool deInitializeSharedMemory)
|
void PhysicsServerSharedMemory::disconnectSharedMemory(bool deInitializeSharedMemory)
|
||||||
{
|
{
|
||||||
|
m_data->m_commandProcessor->deleteDynamicsWorld();
|
||||||
|
|
||||||
m_data->m_commandProcessor->setGuiHelper(0);
|
m_data->m_commandProcessor->setGuiHelper(0);
|
||||||
|
|
||||||
if (m_data->m_verboseOutput)
|
if (m_data->m_verboseOutput)
|
||||||
|
|||||||
@@ -626,6 +626,7 @@ enum eStateLoggingEnums
|
|||||||
STATE_LOGGING_START_LOG=1,
|
STATE_LOGGING_START_LOG=1,
|
||||||
STATE_LOGGING_STOP_LOG=2,
|
STATE_LOGGING_STOP_LOG=2,
|
||||||
STATE_LOGGING_FILTER_OBJECT_UNIQUE_ID=4,
|
STATE_LOGGING_FILTER_OBJECT_UNIQUE_ID=4,
|
||||||
|
STATE_LOGGING_MAX_LOG_DOF=8
|
||||||
};
|
};
|
||||||
|
|
||||||
struct VRCameraState
|
struct VRCameraState
|
||||||
@@ -644,6 +645,7 @@ struct StateLoggingRequest
|
|||||||
int m_numBodyUniqueIds;////only if ROBOT_LOGGING_FILTER_OBJECT_UNIQUE_ID flag is set
|
int m_numBodyUniqueIds;////only if ROBOT_LOGGING_FILTER_OBJECT_UNIQUE_ID flag is set
|
||||||
int m_bodyUniqueIds[MAX_SDF_BODIES];
|
int m_bodyUniqueIds[MAX_SDF_BODIES];
|
||||||
int m_loggingUniqueId;
|
int m_loggingUniqueId;
|
||||||
|
int m_maxLogDof;
|
||||||
};
|
};
|
||||||
|
|
||||||
struct StateLoggingResultArgs
|
struct StateLoggingResultArgs
|
||||||
|
|||||||
@@ -330,7 +330,8 @@ enum b3StateLoggingType
|
|||||||
STATE_LOGGING_MINITAUR = 0,
|
STATE_LOGGING_MINITAUR = 0,
|
||||||
STATE_LOGGING_GENERIC_ROBOT = 1,
|
STATE_LOGGING_GENERIC_ROBOT = 1,
|
||||||
STATE_LOGGING_VR_CONTROLLERS = 2,
|
STATE_LOGGING_VR_CONTROLLERS = 2,
|
||||||
STATE_LOGGING_COMMANDS = 3,
|
STATE_LOGGING_VIDEO_MP4 = 3,
|
||||||
|
STATE_LOGGING_COMMANDS = 4,
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
@@ -36,13 +36,6 @@ subject to the following restrictions:
|
|||||||
#include "../TinyRenderer/model.h"
|
#include "../TinyRenderer/model.h"
|
||||||
#include "../ThirdPartyLibs/stb_image/stb_image.h"
|
#include "../ThirdPartyLibs/stb_image/stb_image.h"
|
||||||
|
|
||||||
enum MyFileType
|
|
||||||
{
|
|
||||||
MY_FILE_STL=1,
|
|
||||||
MY_FILE_COLLADA=2,
|
|
||||||
MY_FILE_OBJ=3,
|
|
||||||
};
|
|
||||||
|
|
||||||
struct MyTexture2
|
struct MyTexture2
|
||||||
{
|
{
|
||||||
unsigned char* textureData;
|
unsigned char* textureData;
|
||||||
@@ -177,7 +170,7 @@ void TinyRendererVisualShapeConverter::setLightSpecularCoeff(float specularCoeff
|
|||||||
m_data->m_hasLightSpecularCoeff = true;
|
m_data->m_hasLightSpecularCoeff = true;
|
||||||
}
|
}
|
||||||
|
|
||||||
void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray<GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut, btAlignedObjectArray<MyTexture2>& texturesOut, b3VisualShapeData& visualShapeOut)
|
void convertURDFToVisualShape(const UrdfShape* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray<GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut, btAlignedObjectArray<MyTexture2>& texturesOut, b3VisualShapeData& visualShapeOut)
|
||||||
{
|
{
|
||||||
|
|
||||||
visualShapeOut.m_visualGeometryType = visual->m_geometry.m_type;
|
visualShapeOut.m_visualGeometryType = visual->m_geometry.m_type;
|
||||||
@@ -185,6 +178,12 @@ void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPref
|
|||||||
visualShapeOut.m_dimensions[1] = 0;
|
visualShapeOut.m_dimensions[1] = 0;
|
||||||
visualShapeOut.m_dimensions[2] = 0;
|
visualShapeOut.m_dimensions[2] = 0;
|
||||||
visualShapeOut.m_meshAssetFileName[0] = 0;
|
visualShapeOut.m_meshAssetFileName[0] = 0;
|
||||||
|
if (visual->m_geometry.m_hasLocalMaterial) {
|
||||||
|
visualShapeOut.m_rgbaColor[0] = visual->m_geometry.m_localMaterial.m_rgbaColor[0];
|
||||||
|
visualShapeOut.m_rgbaColor[1] = visual->m_geometry.m_localMaterial.m_rgbaColor[1];
|
||||||
|
visualShapeOut.m_rgbaColor[2] = visual->m_geometry.m_localMaterial.m_rgbaColor[2];
|
||||||
|
visualShapeOut.m_rgbaColor[3] = visual->m_geometry.m_localMaterial.m_rgbaColor[3];
|
||||||
|
}
|
||||||
|
|
||||||
GLInstanceGraphicsShape* glmesh = 0;
|
GLInstanceGraphicsShape* glmesh = 0;
|
||||||
|
|
||||||
@@ -193,25 +192,63 @@ void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPref
|
|||||||
switch (visual->m_geometry.m_type)
|
switch (visual->m_geometry.m_type)
|
||||||
{
|
{
|
||||||
case URDF_GEOM_CYLINDER:
|
case URDF_GEOM_CYLINDER:
|
||||||
|
case URDF_GEOM_CAPSULE:
|
||||||
{
|
{
|
||||||
|
btVector3 p1 = visual->m_geometry.m_capsuleFrom;
|
||||||
|
btVector3 p2 = visual->m_geometry.m_capsuleTo;
|
||||||
|
btTransform tr;
|
||||||
|
tr.setIdentity();
|
||||||
|
btScalar rad, len;
|
||||||
|
if (visual->m_geometry.m_hasFromTo) {
|
||||||
|
btVector3 v = p2 - p1;
|
||||||
|
btVector3 center = (p2 + p1) * 0.5;
|
||||||
|
btVector3 up_vector(0,0,1);
|
||||||
|
btVector3 dir = v.normalized();
|
||||||
|
btVector3 axis = dir.cross(up_vector);
|
||||||
|
if (axis.fuzzyZero())
|
||||||
|
{
|
||||||
|
axis = btVector3(0,0,1);
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
axis.normalize();
|
||||||
|
}
|
||||||
|
btQuaternion q(axis, -acos(dir.dot(up_vector)));
|
||||||
|
btTransform capsule_orient(q, center);
|
||||||
|
tr = visual->m_linkLocalFrame * capsule_orient;
|
||||||
|
len = v.length();
|
||||||
|
rad = visual->m_geometry.m_capsuleRadius;
|
||||||
|
} else {
|
||||||
|
tr = visual->m_linkLocalFrame;
|
||||||
|
len = visual->m_geometry.m_capsuleHalfHeight;
|
||||||
|
rad = visual->m_geometry.m_capsuleRadius;
|
||||||
|
}
|
||||||
|
visualShapeOut.m_localVisualFrame[0] = tr.getOrigin()[0];
|
||||||
|
visualShapeOut.m_localVisualFrame[1] = tr.getOrigin()[1];
|
||||||
|
visualShapeOut.m_localVisualFrame[2] = tr.getOrigin()[2];
|
||||||
|
visualShapeOut.m_localVisualFrame[3] = tr.getRotation()[0];
|
||||||
|
visualShapeOut.m_localVisualFrame[4] = tr.getRotation()[1];
|
||||||
|
visualShapeOut.m_localVisualFrame[5] = tr.getRotation()[2];
|
||||||
|
visualShapeOut.m_localVisualFrame[6] = tr.getRotation()[3];
|
||||||
|
visualShapeOut.m_dimensions[0] = len;
|
||||||
|
visualShapeOut.m_dimensions[1] = rad;
|
||||||
|
|
||||||
btAlignedObjectArray<btVector3> vertices;
|
btAlignedObjectArray<btVector3> vertices;
|
||||||
|
|
||||||
visualShapeOut.m_dimensions[0] = visual->m_geometry.m_cylinderLength;
|
|
||||||
visualShapeOut.m_dimensions[1] = visual->m_geometry.m_cylinderRadius;
|
|
||||||
|
|
||||||
//int numVerts = sizeof(barrel_vertices)/(9*sizeof(float));
|
|
||||||
int numSteps = 32;
|
int numSteps = 32;
|
||||||
for (int i = 0; i<numSteps; i++)
|
for (int i = 0; i<numSteps; i++)
|
||||||
{
|
{
|
||||||
|
btVector3 vert(rad*btSin(SIMD_2_PI*(float(i) / numSteps)), rad*btCos(SIMD_2_PI*(float(i) / numSteps)), len / 2.);
|
||||||
btScalar cylRadius = visual->m_geometry.m_cylinderRadius;
|
|
||||||
btScalar cylLength = visual->m_geometry.m_cylinderLength;
|
|
||||||
|
|
||||||
btVector3 vert(cylRadius*btSin(SIMD_2_PI*(float(i) / numSteps)), cylRadius*btCos(SIMD_2_PI*(float(i) / numSteps)), cylLength / 2.);
|
|
||||||
vertices.push_back(vert);
|
vertices.push_back(vert);
|
||||||
vert[2] = -cylLength / 2.;
|
vert[2] = -len / 2.;
|
||||||
vertices.push_back(vert);
|
vertices.push_back(vert);
|
||||||
}
|
}
|
||||||
|
if (visual->m_geometry.m_type==URDF_GEOM_CAPSULE) {
|
||||||
|
// TODO: check if tiny renderer works with that, didn't check -- Oleg
|
||||||
|
btVector3 pole1(0, 0, + len / 2. + rad);
|
||||||
|
btVector3 pole2(0, 0, - len / 2. - rad);
|
||||||
|
vertices.push_back(pole1);
|
||||||
|
vertices.push_back(pole2);
|
||||||
|
}
|
||||||
|
|
||||||
btConvexHullShape* cylZShape = new btConvexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3));
|
btConvexHullShape* cylZShape = new btConvexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3));
|
||||||
cylZShape->setMargin(0.001);
|
cylZShape->setMargin(0.001);
|
||||||
@@ -241,226 +278,145 @@ void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPref
|
|||||||
convexColShape = sphereShape;
|
convexColShape = sphereShape;
|
||||||
convexColShape->setMargin(0.001);
|
convexColShape->setMargin(0.001);
|
||||||
break;
|
break;
|
||||||
|
|
||||||
break;
|
|
||||||
}
|
}
|
||||||
case URDF_GEOM_MESH:
|
case URDF_GEOM_MESH:
|
||||||
{
|
{
|
||||||
if (visual->m_name.length())
|
strncpy(visualShapeOut.m_meshAssetFileName, visual->m_geometry.m_meshFileName.c_str(), VISUAL_SHAPE_MAX_PATH_LEN);
|
||||||
|
visualShapeOut.m_meshAssetFileName[VISUAL_SHAPE_MAX_PATH_LEN-1] = 0;
|
||||||
|
|
||||||
|
visualShapeOut.m_dimensions[0] = visual->m_geometry.m_meshScale[0];
|
||||||
|
visualShapeOut.m_dimensions[1] = visual->m_geometry.m_meshScale[1];
|
||||||
|
visualShapeOut.m_dimensions[2] = visual->m_geometry.m_meshScale[2];
|
||||||
|
|
||||||
|
switch (visual->m_geometry.m_meshFileType)
|
||||||
{
|
{
|
||||||
//b3Printf("visual->name=%s\n", visual->m_name.c_str());
|
case UrdfGeometry::FILE_OBJ:
|
||||||
}
|
|
||||||
if (1)//visual->m_geometry)
|
|
||||||
{
|
|
||||||
if (visual->m_geometry.m_meshFileName.length())
|
|
||||||
{
|
{
|
||||||
const char* filename = visual->m_geometry.m_meshFileName.c_str();
|
//glmesh = LoadMeshFromObj(fullPath,visualPathPrefix);
|
||||||
//b3Printf("mesh->filename=%s\n", filename);
|
b3ImportMeshData meshData;
|
||||||
char fullPath[1024];
|
if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(visual->m_geometry.m_meshFileName, meshData))
|
||||||
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"))
|
|
||||||
{
|
{
|
||||||
fileType = MY_FILE_COLLADA;
|
|
||||||
}
|
|
||||||
if (strstr(fullPath, ".stl"))
|
|
||||||
{
|
|
||||||
fileType = MY_FILE_STL;
|
|
||||||
}
|
|
||||||
if (strstr(fullPath,".obj"))
|
|
||||||
{
|
|
||||||
fileType = MY_FILE_OBJ;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
if (meshData.m_textureImage)
|
||||||
sprintf(fullPath, "%s%s", urdfPathPrefix, filename);
|
|
||||||
|
|
||||||
visualShapeOut.m_dimensions[0] = visual->m_geometry.m_meshScale[0];
|
|
||||||
visualShapeOut.m_dimensions[1] = visual->m_geometry.m_meshScale[1];
|
|
||||||
visualShapeOut.m_dimensions[2] = visual->m_geometry.m_meshScale[2];
|
|
||||||
visualShapeOut.m_localVisualFrame[0] = visual->m_linkLocalFrame.getOrigin()[0];
|
|
||||||
visualShapeOut.m_localVisualFrame[1] = visual->m_linkLocalFrame.getOrigin()[1];
|
|
||||||
visualShapeOut.m_localVisualFrame[2] = visual->m_linkLocalFrame.getOrigin()[2];
|
|
||||||
visualShapeOut.m_localVisualFrame[3] = visual->m_linkLocalFrame.getRotation()[0];
|
|
||||||
visualShapeOut.m_localVisualFrame[4] = visual->m_linkLocalFrame.getRotation()[1];
|
|
||||||
visualShapeOut.m_localVisualFrame[5] = visual->m_linkLocalFrame.getRotation()[2];
|
|
||||||
visualShapeOut.m_localVisualFrame[6] = visual->m_linkLocalFrame.getRotation()[3];
|
|
||||||
|
|
||||||
int sl = strlen(fullPath);
|
|
||||||
if (sl < (VISUAL_SHAPE_MAX_PATH_LEN-1))
|
|
||||||
{
|
|
||||||
memcpy(visualShapeOut.m_meshAssetFileName, fullPath, sl);
|
|
||||||
visualShapeOut.m_meshAssetFileName[sl] = 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
FILE* f = fopen(fullPath, "rb");
|
|
||||||
if (f)
|
|
||||||
{
|
|
||||||
fclose(f);
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
switch (fileType)
|
|
||||||
{
|
{
|
||||||
case MY_FILE_OBJ:
|
MyTexture2 texData;
|
||||||
{
|
texData.m_width = meshData.m_textureWidth;
|
||||||
//glmesh = LoadMeshFromObj(fullPath,visualPathPrefix);
|
texData.m_height = meshData.m_textureHeight;
|
||||||
b3ImportMeshData meshData;
|
texData.textureData = meshData.m_textureImage;
|
||||||
if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(fullPath, meshData))
|
texturesOut.push_back(texData);
|
||||||
{
|
|
||||||
|
|
||||||
if (meshData.m_textureImage)
|
|
||||||
{
|
|
||||||
MyTexture2 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 MY_FILE_STL:
|
|
||||||
{
|
|
||||||
glmesh = LoadMeshFromSTL(fullPath);
|
|
||||||
break;
|
|
||||||
}
|
}
|
||||||
case MY_FILE_COLLADA:
|
glmesh = meshData.m_gfxShape;
|
||||||
|
}
|
||||||
|
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];
|
||||||
|
|
||||||
btAlignedObjectArray<GLInstanceGraphicsShape> visualShapes;
|
}
|
||||||
btAlignedObjectArray<ColladaGraphicsInstance> visualShapeInstances;
|
|
||||||
btTransform upAxisTrans; upAxisTrans.setIdentity();
|
|
||||||
float unitMeterScaling = 1;
|
|
||||||
int upAxis = 2;
|
|
||||||
|
|
||||||
LoadMeshFromCollada(fullPath,
|
int curNumIndices = glmesh->m_indices->size();
|
||||||
visualShapes,
|
int additionalIndices = gfxShape->m_indices->size();
|
||||||
visualShapeInstances,
|
glmesh->m_indices->resize(curNumIndices + additionalIndices);
|
||||||
upAxisTrans,
|
for (int k = 0; k<additionalIndices; k++)
|
||||||
unitMeterScaling,
|
{
|
||||||
upAxis);
|
glmesh->m_indices->at(curNumIndices + k) = gfxShape->m_indices->at(k) + baseIndex;
|
||||||
|
}
|
||||||
|
|
||||||
glmesh = new GLInstanceGraphicsShape;
|
//compensate upAxisTrans and unitMeterScaling here
|
||||||
// int index = 0;
|
btMatrix4x4 upAxisMat;
|
||||||
glmesh->m_indices = new b3AlignedObjectArray<int>();
|
upAxisMat.setIdentity();
|
||||||
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());
|
// upAxisMat.setPureRotation(upAxisTrans.getRotation());
|
||||||
btMatrix4x4 unitMeterScalingMat;
|
btMatrix4x4 unitMeterScalingMat;
|
||||||
unitMeterScalingMat.setPureScaling(btVector3(unitMeterScaling, unitMeterScaling, unitMeterScaling));
|
unitMeterScalingMat.setPureScaling(btVector3(unitMeterScaling, unitMeterScaling, unitMeterScaling));
|
||||||
btMatrix4x4 worldMat = unitMeterScalingMat*upAxisMat*instance->m_worldTransform;
|
btMatrix4x4 worldMat = unitMeterScalingMat*upAxisMat*instance->m_worldTransform;
|
||||||
//btMatrix4x4 worldMat = instance->m_worldTransform;
|
//btMatrix4x4 worldMat = instance->m_worldTransform;
|
||||||
int curNumVertices = glmesh->m_vertices->size();
|
int curNumVertices = glmesh->m_vertices->size();
|
||||||
int additionalVertices = verts.size();
|
int additionalVertices = verts.size();
|
||||||
glmesh->m_vertices->reserve(curNumVertices + additionalVertices);
|
glmesh->m_vertices->reserve(curNumVertices + additionalVertices);
|
||||||
|
|
||||||
for (int v = 0; v<verts.size(); v++)
|
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);
|
btVector3 pos(verts[v].xyzw[0], verts[v].xyzw[1], verts[v].xyzw[2]);
|
||||||
btAssert(0);
|
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]);
|
||||||
}
|
}
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
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);
|
|
||||||
}
|
}
|
||||||
|
glmesh->m_numIndices = glmesh->m_indices->size();
|
||||||
|
glmesh->m_numvertices = glmesh->m_vertices->size();
|
||||||
|
//glmesh = LoadMeshFromCollada(visual->m_geometry.m_meshFileName.c_str());
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
default:
|
||||||
|
// should never get here (findExistingMeshFile returns false if it doesn't recognize extension)
|
||||||
|
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", visual->m_geometry.m_meshFileName.c_str());
|
||||||
|
}
|
||||||
break;
|
break;
|
||||||
}
|
} // case mesh
|
||||||
|
|
||||||
default:
|
default:
|
||||||
{
|
{
|
||||||
b3Warning("Error: unknown visual geometry type\n");
|
b3Warning("TinyRenderer: unknown visual geometry type %i\n", visual->m_geometry.m_type);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -547,17 +503,30 @@ void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPref
|
|||||||
|
|
||||||
|
|
||||||
|
|
||||||
void TinyRendererVisualShapeConverter::convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfModel& model, class btCollisionObject* colObj, int bodyUniqueId)
|
void TinyRendererVisualShapeConverter::convertVisualShapes(
|
||||||
|
int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame,
|
||||||
|
const UrdfLink* linkPtr, const UrdfModel* model,
|
||||||
|
class btCollisionObject* colObj, int bodyUniqueId)
|
||||||
{
|
{
|
||||||
|
btAssert(linkPtr); // TODO: remove if (not doing it now, because diff will be 50+ lines)
|
||||||
|
|
||||||
UrdfLink* const* linkPtr = model.m_links.getAtIndex(linkIndex);
|
|
||||||
if (linkPtr)
|
if (linkPtr)
|
||||||
{
|
{
|
||||||
|
const btArray<UrdfVisual>* shapeArray;
|
||||||
|
bool useVisual;
|
||||||
|
int cnt = 0;
|
||||||
|
if (linkPtr->m_visualArray.size() > 0)
|
||||||
|
{
|
||||||
|
useVisual = true;
|
||||||
|
cnt = linkPtr->m_visualArray.size();
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
// We have to see something, take collision shape. Useful for MuJoCo xml, where there is not visual shape.
|
||||||
|
useVisual = false;
|
||||||
|
cnt = linkPtr->m_collisionArray.size();
|
||||||
|
}
|
||||||
|
|
||||||
const UrdfLink* link = *linkPtr;
|
for (int v1=0; v1<cnt; v1++)
|
||||||
|
|
||||||
for (int v1 = 0; v1 < link->m_visualArray.size();v1++)
|
|
||||||
{
|
{
|
||||||
btAlignedObjectArray<MyTexture2> textures;
|
btAlignedObjectArray<MyTexture2> textures;
|
||||||
btAlignedObjectArray<GLInstanceVertex> vertices;
|
btAlignedObjectArray<GLInstanceVertex> vertices;
|
||||||
@@ -565,20 +534,28 @@ void TinyRendererVisualShapeConverter::convertVisualShapes(int linkIndex, const
|
|||||||
btTransform startTrans; startTrans.setIdentity();
|
btTransform startTrans; startTrans.setIdentity();
|
||||||
//int graphicsIndex = -1;
|
//int graphicsIndex = -1;
|
||||||
|
|
||||||
const UrdfVisual& vis = link->m_visualArray[v1];
|
const UrdfShape* vis;
|
||||||
btTransform childTrans = vis.m_linkLocalFrame;
|
if (useVisual) {
|
||||||
btHashString matName(vis.m_materialName.c_str());
|
vis = &linkPtr->m_visualArray[v1];
|
||||||
UrdfMaterial *const * matPtr = model.m_materials[matName];
|
} else {
|
||||||
|
vis = &linkPtr->m_collisionArray[v1];
|
||||||
|
}
|
||||||
|
btTransform childTrans = vis->m_linkLocalFrame;
|
||||||
|
|
||||||
float rgbaColor[4] = {1,1,1,1};
|
float rgbaColor[4] = {1,1,1,1};
|
||||||
|
if (model && useVisual)
|
||||||
if (matPtr)
|
|
||||||
{
|
{
|
||||||
UrdfMaterial *const mat = *matPtr;
|
btHashString matName(linkPtr->m_visualArray[v1].m_materialName.c_str());
|
||||||
for (int i=0;i<4;i++)
|
UrdfMaterial*const* matPtr = model->m_materials[matName];
|
||||||
rgbaColor[i] = mat->m_rgbaColor[i];
|
if (matPtr)
|
||||||
//printf("UrdfMaterial %s, rgba = %f,%f,%f,%f\n",mat->m_name.c_str(),mat->m_rgbaColor[0],mat->m_rgbaColor[1],mat->m_rgbaColor[2],mat->m_rgbaColor[3]);
|
{
|
||||||
//m_data->m_linkColors.insert(linkIndex,mat->m_rgbaColor);
|
for (int i=0; i<4; i++)
|
||||||
|
{
|
||||||
|
rgbaColor[i] = (*matPtr)->m_rgbaColor[i];
|
||||||
|
}
|
||||||
|
//printf("UrdfMaterial %s, rgba = %f,%f,%f,%f\n",mat->m_name.c_str(),mat->m_rgbaColor[0],mat->m_rgbaColor[1],mat->m_rgbaColor[2],mat->m_rgbaColor[3]);
|
||||||
|
//m_data->m_linkColors.insert(linkIndex,mat->m_rgbaColor);
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
TinyRendererObjectArray** visualsPtr = m_data->m_swRenderInstances[colObj];
|
TinyRendererObjectArray** visualsPtr = m_data->m_swRenderInstances[colObj];
|
||||||
@@ -593,19 +570,19 @@ void TinyRendererVisualShapeConverter::convertVisualShapes(int linkIndex, const
|
|||||||
b3VisualShapeData visualShape;
|
b3VisualShapeData visualShape;
|
||||||
visualShape.m_objectUniqueId = bodyUniqueId;
|
visualShape.m_objectUniqueId = bodyUniqueId;
|
||||||
visualShape.m_linkIndex = linkIndex;
|
visualShape.m_linkIndex = linkIndex;
|
||||||
visualShape.m_localVisualFrame[0] = vis.m_linkLocalFrame.getOrigin()[0];
|
visualShape.m_localVisualFrame[0] = vis->m_linkLocalFrame.getOrigin()[0];
|
||||||
visualShape.m_localVisualFrame[1] = vis.m_linkLocalFrame.getOrigin()[1];
|
visualShape.m_localVisualFrame[1] = vis->m_linkLocalFrame.getOrigin()[1];
|
||||||
visualShape.m_localVisualFrame[2] = vis.m_linkLocalFrame.getOrigin()[2];
|
visualShape.m_localVisualFrame[2] = vis->m_linkLocalFrame.getOrigin()[2];
|
||||||
visualShape.m_localVisualFrame[3] = vis.m_linkLocalFrame.getRotation()[0];
|
visualShape.m_localVisualFrame[3] = vis->m_linkLocalFrame.getRotation()[0];
|
||||||
visualShape.m_localVisualFrame[4] = vis.m_linkLocalFrame.getRotation()[1];
|
visualShape.m_localVisualFrame[4] = vis->m_linkLocalFrame.getRotation()[1];
|
||||||
visualShape.m_localVisualFrame[5] = vis.m_linkLocalFrame.getRotation()[2];
|
visualShape.m_localVisualFrame[5] = vis->m_linkLocalFrame.getRotation()[2];
|
||||||
visualShape.m_localVisualFrame[6] = vis.m_linkLocalFrame.getRotation()[3];
|
visualShape.m_localVisualFrame[6] = vis->m_linkLocalFrame.getRotation()[3];
|
||||||
visualShape.m_rgbaColor[0] = rgbaColor[0];
|
visualShape.m_rgbaColor[0] = rgbaColor[0];
|
||||||
visualShape.m_rgbaColor[1] = rgbaColor[1];
|
visualShape.m_rgbaColor[1] = rgbaColor[1];
|
||||||
visualShape.m_rgbaColor[2] = rgbaColor[2];
|
visualShape.m_rgbaColor[2] = rgbaColor[2];
|
||||||
visualShape.m_rgbaColor[3] = rgbaColor[3];
|
visualShape.m_rgbaColor[3] = rgbaColor[3];
|
||||||
|
|
||||||
convertURDFToVisualShape(&vis, pathPrefix, localInertiaFrame.inverse()*childTrans, vertices, indices,textures, visualShape);
|
convertURDFToVisualShape(vis, pathPrefix, localInertiaFrame.inverse()*childTrans, vertices, indices,textures, visualShape);
|
||||||
m_data->m_visualShapes.push_back(visualShape);
|
m_data->m_visualShapes.push_back(visualShape);
|
||||||
|
|
||||||
if (vertices.size() && indices.size())
|
if (vertices.size() && indices.size())
|
||||||
|
|||||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user