diff --git a/_clang-format b/_clang-format new file mode 100644 index 000000000..43a8b1d04 --- /dev/null +++ b/_clang-format @@ -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 +... + diff --git a/build3/premake4_arm64 b/build3/premake4_arm64 new file mode 100755 index 000000000..3b7ee4ab5 Binary files /dev/null and b/build3/premake4_arm64 differ diff --git a/data/MPL/MPL.xml b/data/MPL/MPL.xml index 5800a1200..2b8408323 100644 --- a/data/MPL/MPL.xml +++ b/data/MPL/MPL.xml @@ -49,30 +49,30 @@ - - - - - - - - + + + + + + + + - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + diff --git a/data/MPL/mesh/index0.STL b/data/MPL/mesh/index0.stl similarity index 100% rename from data/MPL/mesh/index0.STL rename to data/MPL/mesh/index0.stl diff --git a/data/MPL/mesh/index0_collision.stl b/data/MPL/mesh/index0_collision.stl new file mode 100644 index 000000000..9d055eb5f Binary files /dev/null and b/data/MPL/mesh/index0_collision.stl differ diff --git a/data/MPL/mesh/index1.STL b/data/MPL/mesh/index1.stl similarity index 100% rename from data/MPL/mesh/index1.STL rename to data/MPL/mesh/index1.stl diff --git a/data/MPL/mesh/index1_collision.stl b/data/MPL/mesh/index1_collision.stl new file mode 100644 index 000000000..9d11ac8bc Binary files /dev/null and b/data/MPL/mesh/index1_collision.stl differ diff --git a/data/MPL/mesh/index2.STL b/data/MPL/mesh/index2.stl similarity index 100% rename from data/MPL/mesh/index2.STL rename to data/MPL/mesh/index2.stl diff --git a/data/MPL/mesh/index2_collision.stl b/data/MPL/mesh/index2_collision.stl new file mode 100644 index 000000000..3739de3fb Binary files /dev/null and b/data/MPL/mesh/index2_collision.stl differ diff --git a/data/MPL/mesh/index3.STL b/data/MPL/mesh/index3.stl similarity index 100% rename from data/MPL/mesh/index3.STL rename to data/MPL/mesh/index3.stl diff --git a/data/MPL/mesh/index3_collision.stl b/data/MPL/mesh/index3_collision.stl new file mode 100644 index 000000000..ec4237308 Binary files /dev/null and b/data/MPL/mesh/index3_collision.stl differ diff --git a/data/MPL/mesh/middle0.STL b/data/MPL/mesh/middle0.stl similarity index 100% rename from data/MPL/mesh/middle0.STL rename to data/MPL/mesh/middle0.stl diff --git a/data/MPL/mesh/middle0_collision.stl b/data/MPL/mesh/middle0_collision.stl new file mode 100644 index 000000000..1b027f78e Binary files /dev/null and b/data/MPL/mesh/middle0_collision.stl differ diff --git a/data/MPL/mesh/middle1.STL b/data/MPL/mesh/middle1.stl similarity index 100% rename from data/MPL/mesh/middle1.STL rename to data/MPL/mesh/middle1.stl diff --git a/data/MPL/mesh/middle1_collision.stl b/data/MPL/mesh/middle1_collision.stl new file mode 100644 index 000000000..b89440459 Binary files /dev/null and b/data/MPL/mesh/middle1_collision.stl differ diff --git a/data/MPL/mesh/middle2.STL b/data/MPL/mesh/middle2.stl similarity index 100% rename from data/MPL/mesh/middle2.STL rename to data/MPL/mesh/middle2.stl diff --git a/data/MPL/mesh/middle2_collision.stl b/data/MPL/mesh/middle2_collision.stl new file mode 100644 index 000000000..447bf7240 Binary files /dev/null and b/data/MPL/mesh/middle2_collision.stl differ diff --git a/data/MPL/mesh/middle3.STL b/data/MPL/mesh/middle3.stl similarity index 100% rename from data/MPL/mesh/middle3.STL rename to data/MPL/mesh/middle3.stl diff --git a/data/MPL/mesh/middle3_collision.stl b/data/MPL/mesh/middle3_collision.stl new file mode 100644 index 000000000..806a150a2 Binary files /dev/null and b/data/MPL/mesh/middle3_collision.stl differ diff --git a/data/MPL/mesh/palm.STL b/data/MPL/mesh/palm.stl similarity index 100% rename from data/MPL/mesh/palm.STL rename to data/MPL/mesh/palm.stl diff --git a/data/MPL/mesh/palm_collision.stl b/data/MPL/mesh/palm_collision.stl new file mode 100644 index 000000000..08625a6f1 Binary files /dev/null and b/data/MPL/mesh/palm_collision.stl differ diff --git a/data/MPL/mesh/pinky0.STL b/data/MPL/mesh/pinky0.stl similarity index 100% rename from data/MPL/mesh/pinky0.STL rename to data/MPL/mesh/pinky0.stl diff --git a/data/MPL/mesh/pinky0_collision.stl b/data/MPL/mesh/pinky0_collision.stl new file mode 100644 index 000000000..ddc637126 Binary files /dev/null and b/data/MPL/mesh/pinky0_collision.stl differ diff --git a/data/MPL/mesh/pinky1.STL b/data/MPL/mesh/pinky1.stl similarity index 100% rename from data/MPL/mesh/pinky1.STL rename to data/MPL/mesh/pinky1.stl diff --git a/data/MPL/mesh/pinky1_collision.stl b/data/MPL/mesh/pinky1_collision.stl new file mode 100644 index 000000000..c4316bff6 Binary files /dev/null and b/data/MPL/mesh/pinky1_collision.stl differ diff --git a/data/MPL/mesh/pinky2.STL b/data/MPL/mesh/pinky2.stl similarity index 100% rename from data/MPL/mesh/pinky2.STL rename to data/MPL/mesh/pinky2.stl diff --git a/data/MPL/mesh/pinky2_collision.stl b/data/MPL/mesh/pinky2_collision.stl new file mode 100644 index 000000000..375aa5f97 Binary files /dev/null and b/data/MPL/mesh/pinky2_collision.stl differ diff --git a/data/MPL/mesh/pinky3.STL b/data/MPL/mesh/pinky3.stl similarity index 100% rename from data/MPL/mesh/pinky3.STL rename to data/MPL/mesh/pinky3.stl diff --git a/data/MPL/mesh/pinky3_collision.stl b/data/MPL/mesh/pinky3_collision.stl new file mode 100644 index 000000000..fc4981f0f Binary files /dev/null and b/data/MPL/mesh/pinky3_collision.stl differ diff --git a/data/MPL/mesh/ring0.STL b/data/MPL/mesh/ring0.stl similarity index 100% rename from data/MPL/mesh/ring0.STL rename to data/MPL/mesh/ring0.stl diff --git a/data/MPL/mesh/ring0_collision.stl b/data/MPL/mesh/ring0_collision.stl new file mode 100644 index 000000000..9d055eb5f Binary files /dev/null and b/data/MPL/mesh/ring0_collision.stl differ diff --git a/data/MPL/mesh/ring1.STL b/data/MPL/mesh/ring1.stl similarity index 100% rename from data/MPL/mesh/ring1.STL rename to data/MPL/mesh/ring1.stl diff --git a/data/MPL/mesh/ring1_collision.stl b/data/MPL/mesh/ring1_collision.stl new file mode 100644 index 000000000..5b1f6af50 Binary files /dev/null and b/data/MPL/mesh/ring1_collision.stl differ diff --git a/data/MPL/mesh/ring2.STL b/data/MPL/mesh/ring2.stl similarity index 100% rename from data/MPL/mesh/ring2.STL rename to data/MPL/mesh/ring2.stl diff --git a/data/MPL/mesh/ring2_collision.stl b/data/MPL/mesh/ring2_collision.stl new file mode 100644 index 000000000..447bf7240 Binary files /dev/null and b/data/MPL/mesh/ring2_collision.stl differ diff --git a/data/MPL/mesh/ring3.STL b/data/MPL/mesh/ring3.stl similarity index 100% rename from data/MPL/mesh/ring3.STL rename to data/MPL/mesh/ring3.stl diff --git a/data/MPL/mesh/ring3_collision.stl b/data/MPL/mesh/ring3_collision.stl new file mode 100644 index 000000000..ec4237308 Binary files /dev/null and b/data/MPL/mesh/ring3_collision.stl differ diff --git a/data/MPL/mesh/thumb0.STL b/data/MPL/mesh/thumb0.stl similarity index 100% rename from data/MPL/mesh/thumb0.STL rename to data/MPL/mesh/thumb0.stl diff --git a/data/MPL/mesh/thumb0_collision.stl b/data/MPL/mesh/thumb0_collision.stl new file mode 100644 index 000000000..dea0dc8f0 Binary files /dev/null and b/data/MPL/mesh/thumb0_collision.stl differ diff --git a/data/MPL/mesh/thumb1.STL b/data/MPL/mesh/thumb1.stl similarity index 100% rename from data/MPL/mesh/thumb1.STL rename to data/MPL/mesh/thumb1.stl diff --git a/data/MPL/mesh/thumb1_collision.stl b/data/MPL/mesh/thumb1_collision.stl new file mode 100644 index 000000000..97fe1dc9c Binary files /dev/null and b/data/MPL/mesh/thumb1_collision.stl differ diff --git a/data/MPL/mesh/thumb2.STL b/data/MPL/mesh/thumb2.stl similarity index 100% rename from data/MPL/mesh/thumb2.STL rename to data/MPL/mesh/thumb2.stl diff --git a/data/MPL/mesh/thumb2_collision.stl b/data/MPL/mesh/thumb2_collision.stl new file mode 100644 index 000000000..5b1f67f53 Binary files /dev/null and b/data/MPL/mesh/thumb2_collision.stl differ diff --git a/data/MPL/mesh/thumb3.STL b/data/MPL/mesh/thumb3.stl similarity index 100% rename from data/MPL/mesh/thumb3.STL rename to data/MPL/mesh/thumb3.stl diff --git a/data/MPL/mesh/thumb3_collision.stl b/data/MPL/mesh/thumb3_collision.stl new file mode 100644 index 000000000..373c2434d Binary files /dev/null and b/data/MPL/mesh/thumb3_collision.stl differ diff --git a/data/MPL/mesh/wristx.STL b/data/MPL/mesh/wristx.stl similarity index 100% rename from data/MPL/mesh/wristx.STL rename to data/MPL/mesh/wristx.stl diff --git a/data/MPL/mesh/wristx_collision.stl b/data/MPL/mesh/wristx_collision.stl new file mode 100644 index 000000000..d6d5ee465 Binary files /dev/null and b/data/MPL/mesh/wristx_collision.stl differ diff --git a/data/MPL/mesh/wristy.STL b/data/MPL/mesh/wristy.stl similarity index 100% rename from data/MPL/mesh/wristy.STL rename to data/MPL/mesh/wristy.stl diff --git a/data/MPL/mesh/wristy_collision.stl b/data/MPL/mesh/wristy_collision.stl new file mode 100644 index 000000000..4aa00412e Binary files /dev/null and b/data/MPL/mesh/wristy_collision.stl differ diff --git a/data/MPL/mesh/wristz.STL b/data/MPL/mesh/wristz.stl similarity index 100% rename from data/MPL/mesh/wristz.STL rename to data/MPL/mesh/wristz.stl diff --git a/data/MPL/mesh/wristz_collision.stl b/data/MPL/mesh/wristz_collision.stl new file mode 100644 index 000000000..95a0f9b48 Binary files /dev/null and b/data/MPL/mesh/wristz_collision.stl differ diff --git a/data/MPL/mpl2.xml b/data/MPL/mpl2.xml index 7205b15ad..327e72778 100644 --- a/data/MPL/mpl2.xml +++ b/data/MPL/mpl2.xml @@ -49,30 +49,30 @@ - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/data/cartpole.urdf b/data/cartpole.urdf new file mode 100644 index 000000000..c22ca920c --- /dev/null +++ b/data/cartpole.urdf @@ -0,0 +1,74 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/data/cube_soft.urdf b/data/cube_soft.urdf index af40dd379..6354ab3b6 100644 --- a/data/cube_soft.urdf +++ b/data/cube_soft.urdf @@ -2,7 +2,8 @@ - + + diff --git a/data/gripper/wsg50_with_r2d2_gripper.sdf b/data/gripper/wsg50_with_r2d2_gripper.sdf index b022faf00..d67959f19 100644 --- a/data/gripper/wsg50_with_r2d2_gripper.sdf +++ b/data/gripper/wsg50_with_r2d2_gripper.sdf @@ -215,6 +215,10 @@ + + 1.0 + 1.5 + 0.062 0 0.145 0 0 1.5708 0.2 @@ -255,6 +259,10 @@ + + 1.0 + 1.5 + -0.062 0 0.145 0 0 4.71239 0.2 diff --git a/data/kitchens/1.sdf b/data/kitchens/1.sdf index 5f1046837..0bf6112e9 100644 --- a/data/kitchens/1.sdf +++ b/data/kitchens/1.sdf @@ -16,7 +16,7 @@ 0.166667 - + .1 .1 .1 @@ -55,7 +55,7 @@ 0.166667 - + .1 .1 .1 @@ -94,7 +94,7 @@ 0.166667 - + .1 .1 .1 @@ -133,7 +133,7 @@ 0.166667 - + .1 .1 .1 @@ -172,7 +172,7 @@ 0.166667 - + .1 .1 .1 @@ -211,7 +211,7 @@ 0.166667 - + .1 .1 .1 @@ -250,7 +250,7 @@ 0.166667 - + .1 .1 .1 @@ -289,7 +289,7 @@ 0.166667 - + .1 .1 .1 @@ -328,7 +328,7 @@ 0.166667 - + .1 .1 .1 @@ -367,7 +367,7 @@ 0.166667 - + .1 .1 .1 @@ -406,7 +406,7 @@ 0.166667 - + .1 .1 .1 @@ -445,7 +445,7 @@ 0.166667 - + .1 .1 .1 @@ -484,7 +484,7 @@ 0.166667 - + .1 .1 .1 @@ -523,7 +523,7 @@ 0.166667 - + .1 .1 .1 @@ -562,7 +562,7 @@ 0.166667 - + .1 .1 .1 @@ -601,7 +601,7 @@ 0.166667 - + .1 .1 .1 @@ -640,7 +640,7 @@ 0.166667 - + .1 .1 .1 @@ -679,7 +679,7 @@ 0.166667 - + .1 .1 .1 @@ -718,7 +718,7 @@ 0.166667 - + .1 .1 .1 @@ -757,7 +757,7 @@ 0.166667 - + .1 .1 .1 @@ -796,7 +796,7 @@ 0.166667 - + .1 .1 .1 @@ -835,7 +835,7 @@ 0.166667 - + .1 .1 .1 @@ -874,7 +874,7 @@ 0.166667 - + .1 .1 .1 @@ -913,7 +913,7 @@ 0.166667 - + .1 .1 .1 @@ -952,7 +952,7 @@ 0.166667 - + .1 .1 .1 @@ -991,7 +991,7 @@ 0.166667 - + .1 .1 .1 @@ -1030,7 +1030,7 @@ 0.166667 - + .1 .1 .1 @@ -1069,7 +1069,7 @@ 0.166667 - + .1 .1 .1 @@ -1108,7 +1108,7 @@ 0.166667 - + .1 .1 .1 @@ -1147,7 +1147,7 @@ 0.166667 - + .1 .1 .1 @@ -1186,7 +1186,7 @@ 0.166667 - + .1 .1 .1 @@ -1225,7 +1225,7 @@ 0.166667 - + .1 .1 .1 @@ -1264,7 +1264,7 @@ 0.166667 - + .1 .1 .1 @@ -1303,7 +1303,7 @@ 0.166667 - + .1 .1 .1 @@ -1342,7 +1342,7 @@ 0.166667 - + .1 .1 .1 @@ -1381,7 +1381,7 @@ 0.166667 - + .1 .1 .1 @@ -1420,7 +1420,7 @@ 0.166667 - + .1 .1 .1 @@ -1459,7 +1459,7 @@ 0.166667 - + .1 .1 .1 @@ -1498,7 +1498,7 @@ 0.166667 - + .1 .1 .1 @@ -1537,7 +1537,7 @@ 0.166667 - + .1 .1 .1 @@ -1576,7 +1576,7 @@ 0.166667 - + .1 .1 .1 @@ -1615,7 +1615,7 @@ 0.166667 - + .1 .1 .1 @@ -1654,7 +1654,7 @@ 0.166667 - + .1 .1 .1 @@ -1693,7 +1693,7 @@ 0.166667 - + .1 .1 .1 @@ -1732,7 +1732,7 @@ 0.166667 - + .1 .1 .1 @@ -1771,7 +1771,7 @@ 0.166667 - + .1 .1 .1 @@ -1810,7 +1810,7 @@ 0.166667 - + .1 .1 .1 @@ -1849,7 +1849,7 @@ 0.166667 - + .1 .1 .1 @@ -1888,7 +1888,7 @@ 0.166667 - + .1 .1 .1 @@ -1927,7 +1927,7 @@ 0.166667 - + .1 .1 .1 @@ -1966,7 +1966,7 @@ 0.166667 - + .1 .1 .1 @@ -2005,7 +2005,7 @@ 0.166667 - + .1 .1 .1 @@ -2044,7 +2044,7 @@ 0.166667 - + .1 .1 .1 @@ -2083,7 +2083,7 @@ 0.166667 - + .1 .1 .1 @@ -2122,7 +2122,7 @@ 0.166667 - + .1 .1 .1 @@ -2161,7 +2161,7 @@ 0.166667 - + .1 .1 .1 @@ -2200,7 +2200,7 @@ 0.166667 - + .1 .1 .1 @@ -2239,7 +2239,7 @@ 0.166667 - + .1 .1 .1 @@ -2278,7 +2278,7 @@ 0.166667 - + .1 .1 .1 @@ -2317,7 +2317,7 @@ 0.166667 - + .1 .1 .1 @@ -2356,7 +2356,7 @@ 0.166667 - + .1 .1 .1 @@ -2395,7 +2395,7 @@ 0.166667 - + .1 .1 .1 @@ -2434,7 +2434,7 @@ 0.166667 - + .1 .1 .1 @@ -2473,7 +2473,7 @@ 0.166667 - + .1 .1 .1 @@ -2512,7 +2512,7 @@ 0.166667 - + .1 .1 .1 @@ -2551,7 +2551,7 @@ 0.166667 - + .1 .1 .1 @@ -2590,7 +2590,7 @@ 0.166667 - + .1 .1 .1 @@ -2629,7 +2629,7 @@ 0.166667 - + .1 .1 .1 @@ -2668,7 +2668,7 @@ 0.166667 - + .1 .1 .1 @@ -2707,7 +2707,7 @@ 0.166667 - + .1 .1 .1 @@ -2746,7 +2746,7 @@ 0.166667 - + .1 .1 .1 @@ -2785,7 +2785,7 @@ 0.166667 - + .1 .1 .1 @@ -2824,7 +2824,7 @@ 0.166667 - + .1 .1 .1 @@ -2863,7 +2863,7 @@ 0.166667 - + .1 .1 .1 @@ -2902,7 +2902,7 @@ 0.166667 - + .1 .1 .1 @@ -2941,7 +2941,7 @@ 0.166667 - + .1 .1 .1 @@ -2980,7 +2980,7 @@ 0.166667 - + .1 .1 .1 @@ -3019,7 +3019,7 @@ 0.166667 - + .1 .1 .1 @@ -3058,7 +3058,7 @@ 0.166667 - + .1 .1 .1 @@ -3097,7 +3097,7 @@ 0.166667 - + .1 .1 .1 @@ -3136,7 +3136,7 @@ 0.166667 - + .1 .1 .1 @@ -3175,7 +3175,7 @@ 0.166667 - + .1 .1 .1 @@ -3214,7 +3214,7 @@ 0.166667 - + .1 .1 .1 @@ -3253,7 +3253,7 @@ 0.166667 - + .1 .1 .1 @@ -3292,7 +3292,7 @@ 0.166667 - + .1 .1 .1 @@ -3331,7 +3331,7 @@ 0.166667 - + .1 .1 .1 @@ -3370,7 +3370,7 @@ 0.166667 - + .1 .1 .1 @@ -3409,7 +3409,7 @@ 0.166667 - + .1 .1 .1 @@ -3448,7 +3448,7 @@ 0.166667 - + .1 .1 .1 @@ -3487,7 +3487,7 @@ 0.166667 - + .1 .1 .1 @@ -3526,7 +3526,7 @@ 0.166667 - + .1 .1 .1 @@ -3565,7 +3565,7 @@ 0.166667 - + .1 .1 .1 @@ -3604,7 +3604,7 @@ 0.166667 - + .1 .1 .1 @@ -3643,7 +3643,7 @@ 0.166667 - + .1 .1 .1 @@ -3682,7 +3682,7 @@ 0.166667 - + .1 .1 .1 @@ -3721,7 +3721,7 @@ 0.166667 - + .1 .1 .1 @@ -3760,7 +3760,7 @@ 0.166667 - + .1 .1 .1 @@ -3799,7 +3799,7 @@ 0.166667 - + .1 .1 .1 @@ -3838,7 +3838,7 @@ 0.166667 - + .1 .1 .1 @@ -3877,7 +3877,7 @@ 0.166667 - + .1 .1 .1 @@ -3916,7 +3916,7 @@ 0.166667 - + .1 .1 .1 @@ -3955,7 +3955,7 @@ 0.166667 - + .1 .1 .1 @@ -3994,7 +3994,7 @@ 0.166667 - + .1 .1 .1 @@ -4033,7 +4033,7 @@ 0.166667 - + .1 .1 .1 @@ -4072,7 +4072,7 @@ 0.166667 - + .1 .1 .1 @@ -4111,7 +4111,7 @@ 0.166667 - + .1 .1 .1 @@ -4150,7 +4150,7 @@ 0.166667 - + .1 .1 .1 @@ -4189,7 +4189,7 @@ 0.166667 - + .1 .1 .1 @@ -4228,7 +4228,7 @@ 0.166667 - + .1 .1 .1 @@ -4267,7 +4267,7 @@ 0.166667 - + .1 .1 .1 @@ -4306,7 +4306,7 @@ 0.166667 - + .1 .1 .1 @@ -4345,7 +4345,7 @@ 0.166667 - + .1 .1 .1 @@ -4384,7 +4384,7 @@ 0.166667 - + .1 .1 .1 @@ -4423,7 +4423,7 @@ 0.166667 - + .1 .1 .1 @@ -4462,7 +4462,7 @@ 0.166667 - + .1 .1 .1 @@ -4501,7 +4501,7 @@ 0.166667 - + .1 .1 .1 @@ -4540,7 +4540,7 @@ 0.166667 - + .1 .1 .1 @@ -4579,7 +4579,7 @@ 0.166667 - + .1 .1 .1 @@ -4618,7 +4618,7 @@ 0.166667 - + .1 .1 .1 @@ -4657,7 +4657,7 @@ 0.166667 - + .1 .1 .1 @@ -4696,7 +4696,7 @@ 0.166667 - + .1 .1 .1 @@ -4735,7 +4735,7 @@ 0.166667 - + .1 .1 .1 @@ -4774,7 +4774,7 @@ 0.166667 - + .1 .1 .1 @@ -4813,7 +4813,7 @@ 0.166667 - + .1 .1 .1 @@ -4852,7 +4852,7 @@ 0.166667 - + .1 .1 .1 @@ -4891,7 +4891,7 @@ 0.166667 - + .1 .1 .1 @@ -4930,7 +4930,7 @@ 0.166667 - + .1 .1 .1 @@ -4969,7 +4969,7 @@ 0.166667 - + .1 .1 .1 @@ -5008,7 +5008,7 @@ 0.166667 - + .1 .1 .1 @@ -5047,7 +5047,7 @@ 0.166667 - + .1 .1 .1 @@ -5086,7 +5086,7 @@ 0.166667 - + .1 .1 .1 @@ -5125,7 +5125,7 @@ 0.166667 - + .1 .1 .1 @@ -5164,7 +5164,7 @@ 0.166667 - + .1 .1 .1 @@ -5203,7 +5203,7 @@ 0.166667 - + .1 .1 .1 @@ -5242,7 +5242,7 @@ 0.166667 - + .1 .1 .1 @@ -5281,7 +5281,7 @@ 0.166667 - + .1 .1 .1 @@ -5320,7 +5320,7 @@ 0.166667 - + .1 .1 .1 @@ -5359,7 +5359,7 @@ 0.166667 - + .1 .1 .1 @@ -5398,7 +5398,7 @@ 0.166667 - + .1 .1 .1 @@ -5437,7 +5437,7 @@ 0.166667 - + .1 .1 .1 @@ -5476,7 +5476,7 @@ 0.166667 - + .1 .1 .1 @@ -5515,7 +5515,7 @@ 0.166667 - + .1 .1 .1 @@ -5554,7 +5554,7 @@ 0.166667 - + .1 .1 .1 @@ -5593,7 +5593,7 @@ 0.166667 - + .1 .1 .1 @@ -5632,7 +5632,7 @@ 0.166667 - + .1 .1 .1 @@ -5671,7 +5671,7 @@ 0.166667 - + .1 .1 .1 @@ -5741,7 +5741,7 @@ 0.166667 - + .1 .1 .1 @@ -5780,7 +5780,7 @@ 0.166667 - + .1 .1 .1 @@ -5819,7 +5819,7 @@ 0.166667 - + .1 .1 .1 @@ -5858,7 +5858,7 @@ 0.166667 - + .1 .1 .1 @@ -5897,7 +5897,7 @@ 0.166667 - + .1 .1 .1 @@ -5936,7 +5936,7 @@ 0.166667 - + .1 .1 .1 @@ -5975,7 +5975,7 @@ 0.166667 - + .1 .1 .1 @@ -6014,7 +6014,7 @@ 0.166667 - + .1 .1 .1 @@ -6053,7 +6053,7 @@ 0.166667 - + .1 .1 .1 @@ -6092,7 +6092,7 @@ 0.166667 - + .1 .1 .1 @@ -6131,7 +6131,7 @@ 0.166667 - + .1 .1 .1 @@ -6170,7 +6170,7 @@ 0.166667 - + .1 .1 .1 @@ -6209,7 +6209,7 @@ 0.166667 - + .1 .1 .1 @@ -6248,7 +6248,7 @@ 0.166667 - + .1 .1 .1 @@ -6287,7 +6287,7 @@ 0.166667 - + .1 .1 .1 @@ -6326,7 +6326,7 @@ 0.166667 - + .1 .1 .1 @@ -6365,7 +6365,7 @@ 0.166667 - + .1 .1 .1 @@ -6404,7 +6404,7 @@ 0.166667 - + .1 .1 .1 @@ -6443,7 +6443,7 @@ 0.166667 - + .1 .1 .1 @@ -6482,7 +6482,7 @@ 0.166667 - + .1 .1 .1 @@ -6521,7 +6521,7 @@ 0.166667 - + .1 .1 .1 @@ -6560,7 +6560,7 @@ 0.166667 - + .1 .1 .1 @@ -6599,7 +6599,7 @@ 0.166667 - + .1 .1 .1 @@ -6638,7 +6638,7 @@ 0.166667 - + .1 .1 .1 @@ -6677,7 +6677,7 @@ 0.166667 - + .1 .1 .1 @@ -6716,7 +6716,7 @@ 0.166667 - + .1 .1 .1 @@ -6755,7 +6755,7 @@ 0.166667 - + .1 .1 .1 diff --git a/data/plane.urdf b/data/plane.urdf index ad10aeedf..1b3d09682 100644 --- a/data/plane.urdf +++ b/data/plane.urdf @@ -2,7 +2,7 @@ - + diff --git a/data/quadruped/LOG00076.TXT b/data/quadruped/LOG00076.TXT new file mode 100644 index 000000000..c1912b4e0 Binary files /dev/null and b/data/quadruped/LOG00076.TXT differ diff --git a/data/quadruped/minitaur.urdf b/data/quadruped/minitaur.urdf new file mode 100644 index 000000000..663ff2a84 --- /dev/null +++ b/data/quadruped/minitaur.urdf @@ -0,0 +1,929 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/data/quadruped/minitaur_fixed_all.urdf b/data/quadruped/minitaur_fixed_all.urdf new file mode 100644 index 000000000..68815043a --- /dev/null +++ b/data/quadruped/minitaur_fixed_all.urdf @@ -0,0 +1,913 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/data/quadruped/minitaur_fixed_knees.urdf b/data/quadruped/minitaur_fixed_knees.urdf new file mode 100644 index 000000000..2c5903e1f --- /dev/null +++ b/data/quadruped/minitaur_fixed_knees.urdf @@ -0,0 +1,929 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/data/quadruped/t-motor.jpg b/data/quadruped/t-motor.jpg new file mode 100644 index 000000000..dbd5d1a6e Binary files /dev/null and b/data/quadruped/t-motor.jpg differ diff --git a/data/quadruped/tmotor.blend b/data/quadruped/tmotor.blend new file mode 100644 index 000000000..ba06c0075 Binary files /dev/null and b/data/quadruped/tmotor.blend differ diff --git a/data/quadruped/tmotor3.mtl b/data/quadruped/tmotor3.mtl new file mode 100644 index 000000000..af216a1cc --- /dev/null +++ b/data/quadruped/tmotor3.mtl @@ -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 diff --git a/data/quadruped/tmotor3.obj b/data/quadruped/tmotor3.obj new file mode 100644 index 000000000..366398a4b --- /dev/null +++ b/data/quadruped/tmotor3.obj @@ -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 diff --git a/docs/pybullet_quickstartguide.pdf b/docs/pybullet_quickstartguide.pdf index d81dbde85..53eef4638 100644 Binary files a/docs/pybullet_quickstartguide.pdf and b/docs/pybullet_quickstartguide.pdf differ diff --git a/examples/CommonInterfaces/CommonGUIHelperInterface.h b/examples/CommonInterfaces/CommonGUIHelperInterface.h index 8e220faef..493b9e123 100644 --- a/examples/CommonInterfaces/CommonGUIHelperInterface.h +++ b/examples/CommonInterfaces/CommonGUIHelperInterface.h @@ -85,6 +85,9 @@ struct GUIHelperInterface virtual void removeAllUserDebugItems( ){}; virtual void setVisualizerFlagCallback(VisualizerFlagCallback callback){} + //empty name stops dumping video + virtual void dumpFramesToVideo(const char* mp4FileName) {}; + }; diff --git a/examples/ExampleBrowser/CMakeLists.txt b/examples/ExampleBrowser/CMakeLists.txt index b7cf1fd06..127ab11f5 100644 --- a/examples/ExampleBrowser/CMakeLists.txt +++ b/examples/ExampleBrowser/CMakeLists.txt @@ -191,6 +191,8 @@ SET(BulletExampleBrowser_SRCS ../SharedMemory/PhysicsLoopBackC_API.h ../SharedMemory/PhysicsServerCommandProcessor.cpp ../SharedMemory/PhysicsServerCommandProcessor.h + ../SharedMemory/SharedMemoryCommands.h + ../SharedMemory/SharedMemoryPublic.h ../BasicDemo/BasicExample.cpp ../BasicDemo/BasicExample.h ../InverseDynamics/InverseDynamicsExample.cpp diff --git a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp index 368cd67cc..42250d589 100644 --- a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp +++ b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp @@ -371,6 +371,7 @@ void OpenGLExampleBrowserVisualizerFlagCallback(int flag, bool enable) if (flag == COV_ENABLE_GUI) { renderGui = enable; + renderGrid = enable; } if (flag == COV_ENABLE_WIREFRAME) diff --git a/examples/ExampleBrowser/OpenGLGuiHelper.cpp b/examples/ExampleBrowser/OpenGLGuiHelper.cpp index 1c5893ce6..b65eaea44 100644 --- a/examples/ExampleBrowser/OpenGLGuiHelper.cpp +++ b/examples/ExampleBrowser/OpenGLGuiHelper.cpp @@ -427,47 +427,67 @@ void OpenGLGuiHelper::copyCameraImageData(const float viewMatrix[16], const floa SimpleCamera tempCam; getRenderInterface()->setActiveCamera(&tempCam); getRenderInterface()->getActiveCamera()->setVRCamera(viewMatrix,projectionMatrix); - getRenderInterface()->renderScene(); + { + BT_PROFILE("renderScene"); + getRenderInterface()->renderScene(); + } getRenderInterface()->setActiveCamera(oldCam); { + BT_PROFILE("copy pixels"); btAlignedObjectArray sourceRgbaPixelBuffer; btAlignedObjectArray sourceDepthBuffer; //copy the image into our local cache sourceRgbaPixelBuffer.resize(sourceWidth*sourceHeight*numBytesPerPixel); sourceDepthBuffer.resize(sourceWidth*sourceHeight); - m_data->m_glApp->getScreenPixels(&(sourceRgbaPixelBuffer[0]),sourceRgbaPixelBuffer.size(), &sourceDepthBuffer[0],sizeof(float)*sourceDepthBuffer.size()); + { + BT_PROFILE("getScreenPixels"); + m_data->m_glApp->getScreenPixels(&(sourceRgbaPixelBuffer[0]),sourceRgbaPixelBuffer.size(), &sourceDepthBuffer[0],sizeof(float)*sourceDepthBuffer.size()); + } m_data->m_rgbaPixelBuffer1.resize(destinationWidth*destinationHeight*numBytesPerPixel); m_data->m_depthBuffer1.resize(destinationWidth*destinationHeight); //rescale and flip - - for (int i=0;im_rgbaPixelBuffer1[(i+j*destinationWidth)*4+0]; + int* src = (int*)&sourceRgbaPixelBuffer[sourcePixelIndex+0]; + *dst = *src; + +#else + m_data->m_rgbaPixelBuffer1[(i+j*destinationWidth)*4+0] = sourceRgbaPixelBuffer[sourcePixelIndex+0]; + m_data->m_rgbaPixelBuffer1[(i+j*destinationWidth)*4+1] = sourceRgbaPixelBuffer[sourcePixelIndex+1]; + m_data->m_rgbaPixelBuffer1[(i+j*destinationWidth)*4+2] = sourceRgbaPixelBuffer[sourcePixelIndex+2]; + m_data->m_rgbaPixelBuffer1[(i+j*destinationWidth)*4+3] = 255; +#endif + if (depthBuffer) + { + m_data->m_depthBuffer1[i+j*destinationWidth] = sourceDepthBuffer[sourceDepthIndex]; + } - m_data->m_rgbaPixelBuffer1[(i+j*destinationWidth)*4+0] = sourceRgbaPixelBuffer[sourcePixelIndex+0]; - m_data->m_rgbaPixelBuffer1[(i+j*destinationWidth)*4+1] = sourceRgbaPixelBuffer[sourcePixelIndex+1]; - m_data->m_rgbaPixelBuffer1[(i+j*destinationWidth)*4+2] = sourceRgbaPixelBuffer[sourcePixelIndex+2]; - m_data->m_rgbaPixelBuffer1[(i+j*destinationWidth)*4+3] = 255; - - m_data->m_depthBuffer1[i+j*destinationWidth] = sourceDepthBuffer[sourceDepthIndex]; - - } + } + } } } } if (pixelsRGBA) { + BT_PROFILE("copy rgba pixels"); + for (int i=0;im_rgbaPixelBuffer1[i+startPixelIndex*numBytesPerPixel]; @@ -475,6 +495,8 @@ void OpenGLGuiHelper::copyCameraImageData(const float viewMatrix[16], const floa } if (depthBuffer) { + BT_PROFILE("copy depth buffer pixels"); + for (int i=0;im_depthBuffer1[i+startPixelIndex]; @@ -546,3 +568,10 @@ struct CommonGraphicsApp* OpenGLGuiHelper::getAppInterface() return m_data->m_glApp; } +void OpenGLGuiHelper::dumpFramesToVideo(const char* mp4FileName) +{ + if (m_data->m_glApp) + { + m_data->m_glApp->dumpFramesToVideo(mp4FileName); + } +} \ No newline at end of file diff --git a/examples/ExampleBrowser/OpenGLGuiHelper.h b/examples/ExampleBrowser/OpenGLGuiHelper.h index 9448b5eef..0e91f0f3b 100644 --- a/examples/ExampleBrowser/OpenGLGuiHelper.h +++ b/examples/ExampleBrowser/OpenGLGuiHelper.h @@ -85,6 +85,8 @@ struct OpenGLGuiHelper : public GUIHelperInterface virtual void setVisualizerFlagCallback(VisualizerFlagCallback callback); + virtual void dumpFramesToVideo(const char* mp4FileName); + }; #endif //OPENGL_GUI_HELPER_H diff --git a/examples/ExampleBrowser/premake4.lua b/examples/ExampleBrowser/premake4.lua index 0c44e93bb..f6a7df31f 100644 --- a/examples/ExampleBrowser/premake4.lua +++ b/examples/ExampleBrowser/premake4.lua @@ -88,6 +88,8 @@ project "App_BulletExampleBrowser" "../SharedMemory/PhysicsServerCommandProcessor.h", "../SharedMemory/TinyRendererVisualShapeConverter.cpp", "../SharedMemory/TinyRendererVisualShapeConverter.h", + "../SharedMemory/SharedMemoryCommands.h", + "../SharedMemory/SharedMemoryPublic.h", "../MultiThreading/MultiThreadingExample.cpp", "../MultiThreading/b3PosixThreadSupport.cpp", "../MultiThreading/b3Win32ThreadSupport.cpp", diff --git a/examples/HelloWorld/HelloWorld.cpp b/examples/HelloWorld/HelloWorld.cpp index e51e6d541..e897bcdd3 100644 --- a/examples/HelloWorld/HelloWorld.cpp +++ b/examples/HelloWorld/HelloWorld.cpp @@ -30,7 +30,7 @@ int main(int argc, char** argv) btDefaultCollisionConfiguration* collisionConfiguration = new btDefaultCollisionConfiguration(); ///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded) - btCollisionDispatcher* dispatcher = new btCollisionDispatcher(collisionConfiguration); + btCollisionDispatcher* dispatcher = new btCollisionDispatcher(collisionConfiguration); ///btDbvtBroadphase is a good general purpose broadphase. You can also try out btAxis3Sweep. btBroadphaseInterface* overlappingPairCache = new btDbvtBroadphase(); @@ -38,9 +38,9 @@ int main(int argc, char** argv) ///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded) btSequentialImpulseConstraintSolver* solver = new btSequentialImpulseConstraintSolver; - btDiscreteDynamicsWorld* dynamicsWorld = new btDiscreteDynamicsWorld(dispatcher,overlappingPairCache,solver,collisionConfiguration); + btDiscreteDynamicsWorld* dynamicsWorld = new btDiscreteDynamicsWorld(dispatcher, overlappingPairCache, solver, collisionConfiguration); - dynamicsWorld->setGravity(btVector3(0,-10,0)); + dynamicsWorld->setGravity(btVector3(0, -10, 0)); ///-----initialization_end----- @@ -48,39 +48,37 @@ int main(int argc, char** argv) //make sure to re-use collision shapes among rigid bodies whenever possible! btAlignedObjectArray collisionShapes; - ///create a few basic rigid bodies //the ground is a cube of side 100 at position y = -56. //the sphere will hit it at y = -6, with center at -5 { - btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(50.),btScalar(50.),btScalar(50.))); + btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(50.), btScalar(50.), btScalar(50.))); collisionShapes.push_back(groundShape); btTransform groundTransform; groundTransform.setIdentity(); - groundTransform.setOrigin(btVector3(0,-56,0)); + groundTransform.setOrigin(btVector3(0, -56, 0)); btScalar mass(0.); //rigidbody is dynamic if and only if mass is non zero, otherwise static bool isDynamic = (mass != 0.f); - btVector3 localInertia(0,0,0); + btVector3 localInertia(0, 0, 0); if (isDynamic) - groundShape->calculateLocalInertia(mass,localInertia); + groundShape->calculateLocalInertia(mass, localInertia); //using motionstate is optional, it provides interpolation capabilities, and only synchronizes 'active' objects btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform); - btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,groundShape,localInertia); + btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, groundShape, localInertia); btRigidBody* body = new btRigidBody(rbInfo); //add the body to the dynamics world dynamicsWorld->addRigidBody(body); } - { //create a dynamic rigidbody @@ -92,37 +90,34 @@ int main(int argc, char** argv) btTransform startTransform; startTransform.setIdentity(); - btScalar mass(1.f); + btScalar mass(1.f); //rigidbody is dynamic if and only if mass is non zero, otherwise static bool isDynamic = (mass != 0.f); - btVector3 localInertia(0,0,0); + btVector3 localInertia(0, 0, 0); if (isDynamic) - colShape->calculateLocalInertia(mass,localInertia); + colShape->calculateLocalInertia(mass, localInertia); - startTransform.setOrigin(btVector3(2,10,0)); - - //using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects - btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform); - btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,colShape,localInertia); - btRigidBody* body = new btRigidBody(rbInfo); + startTransform.setOrigin(btVector3(2, 10, 0)); - dynamicsWorld->addRigidBody(body); + //using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects + btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform); + btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, colShape, localInertia); + btRigidBody* body = new btRigidBody(rbInfo); + + dynamicsWorld->addRigidBody(body); } - - -/// Do some simulation - + /// Do some simulation ///-----stepsimulation_start----- - for (i=0;i<150;i++) + for (i = 0; i < 150; i++) { - dynamicsWorld->stepSimulation(1.f/60.f,10); - + dynamicsWorld->stepSimulation(1.f / 60.f, 10); + //print positions of all objects - for (int j=dynamicsWorld->getNumCollisionObjects()-1; j>=0 ;j--) + for (int j = dynamicsWorld->getNumCollisionObjects() - 1; j >= 0; j--) { btCollisionObject* obj = dynamicsWorld->getCollisionObjectArray()[j]; btRigidBody* body = btRigidBody::upcast(obj); @@ -130,23 +125,23 @@ int main(int argc, char** argv) if (body && body->getMotionState()) { body->getMotionState()->getWorldTransform(trans); - - } else + } + else { trans = obj->getWorldTransform(); } - printf("world pos object %d = %f,%f,%f\n",j,float(trans.getOrigin().getX()),float(trans.getOrigin().getY()),float(trans.getOrigin().getZ())); + printf("world pos object %d = %f,%f,%f\n", j, float(trans.getOrigin().getX()), float(trans.getOrigin().getY()), float(trans.getOrigin().getZ())); } } ///-----stepsimulation_end----- //cleanup in the reverse order of creation/initialization - + ///-----cleanup_start----- //remove the rigidbodies from the dynamics world and delete them - for (i=dynamicsWorld->getNumCollisionObjects()-1; i>=0 ;i--) + for (i = dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--) { btCollisionObject* obj = dynamicsWorld->getCollisionObjectArray()[i]; btRigidBody* body = btRigidBody::upcast(obj); @@ -154,12 +149,12 @@ int main(int argc, char** argv) { delete body->getMotionState(); } - dynamicsWorld->removeCollisionObject( obj ); + dynamicsWorld->removeCollisionObject(obj); delete obj; } //delete collision shapes - for (int j=0;j -enum eMJCF_FILE_TYPE_ENUMS -{ - MJCF_FILE_STL = 1, - MJCF_FILE_OBJ = 2 -}; - enum ePARENT_LINK_ENUMS { BASE_LINK_INDEX=-1, @@ -137,9 +131,11 @@ struct MyMJCFAsset struct BulletMJCFImporterInternalData { GUIHelperInterface* m_guiHelper; + struct LinkVisualShapesConverter* m_customVisualShapesConverter; char m_pathPrefix[1024]; - std::string m_fileModelName; + std::string m_sourceFileName; // with path + std::string m_fileModelName; // without path btHashMap m_assets; btAlignedObjectArray m_models; @@ -150,22 +146,37 @@ struct BulletMJCFImporterInternalData int m_activeModel; + int m_activeBodyUniqueId; //todo: for full MJCF compatibility, we would need a stack of default values int m_defaultCollisionGroup; int m_defaultCollisionMask; btScalar m_defaultCollisionMargin; + // joint defaults + std::string m_defaultJointLimited; + + // geom defaults + std::string m_defaultGeomRgba; + //those collision shapes are deleted by caller (todo: make sure this happens!) btAlignedObjectArray m_allocatedCollisionShapes; BulletMJCFImporterInternalData() :m_activeModel(-1), + m_activeBodyUniqueId(-1), m_defaultCollisionGroup(1), m_defaultCollisionMask(1), m_defaultCollisionMargin(0.001)//assume unit meters, margin is 1mm { m_pathPrefix[0] = 0; } + + std::string sourceFileLocation(TiXmlElement* e) + { + char buf[1024]; + snprintf(buf, sizeof(buf), "%s:%i", m_sourceFileName.c_str(), e->Row()); + return buf; + } const UrdfLink* getLink(int modelIndex, int linkIndex) const { @@ -238,6 +249,17 @@ struct BulletMJCFImporterInternalData { parseAssets(child_xml,logger); } + if (n=="joint") + { + // Other attributes here: + // armature="1" + // damping="1" + // limited="true" + if (const char* conTypeStr = child_xml->Attribute("limited")) + { + m_defaultJointLimited = child_xml->Attribute("limited"); + } + } if (n=="geom") { //contype, conaffinity @@ -251,6 +273,11 @@ struct BulletMJCFImporterInternalData { m_defaultCollisionMask = urdfLexicalCast(conAffinityStr); } + const char* rgba = child_xml->Attribute("rgba"); + if (rgba) + { + m_defaultGeomRgba = rgba; + } } } handled=true; @@ -361,39 +388,57 @@ struct BulletMJCFImporterInternalData bool isLimited = false; double range[2] = {1,0}; + std::string lim = m_defaultJointLimited; if (limitedStr) { - std::string lim = limitedStr; - if (lim=="true") - { - isLimited = true; - //parse the 'range' field - btArray pieces; - btArray sizes; - btAlignedObjectArray strArray; - urdfIsAnyOf(" ", strArray); - urdfStringSplit(pieces, rangeStr, strArray); - for (int i = 0; i < pieces.size(); ++i) - { - if (!pieces[i].empty()) - { - sizes.push_back(urdfLexicalCast(pieces[i].c_str())); - } - } - if (sizes.size()==2) - { - range[0] = sizes[0]; - range[1] = sizes[1]; - } else - { - logger->reportWarning("Expected range[2] in joint with limits"); - } - - } - } else - { -// logger->reportWarning("joint without limited field"); + lim = limitedStr; } + if (lim=="true") + { + isLimited = true; + //parse the 'range' field + btArray pieces; + btArray sizes; + btAlignedObjectArray strArray; + urdfIsAnyOf(" ", strArray); + urdfStringSplit(pieces, rangeStr, strArray); + for (int i = 0; i < pieces.size(); ++i) + { + if (!pieces[i].empty()) + { + sizes.push_back(urdfLexicalCast(pieces[i].c_str())); + } + } + if (sizes.size()==2) + { + // TODO angle units are in " + range[0] = sizes[0] * B3_PI / 180; + range[1] = sizes[1] * B3_PI / 180; + } else + { + logger->reportWarning("Expected range[2] in joint with limits"); + } + } + + // TODO armature : real, "0" Armature inertia (or rotor inertia) of all + // degrees of freedom created by this joint. These are constants added to the + // diagonal of the inertia matrix in generalized coordinates. They make the + // simulation more stable, and often increase physical realism. This is because + // when a motor is attached to the system with a transmission that amplifies + // the motor force by c, the inertia of the rotor (i.e. the moving part of the + // motor) is amplified by c*c. The same holds for gears in the early stages of + // planetary gear boxes. These extra inertias often dominate the inertias of + // the robot parts that are represented explicitly in the model, and the + // armature attribute is the way to model them. + + // TODO damping : real, "0" Damping applied to all degrees of + // freedom created by this joint. Unlike friction loss + // which is computed by the constraint solver, damping is + // simply a force linear in velocity. It is included in + // the passive forces. Despite this simplicity, larger + // damping values can make numerical integrators unstable, + // which is why our Euler integrator handles damping + // implicitly. See Integration in the Computation chapter. bool jointHandled = false; const UrdfLink* linkPtr = getLink(modelIndex,linkIndex); @@ -443,11 +488,11 @@ struct BulletMJCFImporterInternalData jointPtr->m_parentLinkToJointTransform = parentLinkToJointTransform; jointPtr->m_type = ejtype; int numJoints = m_models[modelIndex]->m_joints.size(); - + //range jointPtr->m_lowerLimit = range[0]; jointPtr->m_upperLimit = range[1]; - + if (nameStr) { jointPtr->m_name =nameStr; @@ -489,10 +534,23 @@ struct BulletMJCFImporterInternalData -// const char* rgba = link_xml->Attribute("rgba"); const char* gType = link_xml->Attribute("type"); const char* sz = link_xml->Attribute("size"); const char* posS = link_xml->Attribute("pos"); + + std::string rgba = m_defaultGeomRgba; + if (const char* rgbattr = link_xml->Attribute("rgba")) + { + rgba = rgbattr; + } + if (!rgba.empty()) + { + // "0 0.7 0.7 1" + parseVector4(geom.m_localMaterial.m_rgbaColor, rgba); + geom.m_hasLocalMaterial = true; + geom.m_localMaterial.m_name = rgba; + } + if (posS) { btVector3 pos(0,0,0); @@ -560,10 +618,10 @@ struct BulletMJCFImporterInternalData handledGeomType = true; } - //todo: capsule, cylinder, meshes or heightfields etc - if (geomType == "capsule") + if (geomType == "capsule" || geomType == "cylinder") { - geom.m_type = URDF_GEOM_CAPSULE; + // + geom.m_type = geomType=="cylinder" ? URDF_GEOM_CYLINDER : URDF_GEOM_CAPSULE; btArray pieces; btArray sizes; @@ -621,9 +679,14 @@ struct BulletMJCFImporterInternalData MyMJCFAsset* assetPtr = m_assets[meshStr]; if (assetPtr) { - handledGeomType = true; geom.m_type = URDF_GEOM_MESH; geom.m_meshFileName = assetPtr->m_fileName; + bool exists = findExistingMeshFile( + m_sourceFileName, assetPtr->m_fileName, sourceFileLocation(link_xml), + &geom.m_meshFileName, + &geom.m_meshFileType); + handledGeomType = exists; + geom.m_meshScale.setValue(1,1,1); //todo: parse mesh scale if (sz) @@ -632,13 +695,6 @@ struct BulletMJCFImporterInternalData } } } - #if 0 - if (geomType == "cylinder") - { - geom.m_type = URDF_GEOM_CYLINDER; - handledGeomType = true; - } -#endif if (handledGeomType) { @@ -803,6 +859,7 @@ struct BulletMJCFImporterInternalData return orgChildLinkIndex; } + bool parseBody(TiXmlElement* link_xml, int modelIndex, int orgParentLinkIndex, MJCFErrorLogger* logger) { int newParentLinkIndex = orgParentLinkIndex; @@ -964,10 +1021,6 @@ struct BulletMJCFImporterInternalData } linkPtr->m_linkTransformInWorld = linkTransform; - if (bodyN == "cart1")//front_left_leg") - { - printf("found!\n"); - } if ((newParentLinkIndex != INVALID_LINK_INDEX) && !skipFixedJoint) { //linkPtr->m_linkTransformInWorld.setIdentity(); @@ -1113,10 +1166,11 @@ struct BulletMJCFImporterInternalData }; -BulletMJCFImporter::BulletMJCFImporter(struct GUIHelperInterface* helper) +BulletMJCFImporter::BulletMJCFImporter(struct GUIHelperInterface* helper, LinkVisualShapesConverter* customConverter) { m_data = new BulletMJCFImporterInternalData(); m_data->m_guiHelper = helper; + m_data->m_customVisualShapesConverter = customConverter; } BulletMJCFImporter::~BulletMJCFImporter() @@ -1135,7 +1189,8 @@ bool BulletMJCFImporter::loadMJCF(const char* fileName, MJCFErrorLogger* logger, b3FileUtils fu; //bool fileFound = fu.findFile(fileName, relativeFileName, 1024); - bool fileFound = (b3ResourcePath::findResourcePath(fileName,relativeFileName,1024)>0); + bool fileFound = (b3ResourcePath::findResourcePath(fileName,relativeFileName,1024)>0); + m_data->m_sourceFileName = relativeFileName; std::string xml_string; m_data->m_pathPrefix[0] = 0; @@ -1399,21 +1454,26 @@ bool BulletMJCFImporter::getLinkContactInfo(int linkIndex, URDFLinkContactInfo& return false; } - -void BulletMJCFImporter::convertLinkVisualShapes2(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int objectIndex) const +void BulletMJCFImporter::convertLinkVisualShapes2(int linkIndex, int urdfIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int objectIndex) const { + if (m_data->m_customVisualShapesConverter) + { + const UrdfLink* link = m_data->getLink(m_data->m_activeModel, urdfIndex); + m_data->m_customVisualShapesConverter->convertVisualShapes(linkIndex,pathPrefix,inertialFrame, link, 0, colObj, objectIndex); + } } + void BulletMJCFImporter::setBodyUniqueId(int bodyId) { - + m_data->m_activeBodyUniqueId = bodyId; } int BulletMJCFImporter::getBodyUniqueId() const { - return 0; + b3Assert(m_data->m_activeBodyUniqueId != -1); + return m_data->m_activeBodyUniqueId; } - static btCollisionShape* MjcfCreateConvexHullFromShapes(std::vector& shapes, const btVector3& geomScale, btScalar collisionMargin) { btCompoundShape* compound = new btCompoundShape(); @@ -1496,132 +1556,87 @@ class btCompoundShape* BulletMJCFImporter::convertLinkCollisionShapes(int linkIn } case URDF_GEOM_MESH: { - ////////////////////// - if (1) - { - if (col->m_geometry.m_meshFileName.length()) + GLInstanceGraphicsShape* glmesh = 0; + switch (col->m_geometry.m_meshFileType) { - const char* filename = col->m_geometry.m_meshFileName.c_str(); - //b3Printf("mesh->filename=%s\n",filename); - char fullPath[1024]; - int fileType = 0; - sprintf(fullPath,"%s%s",pathPrefix,filename); - b3FileUtils::toLower(fullPath); - char tmpPathPrefix[1024]; - int maxPathLen = 1024; - b3FileUtils::extractPath(filename,tmpPathPrefix,maxPathLen); - - char collisionPathPrefix[1024]; - sprintf(collisionPathPrefix,"%s%s",pathPrefix,tmpPathPrefix); - - if (strstr(fullPath,".stl")) + case UrdfGeometry::FILE_OBJ: { - fileType = MJCF_FILE_STL; + if (col->m_flags & URDF_FORCE_CONCAVE_TRIMESH) + { + glmesh = LoadMeshFromObj(col->m_geometry.m_meshFileName.c_str(), 0); + } + else + { + std::vector shapes; + std::string err = tinyobj::LoadObj(shapes, col->m_geometry.m_meshFileName.c_str()); + //create a convex hull for each shape, and store it in a btCompoundShape + + childShape = MjcfCreateConvexHullFromShapes(shapes, col->m_geometry.m_meshScale, m_data->m_defaultCollisionMargin); + + } + break; } - if (strstr(fullPath,".obj")) - { - fileType = MJCF_FILE_OBJ; - } - - sprintf(fullPath,"%s%s",pathPrefix,filename); - FILE* f = fopen(fullPath,"rb"); - if (f) + case UrdfGeometry::FILE_STL: { - fclose(f); - GLInstanceGraphicsShape* glmesh = 0; - - - switch (fileType) - { - case MJCF_FILE_OBJ: - { - if (col->m_flags & URDF_FORCE_CONCAVE_TRIMESH) - { - glmesh = LoadMeshFromObj(fullPath, collisionPathPrefix); - } - else - { - std::vector shapes; - std::string err = tinyobj::LoadObj(shapes, fullPath, collisionPathPrefix); - //create a convex hull for each shape, and store it in a btCompoundShape + glmesh = LoadMeshFromSTL(col->m_geometry.m_meshFileName.c_str()); + break; + } + default: + b3Warning("%s: Unsupported file type in Collision: %s (maybe .dae?)\n", col->m_sourceFileLocation.c_str(), col->m_geometry.m_meshFileType); + } - childShape = MjcfCreateConvexHullFromShapes(shapes, col->m_geometry.m_meshScale, m_data->m_defaultCollisionMargin); - - } - break; - } - case MJCF_FILE_STL: - { - glmesh = LoadMeshFromSTL(fullPath); - break; - } - - default: - { - b3Warning("Unsupported file type in Collision: %s\n",fullPath); - - } - } - + if (childShape) + { + // okay! + } + else if (!glmesh || glmesh->m_numvertices<=0) + { + b3Warning("%s: cannot extract anything useful from mesh '%s'\n", col->m_sourceFileLocation.c_str(), col->m_geometry.m_meshFileName.c_str()); + } + else + { + //b3Printf("extracted %d verticed from STL file %s\n", glmesh->m_numvertices,fullPath); + //int shapeId = m_glApp->m_instancingRenderer->registerShape(&gvertices[0].pos[0],gvertices.size(),&indices[0],indices.size()); + //convex->setUserIndex(shapeId); + btAlignedObjectArray convertedVerts; + convertedVerts.reserve(glmesh->m_numvertices); + for (int i=0;im_numvertices;i++) + { + convertedVerts.push_back(btVector3( + glmesh->m_vertices->at(i).xyzw[0]*col->m_geometry.m_meshScale[0], + glmesh->m_vertices->at(i).xyzw[1]*col->m_geometry.m_meshScale[1], + glmesh->m_vertices->at(i).xyzw[2]*col->m_geometry.m_meshScale[2])); + } - if (!childShape && glmesh && (glmesh->m_numvertices>0)) + if (col->m_flags & URDF_FORCE_CONCAVE_TRIMESH) + { + + btTriangleMesh* meshInterface = new btTriangleMesh(); + for (int i=0;im_numIndices/3;i++) { - //b3Printf("extracted %d verticed from STL file %s\n", glmesh->m_numvertices,fullPath); - //int shapeId = m_glApp->m_instancingRenderer->registerShape(&gvertices[0].pos[0],gvertices.size(),&indices[0],indices.size()); - //convex->setUserIndex(shapeId); - btAlignedObjectArray convertedVerts; - convertedVerts.reserve(glmesh->m_numvertices); - for (int i=0;im_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;im_numIndices/3;i++) - { - float* v0 = glmesh->m_vertices->at(glmesh->m_indices->at(i*3)).xyzw; - float* v1 = glmesh->m_vertices->at(glmesh->m_indices->at(i*3+1)).xyzw; - float* v2 = glmesh->m_vertices->at(glmesh->m_indices->at(i*3+2)).xyzw; - meshInterface->addTriangle(btVector3(v0[0],v0[1],v0[2]), - btVector3(v1[0],v1[1],v1[2]), - btVector3(v2[0],v2[1],v2[2])); - } - - btBvhTriangleMeshShape* trimesh = new btBvhTriangleMeshShape(meshInterface,true,true); - childShape = trimesh; - } else - { - btConvexHullShape* convexHull = new btConvexHullShape(&convertedVerts[0].getX(), convertedVerts.size(), sizeof(btVector3)); - convexHull->optimizeConvexHull(); - //convexHull->initializePolyhedralFeatures(); - convexHull->setMargin(m_data->m_defaultCollisionMargin); - childShape = convexHull; - } - } else - { - b3Warning("issue extracting mesh from STL file %s\n", fullPath); + float* v0 = glmesh->m_vertices->at(glmesh->m_indices->at(i*3)).xyzw; + float* v1 = glmesh->m_vertices->at(glmesh->m_indices->at(i*3+1)).xyzw; + float* v2 = glmesh->m_vertices->at(glmesh->m_indices->at(i*3+2)).xyzw; + meshInterface->addTriangle(btVector3(v0[0],v0[1],v0[2]), + btVector3(v1[0],v1[1],v1[2]), + btVector3(v2[0],v2[1],v2[2])); } - delete glmesh; - + btBvhTriangleMeshShape* trimesh = new btBvhTriangleMeshShape(meshInterface,true,true); + childShape = trimesh; } else { - b3Warning("mesh geometry not found %s\n",fullPath); + btConvexHullShape* convexHull = new btConvexHullShape(&convertedVerts[0].getX(), convertedVerts.size(), sizeof(btVector3)); + convexHull->optimizeConvexHull(); + //convexHull->initializePolyhedralFeatures(); + convexHull->setMargin(m_data->m_defaultCollisionMargin); + childShape = convexHull; } - } - } - ////////////////////// + delete glmesh; break; } - case URDF_GEOM_CAPSULE: { //todo: convert fromto to btCapsuleShape + local btTransform @@ -1636,7 +1651,7 @@ class btCompoundShape* BulletMJCFImporter::convertLinkCollisionShapes(int linkIn btVector3 fromto[2] = {f,t}; btScalar radii[2] = {btScalar(col->m_geometry.m_capsuleRadius) ,btScalar(col->m_geometry.m_capsuleRadius)}; - + btMultiSphereShape* ms = new btMultiSphereShape(fromto,radii,2); childShape = ms; } else @@ -1647,11 +1662,8 @@ class btCompoundShape* BulletMJCFImporter::convertLinkCollisionShapes(int linkIn } break; } - default: - { + } // switch geom - } - } if (childShape) { m_data->m_allocatedCollisionShapes.push_back(childShape); diff --git a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.h b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.h index f1b439690..a719c984e 100644 --- a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.h +++ b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.h @@ -18,9 +18,8 @@ class BulletMJCFImporter : public URDFImporterInterface { struct BulletMJCFImporterInternalData* m_data; - public: - BulletMJCFImporter(struct GUIHelperInterface* helper); + BulletMJCFImporter(struct GUIHelperInterface* helper, LinkVisualShapesConverter* customConverter); virtual ~BulletMJCFImporter(); virtual bool parseMJCFString(const char* xmlString, MJCFErrorLogger* logger); @@ -66,7 +65,7 @@ public: virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const; - virtual void convertLinkVisualShapes2(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int objectIndex) const; + virtual void convertLinkVisualShapes2(int linkIndex, int urdfIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int objectIndex) const; virtual void setBodyUniqueId(int bodyId); virtual int getBodyUniqueId() const; diff --git a/examples/Importers/ImportMJCFDemo/ImportMJCFSetup.cpp b/examples/Importers/ImportMJCFDemo/ImportMJCFSetup.cpp index 79fff963f..b5e8cda6c 100644 --- a/examples/Importers/ImportMJCFDemo/ImportMJCFSetup.cpp +++ b/examples/Importers/ImportMJCFDemo/ImportMJCFSetup.cpp @@ -188,12 +188,12 @@ void ImportMJCFSetup::initPhysics() m_filterCallback->m_filterMode = FILTER_GROUPAMASKB_OR_GROUPBMASKA2; //m_dynamicsWorld->getSolverInfo().m_numIterations = 100; - m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); - m_dynamicsWorld->getDebugDrawer()->setDebugMode( - btIDebugDraw::DBG_DrawConstraints - +btIDebugDraw::DBG_DrawContactPoints - +btIDebugDraw::DBG_DrawAabb - );//+btIDebugDraw::DBG_DrawConstraintLimits); + m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); + m_dynamicsWorld->getDebugDrawer()->setDebugMode( + btIDebugDraw::DBG_DrawConstraints + +btIDebugDraw::DBG_DrawContactPoints + +btIDebugDraw::DBG_DrawAabb + );//+btIDebugDraw::DBG_DrawConstraintLimits); if (m_guiHelper->getParameterInterface()) @@ -203,20 +203,23 @@ void ImportMJCFSetup::initPhysics() slider.m_maxVal = 10; m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } - - BulletMJCFImporter importer(m_guiHelper); + + BulletMJCFImporter importer(m_guiHelper, 0); MyMJCFLogger logger; bool result = importer.loadMJCF(m_fileName,&logger); if (result) { - btTransform rootTrans; - rootTrans.setIdentity(); - - for (int m =0; mregisterNameForPointer(name->c_str(),name->c_str()); #endif//TEST_MULTIBODY_SERIALIZATION @@ -287,10 +289,11 @@ void ImportMJCFSetup::initPhysics() m_data->m_numMotors++; } } - } + } else { + // not multibody if (1) { //create motors for each generic joint diff --git a/examples/Importers/ImportSTLDemo/LoadMeshFromSTL.h b/examples/Importers/ImportSTLDemo/LoadMeshFromSTL.h index 65bdd330b..95cd93ef3 100644 --- a/examples/Importers/ImportSTLDemo/LoadMeshFromSTL.h +++ b/examples/Importers/ImportSTLDemo/LoadMeshFromSTL.h @@ -98,8 +98,11 @@ static GLInstanceGraphicsShape* LoadMeshFromSTL(const char* relativeFileName) } fclose(file); } - shape->m_numIndices = shape->m_indices->size(); - shape->m_numvertices = shape->m_vertices->size(); + if (shape) + { + shape->m_numIndices = shape->m_indices->size(); + shape->m_numvertices = shape->m_vertices->size(); + } return shape; } diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp index dfe49e8b6..5ba817b51 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp @@ -32,6 +32,7 @@ static btScalar gUrdfDefaultCollisionMargin = 0.001; #include #include +#include #include "UrdfParser.h" struct MyTexture @@ -47,36 +48,38 @@ ATTRIBUTE_ALIGNED16(struct) BulletURDFInternalData UrdfParser m_urdfParser; struct GUIHelperInterface* m_guiHelper; + std::string m_sourceFile; char m_pathPrefix[1024]; int m_bodyId; btHashMap m_linkColors; btAlignedObjectArray m_allocatedCollisionShapes; LinkVisualShapesConverter* m_customVisualShapesConverter; -}; + void setSourceFile(const std::string& relativeFileName, const std::string& prefix) + { + m_sourceFile = relativeFileName; + m_urdfParser.setSourceFile(relativeFileName); + strncpy(m_pathPrefix, prefix.c_str(), sizeof(m_pathPrefix)); + m_pathPrefix[sizeof(m_pathPrefix)-1] = 0; // required, strncpy doesn't write zero on overflow + } + + BulletURDFInternalData() + { + m_pathPrefix[0] = 0; + } +}; void BulletURDFImporter::printTree() { // btAssert(0); } - -enum MyFileType -{ - FILE_STL=1, - FILE_COLLADA=2, - FILE_OBJ=3, -}; - - - BulletURDFImporter::BulletURDFImporter(struct GUIHelperInterface* helper, LinkVisualShapesConverter* customConverter) { m_data = new BulletURDFInternalData; m_data->m_guiHelper = helper; - m_data->m_pathPrefix[0]=0; m_data->m_customVisualShapesConverter = customConverter; @@ -111,7 +114,6 @@ struct BulletErrorLogger : public ErrorLogger bool BulletURDFImporter::loadURDF(const char* fileName, bool forceFixedBase) { - if (strlen(fileName)==0) return false; @@ -124,17 +126,16 @@ bool BulletURDFImporter::loadURDF(const char* fileName, bool forceFixedBase) bool fileFound = (b3ResourcePath::findResourcePath(fileName,relativeFileName,1024))>0; std::string xml_string; - m_data->m_pathPrefix[0] = 0; - - if (!fileFound){ - std::cerr << "URDF file not found" << std::endl; - return false; - } else - { - - int maxPathLen = 1024; - fu.extractPath(relativeFileName,m_data->m_pathPrefix,maxPathLen); + if (!fileFound){ + b3Warning("URDF file '%s' not found\n", fileName); + return false; + } else + { + + char path[1024]; + fu.extractPath(relativeFileName, path, sizeof(path)); + m_data->setSourceFile(relativeFileName, path); std::fstream xml_file(relativeFileName, std::fstream::in); while ( xml_file.good()) @@ -166,7 +167,7 @@ void BulletURDFImporter::activateModel(int modelIndex) bool BulletURDFImporter::loadSDF(const char* fileName, bool forceFixedBase) { - + //int argc=0; char relativeFileName[1024]; @@ -176,17 +177,16 @@ bool BulletURDFImporter::loadSDF(const char* fileName, bool forceFixedBase) bool fileFound = (b3ResourcePath::findResourcePath(fileName,relativeFileName,1024))>0; std::string xml_string; - m_data->m_pathPrefix[0] = 0; if (!fileFound){ - std::cerr << "SDF file not found" << std::endl; + b3Warning("SDF file '%s' not found\n", fileName); return false; } else { - int maxPathLen = 1024; - fu.extractPath(relativeFileName,m_data->m_pathPrefix,maxPathLen); - + char path[1024]; + fu.extractPath(relativeFileName, path, sizeof(path)); + m_data->setSourceFile(relativeFileName, path); std::fstream xml_file(relativeFileName, std::fstream::in); while ( xml_file.good() ) @@ -447,6 +447,101 @@ static btCollisionShape* createConvexHullFromShapes(std::vector shorter; + shorter.push_back("../.."); + shorter.push_back(".."); + shorter.push_back("."); + int cnt = urdf_path.size(); + for (int i=0; i::iterator x=shorter.begin(); x!=shorter.end(); ++x) + { + std::string attempt = *x + "/" + fn; + FILE* f = fopen(attempt.c_str(), "rb"); + if (!f) + { + //b3Printf("%s: tried '%s'", error_message_prefix.c_str(), attempt.c_str()); + continue; + } + fclose(f); + existing_file = attempt; + //b3Printf("%s: found '%s'", error_message_prefix.c_str(), attempt.c_str()); + break; + } + } + + if (existing_file.empty()) + { + b3Warning("%s: cannot find '%s' in any directory in urdf path\n", error_message_prefix.c_str(), fn.c_str()); + return false; + } + else + { + *out_found_filename = existing_file; + return true; + } +} + btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, const char* urdfPathPrefix) { BT_PROFILE("convertURDFToCollisionShape"); @@ -467,8 +562,8 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co case URDF_GEOM_CYLINDER: { - btScalar cylRadius = collision->m_geometry.m_cylinderRadius; - btScalar cylLength = collision->m_geometry.m_cylinderLength; + btScalar cylRadius = collision->m_geometry.m_capsuleRadius; + btScalar cylLength = collision->m_geometry.m_capsuleHalfHeight; btAlignedObjectArray vertices; //int numVerts = sizeof(barrel_vertices)/(9*sizeof(float)); @@ -507,239 +602,165 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co } case URDF_GEOM_SPHERE: { - btScalar radius = collision->m_geometry.m_sphereRadius; btSphereShape* sphereShape = new btSphereShape(radius); shape = sphereShape; shape ->setMargin(gUrdfDefaultCollisionMargin); break; + } - break; - } - case URDF_GEOM_MESH: - { - if (collision->m_name.length()) + case URDF_GEOM_MESH: + { + GLInstanceGraphicsShape* glmesh = 0; + switch (collision->m_geometry.m_meshFileType) + { + case UrdfGeometry::FILE_OBJ: + if (collision->m_flags & URDF_FORCE_CONCAVE_TRIMESH) { - //b3Printf("collision->name=%s\n",collision->m_name.c_str()); + glmesh = LoadMeshFromObj(collision->m_geometry.m_meshFileName.c_str(), 0); } - if (1) + else { - if (collision->m_geometry.m_meshFileName.length()) + std::vector 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 visualShapes; + btAlignedObjectArray 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(); + glmesh->m_vertices = new b3AlignedObjectArray(); + + for (int i=0;im_geometry.m_meshFileName.c_str(); - //b3Printf("mesh->filename=%s\n",filename); - char fullPath[1024]; - int fileType = 0; - sprintf(fullPath,"%s%s",urdfPathPrefix,filename); - b3FileUtils::toLower(fullPath); - char tmpPathPrefix[1024]; - int maxPathLen = 1024; - b3FileUtils::extractPath(filename,tmpPathPrefix,maxPathLen); - - char collisionPathPrefix[1024]; - sprintf(collisionPathPrefix,"%s%s",urdfPathPrefix,tmpPathPrefix); - - - - if (strstr(fullPath,".dae")) + ColladaGraphicsInstance* instance = &visualShapeInstances[i]; + GLInstanceGraphicsShape* gfxShape = &visualShapes[instance->m_shapeIndex]; + + b3AlignedObjectArray verts; + verts.resize(gfxShape->m_vertices->size()); + + int baseIndex = glmesh->m_vertices->size(); + + for (int i=0;im_vertices->size();i++) { - fileType = FILE_COLLADA; + verts[i].normal[0] = gfxShape->m_vertices->at(i).normal[0]; + verts[i].normal[1] = gfxShape->m_vertices->at(i).normal[1]; + verts[i].normal[2] = gfxShape->m_vertices->at(i).normal[2]; + verts[i].uv[0] = gfxShape->m_vertices->at(i).uv[0]; + verts[i].uv[1] = gfxShape->m_vertices->at(i).uv[1]; + verts[i].xyzw[0] = gfxShape->m_vertices->at(i).xyzw[0]; + verts[i].xyzw[1] = gfxShape->m_vertices->at(i).xyzw[1]; + verts[i].xyzw[2] = gfxShape->m_vertices->at(i).xyzw[2]; + verts[i].xyzw[3] = gfxShape->m_vertices->at(i).xyzw[3]; + } - if (strstr(fullPath,".stl")) + + int curNumIndices = glmesh->m_indices->size(); + int additionalIndices = gfxShape->m_indices->size(); + glmesh->m_indices->resize(curNumIndices+additionalIndices); + for (int k=0;km_indices->at(curNumIndices+k)=gfxShape->m_indices->at(k)+baseIndex; } - if (strstr(fullPath,".obj")) - { - fileType = FILE_OBJ; - } - sprintf(fullPath,"%s%s",urdfPathPrefix,filename); - FILE* f = fopen(fullPath,"rb"); - if (f) + //compensate upAxisTrans and unitMeterScaling here + btMatrix4x4 upAxisMat; +upAxisMat.setIdentity(); + //upAxisMat.setPureRotation(upAxisTrans.getRotation()); + btMatrix4x4 unitMeterScalingMat; + unitMeterScalingMat.setPureScaling(btVector3(unitMeterScaling,unitMeterScaling,unitMeterScaling)); + btMatrix4x4 worldMat = unitMeterScalingMat*instance->m_worldTransform*upAxisMat; + //btMatrix4x4 worldMat = instance->m_worldTransform; + int curNumVertices = glmesh->m_vertices->size(); + int additionalVertices = verts.size(); + glmesh->m_vertices->reserve(curNumVertices+additionalVertices); + + for(int v=0;vm_flags & URDF_FORCE_CONCAVE_TRIMESH) - { - glmesh = LoadMeshFromObj(fullPath, collisionPathPrefix); - } - else - { - std::vector 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 visualShapes; - btAlignedObjectArray 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(); - glmesh->m_vertices = new b3AlignedObjectArray(); - - for (int i=0;im_shapeIndex]; - - b3AlignedObjectArray verts; - verts.resize(gfxShape->m_vertices->size()); - - int baseIndex = glmesh->m_vertices->size(); - - for (int i=0;im_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;km_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;vm_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 convertedVerts; - convertedVerts.reserve(glmesh->m_numvertices); - for (int i=0;im_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;im_numIndices/3;i++) - { - float* v0 = glmesh->m_vertices->at(glmesh->m_indices->at(i*3)).xyzw; - float* v1 = glmesh->m_vertices->at(glmesh->m_indices->at(i*3+1)).xyzw; - float* v2 = glmesh->m_vertices->at(glmesh->m_indices->at(i*3+2)).xyzw; - meshInterface->addTriangle(btVector3(v0[0],v0[1],v0[2]), - btVector3(v1[0],v1[1],v1[2]), - btVector3(v2[0],v2[1],v2[2])); - } - - btBvhTriangleMeshShape* trimesh = new btBvhTriangleMeshShape(meshInterface,true,true); - trimesh->setLocalScaling(collision->m_geometry.m_meshScale); - shape = trimesh; - } else - { - BT_PROFILE("convert btConvexHullShape"); - - btConvexHullShape* convexHull = new btConvexHullShape(&convertedVerts[0].getX(), convertedVerts.size(), sizeof(btVector3)); - convexHull->optimizeConvexHull(); - //convexHull->initializePolyhedralFeatures(); - convexHull->setMargin(gUrdfDefaultCollisionMargin); - convexHull->setLocalScaling(collision->m_geometry.m_meshScale); - shape = convexHull; - } - } else - { - b3Warning("issue extracting mesh from STL file %s\n", fullPath); - } - - delete glmesh; - - } else - { - b3Warning("mesh geometry not found %s\n",fullPath); + btVector3 pos(verts[v].xyzw[0],verts[v].xyzw[1],verts[v].xyzw[2]); + pos = worldMat*pos; + verts[v].xyzw[0] = float(pos[0]); + verts[v].xyzw[1] = float(pos[1]); + verts[v].xyzw[2] = float(pos[2]); + glmesh->m_vertices->push_back(verts[v]); } - } + glmesh->m_numIndices = glmesh->m_indices->size(); + glmesh->m_numvertices = glmesh->m_vertices->size(); + //glmesh = LoadMeshFromCollada(success.c_str()); + break; } + } + + if (!glmesh || glmesh->m_numvertices<=0) + { + b3Warning("%s: cannot extract mesh from '%s'\n", urdfPathPrefix, collision->m_geometry.m_meshFileName.c_str()); + delete glmesh; + break; + } + + btAlignedObjectArray convertedVerts; + convertedVerts.reserve(glmesh->m_numvertices); + for (int i=0; im_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; im_numIndices/3; i++) + { + float* v0 = glmesh->m_vertices->at(glmesh->m_indices->at(i*3)).xyzw; + float* v1 = glmesh->m_vertices->at(glmesh->m_indices->at(i*3+1)).xyzw; + float* v2 = glmesh->m_vertices->at(glmesh->m_indices->at(i*3+2)).xyzw; + meshInterface->addTriangle( + btVector3(v0[0],v0[1],v0[2]), + btVector3(v1[0],v1[1],v1[2]), + btVector3(v2[0],v2[1],v2[2])); + } + btBvhTriangleMeshShape* trimesh = new btBvhTriangleMeshShape(meshInterface,true,true); + trimesh->setLocalScaling(collision->m_geometry.m_meshScale); + shape = trimesh; + + } else + { + BT_PROFILE("convert btConvexHullShape"); + btConvexHullShape* convexHull = new btConvexHullShape(&convertedVerts[0].getX(), convertedVerts.size(), sizeof(btVector3)); + convexHull->optimizeConvexHull(); + //convexHull->initializePolyhedralFeatures(); + convexHull->setMargin(gUrdfDefaultCollisionMargin); + convexHull->setLocalScaling(collision->m_geometry.m_meshScale); + shape = convexHull; + } + + delete glmesh; + break; + } // mesh case - - break; - } default: - { - b3Warning("Error: unknown visual geometry type\n"); - } - } + b3Warning("Error: unknown collision geometry type %i\n", collision->m_geometry.m_type); + // for example, URDF_GEOM_PLANE + } return shape; } @@ -764,8 +785,8 @@ static void convertURDFToVisualShapeInternal(const UrdfVisual* visual, const cha for (int i = 0; im_geometry.m_cylinderRadius; - btScalar cylLength = visual->m_geometry.m_cylinderLength; + btScalar cylRadius = visual->m_geometry.m_capsuleRadius; + btScalar cylLength = visual->m_geometry.m_capsuleHalfHeight; btVector3 vert(cylRadius*btSin(SIMD_2_PI*(float(i) / numSteps)), cylRadius*btCos(SIMD_2_PI*(float(i) / numSteps)), cylLength / 2.); vertices.push_back(vert); @@ -778,17 +799,17 @@ static void convertURDFToVisualShapeInternal(const UrdfVisual* visual, const cha convexColShape = cylZShape; break; } + case URDF_GEOM_BOX: { - btVector3 extents = visual->m_geometry.m_boxSize; - btBoxShape* boxShape = new btBoxShape(extents*0.5f); //btConvexShape* boxShape = new btConeShapeX(extents[2]*0.5,extents[0]*0.5); convexColShape = boxShape; convexColShape->setMargin(gUrdfDefaultCollisionMargin); break; } + case URDF_GEOM_SPHERE: { btScalar radius = visual->m_geometry.m_sphereRadius; @@ -796,206 +817,137 @@ static void convertURDFToVisualShapeInternal(const UrdfVisual* visual, const cha convexColShape = sphereShape; convexColShape->setMargin(gUrdfDefaultCollisionMargin); break; - - break; } + case URDF_GEOM_MESH: { - if (visual->m_name.length()) + switch (visual->m_geometry.m_meshFileType) { - //b3Printf("visual->name=%s\n", visual->m_name.c_str()); - } - if (1)//visual->m_geometry) - { - if (visual->m_geometry.m_meshFileName.length()) + case UrdfGeometry::FILE_OBJ: { - const char* filename = visual->m_geometry.m_meshFileName.c_str(); - //b3Printf("mesh->filename=%s\n", filename); - char fullPath[1024]; - int fileType = 0; - - char tmpPathPrefix[1024]; - std::string xml_string; - int maxPathLen = 1024; - b3FileUtils::extractPath(filename,tmpPathPrefix,maxPathLen); - - char visualPathPrefix[1024]; - sprintf(visualPathPrefix,"%s%s",urdfPathPrefix,tmpPathPrefix); - - - sprintf(fullPath, "%s%s", urdfPathPrefix, filename); - b3FileUtils::toLower(fullPath); - if (strstr(fullPath, ".dae")) + b3ImportMeshData meshData; + if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(visual->m_geometry.m_meshFileName, meshData)) { - fileType = FILE_COLLADA; + + if (meshData.m_textureImage) + { + MyTexture texData; + texData.m_width = meshData.m_textureWidth; + texData.m_height = meshData.m_textureHeight; + texData.textureData = meshData.m_textureImage; + texturesOut.push_back(texData); + } + glmesh = meshData.m_gfxShape; } - if (strstr(fullPath, ".stl")) - { - fileType = FILE_STL; - } - if (strstr(fullPath,".obj")) - { - fileType = FILE_OBJ; - } - - - sprintf(fullPath, "%s%s", urdfPathPrefix, filename); - FILE* f = fopen(fullPath, "rb"); - if (f) - { - fclose(f); - - switch (fileType) - { - case FILE_OBJ: - { -// glmesh = LoadMeshFromObj(fullPath,visualPathPrefix); - - b3ImportMeshData meshData; - if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(fullPath, meshData)) - { - - if (meshData.m_textureImage) - { - MyTexture texData; - texData.m_width = meshData.m_textureWidth; - texData.m_height = meshData.m_textureHeight; - texData.textureData = meshData.m_textureImage; - texturesOut.push_back(texData); - } - glmesh = meshData.m_gfxShape; - } - - break; - } - - case FILE_STL: - { - glmesh = LoadMeshFromSTL(fullPath); - break; - } - case FILE_COLLADA: - { - - btAlignedObjectArray visualShapes; - btAlignedObjectArray 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(); - glmesh->m_vertices = new b3AlignedObjectArray(); - - for (int i = 0; im_shapeIndex]; - - b3AlignedObjectArray verts; - verts.resize(gfxShape->m_vertices->size()); - - int baseIndex = glmesh->m_vertices->size(); - - for (int i = 0; im_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; km_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; vm_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;im_vertices->size();i++) - { - glmesh->m_vertices->at(i).xyzw[0] *= visual->m_geometry.m_meshScale[0]; - glmesh->m_vertices->at(i).xyzw[1] *= visual->m_geometry.m_meshScale[1]; - glmesh->m_vertices->at(i).xyzw[2] *= visual->m_geometry.m_meshScale[2]; - } - - } - else - { - b3Warning("issue extracting mesh from COLLADA/STL file %s\n", fullPath); - } - - } - else - { - b3Warning("mesh geometry not found %s\n", fullPath); - } - - + break; } + + case UrdfGeometry::FILE_STL: + { + glmesh = LoadMeshFromSTL(visual->m_geometry.m_meshFileName.c_str()); + break; + } + + case UrdfGeometry::FILE_COLLADA: + { + btAlignedObjectArray visualShapes; + btAlignedObjectArray 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(); + glmesh->m_vertices = new b3AlignedObjectArray(); + + for (int i = 0; im_shapeIndex]; + + b3AlignedObjectArray verts; + verts.resize(gfxShape->m_vertices->size()); + + int baseIndex = glmesh->m_vertices->size(); + + for (int i = 0; im_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; km_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; vm_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;im_vertices->size();i++) + { + glmesh->m_vertices->at(i).xyzw[0] *= visual->m_geometry.m_meshScale[0]; + glmesh->m_vertices->at(i).xyzw[1] *= visual->m_geometry.m_meshScale[1]; + glmesh->m_vertices->at(i).xyzw[2] *= visual->m_geometry.m_meshScale[2]; + } break; } + default: - { - b3Warning("Error: unknown visual geometry type\n"); - } + b3Warning("Error: unknown visual geometry type %i\n", visual->m_geometry.m_type); } //if we have a convex, tesselate into localVertices/localIndices @@ -1166,15 +1118,17 @@ bool BulletURDFImporter::getLinkContactInfo(int linkIndex, URDFLinkContactInfo& return false; } -void BulletURDFImporter::convertLinkVisualShapes2(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, class btCollisionObject* colObj, int bodyUniqueId) const +void BulletURDFImporter::convertLinkVisualShapes2(int linkIndex, int urdfIndex, const char* pathPrefix, const btTransform& localInertiaFrame, class btCollisionObject* colObj, int bodyUniqueId) const { - if (m_data->m_customVisualShapesConverter) { const UrdfModel& model = m_data->m_urdfParser.getModel(); - m_data->m_customVisualShapesConverter->convertVisualShapes(linkIndex,pathPrefix,localInertiaFrame, model, colObj, bodyUniqueId); + UrdfLink*const* linkPtr = model.m_links.getAtIndex(urdfIndex); + if (linkPtr) + { + m_data->m_customVisualShapesConverter->convertVisualShapes(linkIndex,pathPrefix,localInertiaFrame, *linkPtr, &model, colObj, bodyUniqueId); + } } - } int BulletURDFImporter::getNumAllocatedCollisionShapes() const diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h index 26b5a66e2..2ceb8e0d7 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h @@ -51,7 +51,7 @@ public: virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const; - virtual void convertLinkVisualShapes2(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int bodyUniqueId) const; + virtual void convertLinkVisualShapes2(int linkIndex, int urdfIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int bodyUniqueId) const; ///todo(erwincoumans) refactor this convertLinkCollisionShapes/memory allocation diff --git a/examples/Importers/ImportURDFDemo/LinkVisualShapesConverter.h b/examples/Importers/ImportURDFDemo/LinkVisualShapesConverter.h index 814838ced..8f3d3c411 100644 --- a/examples/Importers/ImportURDFDemo/LinkVisualShapesConverter.h +++ b/examples/Importers/ImportURDFDemo/LinkVisualShapesConverter.h @@ -1,9 +1,14 @@ #ifndef LINK_VISUAL_SHAPES_CONVERTER_H #define LINK_VISUAL_SHAPES_CONVERTER_H +struct UrdfLink; +struct UrdfModel; +class btTransform; +class btCollisionObject; + struct LinkVisualShapesConverter { - virtual void convertVisualShapes(int linkIndex, const char* pathPrefix, const class btTransform& localInertiaFrame, const struct UrdfModel& model, class btCollisionObject* colObj, int objectIndex)=0; + virtual void convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfLink* linkPtr, const UrdfModel* model, class btCollisionObject* colShape, int objectIndex) =0; }; #endif //LINK_VISUAL_SHAPES_CONVERTER_H diff --git a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp index 30f35963a..7cd514804 100644 --- a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp +++ b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp @@ -166,6 +166,10 @@ void processContactParameters(const URDFLinkContactInfo& contactInfo, btCollisio { col->setContactStiffnessAndDamping(contactInfo.m_contactStiffness, contactInfo.m_contactDamping); } + if ((contactInfo.m_flags & URDF_CONTACT_HAS_FRICTION_ANCHOR) != 0) + { + col->setCollisionFlags(col->getCollisionFlags() | btCollisionObject::CF_HAS_FRICTION_ANCHOR); + } } @@ -311,7 +315,7 @@ void ConvertURDF2BulletInternal( - //untested: u2b.convertLinkVisualShapes2(urdfLinkIndex,pathPrefix,localInertialFrame,body); + //untested: u2b.convertLinkVisualShapes2(linkIndex,urdfLinkIndex,pathPrefix,localInertialFrame,body); } else { if (cache.m_bulletMultiBody==0) @@ -469,8 +473,8 @@ void ConvertURDF2BulletInternal( btVector4 color = selectColor2();//(0.0,0.0,0.5); u2b.getLinkColor(urdfLinkIndex,color); creation.createCollisionObjectGraphicsInstance(urdfLinkIndex,col,color); - - u2b.convertLinkVisualShapes2(urdfLinkIndex,pathPrefix,localInertialFrame,col, u2b.getBodyUniqueId()); + + u2b.convertLinkVisualShapes2(mbLinkIndex, urdfLinkIndex, pathPrefix, localInertialFrame,col, u2b.getBodyUniqueId()); URDFLinkContactInfo contactInfo; u2b.getLinkContactInfo(urdfLinkIndex,contactInfo); @@ -487,7 +491,7 @@ void ConvertURDF2BulletInternal( } } else { - //u2b.convertLinkVisualShapes2(urdfLinkIndex,pathPrefix,localInertialFrame,compoundShape); + //u2b.convertLinkVisualShapes2(urdfLinkIndex,urdfIndex,pathPrefix,localInertialFrame,compoundShape); } } diff --git a/examples/Importers/ImportURDFDemo/URDFImporterInterface.h b/examples/Importers/ImportURDFDemo/URDFImporterInterface.h index 6d4c98e57..427f98b3b 100644 --- a/examples/Importers/ImportURDFDemo/URDFImporterInterface.h +++ b/examples/Importers/ImportURDFDemo/URDFImporterInterface.h @@ -49,7 +49,7 @@ public: ///quick hack: need to rethink the API/dependencies of this virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const { return -1;} - virtual void convertLinkVisualShapes2(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int objectIndex) const { } + virtual void convertLinkVisualShapes2(int linkIndex, int urdfIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int objectIndex) const { } virtual void setBodyUniqueId(int bodyId) {} virtual int getBodyUniqueId() const { return 0;} diff --git a/examples/Importers/ImportURDFDemo/URDFJointTypes.h b/examples/Importers/ImportURDFDemo/URDFJointTypes.h index cc39e8913..3a11d981e 100644 --- a/examples/Importers/ImportURDFDemo/URDFJointTypes.h +++ b/examples/Importers/ImportURDFDemo/URDFJointTypes.h @@ -23,7 +23,8 @@ enum URDF_LinkContactFlags URDF_CONTACT_HAS_ROLLING_FRICTION=32, URDF_CONTACT_HAS_SPINNING_FRICTION=64, URDF_CONTACT_HAS_RESTITUTION=128, - + URDF_CONTACT_HAS_FRICTION_ANCHOR=256, + }; struct URDFLinkContactInfo diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.cpp b/examples/Importers/ImportURDFDemo/UrdfParser.cpp index 37cf5eb70..0b42eb2c9 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.cpp +++ b/examples/Importers/ImportURDFDemo/UrdfParser.cpp @@ -8,9 +8,9 @@ UrdfParser::UrdfParser() :m_parseSDF(false), m_activeSdfModel(-1) { + m_urdf2Model.m_sourceFile = "IN_MEMORY_STRING"; // if loadUrdf() called later, source file name will be replaced with real } - UrdfParser::~UrdfParser() { cleanModel(&m_urdf2Model); @@ -401,8 +401,9 @@ bool UrdfParser::parseGeometry(UrdfGeometry& geom, TiXmlElement* g, ErrorLogger* logger->reportError("Cylinder shape must have both length and radius attributes"); return false; } - geom.m_cylinderRadius = urdfLexicalCast(shape->Attribute("radius")); - geom.m_cylinderLength = urdfLexicalCast(shape->Attribute("length")); + geom.m_hasFromTo = false; + geom.m_capsuleRadius = urdfLexicalCast(shape->Attribute("radius")); + geom.m_capsuleHalfHeight = urdfLexicalCast(shape->Attribute("length")); } else if (type_name == "capsule") @@ -410,59 +411,68 @@ bool UrdfParser::parseGeometry(UrdfGeometry& geom, TiXmlElement* g, ErrorLogger* geom.m_type = URDF_GEOM_CAPSULE; if (!shape->Attribute("length") || !shape->Attribute("radius")) - { - logger->reportError("Capsule shape must have both length and radius attributes"); - return false; - } + { + logger->reportError("Capsule shape must have both length and radius attributes"); + return false; + } geom.m_hasFromTo = false; geom.m_capsuleRadius = urdfLexicalCast(shape->Attribute("radius")); geom.m_capsuleHalfHeight = btScalar(0.5)*urdfLexicalCast(shape->Attribute("length")); - } - else if (type_name == "mesh") - { - geom.m_type = URDF_GEOM_MESH; - if (m_parseSDF) - { - TiXmlElement* scale = shape->FirstChildElement("scale"); - if (0==scale) - { - geom.m_meshScale.setValue(1,1,1); - } - else - { - parseVector3(geom.m_meshScale,scale->GetText(),logger); - } - - TiXmlElement* filename = shape->FirstChildElement("uri"); - geom.m_meshFileName = filename->GetText(); - } - else - { - if (!shape->Attribute("filename")) { - logger->reportError("Mesh must contain a filename attribute"); - return false; - } - - geom.m_meshFileName = shape->Attribute("filename"); - geom.m_meshScale.setValue(1,1,1); + else if (type_name == "mesh") + { + geom.m_type = URDF_GEOM_MESH; + geom.m_meshScale.setValue(1,1,1); + std::string fn; - if (shape->Attribute("scale")) - { - if (!parseVector3(geom.m_meshScale,shape->Attribute("scale"),logger)) - { - logger->reportWarning("scale should be a vector3, not single scalar. Workaround activated.\n"); - std::string scalar_str = shape->Attribute("scale"); - double scaleFactor = urdfLexicalCast(scalar_str.c_str()); - if (scaleFactor) - { - geom.m_meshScale.setValue(scaleFactor,scaleFactor,scaleFactor); - } - } - } else - { - } - } + if (m_parseSDF) + { + if (TiXmlElement* scale = shape->FirstChildElement("scale")) + { + parseVector3(geom.m_meshScale,scale->GetText(),logger); + } + if (TiXmlElement* filename = shape->FirstChildElement("uri")) + { + fn = filename->GetText(); + } + } + else + { + // URDF + if (shape->Attribute("filename")) + { + fn = shape->Attribute("filename"); + } + if (shape->Attribute("scale")) + { + if (!parseVector3(geom.m_meshScale, shape->Attribute("scale"), logger)) + { + logger->reportWarning("Scale should be a vector3, not single scalar. Workaround activated.\n"); + std::string scalar_str = shape->Attribute("scale"); + double scaleFactor = urdfLexicalCast(scalar_str.c_str()); + if (scaleFactor) + { + geom.m_meshScale.setValue(scaleFactor, scaleFactor, scaleFactor); + } + } + } + } + + if (fn.empty()) + { + logger->reportError("Mesh filename is empty"); + return false; + } + + geom.m_meshFileName = fn; + bool success = findExistingMeshFile( + m_urdf2Model.m_sourceFile, fn, sourceFileLocation(shape), + &geom.m_meshFileName, &geom.m_meshFileType); + if (!success) + { + // warning already printed + return false; + } } else { @@ -566,7 +576,7 @@ bool UrdfParser::parseVisual(UrdfModel& model, UrdfVisual& visual, TiXmlElement* if (name_char) visual.m_name = name_char; - visual.m_hasLocalMaterial = false; + visual.m_geometry.m_hasLocalMaterial = false; // Material TiXmlElement *mat = config->FirstChildElement("material"); @@ -588,7 +598,7 @@ bool UrdfParser::parseVisual(UrdfModel& model, UrdfVisual& visual, TiXmlElement* matPtr->m_rgbaColor = rgba; visual.m_materialName = matPtr->m_name; - visual.m_hasLocalMaterial = true; + visual.m_geometry.m_hasLocalMaterial = true; } } else @@ -607,11 +617,11 @@ bool UrdfParser::parseVisual(UrdfModel& model, UrdfVisual& visual, TiXmlElement* TiXmlElement *c = mat->FirstChildElement("color"); if (t||c) { - if (parseMaterial(visual.m_localMaterial, mat,logger)) + if (parseMaterial(visual.m_geometry.m_localMaterial, mat,logger)) { - UrdfMaterial* matPtr = new UrdfMaterial(visual.m_localMaterial); + UrdfMaterial* matPtr = new UrdfMaterial(visual.m_geometry.m_localMaterial); model.m_materials.insert(matPtr->m_name.c_str(),matPtr); - visual.m_hasLocalMaterial = true; + visual.m_geometry.m_hasLocalMaterial = true; } } } @@ -758,6 +768,13 @@ bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *confi } } + } + { + TiXmlElement *friction_anchor = ci->FirstChildElement("friction_anchor"); + if (friction_anchor) + { + link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_FRICTION_ANCHOR; + } } { @@ -845,7 +862,8 @@ bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *confi for (TiXmlElement* vis_xml = config->FirstChildElement("visual"); vis_xml; vis_xml = vis_xml->NextSiblingElement("visual")) { UrdfVisual visual; - + visual.m_sourceFileLocation = sourceFileLocation(vis_xml); + if (parseVisual(model, visual, vis_xml,logger)) { link.m_visualArray.push_back(visual); @@ -864,6 +882,8 @@ bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *confi for (TiXmlElement* col_xml = config->FirstChildElement("collision"); col_xml; col_xml = col_xml->NextSiblingElement("collision")) { UrdfCollision col; + col.m_sourceFileLocation = sourceFileLocation(col_xml); + if (parseCollision(col, col_xml,logger)) { link.m_collisionArray.push_back(col); @@ -1396,12 +1416,12 @@ bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceF for (int i=0;im_visualArray.size();i++) { UrdfVisual& vis = link->m_visualArray.at(i); - if (!vis.m_hasLocalMaterial && vis.m_materialName.c_str()) + if (!vis.m_geometry.m_hasLocalMaterial && vis.m_materialName.c_str()) { UrdfMaterial** mat = m_urdf2Model.m_materials.find(vis.m_materialName.c_str()); if (mat && *mat) { - vis.m_localMaterial = **mat; + vis.m_geometry.m_localMaterial = **mat; } else { //logger->reportError("Cannot find material with name:"); @@ -1591,12 +1611,12 @@ bool UrdfParser::loadSDF(const char* sdfText, ErrorLogger* logger) for (int i=0;im_visualArray.size();i++) { UrdfVisual& vis = link->m_visualArray.at(i); - if (!vis.m_hasLocalMaterial && vis.m_materialName.c_str()) + if (!vis.m_geometry.m_hasLocalMaterial && vis.m_materialName.c_str()) { UrdfMaterial** mat = localModel->m_materials.find(vis.m_materialName.c_str()); if (mat && *mat) { - vis.m_localMaterial = **mat; + vis.m_geometry.m_localMaterial = **mat; } else { //logger->reportError("Cannot find material with name:"); @@ -1657,3 +1677,9 @@ bool UrdfParser::loadSDF(const char* sdfText, ErrorLogger* logger) return true; } +std::string UrdfParser::sourceFileLocation(TiXmlElement* e) +{ + char buf[1024]; + snprintf(buf, sizeof(buf), "%s:%i", m_urdf2Model.m_sourceFile.c_str(), e->Row()); + return buf; +} diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.h b/examples/Importers/ImportURDFDemo/UrdfParser.h index 64088a1d9..015342a0a 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.h +++ b/examples/Importers/ImportURDFDemo/UrdfParser.h @@ -18,9 +18,13 @@ struct ErrorLogger struct UrdfMaterial { - std::string m_name; + std::string m_name; std::string m_textureFilename; - btVector4 m_rgbaColor; + btVector4 m_rgbaColor; // [0]==r [1]==g [2]==b [3]==a + UrdfMaterial(): + m_rgbaColor(0.8, 0.8, 0.8, 1) + { + } }; struct UrdfInertia @@ -66,33 +70,40 @@ struct UrdfGeometry btVector3 m_capsuleFrom; btVector3 m_capsuleTo; - double m_cylinderRadius; - double m_cylinderLength; - - btVector3 m_planeNormal; + btVector3 m_planeNormal; + enum { + FILE_STL =1, + FILE_COLLADA =2, + FILE_OBJ =3, + }; + int m_meshFileType; std::string m_meshFileName; - btVector3 m_meshScale; -}; + btVector3 m_meshScale; -struct UrdfVisual -{ - btTransform m_linkLocalFrame; - UrdfGeometry m_geometry; - std::string m_name; - std::string m_materialName; - bool m_hasLocalMaterial; UrdfMaterial m_localMaterial; + bool m_hasLocalMaterial; }; +bool findExistingMeshFile(const std::string& urdf_path, std::string fn, + const std::string& error_message_prefix, + std::string* out_found_filename, int* out_type); // intended to fill UrdfGeometry::m_meshFileName and Type, but can be used elsewhere - - -struct UrdfCollision +struct UrdfShape { + std::string m_sourceFileLocation; btTransform m_linkLocalFrame; UrdfGeometry m_geometry; std::string m_name; +}; + +struct UrdfVisual: UrdfShape +{ + std::string m_materialName; +}; + +struct UrdfCollision: UrdfShape +{ int m_flags; int m_collisionGroup; int m_collisionMask; @@ -159,6 +170,7 @@ struct UrdfJoint struct UrdfModel { std::string m_name; + std::string m_sourceFile; btTransform m_rootTransformInWorld; btHashMap m_materials; btHashMap m_links; @@ -204,7 +216,7 @@ public: UrdfParser(); virtual ~UrdfParser(); - + void setParseSDF(bool useSDF) { m_parseSDF = useSDF; @@ -263,6 +275,13 @@ public: } return m_urdf2Model; } + + std::string sourceFileLocation(TiXmlElement* e); + + void setSourceFile(const std::string& sourceFile) + { + m_urdf2Model.m_sourceFile = sourceFile; + } }; #endif diff --git a/examples/OpenGLWindow/SimpleOpenGL3App.cpp b/examples/OpenGLWindow/SimpleOpenGL3App.cpp index 212d80a6f..f7e37688a 100644 --- a/examples/OpenGLWindow/SimpleOpenGL3App.cpp +++ b/examples/OpenGLWindow/SimpleOpenGL3App.cpp @@ -774,16 +774,18 @@ void SimpleOpenGL3App::swapBuffer() m_data->m_frameDumpPngFileName = 0; } } - m_window->endRendering(); - m_window->startRendering(); + m_window->endRendering(); + m_window->startRendering(); } // see also http://blog.mmacklin.com/2013/06/11/real-time-video-capture-with-ffmpeg/ void SimpleOpenGL3App::dumpFramesToVideo(const char* mp4FileName) { - int width = (int)m_window->getRetinaScale()*m_instancingRenderer->getScreenWidth(); - int height = (int)m_window->getRetinaScale()*m_instancingRenderer->getScreenHeight(); - char cmd[8192]; + if (mp4FileName) + { + int width = (int)m_window->getRetinaScale()*m_instancingRenderer->getScreenWidth(); + int height = (int)m_window->getRetinaScale()*m_instancingRenderer->getScreenHeight(); + char cmd[8192]; #ifdef _WIN32 sprintf(cmd, "ffmpeg -r 60 -f rawvideo -pix_fmt rgba -s %dx%d -i - " @@ -803,15 +805,25 @@ void SimpleOpenGL3App::dumpFramesToVideo(const char* mp4FileName) // sprintf(cmd,"ffmpeg -r 60 -f rawvideo -pix_fmt rgba -s %dx%d -i - " // "-threads 0 -preset fast -y -crf 21 -vf vflip %s",width,height,mp4FileName); - if (m_data->m_ffmpegFile) - { - pclose(m_data->m_ffmpegFile); - } - if (mp4FileName) - { - m_data->m_ffmpegFile = popen(cmd, "w"); + if (m_data->m_ffmpegFile) + { + pclose(m_data->m_ffmpegFile); + } + if (mp4FileName) + { + m_data->m_ffmpegFile = popen(cmd, "w"); - m_data->m_frameDumpPngFileName = mp4FileName; + m_data->m_frameDumpPngFileName = mp4FileName; + } + } else + { + if (m_data->m_ffmpegFile) + { + fflush(m_data->m_ffmpegFile); + pclose(m_data->m_ffmpegFile); + m_data->m_frameDumpPngFileName = 0; + } + m_data->m_ffmpegFile = 0; } } void SimpleOpenGL3App::dumpNextFrameToPng(const char* filename) diff --git a/examples/RobotSimulator/CMakeLists.txt b/examples/RobotSimulator/CMakeLists.txt index 0dbf71053..c9e0a66c3 100644 --- a/examples/RobotSimulator/CMakeLists.txt +++ b/examples/RobotSimulator/CMakeLists.txt @@ -138,8 +138,34 @@ IF(BUILD_CLSOCKET) ) ENDIF() -ADD_EXECUTABLE(App_RobotSimulator ${RobotSimulator_SRCS}) +#some code to support OpenGL and Glew cross platform +IF (WIN32) + INCLUDE_DIRECTORIES( + ${BULLET_PHYSICS_SOURCE_DIR}/btgui/OpenGLWindow/GlewWindows + ) + ADD_DEFINITIONS(-DGLEW_STATIC) + LINK_LIBRARIES( ${OPENGL_gl_LIBRARY} ${OPENGL_glu_LIBRARY} ) +ELSE(WIN32) + IF(APPLE) + find_library(COCOA NAMES Cocoa) + MESSAGE(${COCOA}) + link_libraries(${COCOA} ${OPENGL_gl_LIBRARY} ${OPENGL_glu_LIBRARY}) + + ELSE(APPLE) + INCLUDE_DIRECTORIES( + ${BULLET_PHYSICS_SOURCE_DIR}/btgui/OpenGLWindow/GlewWindows + ) + ADD_DEFINITIONS("-DGLEW_INIT_OPENGL11_FUNCTIONS=1") + ADD_DEFINITIONS("-DGLEW_STATIC") + ADD_DEFINITIONS("-DGLEW_DYNAMIC_LOAD_ALL_GLX_FUNCTIONS=1") + + LINK_LIBRARIES( pthread dl ) + ENDIF(APPLE) +ENDIF(WIN32) + + +ADD_EXECUTABLE(App_RobotSimulator ${RobotSimulator_SRCS}) SET_TARGET_PROPERTIES(App_RobotSimulator PROPERTIES VERSION ${BULLET_VERSION}) SET_TARGET_PROPERTIES(App_RobotSimulator PROPERTIES DEBUG_POSTFIX "_d") @@ -153,7 +179,6 @@ ENDIF(WIN32) - TARGET_LINK_LIBRARIES(App_RobotSimulator BulletExampleBrowserLib BulletFileLoader BulletWorldImporter BulletSoftBody BulletDynamics BulletCollision BulletInverseDynamicsUtils BulletInverseDynamics LinearMath OpenGLWindow gwen BussIK Bullet3Common) diff --git a/examples/RobotSimulator/MinitaurSetup.cpp b/examples/RobotSimulator/MinitaurSetup.cpp index 0d1238c87..e867d0583 100644 --- a/examples/RobotSimulator/MinitaurSetup.cpp +++ b/examples/RobotSimulator/MinitaurSetup.cpp @@ -49,57 +49,69 @@ void MinitaurSetup::resetPose(class b3RobotSimulatorClientAPI* sim) sim->setJointMotorControl(m_data->m_quadrupedUniqueId,i,controlArgs); } - //right front leg - sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["motor_front_rightR_joint"],1.57); - sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_front_rightR_link"],-2.2); - sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["motor_front_rightL_joint"],-1.57); - sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_front_rightL_link"],2.2); + b3Scalar startAngle = B3_HALF_PI; + b3Scalar upperLegLength = 11.5; + b3Scalar lowerLegLength = 20; + b3Scalar kneeAngle = B3_PI+b3Acos(upperLegLength/lowerLegLength); + + b3Scalar motorDirs[8] = {-1,-1,-1,-1,1,1,1,1}; b3JointInfo jointInfo; jointInfo.m_jointType = ePoint2PointType; - jointInfo.m_parentFrame[0] = 0; jointInfo.m_parentFrame[1] = 0.01; jointInfo.m_parentFrame[2] = 0.2; - jointInfo.m_childFrame[0] = 0; jointInfo.m_childFrame[1] = -0.015; jointInfo.m_childFrame[2] = 0.2; - sim->createConstraint(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_front_rightR_link"], - m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_front_rightL_link"],&jointInfo); - setDesiredMotorAngle(sim,"motor_front_rightR_joint",1.57); - setDesiredMotorAngle(sim,"motor_front_rightL_joint",-1.57); - - //left front leg - sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["motor_front_leftR_joint"],1.57); - sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_front_leftR_link"],-2.2); - sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["motor_front_leftL_joint"],-1.57); - sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_front_leftL_link"],2.2); - jointInfo.m_parentFrame[0] = 0; jointInfo.m_parentFrame[1] = -0.01; jointInfo.m_parentFrame[2] = 0.2; - jointInfo.m_childFrame[0] = 0; jointInfo.m_childFrame[1] = 0.015; jointInfo.m_childFrame[2] = 0.2; + //left front leg + sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["motor_front_leftL_joint"],motorDirs[0] * startAngle); + sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_front_leftL_link"],motorDirs[0]*kneeAngle); + sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["motor_front_leftR_joint"],motorDirs[1] * startAngle); + sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_front_leftR_link"],motorDirs[1]*kneeAngle); + + jointInfo.m_parentFrame[0] = 0; jointInfo.m_parentFrame[1] = 0.005; jointInfo.m_parentFrame[2] = 0.2; + jointInfo.m_childFrame[0] = 0; jointInfo.m_childFrame[1] = 0.01; jointInfo.m_childFrame[2] = 0.2; sim->createConstraint(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_front_leftR_link"], m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_front_leftL_link"],&jointInfo); - setDesiredMotorAngle(sim,"motor_front_leftR_joint", 1.57); - setDesiredMotorAngle(sim,"motor_front_leftL_joint", -1.57); - - //right back leg - sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["motor_back_rightR_joint"],1.57); - sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_back_rightR_link"],-2.2); - sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["motor_back_rightL_joint"],-1.57); - sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_back_rightL_link"],2.2); - jointInfo.m_parentFrame[0] = 0; jointInfo.m_parentFrame[1] = 0.01; jointInfo.m_parentFrame[2] = 0.2; - jointInfo.m_childFrame[0] = 0; jointInfo.m_childFrame[1] = -0.015; jointInfo.m_childFrame[2] = 0.2; - sim->createConstraint(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_back_rightR_link"], - m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_back_rightL_link"],&jointInfo); - setDesiredMotorAngle(sim,"motor_back_rightR_joint", 1.57); - setDesiredMotorAngle(sim,"motor_back_rightL_joint", -1.57); - + setDesiredMotorAngle(sim,"motor_front_leftL_joint", motorDirs[0] * startAngle); + setDesiredMotorAngle(sim,"motor_front_leftR_joint", motorDirs[1] * startAngle); + //left back leg - sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["motor_back_leftR_joint"],1.57); - sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_back_leftR_link"],-2.2); - sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["motor_back_leftL_joint"],-1.57); - sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_back_leftL_link"],2.2); - jointInfo.m_parentFrame[0] = 0; jointInfo.m_parentFrame[1] = -0.01; jointInfo.m_parentFrame[2] = 0.2; - jointInfo.m_childFrame[0] = 0; jointInfo.m_childFrame[1] = 0.015; jointInfo.m_childFrame[2] = 0.2; + sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["motor_back_leftL_joint"],motorDirs[2] * startAngle); + sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_back_leftL_link"],motorDirs[2] * kneeAngle); + sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["motor_back_leftR_joint"],motorDirs[3] * startAngle); + sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_back_leftR_link"],motorDirs[3] * kneeAngle); + jointInfo.m_parentFrame[0] = 0; jointInfo.m_parentFrame[1] = 0.005; jointInfo.m_parentFrame[2] = 0.2; + jointInfo.m_childFrame[0] = 0; jointInfo.m_childFrame[1] = 0.01; jointInfo.m_childFrame[2] = 0.2; sim->createConstraint(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_back_leftR_link"], m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_back_leftL_link"],&jointInfo); - setDesiredMotorAngle(sim,"motor_back_leftR_joint", 1.57); - setDesiredMotorAngle(sim,"motor_back_leftL_joint", -1.57); + setDesiredMotorAngle(sim,"motor_back_leftL_joint", motorDirs[2] * startAngle); + setDesiredMotorAngle(sim,"motor_back_leftR_joint", motorDirs[3] * startAngle); + //right front leg + sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["motor_front_rightL_joint"],motorDirs[4] * startAngle); + sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_front_rightL_link"],motorDirs[4] * kneeAngle); + sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["motor_front_rightR_joint"],motorDirs[5]*startAngle); + sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_front_rightR_link"],motorDirs[5] * kneeAngle); + + + + jointInfo.m_parentFrame[0] = 0; jointInfo.m_parentFrame[1] = 0.005; jointInfo.m_parentFrame[2] = 0.2; + jointInfo.m_childFrame[0] = 0; jointInfo.m_childFrame[1] = 0.01; jointInfo.m_childFrame[2] = 0.2; + sim->createConstraint(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_front_rightR_link"], + m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_front_rightL_link"],&jointInfo); + setDesiredMotorAngle(sim,"motor_front_rightL_joint",motorDirs[4] * startAngle); + setDesiredMotorAngle(sim,"motor_front_rightR_joint",motorDirs[5] * startAngle); + + + //right back leg + sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["motor_back_rightL_joint"],motorDirs[6] * startAngle); + sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_back_rightL_link"],motorDirs[6] * kneeAngle); + sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["motor_back_rightR_joint"],motorDirs[7] * startAngle); + sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_back_rightR_link"],motorDirs[7] * kneeAngle); + + jointInfo.m_parentFrame[0] = 0; jointInfo.m_parentFrame[1] = 0.005; jointInfo.m_parentFrame[2] = 0.2; + jointInfo.m_childFrame[0] = 0; jointInfo.m_childFrame[1] = 0.01; jointInfo.m_childFrame[2] = 0.2; + sim->createConstraint(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_back_rightR_link"], + m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_back_rightL_link"],&jointInfo); + setDesiredMotorAngle(sim,"motor_back_rightL_joint", motorDirs[6] * startAngle); + setDesiredMotorAngle(sim,"motor_back_rightR_joint", motorDirs[7] * startAngle); + } int MinitaurSetup::setupMinitaur(class b3RobotSimulatorClientAPI* sim, const b3Vector3& startPos, const b3Quaternion& startOrn) @@ -109,7 +121,7 @@ int MinitaurSetup::setupMinitaur(class b3RobotSimulatorClientAPI* sim, const b3V args.m_startPosition = startPos; args.m_startOrientation = startOrn; - m_data->m_quadrupedUniqueId = sim->loadURDF("quadruped/quadruped.urdf",args); + m_data->m_quadrupedUniqueId = sim->loadURDF("quadruped/minitaur.urdf",args); int numJoints = sim->getNumJoints(m_data->m_quadrupedUniqueId); for (int i=0;iconnect(eCONNECT_UDP, "localhost", 1234); sim->configureDebugVisualizer( COV_ENABLE_GUI, 0); // sim->configureDebugVisualizer( COV_ENABLE_SHADOWS, 0);//COV_ENABLE_WIREFRAME - + sim->setTimeOut(10); //syncBodies is only needed when connecting to an existing physics server that has already some bodies sim->syncBodies(); - - sim->setTimeStep(1./240.); + b3Scalar fixedTimeStep = 1./240.; - sim->setGravity(b3MakeVector3(0,0,-10)); + sim->setTimeStep(fixedTimeStep); - int blockId = sim->loadURDF("cube.urdf"); - b3BodyInfo bodyInfo; - sim->getBodyInfo(blockId,&bodyInfo); + b3Quaternion q = sim->getQuaternionFromEuler(b3MakeVector3(0.1,0.2,0.3)); + b3Vector3 rpy; + rpy = sim->getEulerFromQuaternion(q); + + sim->setGravity(b3MakeVector3(0,0,-9.8)); + + //int blockId = sim->loadURDF("cube.urdf"); + //b3BodyInfo bodyInfo; + //sim->getBodyInfo(blockId,&bodyInfo); sim->loadURDF("plane.urdf"); @@ -34,15 +39,15 @@ int main(int argc, char* argv[]) int minitaurUid = minitaur.setupMinitaur(sim, b3MakeVector3(0,0,.3)); - b3RobotSimulatorLoadUrdfFileArgs args; - args.m_startPosition.setValue(2,0,1); - int r2d2 = sim->loadURDF("r2d2.urdf",args); + //b3RobotSimulatorLoadUrdfFileArgs args; + //args.m_startPosition.setValue(2,0,1); + //int r2d2 = sim->loadURDF("r2d2.urdf",args); - b3RobotSimulatorLoadFileResults sdfResults; - if (!sim->loadSDF("two_cubes.sdf",sdfResults)) - { - b3Warning("Can't load SDF!\n"); - } + //b3RobotSimulatorLoadFileResults sdfResults; + //if (!sim->loadSDF("two_cubes.sdf",sdfResults)) + //{ +// b3Warning("Can't load SDF!\n"); + //} b3Clock clock; double startTime = clock.getTimeInSeconds(); @@ -54,7 +59,10 @@ int main(int argc, char* argv[]) } #endif sim->setRealTimeSimulation(false); - + int vidLogId = -1; + int minitaurLogId = -1; + int rotateCamera = 0; + while (sim->canSubmitCommand()) { b3KeyboardEventsData keyEvents; @@ -62,14 +70,63 @@ int main(int argc, char* argv[]) if (keyEvents.m_numKeyboardEvents) { - printf("num key events = %d]\n", keyEvents.m_numKeyboardEvents); + //printf("num key events = %d]\n", keyEvents.m_numKeyboardEvents); //m_keyState is a flag combination of eButtonIsDown,eButtonTriggered, eButtonReleased for (int i=0;istartStateLogging(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"); diff --git a/examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp b/examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp index 65692358d..64b062964 100644 --- a/examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp +++ b/examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp @@ -1,17 +1,16 @@ #include "b3RobotSimulatorClientAPI.h" - //#include "SharedMemoryCommands.h" #include "SharedMemory/PhysicsClientC_API.h" #ifdef BT_ENABLE_ENET #include "SharedMemory/PhysicsClientUDP_C_API.h" -#endif//PHYSICS_UDP +#endif //PHYSICS_UDP #ifdef BT_ENABLE_CLSOCKET #include "SharedMemory/PhysicsClientTCP_C_API.h" -#endif//PHYSICS_TCP +#endif //PHYSICS_TCP #include "SharedMemory/PhysicsDirectC_API.h" @@ -25,7 +24,7 @@ struct b3RobotSimulatorClientAPI_InternalData b3PhysicsClientHandle m_physicsClientHandle; b3RobotSimulatorClientAPI_InternalData() - :m_physicsClientHandle(0) + : m_physicsClientHandle(0) { } }; @@ -35,17 +34,16 @@ b3RobotSimulatorClientAPI::b3RobotSimulatorClientAPI() m_data = new b3RobotSimulatorClientAPI_InternalData(); } - b3RobotSimulatorClientAPI::~b3RobotSimulatorClientAPI() +b3RobotSimulatorClientAPI::~b3RobotSimulatorClientAPI() { - delete m_data; + delete m_data; } - bool b3RobotSimulatorClientAPI::connect(int mode, const std::string& hostName, int portOrKey) { if (m_data->m_physicsClientHandle) { - b3Warning ("Already connected, disconnect first."); + b3Warning("Already connected, disconnect first."); return false; } b3PhysicsClientHandle sm = 0; @@ -54,86 +52,97 @@ bool b3RobotSimulatorClientAPI::connect(int mode, const std::string& hostName, i int tcpPort = 6667; int key = SHARED_MEMORY_KEY; bool connected = false; - - switch (mode) + switch (mode) { - case eCONNECT_GUI: + case eCONNECT_GUI: { - int argc = 0; - char* argv[1] = {0}; - #ifdef __APPLE__ - sm = b3CreateInProcessPhysicsServerAndConnectMainThread(argc, argv); - #else - sm = b3CreateInProcessPhysicsServerAndConnect(argc, argv); - #endif - break; + int argc = 0; + char* argv[1] = {0}; +#ifdef __APPLE__ + sm = b3CreateInProcessPhysicsServerAndConnectMainThread(argc, argv); +#else + sm = b3CreateInProcessPhysicsServerAndConnect(argc, argv); +#endif + break; } - case eCONNECT_DIRECT: { - sm = b3ConnectPhysicsDirect(); - break; + case eCONNECT_DIRECT: + { + sm = b3ConnectPhysicsDirect(); + break; } - case eCONNECT_SHARED_MEMORY: { - if (portOrKey>=0) + case eCONNECT_SHARED_MEMORY: + { + if (portOrKey >= 0) { key = portOrKey; } - sm = b3ConnectSharedMemory(key); - break; + sm = b3ConnectSharedMemory(key); + break; } - case eCONNECT_UDP: - { - if (portOrKey>=0) + case eCONNECT_UDP: + { + if (portOrKey >= 0) { udpPort = portOrKey; } - #ifdef BT_ENABLE_ENET +#ifdef BT_ENABLE_ENET sm = b3ConnectPhysicsUDP(hostName.c_str(), udpPort); - #else - b3Warning("UDP is not enabled in this build"); - #endif //BT_ENABLE_ENET +#else + b3Warning("UDP is not enabled in this build"); +#endif //BT_ENABLE_ENET break; - } - case eCONNECT_TCP: + } + case eCONNECT_TCP: { - if (portOrKey>=0) + if (portOrKey >= 0) { tcpPort = portOrKey; } - #ifdef BT_ENABLE_CLSOCKET - +#ifdef BT_ENABLE_CLSOCKET + sm = b3ConnectPhysicsTCP(hostName.c_str(), tcpPort); - #else +#else b3Warning("TCP is not enabled in this pybullet build"); - #endif //BT_ENABLE_CLSOCKET +#endif //BT_ENABLE_CLSOCKET break; } - - - default: { - b3Warning("connectPhysicsServer unexpected argument"); + default: + { + b3Warning("connectPhysicsServer unexpected argument"); } }; - if (sm) - { - m_data->m_physicsClientHandle = sm; - if (!b3CanSubmitCommand(m_data->m_physicsClientHandle)) - { - disconnect(); - return false; - } - return true; - } - return false; + if (sm) + { + m_data->m_physicsClientHandle = sm; + if (!b3CanSubmitCommand(m_data->m_physicsClientHandle)) + { + disconnect(); + return false; + } + return true; + } + return false; } bool b3RobotSimulatorClientAPI::isConnected() const { - return (m_data->m_physicsClientHandle!=0); + return (m_data->m_physicsClientHandle != 0); +} + +void b3RobotSimulatorClientAPI::setTimeOut(double timeOutInSec) +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return; + } + b3SetTimeOut(m_data->m_physicsClientHandle,timeOutInSec); + } @@ -157,15 +166,13 @@ void b3RobotSimulatorClientAPI::syncBodies() return; } - - b3SharedMemoryCommandHandle command; + b3SharedMemoryCommandHandle command; b3SharedMemoryStatusHandle statusHandle; int statusType; command = b3InitSyncBodyInfoCommand(m_data->m_physicsClientHandle); statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); statusType = b3GetStatusType(statusHandle); - } void b3RobotSimulatorClientAPI::resetSimulation() @@ -175,9 +182,9 @@ void b3RobotSimulatorClientAPI::resetSimulation() b3Warning("Not connected"); return; } - b3SharedMemoryStatusHandle statusHandle; - statusHandle = b3SubmitClientCommandAndWaitStatus( - m_data->m_physicsClientHandle, b3InitResetSimulationCommand(m_data->m_physicsClientHandle)); + b3SharedMemoryStatusHandle statusHandle; + statusHandle = b3SubmitClientCommandAndWaitStatus( + m_data->m_physicsClientHandle, b3InitResetSimulationCommand(m_data->m_physicsClientHandle)); } bool b3RobotSimulatorClientAPI::canSubmitCommand() const @@ -186,7 +193,7 @@ bool b3RobotSimulatorClientAPI::canSubmitCommand() const { return false; } - return (b3CanSubmitCommand(m_data->m_physicsClientHandle)); + return (b3CanSubmitCommand(m_data->m_physicsClientHandle) != 0); } void b3RobotSimulatorClientAPI::stepSimulation() @@ -200,11 +207,11 @@ void b3RobotSimulatorClientAPI::stepSimulation() b3SharedMemoryStatusHandle statusHandle; int statusType; if (b3CanSubmitCommand(m_data->m_physicsClientHandle)) - { - statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, b3InitStepSimulationCommand(m_data->m_physicsClientHandle)); - statusType = b3GetStatusType(statusHandle); - b3Assert(statusType==CMD_STEP_FORWARD_SIMULATION_COMPLETED); - } + { + statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, b3InitStepSimulationCommand(m_data->m_physicsClientHandle)); + statusType = b3GetStatusType(statusHandle); + //b3Assert(statusType == CMD_STEP_FORWARD_SIMULATION_COMPLETED); + } } void b3RobotSimulatorClientAPI::setGravity(const b3Vector3& gravityAcceleration) @@ -215,57 +222,26 @@ void b3RobotSimulatorClientAPI::setGravity(const b3Vector3& gravityAcceleration) return; } - b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle); - b3SharedMemoryStatusHandle statusHandle; - b3PhysicsParamSetGravity(command, gravityAcceleration[0],gravityAcceleration[1],gravityAcceleration[2]); + b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle); + b3SharedMemoryStatusHandle statusHandle; + b3PhysicsParamSetGravity(command, gravityAcceleration[0], gravityAcceleration[1], gravityAcceleration[2]); statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); - b3Assert(b3GetStatusType(statusHandle)==CMD_CLIENT_COMMAND_COMPLETED); +// b3Assert(b3GetStatusType(statusHandle) == CMD_CLIENT_COMMAND_COMPLETED); } b3Quaternion b3RobotSimulatorClientAPI::getQuaternionFromEuler(const b3Vector3& rollPitchYaw) { - double phi, the, psi; - phi = rollPitchYaw[0] / 2.0; - the = rollPitchYaw[1] / 2.0; - psi = rollPitchYaw[2] / 2.0; - double quat[4] = { - sin(phi) * cos(the) * cos(psi) - cos(phi) * sin(the) * sin(psi), - cos(phi) * sin(the) * cos(psi) + sin(phi) * cos(the) * sin(psi), - cos(phi) * cos(the) * sin(psi) - sin(phi) * sin(the) * cos(psi), - cos(phi) * cos(the) * cos(psi) + sin(phi) * sin(the) * sin(psi)}; - - // normalize the quaternion - double len = sqrt(quat[0] * quat[0] + quat[1] * quat[1] + - quat[2] * quat[2] + quat[3] * quat[3]); - quat[0] /= len; - quat[1] /= len; - quat[2] /= len; - quat[3] /= len; - b3Quaternion q(quat[0],quat[1],quat[2],quat[3]); + b3Quaternion q; + q.setEulerZYX(rollPitchYaw[2],rollPitchYaw[1],rollPitchYaw[0]); return q; } b3Vector3 b3RobotSimulatorClientAPI::getEulerFromQuaternion(const b3Quaternion& quat) { - double squ; - double sqx; - double sqy; - double sqz; - double sarg; - - b3Vector3 rpy; - sqx = quat[0] * quat[0]; - sqy = quat[1] * quat[1]; - sqz = quat[2] * quat[2]; - squ = quat[3] * quat[3]; - rpy[0] = atan2(2 * (quat[1] * quat[2] + quat[3] * quat[0]), - squ - sqx - sqy + sqz); - sarg = -2 * (quat[0] * quat[2] - quat[3] * quat[1]); - rpy[1] = sarg <= -1.0 ? -0.5 * 3.141592538 - : (sarg >= 1.0 ? 0.5 * 3.141592538 : asin(sarg)); - rpy[2] = atan2(2 * (quat[0] * quat[1] + quat[3] * quat[2]), - squ + sqx - sqy - sqz); - return rpy; + b3Scalar roll,pitch,yaw; + quat.getEulerZYX(yaw,pitch,roll); + b3Vector3 rpy2 = b3MakeVector3(roll,pitch,yaw); + return rpy2; } int b3RobotSimulatorClientAPI::loadURDF(const std::string& fileName, const struct b3RobotSimulatorLoadUrdfFileArgs& args) @@ -278,31 +254,26 @@ int b3RobotSimulatorClientAPI::loadURDF(const std::string& fileName, const struc return robotUniqueId; } - - b3SharedMemoryStatusHandle statusHandle; int statusType; b3SharedMemoryCommandHandle command = b3LoadUrdfCommandInit(m_data->m_physicsClientHandle, fileName.c_str()); - + //setting the initial position, orientation and other arguments are optional - + b3LoadUrdfCommandSetStartPosition(command, args.m_startPosition[0], - args.m_startPosition[1], - args.m_startPosition[2]); - b3LoadUrdfCommandSetStartOrientation(command,args.m_startOrientation[0] - ,args.m_startOrientation[1] - ,args.m_startOrientation[2] - ,args.m_startOrientation[3]); + args.m_startPosition[1], + args.m_startPosition[2]); + b3LoadUrdfCommandSetStartOrientation(command, args.m_startOrientation[0], args.m_startOrientation[1], args.m_startOrientation[2], args.m_startOrientation[3]); if (args.m_forceOverrideFixedBase) { - b3LoadUrdfCommandSetUseFixedBase(command,true); + b3LoadUrdfCommandSetUseFixedBase(command, true); } b3LoadUrdfCommandSetUseMultiBody(command, args.m_useMultiBody); statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); statusType = b3GetStatusType(statusHandle); - b3Assert(statusType==CMD_URDF_LOADING_COMPLETED); - if (statusType==CMD_URDF_LOADING_COMPLETED) + b3Assert(statusType == CMD_URDF_LOADING_COMPLETED); + if (statusType == CMD_URDF_LOADING_COMPLETED) { robotUniqueId = b3GetStatusBodyIndex(statusHandle); } @@ -329,12 +300,12 @@ bool b3RobotSimulatorClientAPI::loadMJCF(const std::string& fileName, b3RobotSim b3Warning("Couldn't load .mjcf file."); return false; } - int numBodies = b3GetStatusBodyIndices(statusHandle, 0,0); + int numBodies = b3GetStatusBodyIndices(statusHandle, 0, 0); if (numBodies) { results.m_uniqueObjectIds.resize(numBodies); int numBodies; - numBodies = b3GetStatusBodyIndices(statusHandle, &results.m_uniqueObjectIds[0],results.m_uniqueObjectIds.size()); + numBodies = b3GetStatusBodyIndices(statusHandle, &results.m_uniqueObjectIds[0], results.m_uniqueObjectIds.size()); } return true; @@ -359,12 +330,12 @@ bool b3RobotSimulatorClientAPI::loadBullet(const std::string& fileName, b3RobotS { return false; } - int numBodies = b3GetStatusBodyIndices(statusHandle, 0,0); + int numBodies = b3GetStatusBodyIndices(statusHandle, 0, 0); if (numBodies) { results.m_uniqueObjectIds.resize(numBodies); int numBodies; - numBodies = b3GetStatusBodyIndices(statusHandle, &results.m_uniqueObjectIds[0],results.m_uniqueObjectIds.size()); + numBodies = b3GetStatusBodyIndices(statusHandle, &results.m_uniqueObjectIds[0], results.m_uniqueObjectIds.size()); } return true; @@ -383,19 +354,18 @@ bool b3RobotSimulatorClientAPI::loadSDF(const std::string& fileName, b3RobotSimu b3SharedMemoryStatusHandle statusHandle; int statusType; b3SharedMemoryCommandHandle command = b3LoadSdfCommandInit(m_data->m_physicsClientHandle, fileName.c_str()); - b3LoadSdfCommandSetUseMultiBody(command, args.m_useMultiBody); + b3LoadSdfCommandSetUseMultiBody(command, args.m_useMultiBody); statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); statusType = b3GetStatusType(statusHandle); b3Assert(statusType == CMD_SDF_LOADING_COMPLETED); if (statusType == CMD_SDF_LOADING_COMPLETED) { - int numBodies = b3GetStatusBodyIndices(statusHandle, 0,0); + int numBodies = b3GetStatusBodyIndices(statusHandle, 0, 0); if (numBodies) { results.m_uniqueObjectIds.resize(numBodies); int numBodies; - numBodies = b3GetStatusBodyIndices(statusHandle, &results.m_uniqueObjectIds[0],results.m_uniqueObjectIds.size()); - + numBodies = b3GetStatusBodyIndices(statusHandle, &results.m_uniqueObjectIds[0], results.m_uniqueObjectIds.size()); } statusOk = true; } @@ -411,7 +381,7 @@ bool b3RobotSimulatorClientAPI::getBodyInfo(int bodyUniqueId, struct b3BodyInfo* return false; } - int result = b3GetBodyInfo(m_data->m_physicsClientHandle, bodyUniqueId, bodyInfo); + int result = b3GetBodyInfo(m_data->m_physicsClientHandle, bodyUniqueId, bodyInfo); return (result != 0); } @@ -431,7 +401,8 @@ bool b3RobotSimulatorClientAPI::getBasePositionAndOrientation(int bodyUniqueId, const int status_type = b3GetStatusType(status_handle); const double* actualStateQ; - if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED) { + if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED) + { return false; } @@ -466,14 +437,72 @@ bool b3RobotSimulatorClientAPI::resetBasePositionAndOrientation(int bodyUniqueId b3CreatePoseCommandSetBasePosition(commandHandle, basePosition[0], basePosition[1], basePosition[2]); b3CreatePoseCommandSetBaseOrientation(commandHandle, baseOrientation[0], baseOrientation[1], - baseOrientation[2], baseOrientation[3]); + baseOrientation[2], baseOrientation[3]); b3SharedMemoryStatusHandle statusHandle; statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); - + return true; } +bool b3RobotSimulatorClientAPI::getBaseVelocity(int bodyUniqueId, b3Vector3& baseLinearVelocity, b3Vector3& baseAngularVelocity) const +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return false; + } + + b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(m_data->m_physicsClientHandle, bodyUniqueId); + b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); + + const int status_type = b3GetStatusType(statusHandle); + const double* actualStateQdot; + + if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED) + { + return false; + } + + b3GetStatusActualState(statusHandle, 0 /* body_unique_id */, + 0 /* num_degree_of_freedom_q */, 0 /* num_degree_of_freedom_u */, + 0 /*root_local_inertial_frame*/, 0, + &actualStateQdot, 0 /* joint_reaction_forces */); + + baseLinearVelocity[0] = actualStateQdot[0]; + baseLinearVelocity[1] = actualStateQdot[1]; + baseLinearVelocity[2] = actualStateQdot[2]; + + baseAngularVelocity[0] = actualStateQdot[3]; + baseAngularVelocity[1] = actualStateQdot[4]; + baseAngularVelocity[2] = actualStateQdot[5]; + return true; +} + +bool b3RobotSimulatorClientAPI::resetBaseVelocity(int bodyUniqueId, const b3Vector3& linearVelocity, const b3Vector3& angularVelocity) const +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return false; + } + + b3SharedMemoryCommandHandle commandHandle; + b3SharedMemoryStatusHandle statusHandle; + + commandHandle = b3CreatePoseCommandInit(m_data->m_physicsClientHandle, bodyUniqueId); + + b3Vector3DoubleData linVelDouble; + linearVelocity.serializeDouble(linVelDouble); + b3CreatePoseCommandSetBaseLinearVelocity(commandHandle, linVelDouble.m_floats); + + b3Vector3DoubleData angVelDouble; + angularVelocity.serializeDouble(angVelDouble); + b3CreatePoseCommandSetBaseAngularVelocity(commandHandle, angVelDouble.m_floats); + + statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); + return true; +} void b3RobotSimulatorClientAPI::setRealTimeSimulation(bool enableRealTimeSimulation) { @@ -500,10 +529,9 @@ int b3RobotSimulatorClientAPI::getNumJoints(int bodyUniqueId) const b3Warning("Not connected"); return false; } - return b3GetNumJoints(m_data->m_physicsClientHandle,bodyUniqueId); + return b3GetNumJoints(m_data->m_physicsClientHandle, bodyUniqueId); } - bool b3RobotSimulatorClientAPI::getJointInfo(int bodyUniqueId, int jointIndex, b3JointInfo* jointInfo) { if (!isConnected()) @@ -511,7 +539,7 @@ bool b3RobotSimulatorClientAPI::getJointInfo(int bodyUniqueId, int jointIndex, b b3Warning("Not connected"); return false; } - return (b3GetJointInfo(m_data->m_physicsClientHandle,bodyUniqueId, jointIndex,jointInfo)!=0); + return (b3GetJointInfo(m_data->m_physicsClientHandle, bodyUniqueId, jointIndex, jointInfo) != 0); } void b3RobotSimulatorClientAPI::createConstraint(int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, b3JointInfo* jointInfo) @@ -521,55 +549,55 @@ void b3RobotSimulatorClientAPI::createConstraint(int parentBodyIndex, int parent b3Warning("Not connected"); return; } - b3SharedMemoryStatusHandle statusHandle; - b3Assert(b3CanSubmitCommand(m_data->m_physicsClientHandle)); - if (b3CanSubmitCommand(m_data->m_physicsClientHandle)) - { - statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, b3InitCreateUserConstraintCommand(m_data->m_physicsClientHandle, parentBodyIndex, parentJointIndex, childBodyIndex, childJointIndex, jointInfo)); - } + b3SharedMemoryStatusHandle statusHandle; + b3Assert(b3CanSubmitCommand(m_data->m_physicsClientHandle)); + if (b3CanSubmitCommand(m_data->m_physicsClientHandle)) + { + statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, b3InitCreateUserConstraintCommand(m_data->m_physicsClientHandle, parentBodyIndex, parentJointIndex, childBodyIndex, childJointIndex, jointInfo)); + } } -bool b3RobotSimulatorClientAPI::getJointState(int bodyUniqueId, int jointIndex, struct b3JointSensorState *state) +bool b3RobotSimulatorClientAPI::getJointState(int bodyUniqueId, int jointIndex, struct b3JointSensorState* state) { if (!isConnected()) { b3Warning("Not connected"); return false; } - b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(m_data->m_physicsClientHandle,bodyUniqueId); - b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); - int statusType = b3GetStatusType(statusHandle); - if (statusType != CMD_ACTUAL_STATE_UPDATE_COMPLETED) + b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(m_data->m_physicsClientHandle, bodyUniqueId); + b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); + int statusType = b3GetStatusType(statusHandle); + if (statusType == CMD_ACTUAL_STATE_UPDATE_COMPLETED) { - if (b3GetJointState(m_data->m_physicsClientHandle, statusHandle, jointIndex, state)) - { - return true; - } + if (b3GetJointState(m_data->m_physicsClientHandle, statusHandle, jointIndex, state)) + { + return true; + } } - return false; + return false; } bool b3RobotSimulatorClientAPI::resetJointState(int bodyUniqueId, int jointIndex, double targetValue) { - b3SharedMemoryCommandHandle commandHandle; - b3SharedMemoryStatusHandle statusHandle; - int numJoints; + b3SharedMemoryCommandHandle commandHandle; + b3SharedMemoryStatusHandle statusHandle; + int numJoints; - numJoints = b3GetNumJoints(m_data->m_physicsClientHandle, bodyUniqueId); - if ((jointIndex >= numJoints) || (jointIndex < 0)) { - return false; - } + numJoints = b3GetNumJoints(m_data->m_physicsClientHandle, bodyUniqueId); + if ((jointIndex >= numJoints) || (jointIndex < 0)) + { + return false; + } - commandHandle = b3CreatePoseCommandInit(m_data->m_physicsClientHandle, bodyUniqueId); + commandHandle = b3CreatePoseCommandInit(m_data->m_physicsClientHandle, bodyUniqueId); - b3CreatePoseCommandSetJointPosition(m_data->m_physicsClientHandle, commandHandle, jointIndex, - targetValue); + b3CreatePoseCommandSetJointPosition(m_data->m_physicsClientHandle, commandHandle, jointIndex, + targetValue); - statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); + statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); return false; } - void b3RobotSimulatorClientAPI::setJointMotorControl(int bodyUniqueId, int jointIndex, const b3RobotSimulatorJointMotorArgs& args) { if (!isConnected()) @@ -582,39 +610,39 @@ void b3RobotSimulatorClientAPI::setJointMotorControl(int bodyUniqueId, int joint { case CONTROL_MODE_VELOCITY: { - b3SharedMemoryCommandHandle command = b3JointControlCommandInit2( m_data->m_physicsClientHandle, bodyUniqueId, CONTROL_MODE_VELOCITY); + b3SharedMemoryCommandHandle command = b3JointControlCommandInit2(m_data->m_physicsClientHandle, bodyUniqueId, CONTROL_MODE_VELOCITY); b3JointInfo jointInfo; b3GetJointInfo(m_data->m_physicsClientHandle, bodyUniqueId, jointIndex, &jointInfo); int uIndex = jointInfo.m_uIndex; - b3JointControlSetKd(command,uIndex,args.m_kd); - b3JointControlSetDesiredVelocity(command,uIndex,args.m_targetVelocity); - b3JointControlSetMaximumForce(command,uIndex,args.m_maxTorqueValue); + b3JointControlSetKd(command, uIndex, args.m_kd); + b3JointControlSetDesiredVelocity(command, uIndex, args.m_targetVelocity); + b3JointControlSetMaximumForce(command, uIndex, args.m_maxTorqueValue); statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); break; } case CONTROL_MODE_POSITION_VELOCITY_PD: { - b3SharedMemoryCommandHandle command = b3JointControlCommandInit2( m_data->m_physicsClientHandle, bodyUniqueId, CONTROL_MODE_POSITION_VELOCITY_PD); + b3SharedMemoryCommandHandle command = b3JointControlCommandInit2(m_data->m_physicsClientHandle, bodyUniqueId, CONTROL_MODE_POSITION_VELOCITY_PD); b3JointInfo jointInfo; b3GetJointInfo(m_data->m_physicsClientHandle, bodyUniqueId, jointIndex, &jointInfo); int uIndex = jointInfo.m_uIndex; int qIndex = jointInfo.m_qIndex; - b3JointControlSetDesiredPosition(command,qIndex,args.m_targetPosition); - b3JointControlSetKp(command,uIndex,args.m_kp); - b3JointControlSetDesiredVelocity(command,uIndex,args.m_targetVelocity); - b3JointControlSetKd(command,uIndex,args.m_kd); - b3JointControlSetMaximumForce(command,uIndex,args.m_maxTorqueValue); + b3JointControlSetDesiredPosition(command, qIndex, args.m_targetPosition); + b3JointControlSetKp(command, uIndex, args.m_kp); + b3JointControlSetDesiredVelocity(command, uIndex, args.m_targetVelocity); + b3JointControlSetKd(command, uIndex, args.m_kd); + b3JointControlSetMaximumForce(command, uIndex, args.m_maxTorqueValue); statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); break; } case CONTROL_MODE_TORQUE: { - b3SharedMemoryCommandHandle command = b3JointControlCommandInit2( m_data->m_physicsClientHandle, bodyUniqueId, CONTROL_MODE_TORQUE); + b3SharedMemoryCommandHandle command = b3JointControlCommandInit2(m_data->m_physicsClientHandle, bodyUniqueId, CONTROL_MODE_TORQUE); b3JointInfo jointInfo; b3GetJointInfo(m_data->m_physicsClientHandle, bodyUniqueId, jointIndex, &jointInfo); int uIndex = jointInfo.m_uIndex; - b3JointControlSetDesiredForceTorque(command,uIndex,args.m_maxTorqueValue); + b3JointControlSetDesiredForceTorque(command, uIndex, args.m_maxTorqueValue); statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); break; } @@ -625,7 +653,6 @@ void b3RobotSimulatorClientAPI::setJointMotorControl(int bodyUniqueId, int joint } } - void b3RobotSimulatorClientAPI::setNumSolverIterations(int numIterations) { if (!isConnected()) @@ -633,12 +660,11 @@ void b3RobotSimulatorClientAPI::setNumSolverIterations(int numIterations) b3Warning("Not connected"); return; } - b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle); - b3SharedMemoryStatusHandle statusHandle; - b3PhysicsParamSetNumSolverIterations(command, numIterations); - statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); - b3Assert(b3GetStatusType(statusHandle)==CMD_CLIENT_COMMAND_COMPLETED); - + b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle); + b3SharedMemoryStatusHandle statusHandle; + b3PhysicsParamSetNumSolverIterations(command, numIterations); + statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); + b3Assert(b3GetStatusType(statusHandle) == CMD_CLIENT_COMMAND_COMPLETED); } void b3RobotSimulatorClientAPI::setTimeStep(double timeStepInSeconds) @@ -654,10 +680,8 @@ void b3RobotSimulatorClientAPI::setTimeStep(double timeStepInSeconds) int ret; ret = b3PhysicsParamSetTimeStep(command, timeStepInSeconds); statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); - } - void b3RobotSimulatorClientAPI::setNumSimulationSubSteps(int numSubSteps) { if (!isConnected()) @@ -665,14 +689,13 @@ void b3RobotSimulatorClientAPI::setNumSimulationSubSteps(int numSubSteps) b3Warning("Not connected"); return; } - b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle); - b3SharedMemoryStatusHandle statusHandle; - b3PhysicsParamSetNumSubSteps(command, numSubSteps); - statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); - b3Assert(b3GetStatusType(statusHandle)==CMD_CLIENT_COMMAND_COMPLETED); + b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle); + b3SharedMemoryStatusHandle statusHandle; + b3PhysicsParamSetNumSubSteps(command, numSubSteps); + statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); + b3Assert(b3GetStatusType(statusHandle) == CMD_CLIENT_COMMAND_COMPLETED); } - bool b3RobotSimulatorClientAPI::calculateInverseKinematics(const struct b3RobotSimulatorInverseKinematicArgs& args, struct b3RobotSimulatorInverseKinematicsResults& results) { if (!isConnected()) @@ -680,55 +703,52 @@ bool b3RobotSimulatorClientAPI::calculateInverseKinematics(const struct b3RobotS b3Warning("Not connected"); return false; } - b3Assert(args.m_endEffectorLinkIndex>=0); - b3Assert(args.m_bodyUniqueId>=0); - + b3Assert(args.m_endEffectorLinkIndex >= 0); + b3Assert(args.m_bodyUniqueId >= 0); - b3SharedMemoryCommandHandle command = b3CalculateInverseKinematicsCommandInit(m_data->m_physicsClientHandle,args.m_bodyUniqueId); + b3SharedMemoryCommandHandle command = b3CalculateInverseKinematicsCommandInit(m_data->m_physicsClientHandle, args.m_bodyUniqueId); if ((args.m_flags & B3_HAS_IK_TARGET_ORIENTATION) && (args.m_flags & B3_HAS_NULL_SPACE_VELOCITY)) - { - b3CalculateInverseKinematicsPosOrnWithNullSpaceVel(command, args.m_numDegreeOfFreedom, args.m_endEffectorLinkIndex, args.m_endEffectorTargetPosition, args.m_endEffectorTargetOrientation, &args.m_lowerLimits[0], &args.m_upperLimits[0], &args.m_jointRanges[0], &args.m_restPoses[0]); - } else if (args.m_flags & B3_HAS_IK_TARGET_ORIENTATION) { - b3CalculateInverseKinematicsAddTargetPositionWithOrientation(command,args.m_endEffectorLinkIndex,args.m_endEffectorTargetPosition,args.m_endEffectorTargetOrientation); - } else if (args.m_flags & B3_HAS_NULL_SPACE_VELOCITY) - { - b3CalculateInverseKinematicsPosWithNullSpaceVel(command, args.m_numDegreeOfFreedom, args.m_endEffectorLinkIndex, args.m_endEffectorTargetPosition, &args.m_lowerLimits[0], &args.m_upperLimits[0], &args.m_jointRanges[0], &args.m_restPoses[0]); - } else - { - b3CalculateInverseKinematicsAddTargetPurePosition(command,args.m_endEffectorLinkIndex,args.m_endEffectorTargetPosition); + b3CalculateInverseKinematicsPosOrnWithNullSpaceVel(command, args.m_numDegreeOfFreedom, args.m_endEffectorLinkIndex, args.m_endEffectorTargetPosition, args.m_endEffectorTargetOrientation, &args.m_lowerLimits[0], &args.m_upperLimits[0], &args.m_jointRanges[0], &args.m_restPoses[0]); + } + else if (args.m_flags & B3_HAS_IK_TARGET_ORIENTATION) + { + b3CalculateInverseKinematicsAddTargetPositionWithOrientation(command, args.m_endEffectorLinkIndex, args.m_endEffectorTargetPosition, args.m_endEffectorTargetOrientation); + } + else if (args.m_flags & B3_HAS_NULL_SPACE_VELOCITY) + { + b3CalculateInverseKinematicsPosWithNullSpaceVel(command, args.m_numDegreeOfFreedom, args.m_endEffectorLinkIndex, args.m_endEffectorTargetPosition, &args.m_lowerLimits[0], &args.m_upperLimits[0], &args.m_jointRanges[0], &args.m_restPoses[0]); + } + else + { + b3CalculateInverseKinematicsAddTargetPurePosition(command, args.m_endEffectorLinkIndex, args.m_endEffectorTargetPosition); } - - if (args.m_flags & B3_HAS_JOINT_DAMPING) - { - b3CalculateInverseKinematicsSetJointDamping(command, args.m_numDegreeOfFreedom, &args.m_jointDamping[0]); - } - b3SharedMemoryStatusHandle statusHandle; + if (args.m_flags & B3_HAS_JOINT_DAMPING) + { + b3CalculateInverseKinematicsSetJointDamping(command, args.m_numDegreeOfFreedom, &args.m_jointDamping[0]); + } + + b3SharedMemoryStatusHandle statusHandle; statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); - + int numPos = 0; - int numPos=0; - bool result = b3GetStatusInverseKinematicsJointPositions(statusHandle, - &results.m_bodyUniqueId, - &numPos, - 0)!=0; - if (result && numPos) - { - results.m_calculatedJointPositions.resize(numPos); - result = b3GetStatusInverseKinematicsJointPositions(statusHandle, - &results.m_bodyUniqueId, - &numPos, - &results.m_calculatedJointPositions[0])!=0; - - } + &results.m_bodyUniqueId, + &numPos, + 0) != 0; + if (result && numPos) + { + results.m_calculatedJointPositions.resize(numPos); + result = b3GetStatusInverseKinematicsJointPositions(statusHandle, + &results.m_bodyUniqueId, + &numPos, + &results.m_calculatedJointPositions[0]) != 0; + } return result; } - - bool b3RobotSimulatorClientAPI::getBodyJacobian(int bodyUniqueId, int linkIndex, const double* localPosition, const double* jointPositions, const double* jointVelocities, const double* jointAccelerations, double* linearJacobian, double* angularJacobian) { if (!isConnected()) @@ -736,12 +756,12 @@ bool b3RobotSimulatorClientAPI::getBodyJacobian(int bodyUniqueId, int linkIndex, b3Warning("Not connected"); return false; } - b3SharedMemoryCommandHandle command = b3CalculateJacobianCommandInit(m_data->m_physicsClientHandle, bodyUniqueId, linkIndex, localPosition, jointPositions, jointVelocities, jointAccelerations); - b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); - - if (b3GetStatusType(statusHandle) == CMD_CALCULATED_JACOBIAN_COMPLETED) - { - b3GetStatusJacobian(statusHandle, linearJacobian, angularJacobian); + b3SharedMemoryCommandHandle command = b3CalculateJacobianCommandInit(m_data->m_physicsClientHandle, bodyUniqueId, linkIndex, localPosition, jointPositions, jointVelocities, jointAccelerations); + b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); + + if (b3GetStatusType(statusHandle) == CMD_CALCULATED_JACOBIAN_COMPLETED) + { + b3GetStatusJacobian(statusHandle, linearJacobian, angularJacobian); return true; } return false; @@ -754,14 +774,14 @@ bool b3RobotSimulatorClientAPI::getLinkState(int bodyUniqueId, int linkIndex, b3 b3Warning("Not connected"); return false; } - b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(m_data->m_physicsClientHandle,bodyUniqueId); - b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); - - if (b3GetStatusType(statusHandle) == CMD_ACTUAL_STATE_UPDATE_COMPLETED) - { - b3GetLinkState(m_data->m_physicsClientHandle, statusHandle, linkIndex, linkState); + b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(m_data->m_physicsClientHandle, bodyUniqueId); + b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); + + if (b3GetStatusType(statusHandle) == CMD_ACTUAL_STATE_UPDATE_COMPLETED) + { + b3GetLinkState(m_data->m_physicsClientHandle, statusHandle, linkIndex, linkState); return true; - } + } return false; } @@ -787,9 +807,9 @@ void b3RobotSimulatorClientAPI::getVREvents(b3VREventsData* vrEventsData) return; } - b3SharedMemoryCommandHandle commandHandle = b3RequestVREventsCommandInit(m_data->m_physicsClientHandle); + b3SharedMemoryCommandHandle commandHandle = b3RequestVREventsCommandInit(m_data->m_physicsClientHandle); b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); - b3GetVREventsData(m_data->m_physicsClientHandle, vrEventsData); + b3GetVREventsData(m_data->m_physicsClientHandle, vrEventsData); } void b3RobotSimulatorClientAPI::getKeyboardEvents(b3KeyboardEventsData* keyboardEventsData) @@ -802,32 +822,35 @@ void b3RobotSimulatorClientAPI::getKeyboardEvents(b3KeyboardEventsData* keyboard return; } - b3SharedMemoryCommandHandle commandHandle = b3RequestKeyboardEventsCommandInit(m_data->m_physicsClientHandle); + b3SharedMemoryCommandHandle commandHandle = b3RequestKeyboardEventsCommandInit(m_data->m_physicsClientHandle); b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); - b3GetKeyboardEventsData(m_data->m_physicsClientHandle,keyboardEventsData); - + b3GetKeyboardEventsData(m_data->m_physicsClientHandle, keyboardEventsData); } - -int b3RobotSimulatorClientAPI::startStateLogging(b3StateLoggingType loggingType, const std::string& fileName, const b3AlignedObjectArray& objectUniqueIds) +int b3RobotSimulatorClientAPI::startStateLogging(b3StateLoggingType loggingType, const std::string& fileName, const b3AlignedObjectArray& objectUniqueIds, int maxLogDof) { int loggingUniqueId = -1; b3SharedMemoryCommandHandle commandHandle; commandHandle = b3StateLoggingCommandInit(m_data->m_physicsClientHandle); - - b3StateLoggingStart(commandHandle,loggingType,fileName.c_str()); - for ( int i=0;i 0) + { + b3StateLoggingSetMaxLogDof(commandHandle, maxLogDof); } b3SharedMemoryStatusHandle statusHandle; int statusType; statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); statusType = b3GetStatusType(statusHandle); - if (statusType==CMD_STATE_LOGGING_START_COMPLETED) + if (statusType == CMD_STATE_LOGGING_START_COMPLETED) { loggingUniqueId = b3GetStatusLoggingUniqueId(statusHandle); } @@ -843,3 +866,17 @@ void b3RobotSimulatorClientAPI::stopStateLogging(int stateLoggerUniqueId) statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); } +void b3RobotSimulatorClientAPI::resetDebugVisualizerCamera(double cameraDistance, double cameraPitch, double cameraYaw, const b3Vector3& targetPos) +{ + b3SharedMemoryCommandHandle commandHandle = b3InitConfigureOpenGLVisualizer(m_data->m_physicsClientHandle); + if (commandHandle) + { + if ((cameraDistance >= 0)) + { + b3Vector3FloatData camTargetPos; + targetPos.serializeFloat(camTargetPos); + b3ConfigureOpenGLVisualizerSetViewMatrix(commandHandle, cameraDistance, cameraPitch, cameraYaw, camTargetPos.m_floats); + } + b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); + } +} diff --git a/examples/RobotSimulator/b3RobotSimulatorClientAPI.h b/examples/RobotSimulator/b3RobotSimulatorClientAPI.h index f1ca5fe98..bac77009a 100644 --- a/examples/RobotSimulator/b3RobotSimulatorClientAPI.h +++ b/examples/RobotSimulator/b3RobotSimulatorClientAPI.h @@ -9,23 +9,18 @@ #include - - - - struct b3RobotSimulatorLoadUrdfFileArgs { b3Vector3 m_startPosition; b3Quaternion m_startOrientation; bool m_forceOverrideFixedBase; - bool m_useMultiBody; + bool m_useMultiBody; b3RobotSimulatorLoadUrdfFileArgs() - : - m_startPosition(b3MakeVector3(0,0,0)), - m_startOrientation(b3Quaternion(0,0,0,1)), - m_forceOverrideFixedBase(false), - m_useMultiBody(true) + : m_startPosition(b3MakeVector3(0, 0, 0)), + m_startOrientation(b3Quaternion(0, 0, 0, 1)), + m_forceOverrideFixedBase(false), + m_useMultiBody(true) { } }; @@ -33,17 +28,15 @@ struct b3RobotSimulatorLoadUrdfFileArgs struct b3RobotSimulatorLoadSdfFileArgs { bool m_forceOverrideFixedBase; - bool m_useMultiBody; + bool m_useMultiBody; b3RobotSimulatorLoadSdfFileArgs() - : - m_forceOverrideFixedBase(false), - m_useMultiBody(true) + : m_forceOverrideFixedBase(false), + m_useMultiBody(true) { } }; - struct b3RobotSimulatorLoadFileResults { b3AlignedObjectArray m_uniqueObjectIds; @@ -55,7 +48,7 @@ struct b3RobotSimulatorLoadFileResults struct b3RobotSimulatorJointMotorArgs { int m_controlMode; - + double m_targetPosition; double m_kp; @@ -65,52 +58,52 @@ struct b3RobotSimulatorJointMotorArgs double m_maxTorqueValue; b3RobotSimulatorJointMotorArgs(int controlMode) - :m_controlMode(controlMode), - m_targetPosition(0), - m_kp(0.1), - m_targetVelocity(0), - m_kd(0.9), - m_maxTorqueValue(1000) + : m_controlMode(controlMode), + m_targetPosition(0), + m_kp(0.1), + m_targetVelocity(0), + m_kd(0.9), + m_maxTorqueValue(1000) { } }; enum b3RobotSimulatorInverseKinematicsFlags { - B3_HAS_IK_TARGET_ORIENTATION=1, - B3_HAS_NULL_SPACE_VELOCITY=2, - B3_HAS_JOINT_DAMPING=4, + B3_HAS_IK_TARGET_ORIENTATION = 1, + B3_HAS_NULL_SPACE_VELOCITY = 2, + B3_HAS_JOINT_DAMPING = 4, }; struct b3RobotSimulatorInverseKinematicArgs { int m_bodyUniqueId; -// double* m_currentJointPositions; -// int m_numPositions; + // double* m_currentJointPositions; + // int m_numPositions; double m_endEffectorTargetPosition[3]; double m_endEffectorTargetOrientation[4]; - int m_endEffectorLinkIndex; + int m_endEffectorLinkIndex; int m_flags; - int m_numDegreeOfFreedom; - b3AlignedObjectArray m_lowerLimits; - b3AlignedObjectArray m_upperLimits; - b3AlignedObjectArray m_jointRanges; - b3AlignedObjectArray m_restPoses; - b3AlignedObjectArray m_jointDamping; + int m_numDegreeOfFreedom; + b3AlignedObjectArray m_lowerLimits; + b3AlignedObjectArray m_upperLimits; + b3AlignedObjectArray m_jointRanges; + b3AlignedObjectArray m_restPoses; + b3AlignedObjectArray m_jointDamping; b3RobotSimulatorInverseKinematicArgs() - :m_bodyUniqueId(-1), - m_endEffectorLinkIndex(-1), - m_flags(0) + : m_bodyUniqueId(-1), + m_endEffectorLinkIndex(-1), + m_flags(0) { - m_endEffectorTargetPosition[0]=0; - m_endEffectorTargetPosition[1]=0; - m_endEffectorTargetPosition[2]=0; + m_endEffectorTargetPosition[0] = 0; + m_endEffectorTargetPosition[1] = 0; + m_endEffectorTargetPosition[2] = 0; - m_endEffectorTargetOrientation[0]=0; - m_endEffectorTargetOrientation[1]=0; - m_endEffectorTargetOrientation[2]=0; - m_endEffectorTargetOrientation[3]=1; + m_endEffectorTargetOrientation[0] = 0; + m_endEffectorTargetOrientation[1] = 0; + m_endEffectorTargetOrientation[2] = 0; + m_endEffectorTargetOrientation[3] = 1; } }; @@ -120,24 +113,25 @@ struct b3RobotSimulatorInverseKinematicsResults b3AlignedObjectArray m_calculatedJointPositions; }; - ///The b3RobotSimulatorClientAPI is pretty much the C++ version of pybullet ///as documented in the pybullet Quickstart Guide ///https://docs.google.com/document/d/10sXEhzFRSnvFcl3XxNGhnD4N2SedqwdAvK3dsihxVUA class b3RobotSimulatorClientAPI { struct b3RobotSimulatorClientAPI_InternalData* m_data; - + public: b3RobotSimulatorClientAPI(); virtual ~b3RobotSimulatorClientAPI(); - bool connect(int mode, const std::string& hostName="localhost", int portOrKey=-1); - + bool connect(int mode, const std::string& hostName = "localhost", int portOrKey = -1); + void disconnect(); bool isConnected() const; + void setTimeOut(double timeOutInSec); + void syncBodies(); void resetSimulation(); @@ -145,8 +139,8 @@ public: b3Quaternion getQuaternionFromEuler(const b3Vector3& rollPitchYaw); b3Vector3 getEulerFromQuaternion(const b3Quaternion& quat); - int loadURDF(const std::string& fileName, const struct b3RobotSimulatorLoadUrdfFileArgs& args=b3RobotSimulatorLoadUrdfFileArgs()); - bool loadSDF(const std::string& fileName, b3RobotSimulatorLoadFileResults& results, const struct b3RobotSimulatorLoadSdfFileArgs& args=b3RobotSimulatorLoadSdfFileArgs()); + int loadURDF(const std::string& fileName, const struct b3RobotSimulatorLoadUrdfFileArgs& args = b3RobotSimulatorLoadUrdfFileArgs()); + bool loadSDF(const std::string& fileName, b3RobotSimulatorLoadFileResults& results, const struct b3RobotSimulatorLoadSdfFileArgs& args = b3RobotSimulatorLoadSdfFileArgs()); bool loadMJCF(const std::string& fileName, b3RobotSimulatorLoadFileResults& results); bool loadBullet(const std::string& fileName, b3RobotSimulatorLoadFileResults& results); @@ -155,14 +149,17 @@ public: bool getBasePositionAndOrientation(int bodyUniqueId, b3Vector3& basePosition, b3Quaternion& baseOrientation) const; bool resetBasePositionAndOrientation(int bodyUniqueId, b3Vector3& basePosition, b3Quaternion& baseOrientation); + bool getBaseVelocity(int bodyUniqueId, b3Vector3& baseLinearVelocity, b3Vector3& baseAngularVelocity) const; + bool resetBaseVelocity(int bodyUniqueId, const b3Vector3& linearVelocity, const b3Vector3& angularVelocity) const; + int getNumJoints(int bodyUniqueId) const; bool getJointInfo(int bodyUniqueId, int jointIndex, b3JointInfo* jointInfo); - - void createConstraint(int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, b3JointInfo* jointInfo); - bool getJointState(int bodyUniqueId, int jointIndex, struct b3JointSensorState *state); - + void createConstraint(int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, b3JointInfo* jointInfo); + + bool getJointState(int bodyUniqueId, int jointIndex, struct b3JointSensorState* state); + bool resetJointState(int bodyUniqueId, int jointIndex, double targetValue); void setJointMotorControl(int bodyUniqueId, int jointIndex, const struct b3RobotSimulatorJointMotorArgs& args); @@ -170,29 +167,29 @@ public: void stepSimulation(); bool canSubmitCommand() const; - + void setRealTimeSimulation(bool enableRealTimeSimulation); void setGravity(const b3Vector3& gravityAcceleration); - + void setTimeStep(double timeStepInSeconds); - void setNumSimulationSubSteps(int numSubSteps); + void setNumSimulationSubSteps(int numSubSteps); void setNumSolverIterations(int numIterations); bool calculateInverseKinematics(const struct b3RobotSimulatorInverseKinematicArgs& args, struct b3RobotSimulatorInverseKinematicsResults& results); - bool getBodyJacobian(int bodyUniqueId, int linkIndex, const double* localPosition, const double* jointPositions, const double* jointVelocities, const double* jointAccelerations, double* linearJacobian, double* angularJacobian); - + bool getBodyJacobian(int bodyUniqueId, int linkIndex, const double* localPosition, const double* jointPositions, const double* jointVelocities, const double* jointAccelerations, double* linearJacobian, double* angularJacobian); + bool getLinkState(int bodyUniqueId, int linkIndex, b3LinkState* linkState); void configureDebugVisualizer(enum b3ConfigureDebugVisualizerEnum flag, int enable); + void resetDebugVisualizerCamera(double cameraDistance, double cameraPitch, double cameraYaw, const b3Vector3& targetPos); - int startStateLogging(b3StateLoggingType loggingType, const std::string& fileName, const b3AlignedObjectArray& objectUniqueIds); + int startStateLogging(b3StateLoggingType loggingType, const std::string& fileName, const b3AlignedObjectArray& objectUniqueIds=b3AlignedObjectArray(), int maxLogDof = -1); void stopStateLogging(int stateLoggerUniqueId); void getVREvents(b3VREventsData* vrEventsData); void getKeyboardEvents(b3KeyboardEventsData* keyboardEventsData); - }; -#endif //B3_ROBOT_SIMULATOR_CLIENT_API_H +#endif //B3_ROBOT_SIMULATOR_CLIENT_API_H diff --git a/examples/SharedMemory/CMakeLists.txt b/examples/SharedMemory/CMakeLists.txt index b282c09cb..d2754f8c5 100644 --- a/examples/SharedMemory/CMakeLists.txt +++ b/examples/SharedMemory/CMakeLists.txt @@ -38,6 +38,8 @@ SET(SharedMemory_SRCS PhysicsServerCommandProcessor.h TinyRendererVisualShapeConverter.cpp TinyRendererVisualShapeConverter.h + SharedMemoryCommands.h + SharedMemoryPublic.h ../TinyRenderer/geometry.cpp ../TinyRenderer/model.cpp ../TinyRenderer/tgaimage.cpp diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 0ec231e4a..5ac6ea74a 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -914,7 +914,7 @@ b3SharedMemoryStatusHandle b3ProcessServerStatus(b3PhysicsClientHandle physClien int b3GetStatusType(b3SharedMemoryStatusHandle statusHandle) { const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle; - b3Assert(status); + //b3Assert(status); if (status) { return status->m_type; @@ -1063,7 +1063,7 @@ b3SharedMemoryStatusHandle b3SubmitClientCommandAndWaitStatus(b3PhysicsClientHan b3SubmitClientCommand(physClient, commandHandle); - while ((statusHandle == 0) && (clock.getTimeInSeconds()-startTime < timeOutInSeconds)) + while (cl->isConnected() && (statusHandle == 0) && (clock.getTimeInSeconds()-startTime < timeOutInSeconds)) { statusHandle = b3ProcessServerStatus(physClient); } @@ -2585,6 +2585,20 @@ int b3StateLoggingAddLoggingObjectUniqueId(b3SharedMemoryCommandHandle commandHa } return 0; } + +int b3StateLoggingSetMaxLogDof(b3SharedMemoryCommandHandle commandHandle, int maxLogDof) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_STATE_LOGGING); + if (command->m_type == CMD_STATE_LOGGING) + { + command->m_updateFlags |= STATE_LOGGING_MAX_LOG_DOF; + command->m_stateLoggingArguments.m_maxLogDof = maxLogDof; + } + return 0; +} + int b3StateLoggingStop(b3SharedMemoryCommandHandle commandHandle, int loggingUid) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index e71203700..01eb55cc1 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -356,9 +356,13 @@ void b3GetKeyboardEventsData(b3PhysicsClientHandle physClient, struct b3Keyboard b3SharedMemoryCommandHandle b3StateLoggingCommandInit(b3PhysicsClientHandle physClient); int b3StateLoggingStart(b3SharedMemoryCommandHandle commandHandle, int loggingType, const char* fileName); int b3StateLoggingAddLoggingObjectUniqueId(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId); +int b3StateLoggingSetMaxLogDof(b3SharedMemoryCommandHandle commandHandle, int maxLogDof); + int b3GetStatusLoggingUniqueId(b3SharedMemoryStatusHandle statusHandle); int b3StateLoggingStop(b3SharedMemoryCommandHandle commandHandle, int loggingUniqueId); + + void b3SetTimeOut(b3PhysicsClientHandle physClient, double timeOutInSeconds); double b3GetTimeOut(b3PhysicsClientHandle physClient); diff --git a/examples/SharedMemory/PhysicsDirect.cpp b/examples/SharedMemory/PhysicsDirect.cpp index 0989a4962..5ca0f35a2 100644 --- a/examples/SharedMemory/PhysicsDirect.cpp +++ b/examples/SharedMemory/PhysicsDirect.cpp @@ -1067,4 +1067,4 @@ void PhysicsDirect::setTimeOut(double timeOutInSeconds) double PhysicsDirect::getTimeOut() const { return m_data->m_timeOutInSeconds; -} \ No newline at end of file +} diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 94d226be4..4061cea95 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -48,7 +48,7 @@ bool gResetSimulation = 0; int gVRTrackingObjectUniqueId = -1; btTransform gVRTrackingObjectTr = btTransform::getIdentity(); -int gMaxNumCmdPer1ms = 10;//experiment: add some delay to avoid threads starving other threads +int gMaxNumCmdPer1ms = -1;//experiment: add some delay to avoid threads starving other threads int gCreateObjectSimVR = -1; int gEnableKukaControl = 0; btVector3 gVRTeleportPos1(0,0,0); @@ -438,6 +438,31 @@ struct InternalStateLogger }; +struct VideoMP4Loggger : public InternalStateLogger +{ + + struct GUIHelperInterface* m_guiHelper; + std::string m_fileName; + VideoMP4Loggger(int loggerUid,const char* fileName,GUIHelperInterface* guiHelper) + :m_guiHelper(guiHelper) + { + m_fileName = fileName; + m_loggingUniqueId = loggerUid; + m_loggingType = STATE_LOGGING_VIDEO_MP4; + m_guiHelper->dumpFramesToVideo(fileName); + } + + virtual void stop() + { + m_guiHelper->dumpFramesToVideo(0); + } + virtual void logState(btScalar timeStamp) + { + //dumping video frames happens in another thread + //we could add some overlay of timestamp here, if needed/wanted + } +}; + struct MinitaurStateLogger : public InternalStateLogger { int m_loggingTimeStamp; @@ -454,6 +479,7 @@ struct MinitaurStateLogger : public InternalStateLogger m_logFileHandle(0), m_minitaurMultiBody(minitaurMultiBody) { + m_loggingUniqueId = loggingUniqueId; m_loggingType = STATE_LOGGING_MINITAUR; m_motorIdList.resize(motorIdList.size()); for (int m=0;mgetBaseWorldTransform().getRotation(); btMatrix3x3 mat(orn); @@ -568,13 +595,16 @@ struct GenericRobotStateLogger : public InternalStateLogger btMultiBodyDynamicsWorld* m_dynamicsWorld; btAlignedObjectArray m_bodyIdList; bool m_filterObjectUniqueId; - - GenericRobotStateLogger(int loggingUniqueId, const std::string& fileName, btMultiBodyDynamicsWorld* dynamicsWorld) + int m_maxLogDof; + + GenericRobotStateLogger(int loggingUniqueId, const std::string& fileName, btMultiBodyDynamicsWorld* dynamicsWorld, int maxLogDof) :m_loggingTimeStamp(0), m_logFileHandle(0), m_dynamicsWorld(dynamicsWorld), - m_filterObjectUniqueId(false) + m_filterObjectUniqueId(false), + m_maxLogDof(maxLogDof) { + m_loggingUniqueId = loggingUniqueId; m_loggingType = STATE_LOGGING_GENERIC_ROBOT; btAlignedObjectArray structNames; @@ -595,32 +625,24 @@ struct GenericRobotStateLogger : public InternalStateLogger structNames.push_back("omegaY"); structNames.push_back("omegaZ"); structNames.push_back("qNum"); - structNames.push_back("q0"); - structNames.push_back("q1"); - structNames.push_back("q2"); - structNames.push_back("q3"); - structNames.push_back("q4"); - structNames.push_back("q5"); - structNames.push_back("q6"); - structNames.push_back("q7"); - structNames.push_back("q8"); - structNames.push_back("q9"); - structNames.push_back("q10"); - structNames.push_back("q11"); - structNames.push_back("u0"); - structNames.push_back("u1"); - structNames.push_back("u2"); - structNames.push_back("u3"); - structNames.push_back("u4"); - structNames.push_back("u5"); - structNames.push_back("u6"); - structNames.push_back("u7"); - structNames.push_back("u8"); - structNames.push_back("u9"); - structNames.push_back("u10"); - structNames.push_back("u11"); + + m_structTypes = "IfIfffffffffffffI"; + + for (int i=0;im_dynamicsWorld->setGravity(btVector3(0, 0, 0)); m_data->m_dynamicsWorld->getSolverInfo().m_erp2 = 0.08; - + + m_data->m_dynamicsWorld->getSolverInfo().m_frictionERP = 0.2;//need to check if there are artifacts with frictionERP m_data->m_dynamicsWorld->getSolverInfo().m_linearSlop = 0.00001; m_data->m_dynamicsWorld->getSolverInfo().m_numIterations = 50; m_data->m_dynamicsWorld->getSolverInfo().m_leastSquaresResidualThreshold = 1e-7; @@ -1475,7 +1499,7 @@ bool PhysicsServerCommandProcessor::loadMjcf(const char* fileName, char* bufferS m_data->m_sdfRecentLoadedBodies.clear(); - BulletMJCFImporter u2b(m_data->m_guiHelper); //, &m_data->m_visualConverter + BulletMJCFImporter u2b(m_data->m_guiHelper, &m_data->m_visualConverter); bool useFixedBase = false; MyMJCFLogger2 logger; @@ -1800,6 +1824,17 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm if (clientCmd.m_updateFlags & STATE_LOGGING_START_LOG) { + if (clientCmd.m_stateLoggingArguments.m_logType == STATE_LOGGING_VIDEO_MP4) + { + //if (clientCmd.m_stateLoggingArguments.m_fileName) + { + int loggerUid = m_data->m_stateLoggersUniqueId++; + VideoMP4Loggger* logger = new VideoMP4Loggger(loggerUid,clientCmd.m_stateLoggingArguments.m_fileName,this->m_data->m_guiHelper); + m_data->m_stateLoggers.push_back(logger); + serverStatusOut.m_type = CMD_STATE_LOGGING_START_COMPLETED; + serverStatusOut.m_stateLoggingResultArgs.m_loggingUniqueId = loggerUid; + } + } if (clientCmd.m_stateLoggingArguments.m_logType == STATE_LOGGING_MINITAUR) { @@ -1860,14 +1895,20 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm std::string fileName = clientCmd.m_stateLoggingArguments.m_fileName; int loggerUid = m_data->m_stateLoggersUniqueId++; - GenericRobotStateLogger* logger = new GenericRobotStateLogger(loggerUid,fileName,m_data->m_dynamicsWorld); + int maxLogDof = 12; + if ((clientCmd.m_updateFlags & STATE_LOGGING_MAX_LOG_DOF)) + { + maxLogDof = clientCmd.m_stateLoggingArguments.m_maxLogDof; + } + GenericRobotStateLogger* logger = new GenericRobotStateLogger(loggerUid,fileName,m_data->m_dynamicsWorld,maxLogDof); if ((clientCmd.m_updateFlags & STATE_LOGGING_FILTER_OBJECT_UNIQUE_ID) && (clientCmd.m_stateLoggingArguments.m_numBodyUniqueIds>0)) { logger->m_filterObjectUniqueId = true; for (int i = 0; i < clientCmd.m_stateLoggingArguments.m_numBodyUniqueIds; ++i) { - logger->m_bodyIdList.push_back(clientCmd.m_stateLoggingArguments.m_bodyUniqueIds[i]); + int objectUniqueId = clientCmd.m_stateLoggingArguments.m_bodyUniqueIds[i]; + logger->m_bodyIdList.push_back(objectUniqueId); } } @@ -4571,20 +4612,19 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm int remain = totalNumVisualShapes - clientCmd.m_requestVisualShapeDataArguments.m_startingVisualShapeIndex; int shapeIndex = clientCmd.m_requestVisualShapeDataArguments.m_startingVisualShapeIndex; - m_data->m_visualConverter.getVisualShapesData(clientCmd.m_requestVisualShapeDataArguments.m_bodyUniqueId, + int success = m_data->m_visualConverter.getVisualShapesData(clientCmd.m_requestVisualShapeDataArguments.m_bodyUniqueId, shapeIndex, visualShapeStoragePtr); - - - //m_visualConverter - serverCmd.m_sendVisualShapeArgs.m_numRemainingVisualShapes = remain-1; - serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied = 1; - serverCmd.m_sendVisualShapeArgs.m_startingVisualShapeIndex = clientCmd.m_requestVisualShapeDataArguments.m_startingVisualShapeIndex; - serverCmd.m_sendVisualShapeArgs.m_bodyUniqueId = clientCmd.m_requestVisualShapeDataArguments.m_bodyUniqueId; - serverCmd.m_numDataStreamBytes = sizeof(b3VisualShapeData)*serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied; - serverCmd.m_type =CMD_VISUAL_SHAPE_INFO_COMPLETED; - hasStatus = true; - break; + if (success) { + serverCmd.m_sendVisualShapeArgs.m_numRemainingVisualShapes = remain-1; + serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied = 1; + serverCmd.m_sendVisualShapeArgs.m_startingVisualShapeIndex = clientCmd.m_requestVisualShapeDataArguments.m_startingVisualShapeIndex; + serverCmd.m_sendVisualShapeArgs.m_bodyUniqueId = clientCmd.m_requestVisualShapeDataArguments.m_bodyUniqueId; + serverCmd.m_numDataStreamBytes = sizeof(b3VisualShapeData)*serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied; + serverCmd.m_type = CMD_VISUAL_SHAPE_INFO_COMPLETED; + } + hasStatus = true; + break; } case CMD_UPDATE_VISUAL_SHAPE: { @@ -4939,6 +4979,7 @@ bool PhysicsServerCommandProcessor::pickBody(const btVector3& rayFromWorld, cons if (!(body->isStaticObject() || body->isKinematicObject())) { m_data->m_pickedBody = body; + m_data->m_savedActivationState = body->getActivationState(); m_data->m_pickedBody->setActivationState(DISABLE_DEACTIVATION); //printf("pickPos=%f,%f,%f\n",pickPos.getX(),pickPos.getY(),pickPos.getZ()); btVector3 localPivot = body->getCenterOfMassTransform().inverse() * pickPos; @@ -5031,7 +5072,7 @@ void PhysicsServerCommandProcessor::removePickingConstraint() m_data->m_dynamicsWorld->removeConstraint(m_data->m_pickedConstraint); delete m_data->m_pickedConstraint; m_data->m_pickedConstraint = 0; - m_data->m_pickedBody->forceActivationState(ACTIVE_TAG); + m_data->m_pickedBody->forceActivationState(m_data->m_savedActivationState); m_data->m_pickedBody = 0; } if (m_data->m_pickingMultiBodyPoint2Point) diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index ed15aa1b1..970b209d3 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -179,8 +179,10 @@ enum MultiThreadedGUIHelperCommunicationEnums eGUIUserDebugAddParameter, eGUIUserDebugRemoveItem, eGUIUserDebugRemoveAllItems, + eGUIDumpFramesToVideo, }; + #include //#include "BulletMultiThreaded/PlatformDefinitions.h" @@ -313,12 +315,15 @@ void MotionThreadFunc(void* userPtr,void* lsMemory) b3Clock::usleep(0); } - if (numCmdSinceSleep1ms>gMaxNumCmdPer1ms) + if (gMaxNumCmdPer1ms>0) { - BT_PROFILE("usleep(1000)"); - b3Clock::usleep(1000); - numCmdSinceSleep1ms = 0; - sleepClock.reset(); + if (numCmdSinceSleep1ms>gMaxNumCmdPer1ms) + { + BT_PROFILE("usleep(10)"); + b3Clock::usleep(10); + numCmdSinceSleep1ms = 0; + sleepClock.reset(); + } } if (sleepClock.getTimeMilliseconds()>1) { @@ -486,7 +491,7 @@ void MotionThreadFunc(void* userPtr,void* lsMemory) args->m_cs->unlock(); } - + args->m_physicsServerPtr->disconnectSharedMemory(true); //do nothing } @@ -956,6 +961,16 @@ public: } + const char* m_mp4FileName; + virtual void dumpFramesToVideo(const char* mp4FileName) + { + m_cs->lock(); + m_mp4FileName = mp4FileName; + m_cs->setSharedParam(1, eGUIDumpFramesToVideo); + workerThreadWait(); + m_mp4FileName = 0; + } + }; @@ -1611,6 +1626,14 @@ void PhysicsServerExample::updateGraphics() m_multiThreadedHelper->mainThreadRelease(); break; } + + case eGUIDumpFramesToVideo: + { + m_multiThreadedHelper->m_childGuiHelper->dumpFramesToVideo(m_multiThreadedHelper->m_mp4FileName); + m_multiThreadedHelper->mainThreadRelease(); + break; + } + case eGUIHelperIdle: { break; diff --git a/examples/SharedMemory/PhysicsServerSharedMemory.cpp b/examples/SharedMemory/PhysicsServerSharedMemory.cpp index 182187885..70d173784 100644 --- a/examples/SharedMemory/PhysicsServerSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsServerSharedMemory.cpp @@ -188,6 +188,8 @@ bool PhysicsServerSharedMemory::connectSharedMemory( struct GUIHelperInterface* void PhysicsServerSharedMemory::disconnectSharedMemory(bool deInitializeSharedMemory) { + m_data->m_commandProcessor->deleteDynamicsWorld(); + m_data->m_commandProcessor->setGuiHelper(0); if (m_data->m_verboseOutput) diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 725849d20..1c119c4d1 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -626,6 +626,7 @@ enum eStateLoggingEnums STATE_LOGGING_START_LOG=1, STATE_LOGGING_STOP_LOG=2, STATE_LOGGING_FILTER_OBJECT_UNIQUE_ID=4, + STATE_LOGGING_MAX_LOG_DOF=8 }; struct VRCameraState @@ -644,6 +645,7 @@ struct StateLoggingRequest int m_numBodyUniqueIds;////only if ROBOT_LOGGING_FILTER_OBJECT_UNIQUE_ID flag is set int m_bodyUniqueIds[MAX_SDF_BODIES]; int m_loggingUniqueId; + int m_maxLogDof; }; struct StateLoggingResultArgs diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 73760a130..6421a37e0 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -330,7 +330,8 @@ enum b3StateLoggingType STATE_LOGGING_MINITAUR = 0, STATE_LOGGING_GENERIC_ROBOT = 1, STATE_LOGGING_VR_CONTROLLERS = 2, - STATE_LOGGING_COMMANDS = 3, + STATE_LOGGING_VIDEO_MP4 = 3, + STATE_LOGGING_COMMANDS = 4, }; diff --git a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp index cfe25c938..520ca6710 100644 --- a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp +++ b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp @@ -36,13 +36,6 @@ subject to the following restrictions: #include "../TinyRenderer/model.h" #include "../ThirdPartyLibs/stb_image/stb_image.h" -enum MyFileType -{ - MY_FILE_STL=1, - MY_FILE_COLLADA=2, - MY_FILE_OBJ=3, -}; - struct MyTexture2 { unsigned char* textureData; @@ -177,7 +170,7 @@ void TinyRendererVisualShapeConverter::setLightSpecularCoeff(float specularCoeff m_data->m_hasLightSpecularCoeff = true; } -void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray& verticesOut, btAlignedObjectArray& indicesOut, btAlignedObjectArray& texturesOut, b3VisualShapeData& visualShapeOut) +void convertURDFToVisualShape(const UrdfShape* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray& verticesOut, btAlignedObjectArray& indicesOut, btAlignedObjectArray& texturesOut, b3VisualShapeData& visualShapeOut) { 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[2] = 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; @@ -193,25 +192,63 @@ void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPref switch (visual->m_geometry.m_type) { 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 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; for (int i = 0; im_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.); + btVector3 vert(rad*btSin(SIMD_2_PI*(float(i) / numSteps)), rad*btCos(SIMD_2_PI*(float(i) / numSteps)), len / 2.); vertices.push_back(vert); - vert[2] = -cylLength / 2.; + vert[2] = -len / 2.; 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)); cylZShape->setMargin(0.001); @@ -235,232 +272,151 @@ void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPref case URDF_GEOM_SPHERE: { visualShapeOut.m_dimensions[0] = visual->m_geometry.m_sphereRadius; - + btScalar radius = visual->m_geometry.m_sphereRadius; btSphereShape* sphereShape = new btSphereShape(radius); convexColShape = sphereShape; convexColShape->setMargin(0.001); break; - - break; } 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()); - } - if (1)//visual->m_geometry) - { - if (visual->m_geometry.m_meshFileName.length()) + case UrdfGeometry::FILE_OBJ: { - const char* filename = visual->m_geometry.m_meshFileName.c_str(); - //b3Printf("mesh->filename=%s\n", filename); - char fullPath[1024]; - int fileType = 0; - - char tmpPathPrefix[1024]; - std::string xml_string; - int maxPathLen = 1024; - b3FileUtils::extractPath(filename,tmpPathPrefix,maxPathLen); - - char visualPathPrefix[1024]; - sprintf(visualPathPrefix,"%s%s",urdfPathPrefix,tmpPathPrefix); - - - sprintf(fullPath, "%s%s", urdfPathPrefix, filename); - b3FileUtils::toLower(fullPath); - if (strstr(fullPath, ".dae")) + //glmesh = LoadMeshFromObj(fullPath,visualPathPrefix); + b3ImportMeshData meshData; + if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(visual->m_geometry.m_meshFileName, meshData)) { - fileType = MY_FILE_COLLADA; - } - if (strstr(fullPath, ".stl")) - { - fileType = MY_FILE_STL; - } - if (strstr(fullPath,".obj")) - { - fileType = MY_FILE_OBJ; - } - - 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) + if (meshData.m_textureImage) { - case MY_FILE_OBJ: - { - //glmesh = LoadMeshFromObj(fullPath,visualPathPrefix); - b3ImportMeshData meshData; - if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(fullPath, meshData)) - { - - 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; + MyTexture2 texData; + texData.m_width = meshData.m_textureWidth; + texData.m_height = meshData.m_textureHeight; + texData.textureData = meshData.m_textureImage; + texturesOut.push_back(texData); } - 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 visualShapes; + btAlignedObjectArray 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(); + glmesh->m_vertices = new b3AlignedObjectArray(); + + for (int i = 0; im_shapeIndex]; + + b3AlignedObjectArray verts; + verts.resize(gfxShape->m_vertices->size()); + + int baseIndex = glmesh->m_vertices->size(); + + for (int i = 0; im_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 visualShapes; - btAlignedObjectArray visualShapeInstances; - btTransform upAxisTrans; upAxisTrans.setIdentity(); - float unitMeterScaling = 1; - int upAxis = 2; + } - LoadMeshFromCollada(fullPath, - visualShapes, - visualShapeInstances, - upAxisTrans, - unitMeterScaling, - upAxis); + int curNumIndices = glmesh->m_indices->size(); + int additionalIndices = gfxShape->m_indices->size(); + glmesh->m_indices->resize(curNumIndices + additionalIndices); + for (int k = 0; km_indices->at(curNumIndices + k) = gfxShape->m_indices->at(k) + baseIndex; + } - glmesh = new GLInstanceGraphicsShape; - // int index = 0; - glmesh->m_indices = new b3AlignedObjectArray(); - glmesh->m_vertices = new b3AlignedObjectArray(); - - for (int i = 0; im_shapeIndex]; - - b3AlignedObjectArray verts; - verts.resize(gfxShape->m_vertices->size()); - - int baseIndex = glmesh->m_vertices->size(); - - for (int i = 0; im_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; km_indices->at(curNumIndices + k) = gfxShape->m_indices->at(k) + baseIndex; - } - - //compensate upAxisTrans and unitMeterScaling here - btMatrix4x4 upAxisMat; - upAxisMat.setIdentity(); + //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); + 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; vm_vertices->push_back(verts[v]); - } - } - glmesh->m_numIndices = glmesh->m_indices->size(); - glmesh->m_numvertices = glmesh->m_vertices->size(); - //glmesh = LoadMeshFromCollada(fullPath); - - break; - } - default: + for (int v = 0; vm_vertices->push_back(verts[v]); } - } - - - if (glmesh && glmesh->m_vertices && (glmesh->m_numvertices>0)) - { - //apply the geometry scaling - for (int i=0;im_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;im_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; - } + } // case mesh + 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) { - - - UrdfLink* const* linkPtr = model.m_links.getAtIndex(linkIndex); + btAssert(linkPtr); // TODO: remove if (not doing it now, because diff will be 50+ lines) if (linkPtr) { + const btArray* 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 < link->m_visualArray.size();v1++) + for (int v1=0; v1 textures; btAlignedObjectArray vertices; @@ -565,20 +534,28 @@ void TinyRendererVisualShapeConverter::convertVisualShapes(int linkIndex, const btTransform startTrans; startTrans.setIdentity(); //int graphicsIndex = -1; - const UrdfVisual& vis = link->m_visualArray[v1]; - btTransform childTrans = vis.m_linkLocalFrame; - btHashString matName(vis.m_materialName.c_str()); - UrdfMaterial *const * matPtr = model.m_materials[matName]; - - float rgbaColor[4] = {1,1,1,1}; - - if (matPtr) + const UrdfShape* vis; + if (useVisual) { + vis = &linkPtr->m_visualArray[v1]; + } else { + vis = &linkPtr->m_collisionArray[v1]; + } + btTransform childTrans = vis->m_linkLocalFrame; + + float rgbaColor[4] = {1,1,1,1}; + if (model && useVisual) { - UrdfMaterial *const mat = *matPtr; - for (int i=0;i<4;i++) - rgbaColor[i] = mat->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); + btHashString matName(linkPtr->m_visualArray[v1].m_materialName.c_str()); + UrdfMaterial*const* matPtr = model->m_materials[matName]; + if (matPtr) + { + 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]; @@ -593,19 +570,19 @@ void TinyRendererVisualShapeConverter::convertVisualShapes(int linkIndex, const b3VisualShapeData visualShape; visualShape.m_objectUniqueId = bodyUniqueId; visualShape.m_linkIndex = linkIndex; - visualShape.m_localVisualFrame[0] = vis.m_linkLocalFrame.getOrigin()[0]; - visualShape.m_localVisualFrame[1] = vis.m_linkLocalFrame.getOrigin()[1]; - visualShape.m_localVisualFrame[2] = vis.m_linkLocalFrame.getOrigin()[2]; - visualShape.m_localVisualFrame[3] = vis.m_linkLocalFrame.getRotation()[0]; - visualShape.m_localVisualFrame[4] = vis.m_linkLocalFrame.getRotation()[1]; - visualShape.m_localVisualFrame[5] = vis.m_linkLocalFrame.getRotation()[2]; - visualShape.m_localVisualFrame[6] = vis.m_linkLocalFrame.getRotation()[3]; - visualShape.m_rgbaColor[0] = rgbaColor[0]; - visualShape.m_rgbaColor[1] = rgbaColor[1]; - visualShape.m_rgbaColor[2] = rgbaColor[2]; - visualShape.m_rgbaColor[3] = rgbaColor[3]; + visualShape.m_localVisualFrame[0] = vis->m_linkLocalFrame.getOrigin()[0]; + visualShape.m_localVisualFrame[1] = vis->m_linkLocalFrame.getOrigin()[1]; + visualShape.m_localVisualFrame[2] = vis->m_linkLocalFrame.getOrigin()[2]; + visualShape.m_localVisualFrame[3] = vis->m_linkLocalFrame.getRotation()[0]; + visualShape.m_localVisualFrame[4] = vis->m_linkLocalFrame.getRotation()[1]; + visualShape.m_localVisualFrame[5] = vis->m_linkLocalFrame.getRotation()[2]; + visualShape.m_localVisualFrame[6] = vis->m_linkLocalFrame.getRotation()[3]; + visualShape.m_rgbaColor[0] = rgbaColor[0]; + visualShape.m_rgbaColor[1] = rgbaColor[1]; + visualShape.m_rgbaColor[2] = rgbaColor[2]; + 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); if (vertices.size() && indices.size()) diff --git a/examples/SharedMemory/TinyRendererVisualShapeConverter.h b/examples/SharedMemory/TinyRendererVisualShapeConverter.h index 78b4c62d6..e9e60948f 100644 --- a/examples/SharedMemory/TinyRendererVisualShapeConverter.h +++ b/examples/SharedMemory/TinyRendererVisualShapeConverter.h @@ -16,7 +16,7 @@ struct TinyRendererVisualShapeConverter : public LinkVisualShapesConverter virtual ~TinyRendererVisualShapeConverter(); - virtual void convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfModel& model, class btCollisionObject* colShape, int objectIndex); + virtual void convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfLink* linkPtr, const UrdfModel* model, class btCollisionObject* colShape, int objectIndex); virtual int getNumVisualShapes(int bodyUniqueId); diff --git a/examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp b/examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp index ca4490aee..f56de333a 100644 --- a/examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp +++ b/examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp @@ -218,8 +218,18 @@ updateVertex( } if (i.vt_idx >= 0) { - texcoords.push_back(in_texcoords[2*i.vt_idx+0]); - texcoords.push_back(in_texcoords[2*i.vt_idx+1]); + int numTexCoords = in_texcoords.size(); + int index0 = 2*i.vt_idx+0; + int index1 = 2*i.vt_idx+1; + + if (index0>=0 && (index0)=0 && (index1) allIndices; diff --git a/examples/pybullet/gym/cartpole_bullet_gym_example.py b/examples/pybullet/gym/cartpole_bullet_gym_example.py new file mode 100644 index 000000000..52cdab258 --- /dev/null +++ b/examples/pybullet/gym/cartpole_bullet_gym_example.py @@ -0,0 +1,31 @@ +"""One-line documentation for gym_example module. + +A detailed description of gym_example. +""" + +import gym +from envs.bullet.cartpole_bullet import CartPoleBulletEnv +import setuptools +import time +import numpy as np + + +w = [0.3, 0.02, 0.02, 0.012] + +def main(): + env = gym.make('CartPoleBulletEnv-v0') + for i_episode in range(1): + observation = env.reset() + done = False + t = 0 + while not done: + print(observation) + action = np.array([np.inner(observation, w)]) + print(action) + observation, reward, done, info = env.step(action) + t = t + 1 + if done: + print("Episode finished after {} timesteps".format(t+1)) + break + +main() diff --git a/examples/pybullet/gym/envs/__init__.py b/examples/pybullet/gym/envs/__init__.py new file mode 100644 index 000000000..605d5e531 --- /dev/null +++ b/examples/pybullet/gym/envs/__init__.py @@ -0,0 +1,17 @@ +from gym.envs.registration import registry, register, make, spec + +# ------------bullet------------- + +register( + id='CartPoleBulletEnv-v0', + entry_point='envs.bullet:CartPoleBulletEnv', + timestep_limit=1000, + reward_threshold=950.0, +) + +register( + id='MinitaurBulletEnv-v0', + entry_point='envs.bullet:MinitaurBulletEnv', + timestep_limit=1000, + reward_threshold=5.0, +) diff --git a/examples/pybullet/gym/envs/bullet/__init__.py b/examples/pybullet/gym/envs/bullet/__init__.py new file mode 100644 index 000000000..25da62f87 --- /dev/null +++ b/examples/pybullet/gym/envs/bullet/__init__.py @@ -0,0 +1,2 @@ +from envs.bullet.cartpole_bullet import CartPoleBulletEnv +from envs.bullet.minitaur_bullet import MinitaurBulletEnv diff --git a/examples/pybullet/gym/envs/bullet/cartpole_bullet.py b/examples/pybullet/gym/envs/bullet/cartpole_bullet.py new file mode 100644 index 000000000..b1f1a1e35 --- /dev/null +++ b/examples/pybullet/gym/envs/bullet/cartpole_bullet.py @@ -0,0 +1,89 @@ +""" +Classic cart-pole system implemented by Rich Sutton et al. +Copied from https://webdocs.cs.ualberta.ca/~sutton/book/code/pole.c +""" +import os +import logging +import math +import gym +from gym import spaces +from gym.utils import seeding +import numpy as np +import time +import subprocess +import pybullet as p + + +logger = logging.getLogger(__name__) + +class CartPoleBulletEnv(gym.Env): + metadata = { + 'render.modes': ['human', 'rgb_array'], + 'video.frames_per_second' : 50 + } + + def __init__(self): + # start the bullet physics server + p.connect(p.GUI) +# p.connect(p.DIRECT) + observation_high = np.array([ + np.finfo(np.float32).max, + np.finfo(np.float32).max, + np.finfo(np.float32).max, + np.finfo(np.float32).max]) + action_high = np.array([0.1]) + + self.action_space = spaces.Box(-action_high, action_high) + self.observation_space = spaces.Box(-observation_high, observation_high) + + self.theta_threshold_radians = 1 + self.x_threshold = 2.4 + self._seed() +# self.reset() + self.viewer = None + self._configure() + + def _configure(self, display=None): + self.display = display + + def _seed(self, seed=None): + self.np_random, seed = seeding.np_random(seed) + return [seed] + + def _step(self, action): + p.stepSimulation() +# time.sleep(self.timeStep) + self.state = p.getJointState(self.cartpole, 1)[0:2] + p.getJointState(self.cartpole, 0)[0:2] + theta, theta_dot, x, x_dot = self.state + force = action + p.setJointMotorControl2(self.cartpole, 0, p.VELOCITY_CONTROL, targetVelocity=(action + self.state[3])) + + done = x < -self.x_threshold \ + or x > self.x_threshold \ + or theta < -self.theta_threshold_radians \ + or theta > self.theta_threshold_radians + reward = 1.0 + + return np.array(self.state), reward, done, {} + + def _reset(self): +# print("-----------reset simulation---------------") + p.resetSimulation() + self.cartpole = p.loadURDF("cartpole.urdf",[0,0,0]) + self.timeStep = 0.01 + p.setJointMotorControl2(self.cartpole, 1, p.VELOCITY_CONTROL, force=0) + p.setGravity(0,0, -10) + p.setTimeStep(self.timeStep) + p.setRealTimeSimulation(0) + + initialCartPos = self.np_random.uniform(low=-0.5, high=0.5, size=(1,)) + initialAngle = self.np_random.uniform(low=-0.5, high=0.5, size=(1,)) + p.resetJointState(self.cartpole, 1, initialAngle) + p.resetJointState(self.cartpole, 0, initialCartPos) + + self.state = p.getJointState(self.cartpole, 1)[0:2] + p.getJointState(self.cartpole, 0)[0:2] + + return np.array(self.state) + + def _render(self, mode='human', close=False): + return diff --git a/examples/pybullet/gym/envs/bullet/minitaur.py b/examples/pybullet/gym/envs/bullet/minitaur.py new file mode 100644 index 000000000..74ddedf84 --- /dev/null +++ b/examples/pybullet/gym/envs/bullet/minitaur.py @@ -0,0 +1,142 @@ +import pybullet as p +import numpy as np + +class Minitaur: + def __init__(self, urdfRootPath=''): + self.urdfRootPath = urdfRootPath + self.reset() + + def applyAction(self, motorCommands): + motorCommandsWithDir = np.multiply(motorCommands, self.motorDir) + for i in range(self.nMotors): + self.setMotorAngleById(self.motorIdList[i], motorCommandsWithDir[i]) + + def getObservation(self): + observation = [] + observation.extend(self.getMotorAngles().tolist()) + observation.extend(self.getMotorVelocities().tolist()) + observation.extend(self.getMotorTorques().tolist()) + observation.extend(list(self.getBaseOrientation())) + observation.extend(list(self.getBasePosition())) + return observation + + def getActionDimension(self): + return self.nMotors + + def getObservationDimension(self): + return len(self.getObservation()) + + def buildJointNameToIdDict(self): + nJoints = p.getNumJoints(self.quadruped) + self.jointNameToId = {} + for i in range(nJoints): + jointInfo = p.getJointInfo(self.quadruped, i) + self.jointNameToId[jointInfo[1].decode('UTF-8')] = jointInfo[0] + self.resetPose() + for i in range(100): + p.stepSimulation() + + def buildMotorIdList(self): + self.motorIdList.append(self.jointNameToId['motor_front_leftR_joint']) + self.motorIdList.append(self.jointNameToId['motor_front_leftL_joint']) + self.motorIdList.append(self.jointNameToId['motor_back_leftR_joint']) + self.motorIdList.append(self.jointNameToId['motor_back_leftL_joint']) + self.motorIdList.append(self.jointNameToId['motor_front_rightL_joint']) + self.motorIdList.append(self.jointNameToId['motor_front_rightR_joint']) + self.motorIdList.append(self.jointNameToId['motor_back_rightL_joint']) + self.motorIdList.append(self.jointNameToId['motor_back_rightR_joint']) + + + def reset(self): + self.quadruped = p.loadURDF("%s/quadruped/quadruped.urdf" % self.urdfRootPath,0,0,.3) + self.kp = 1 + self.kd = 0.1 + self.maxForce = 3.5 + self.nMotors = 8 + self.motorIdList = [] + self.motorDir = [1, -1, 1, -1, -1, 1, -1, 1] + self.buildJointNameToIdDict() + self.buildMotorIdList() + + + def disableAllMotors(self): + nJoints = p.getNumJoints(self.quadruped) + for i in range(nJoints): + p.setJointMotorControl2(bodyIndex=self.quadruped, jointIndex=i, controlMode=p.VELOCITY_CONTROL, force=0) + + def setMotorAngleById(self, motorId, desiredAngle): + p.setJointMotorControl2(bodyIndex=self.quadruped, jointIndex=motorId, controlMode=p.POSITION_CONTROL, targetPosition=desiredAngle, positionGain=self.kp, velocityGain=self.kd, force=self.maxForce) + + def setMotorAngleByName(self, motorName, desiredAngle): + self.setMotorAngleById(self.jointNameToId[motorName], desiredAngle) + + + def resetPose(self): + #right front leg + self.disableAllMotors() + p.resetJointState(self.quadruped,self.jointNameToId['motor_front_rightR_joint'],1.57) + p.resetJointState(self.quadruped,self.jointNameToId['knee_front_rightR_link'],-2.2) + p.resetJointState(self.quadruped,self.jointNameToId['motor_front_rightL_joint'],-1.57) + p.resetJointState(self.quadruped,self.jointNameToId['knee_front_rightL_link'],2.2) + p.createConstraint(self.quadruped,self.jointNameToId['knee_front_rightR_link'],self.quadruped,self.jointNameToId['knee_front_rightL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,0.01,0.2],[0,-0.015,0.2]) + self.setMotorAngleByName('motor_front_rightR_joint', 1.57) + self.setMotorAngleByName('motor_front_rightL_joint',-1.57) + + #left front leg + p.resetJointState(self.quadruped,self.jointNameToId['motor_front_leftR_joint'],1.57) + p.resetJointState(self.quadruped,self.jointNameToId['knee_front_leftR_link'],-2.2) + p.resetJointState(self.quadruped,self.jointNameToId['motor_front_leftL_joint'],-1.57) + p.resetJointState(self.quadruped,self.jointNameToId['knee_front_leftL_link'],2.2) + p.createConstraint(self.quadruped,self.jointNameToId['knee_front_leftR_link'],self.quadruped,self.jointNameToId['knee_front_leftL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,-0.01,0.2],[0,0.015,0.2]) + self.setMotorAngleByName('motor_front_leftR_joint', 1.57) + self.setMotorAngleByName('motor_front_leftL_joint',-1.57) + + #right back leg + p.resetJointState(self.quadruped,self.jointNameToId['motor_back_rightR_joint'],1.57) + p.resetJointState(self.quadruped,self.jointNameToId['knee_back_rightR_link'],-2.2) + p.resetJointState(self.quadruped,self.jointNameToId['motor_back_rightL_joint'],-1.57) + p.resetJointState(self.quadruped,self.jointNameToId['knee_back_rightL_link'],2.2) + p.createConstraint(self.quadruped,self.jointNameToId['knee_back_rightR_link'],self.quadruped,self.jointNameToId['knee_back_rightL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,0.01,0.2],[0,-0.015,0.2]) + self.setMotorAngleByName('motor_back_rightR_joint', 1.57) + self.setMotorAngleByName('motor_back_rightL_joint',-1.57) + + #left back leg + p.resetJointState(self.quadruped,self.jointNameToId['motor_back_leftR_joint'],1.57) + p.resetJointState(self.quadruped,self.jointNameToId['knee_back_leftR_link'],-2.2) + p.resetJointState(self.quadruped,self.jointNameToId['motor_back_leftL_joint'],-1.57) + p.resetJointState(self.quadruped,self.jointNameToId['knee_back_leftL_link'],2.2) + p.createConstraint(self.quadruped,self.jointNameToId['knee_back_leftR_link'],self.quadruped,self.jointNameToId['knee_back_leftL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,-0.01,0.2],[0,0.015,0.2]) + self.setMotorAngleByName('motor_back_leftR_joint', 1.57) + self.setMotorAngleByName('motor_back_leftL_joint',-1.57) + + def getBasePosition(self): + position, orientation = p.getBasePositionAndOrientation(self.quadruped) + return position + + def getBaseOrientation(self): + position, orientation = p.getBasePositionAndOrientation(self.quadruped) + return orientation + + def getMotorAngles(self): + motorAngles = [] + for i in range(self.nMotors): + jointState = p.getJointState(self.quadruped, self.motorIdList[i]) + motorAngles.append(jointState[0]) + motorAngles = np.multiply(motorAngles, self.motorDir) + return motorAngles + + def getMotorVelocities(self): + motorVelocities = [] + for i in range(self.nMotors): + jointState = p.getJointState(self.quadruped, self.motorIdList[i]) + motorVelocities.append(jointState[1]) + motorVelocities = np.multiply(motorVelocities, self.motorDir) + return motorVelocities + + def getMotorTorques(self): + motorTorques = [] + for i in range(self.nMotors): + jointState = p.getJointState(self.quadruped, self.motorIdList[i]) + motorTorques.append(jointState[3]) + motorTorques = np.multiply(motorTorques, self.motorDir) + return motorTorques diff --git a/examples/pybullet/gym/envs/bullet/minitaur_bullet.py b/examples/pybullet/gym/envs/bullet/minitaur_bullet.py new file mode 100644 index 000000000..cd4d5f658 --- /dev/null +++ b/examples/pybullet/gym/envs/bullet/minitaur_bullet.py @@ -0,0 +1,90 @@ +import math +import gym +from gym import spaces +from gym.utils import seeding +import numpy as np +import time + +import pybullet as p +from envs.bullet.minitaur import Minitaur + +class MinitaurBulletEnv(gym.Env): + metadata = { + 'render.modes': ['human', 'rgb_array'], + 'video.frames_per_second' : 50 + } + + def __init__(self): + self._timeStep = 0.01 + self._observation = [] + self._envStepCounter = 0 + self._lastBasePosition = [0, 0, 0] + + p.connect(p.GUI) + + p.resetSimulation() + p.setTimeStep(self._timeStep) + p.loadURDF("plane.urdf") + p.setGravity(0,0,-10) + self._minitaur = Minitaur() + + observationDim = self._minitaur.getObservationDimension() + observation_high = np.array([np.finfo(np.float32).max] * observationDim) + actionDim = 8 + action_high = np.array([math.pi / 2.0] * actionDim) + self.action_space = spaces.Box(-action_high, action_high) + self.observation_space = spaces.Box(-observation_high, observation_high) + + self._seed() + self.reset() + self.viewer = None + + def __del__(self): + p.disconnect() + + def _seed(self, seed=None): + self.np_random, seed = seeding.np_random(seed) + return [seed] + + def _step(self, action): + if len(action) != self._minitaur.getActionDimension(): + raise ValueError("We expect {} continuous action not {}.".format(self._minitaur.getActionDimension(), len(actions.continuous_actions))) + + for i in range(len(action)): + if not -math.pi/2 <= action[i] <= math.pi/2: + raise ValueError("{}th action should be between -1 and 1 not {}.".format(i, action[i])) + action[i] += math.pi / 2 + + self._minitaur.applyAction(action) + p.stepSimulation() + self._observation = self._minitaur.getObservation() + self._envStepCounter += 1 + reward = self._reward() + done = self._termination() + return np.array(self._observation), reward, done, {} + + def _reset(self): + p.resetSimulation() + p.setTimeStep(self._timeStep) + p.loadURDF("plane.urdf") + p.setGravity(0,0,-10) + self._minitaur = Minitaur() + self._observation = self._minitaur.getObservation() + + def _render(self, mode='human', close=False): + return + + def is_fallen(self): + orientation = self._minitaur.getBaseOrientation() + rotMat = p.getMatrixFromQuaternion(orientation) + localUp = rotMat[6:] + return np.dot(np.asarray([0, 0, 1]), np.asarray(localUp)) < 0 + + def _termination(self): + return self.is_fallen() + + def _reward(self): + currentBasePosition = self._minitaur.getBasePosition() + reward = np.dot(np.asarray([-1, 0, 0]), np.asarray(currentBasePosition) - np.asarray(self._lastBasePosition)) + self._lastBasePosition = currentBasePosition + return reward diff --git a/examples/pybullet/gym/trpo_cartpole_bullet_gym.py b/examples/pybullet/gym/trpo_cartpole_bullet_gym.py new file mode 100644 index 000000000..bac943217 --- /dev/null +++ b/examples/pybullet/gym/trpo_cartpole_bullet_gym.py @@ -0,0 +1,31 @@ +from envs.bullet.cartpole_bullet import CartPoleBulletEnv +from rllab.algos.trpo import TRPO +from rllab.baselines.linear_feature_baseline import LinearFeatureBaseline +from rllab.envs.gym_env import GymEnv +from rllab.envs.normalized_env import normalize +from rllab.policies.gaussian_mlp_policy import GaussianMLPPolicy + +env = normalize(GymEnv("CartPoleBulletEnv-v0")) + +policy = GaussianMLPPolicy( + env_spec=env.spec, + # The neural network policy should have two hidden layers, each with 32 hidden units. + hidden_sizes=(8,) +) + +baseline = LinearFeatureBaseline(env_spec=env.spec) + +algo = TRPO( + env=env, + policy=policy, + baseline=baseline, + batch_size=5000, + max_path_length=env.horizon, + n_itr=50, + discount=0.999, + step_size=0.01, + # Uncomment both lines (this and the plot parameter below) to enable plotting +# plot=True, +) + +algo.train() diff --git a/examples/pybullet/gym/trpo_tf_cartpole_bullet_gym.py b/examples/pybullet/gym/trpo_tf_cartpole_bullet_gym.py new file mode 100644 index 000000000..6e055be15 --- /dev/null +++ b/examples/pybullet/gym/trpo_tf_cartpole_bullet_gym.py @@ -0,0 +1,35 @@ +from envs.bullet.cartpole_bullet import CartPoleBulletEnv +from sandbox.rocky.tf.algos.trpo import TRPO +from sandbox.rocky.tf.policies.gaussian_mlp_policy import GaussianMLPPolicy +from sandbox.rocky.tf.envs.base import TfEnv + +from rllab.baselines.linear_feature_baseline import LinearFeatureBaseline +from rllab.envs.gym_env import GymEnv +from rllab.envs.normalized_env import normalize + +env = TfEnv(normalize(GymEnv("CartPoleBulletEnv-v0"))) + +policy = GaussianMLPPolicy( + name = "tf_gaussian_mlp", + env_spec=env.spec, + # The neural network policy should have two hidden layers, each with 32 hidden units. + hidden_sizes=(8,) +) + +baseline = LinearFeatureBaseline(env_spec=env.spec) + +algo = TRPO( + env=env, + policy=policy, + baseline=baseline, + batch_size=5000, + max_path_length=env.horizon, + n_itr=50, + discount=0.999, + step_size=0.01, + force_batch_sampler=True, + # Uncomment both lines (this and the plot parameter below) to enable plotting + #plot=True, +) + +algo.train() diff --git a/examples/pybullet/hand.py b/examples/pybullet/hand.py index d0428c555..cb4b45f87 100644 --- a/examples/pybullet/hand.py +++ b/examples/pybullet/hand.py @@ -16,7 +16,7 @@ if (c<0): p.connect(p.GUI) #load the MuJoCo MJCF hand -objects = p.loadMJCF("MPL/mpl.xml") +objects = p.loadMJCF("MPL/MPL.xml") hand=objects[0] #clamp in range 400-600 @@ -75,4 +75,4 @@ if (ser.isOpen()): #print(middle) #print(pink) #print(index) - #print(thumb) \ No newline at end of file + #print(thumb) diff --git a/examples/pybullet/kuka_with_cube_playback.py b/examples/pybullet/kuka_with_cube_playback.py index 64b9a52c7..f84fbc45c 100644 --- a/examples/pybullet/kuka_with_cube_playback.py +++ b/examples/pybullet/kuka_with_cube_playback.py @@ -16,8 +16,8 @@ def readLogFile(filename, verbose = True): print('Opened'), print(filename) - keys = f.readline().rstrip('\n').split(',') - fmt = f.readline().rstrip('\n') + keys = f.readline().decode('utf8').rstrip('\n').split(',') + fmt = f.readline().decode('utf8').rstrip('\n') # The byte number of one record sz = struct.calcsize(fmt) @@ -37,7 +37,7 @@ def readLogFile(filename, verbose = True): # Read data wholeFile = f.read() # split by alignment word - chunks = wholeFile.split('\xaa\xbb') + chunks = wholeFile.split(b'\xaa\xbb') log = list() for chunk in chunks: if len(chunk) == sz: diff --git a/examples/pybullet/minitaur.py b/examples/pybullet/minitaur.py index 58aab4a38..1db778237 100644 --- a/examples/pybullet/minitaur.py +++ b/examples/pybullet/minitaur.py @@ -17,10 +17,10 @@ class Minitaur: p.stepSimulation() def buildMotorIdList(self): - self.motorIdList.append(self.jointNameToId['motor_front_leftR_joint']) self.motorIdList.append(self.jointNameToId['motor_front_leftL_joint']) - self.motorIdList.append(self.jointNameToId['motor_back_leftR_joint']) + self.motorIdList.append(self.jointNameToId['motor_front_leftR_joint']) self.motorIdList.append(self.jointNameToId['motor_back_leftL_joint']) + self.motorIdList.append(self.jointNameToId['motor_back_leftR_joint']) self.motorIdList.append(self.jointNameToId['motor_front_rightL_joint']) self.motorIdList.append(self.jointNameToId['motor_front_rightR_joint']) self.motorIdList.append(self.jointNameToId['motor_back_rightL_joint']) @@ -28,21 +28,17 @@ class Minitaur: def reset(self): - self.quadruped = p.loadURDF("%s/quadruped/quadruped.urdf" % self.urdfRootPath,0,0,.3) + self.quadruped = p.loadURDF("%s/quadruped/minitaur.urdf" % self.urdfRootPath,0,0,.2) self.kp = 1 self.kd = 0.1 self.maxForce = 3.5 self.nMotors = 8 self.motorIdList = [] - self.motorDir = [1, -1, 1, -1, -1, 1, -1, 1] + self.motorDir = [1, 1, 1, 1, 1, 1, 1, 1] self.buildJointNameToIdDict() self.buildMotorIdList() - def disableAllMotors(self): - nJoints = p.getNumJoints(self.quadruped) - for i in range(nJoints): - p.setJointMotorControl2(bodyIndex=self.quadruped, jointIndex=i, controlMode=p.VELOCITY_CONTROL, force=0) def setMotorAngleById(self, motorId, desiredAngle): p.setJointMotorControl2(bodyIndex=self.quadruped, jointIndex=motorId, controlMode=p.POSITION_CONTROL, targetPosition=desiredAngle, positionGain=self.kp, velocityGain=self.kd, force=self.maxForce) @@ -51,42 +47,55 @@ class Minitaur: self.setMotorAngleById(self.jointNameToId[motorName], desiredAngle) def resetPose(self): - #right front leg - self.disableAllMotors() - p.resetJointState(self.quadruped,self.jointNameToId['motor_front_rightR_joint'],1.57) - p.resetJointState(self.quadruped,self.jointNameToId['knee_front_rightR_link'],-2.2) - p.resetJointState(self.quadruped,self.jointNameToId['motor_front_rightL_joint'],-1.57) - p.resetJointState(self.quadruped,self.jointNameToId['knee_front_rightL_link'],2.2) - p.createConstraint(self.quadruped,self.jointNameToId['knee_front_rightR_link'],self.quadruped,self.jointNameToId['knee_front_rightL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,0.01,0.2],[0,-0.015,0.2]) - self.setMotorAngleByName('motor_front_rightR_joint', 1.57) - self.setMotorAngleByName('motor_front_rightL_joint',-1.57) + kneeFrictionForce = 0 + halfpi = 1.57079632679 + kneeangle = -2.1834 #halfpi - acos(upper_leg_length / lower_leg_length) #left front leg - p.resetJointState(self.quadruped,self.jointNameToId['motor_front_leftR_joint'],1.57) - p.resetJointState(self.quadruped,self.jointNameToId['knee_front_leftR_link'],-2.2) - p.resetJointState(self.quadruped,self.jointNameToId['motor_front_leftL_joint'],-1.57) - p.resetJointState(self.quadruped,self.jointNameToId['knee_front_leftL_link'],2.2) - p.createConstraint(self.quadruped,self.jointNameToId['knee_front_leftR_link'],self.quadruped,self.jointNameToId['knee_front_leftL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,-0.01,0.2],[0,0.015,0.2]) - self.setMotorAngleByName('motor_front_leftR_joint', 1.57) - self.setMotorAngleByName('motor_front_leftL_joint',-1.57) - - #right back leg - p.resetJointState(self.quadruped,self.jointNameToId['motor_back_rightR_joint'],1.57) - p.resetJointState(self.quadruped,self.jointNameToId['knee_back_rightR_link'],-2.2) - p.resetJointState(self.quadruped,self.jointNameToId['motor_back_rightL_joint'],-1.57) - p.resetJointState(self.quadruped,self.jointNameToId['knee_back_rightL_link'],2.2) - p.createConstraint(self.quadruped,self.jointNameToId['knee_back_rightR_link'],self.quadruped,self.jointNameToId['knee_back_rightL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,0.01,0.2],[0,-0.015,0.2]) - self.setMotorAngleByName('motor_back_rightR_joint', 1.57) - self.setMotorAngleByName('motor_back_rightL_joint',-1.57) + p.resetJointState(self.quadruped,self.jointNameToId['motor_front_leftL_joint'],self.motorDir[0]*halfpi) + p.resetJointState(self.quadruped,self.jointNameToId['knee_front_leftL_link'],self.motorDir[0]*kneeangle) + p.resetJointState(self.quadruped,self.jointNameToId['motor_front_leftR_joint'],self.motorDir[1]*halfpi) + p.resetJointState(self.quadruped,self.jointNameToId['knee_front_leftR_link'],self.motorDir[1]*kneeangle) + p.createConstraint(self.quadruped,self.jointNameToId['knee_front_leftR_link'],self.quadruped,self.jointNameToId['knee_front_leftL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2]) + self.setMotorAngleByName('motor_front_leftL_joint', self.motorDir[0]*halfpi) + self.setMotorAngleByName('motor_front_leftR_joint', self.motorDir[1]*halfpi) + p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_front_leftL_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce) + p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_front_leftR_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce) #left back leg - p.resetJointState(self.quadruped,self.jointNameToId['motor_back_leftR_joint'],1.57) - p.resetJointState(self.quadruped,self.jointNameToId['knee_back_leftR_link'],-2.2) - p.resetJointState(self.quadruped,self.jointNameToId['motor_back_leftL_joint'],-1.57) - p.resetJointState(self.quadruped,self.jointNameToId['knee_back_leftL_link'],2.2) - p.createConstraint(self.quadruped,self.jointNameToId['knee_back_leftR_link'],self.quadruped,self.jointNameToId['knee_back_leftL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,-0.01,0.2],[0,0.015,0.2]) - self.setMotorAngleByName('motor_back_leftR_joint', 1.57) - self.setMotorAngleByName('motor_back_leftL_joint',-1.57) + p.resetJointState(self.quadruped,self.jointNameToId['motor_back_leftL_joint'],self.motorDir[2]*halfpi) + p.resetJointState(self.quadruped,self.jointNameToId['knee_back_leftL_link'],self.motorDir[2]*kneeangle) + p.resetJointState(self.quadruped,self.jointNameToId['motor_back_leftR_joint'],self.motorDir[3]*halfpi) + p.resetJointState(self.quadruped,self.jointNameToId['knee_back_leftR_link'],self.motorDir[3]*kneeangle) + p.createConstraint(self.quadruped,self.jointNameToId['knee_back_leftR_link'],self.quadruped,self.jointNameToId['knee_back_leftL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2]) + self.setMotorAngleByName('motor_back_leftL_joint',self.motorDir[2]*halfpi) + self.setMotorAngleByName('motor_back_leftR_joint',self.motorDir[3]*halfpi) + p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_back_leftL_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce) + p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_back_leftR_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce) + + #right front leg + p.resetJointState(self.quadruped,self.jointNameToId['motor_front_rightL_joint'],self.motorDir[4]*halfpi) + p.resetJointState(self.quadruped,self.jointNameToId['knee_front_rightL_link'],self.motorDir[4]*kneeangle) + p.resetJointState(self.quadruped,self.jointNameToId['motor_front_rightR_joint'],self.motorDir[5]*halfpi) + p.resetJointState(self.quadruped,self.jointNameToId['knee_front_rightR_link'],self.motorDir[5]*kneeangle) + p.createConstraint(self.quadruped,self.jointNameToId['knee_front_rightR_link'],self.quadruped,self.jointNameToId['knee_front_rightL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2]) + self.setMotorAngleByName('motor_front_rightL_joint',self.motorDir[4]*halfpi) + self.setMotorAngleByName('motor_front_rightR_joint',self.motorDir[5]*halfpi) + p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_front_rightL_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce) + p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_front_rightR_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce) + + + #right back leg + p.resetJointState(self.quadruped,self.jointNameToId['motor_back_rightL_joint'],self.motorDir[6]*halfpi) + p.resetJointState(self.quadruped,self.jointNameToId['knee_back_rightL_link'],self.motorDir[6]*kneeangle) + p.resetJointState(self.quadruped,self.jointNameToId['motor_back_rightR_joint'],self.motorDir[7]*halfpi) + p.resetJointState(self.quadruped,self.jointNameToId['knee_back_rightR_link'],self.motorDir[7]*kneeangle) + p.createConstraint(self.quadruped,self.jointNameToId['knee_back_rightR_link'],self.quadruped,self.jointNameToId['knee_back_rightL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2]) + self.setMotorAngleByName('motor_back_rightL_joint',self.motorDir[6]*halfpi) + self.setMotorAngleByName('motor_back_rightR_joint',self.motorDir[7]*halfpi) + p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_back_rightL_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce) + p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_back_rightR_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce) + def getBasePosition(self): position, orientation = p.getBasePositionAndOrientation(self.quadruped) diff --git a/examples/pybullet/minitaur_evaluate.py b/examples/pybullet/minitaur_evaluate.py index 3c3e65280..0975bef0c 100644 --- a/examples/pybullet/minitaur_evaluate.py +++ b/examples/pybullet/minitaur_evaluate.py @@ -18,7 +18,7 @@ def current_position(): def is_fallen(): global minitaur orientation = minitaur.getBaseOrientation() - rotMat = p.getMatrixFromQuaterion(orientation) + rotMat = p.getMatrixFromQuaternion(orientation) localUp = rotMat[6:] return np.dot(np.asarray([0, 0, 1]), np.asarray(localUp)) < 0 @@ -58,7 +58,7 @@ evaluate_func_map['evaluate_desired_motorAngle_hop'] = evaluate_desired_motorAng -def evaluate_params(evaluateFunc, params, objectiveParams, urdfRoot='', timeStep=0.01, maxNumSteps=1000, sleepTime=0): +def evaluate_params(evaluateFunc, params, objectiveParams, urdfRoot='', timeStep=0.01, maxNumSteps=10000, sleepTime=0): print('start evaluation') beforeTime = time.time() p.resetSimulation() diff --git a/examples/pybullet/minitaur_test.py b/examples/pybullet/minitaur_test.py index e98cd8cde..1d156e5a3 100644 --- a/examples/pybullet/minitaur_test.py +++ b/examples/pybullet/minitaur_test.py @@ -1,3 +1,7 @@ +import sys +#some python interpreters need '.' added +sys.path.append(".") + import pybullet as p from minitaur import Minitaur from minitaur_evaluate import * diff --git a/examples/pybullet/mylittleminitaur.py b/examples/pybullet/mylittleminitaur.py new file mode 100644 index 000000000..fd1ad4718 --- /dev/null +++ b/examples/pybullet/mylittleminitaur.py @@ -0,0 +1,21 @@ +import pybullet as p +import time +import math +import sys +sys.path.append(".") + +from minitaur import Minitaur +p.connect(p.GUI) +p.setTimeOut(5) +#p.setPhysicsEngineParameter(numSolverIterations=50) +p.setGravity(0,0,-10) +p.setTimeStep(0.01) + +urdfRoot = '' +p.loadURDF("%s/plane.urdf" % urdfRoot) +minitaur = Minitaur(urdfRoot) + +while (True): + p.stepSimulation() + time.sleep(0.01) + \ No newline at end of file diff --git a/examples/pybullet/premake4.lua b/examples/pybullet/premake4.lua index d424ef209..7892d3da2 100644 --- a/examples/pybullet/premake4.lua +++ b/examples/pybullet/premake4.lua @@ -125,6 +125,8 @@ if not _OPTIONS["no-enet"] then "../../examples/SharedMemory/Win32SharedMemory.h", "../../examples/SharedMemory/PosixSharedMemory.cpp", "../../examples/SharedMemory/PosixSharedMemory.h", + "../../examples/SharedMemory/SharedMemoryCommands.h", + "../../examples/SharedMemory/SharedMemoryPublic.h", "../../examples/Utils/b3ResourcePath.cpp", "../../examples/Utils/b3ResourcePath.h", "../../examples/Utils/RobotLoggingUtil.cpp", diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index da71a68dc..36ceb794c 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -3,11 +3,11 @@ #include "../SharedMemory/SharedMemoryInProcessPhysicsC_API.h" #ifdef BT_ENABLE_ENET #include "../SharedMemory/PhysicsClientUDP_C_API.h" -#endif //BT_ENABLE_ENET +#endif //BT_ENABLE_ENET #ifdef BT_ENABLE_CLSOCKET #include "../SharedMemory/PhysicsClientTCP_C_API.h" -#endif //BT_ENABLE_CLSOCKET +#endif //BT_ENABLE_CLSOCKET #ifdef __APPLE__ #include @@ -24,19 +24,17 @@ #define PyString_FromString PyBytes_FromString #endif - - static PyObject* SpamError; #define MAX_PHYSICS_CLIENTS 1024 static b3PhysicsClientHandle sPhysicsClients1[MAX_PHYSICS_CLIENTS] = {0}; static int sPhysicsClientsGUI[MAX_PHYSICS_CLIENTS] = {0}; -static int sNumPhysicsClients=0; +static int sNumPhysicsClients = 0; b3PhysicsClientHandle getPhysicsClient(int physicsClientId) { b3PhysicsClientHandle sm; - if ((physicsClientId <0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients1[physicsClientId])) + if ((physicsClientId < 0) || (physicsClientId >= MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients1[physicsClientId])) { return 0; } @@ -55,32 +53,34 @@ b3PhysicsClientHandle getPhysicsClient(int physicsClientId) sNumPhysicsClients--; } return 0; - } -static double pybullet_internalGetFloatFromSequence(PyObject* seq, int index) { +static double pybullet_internalGetFloatFromSequence(PyObject* seq, int index) +{ double v = 0.0; PyObject* item; - if (PyList_Check(seq)) { + if (PyList_Check(seq)) + { item = PyList_GET_ITEM(seq, index); v = PyFloat_AsDouble(item); } - else { + else + { item = PyTuple_GET_ITEM(seq, index); v = PyFloat_AsDouble(item); } return v; } - // internal function to set a float matrix[16] // used to initialize camera position with // a view and projection matrix in renderImage() // // // Args: // matrix - float[16] which will be set by values from objMat -static int pybullet_internalSetMatrix(PyObject* objMat, float matrix[16]) { +static int pybullet_internalSetMatrix(PyObject* objMat, float matrix[16]) +{ int i, len; PyObject* seq; @@ -88,8 +88,10 @@ static int pybullet_internalSetMatrix(PyObject* objMat, float matrix[16]) { if (seq) { len = PySequence_Size(objMat); - if (len == 16) { - for (i = 0; i < len; i++) { + if (len == 16) + { + for (i = 0; i < len; i++) + { matrix[i] = pybullet_internalGetFloatFromSequence(seq, i); } Py_DECREF(seq); @@ -106,7 +108,8 @@ static int pybullet_internalSetMatrix(PyObject* objMat, float matrix[16]) { // // // Args: // vector - float[3] which will be set by values from objMat -static int pybullet_internalSetVector(PyObject* objVec, float vector[3]) { +static int pybullet_internalSetVector(PyObject* objVec, float vector[3]) +{ int i, len; PyObject* seq = 0; if (objVec == NULL) @@ -115,10 +118,11 @@ static int pybullet_internalSetVector(PyObject* objVec, float vector[3]) { seq = PySequence_Fast(objVec, "expected a sequence"); if (seq) { - len = PySequence_Size(objVec); - if (len == 3) { - for (i = 0; i < len; i++) { + if (len == 3) + { + for (i = 0; i < len; i++) + { vector[i] = pybullet_internalGetFloatFromSequence(seq, i); } Py_DECREF(seq); @@ -130,7 +134,8 @@ static int pybullet_internalSetVector(PyObject* objVec, float vector[3]) { } // vector - double[3] which will be set by values from obVec -static int pybullet_internalSetVectord(PyObject* obVec, double vector[3]) { +static int pybullet_internalSetVectord(PyObject* obVec, double vector[3]) +{ int i, len; PyObject* seq; if (obVec == NULL) @@ -140,8 +145,10 @@ static int pybullet_internalSetVectord(PyObject* obVec, double vector[3]) { if (seq) { len = PySequence_Size(obVec); - if (len == 3) { - for (i = 0; i < len; i++) { + if (len == 3) + { + for (i = 0; i < len; i++) + { vector[i] = pybullet_internalGetFloatFromSequence(seq, i); } Py_DECREF(seq); @@ -153,7 +160,8 @@ static int pybullet_internalSetVectord(PyObject* obVec, double vector[3]) { } // vector - double[3] which will be set by values from obVec -static int pybullet_internalSetVector4d(PyObject* obVec, double vector[4]) { +static int pybullet_internalSetVector4d(PyObject* obVec, double vector[4]) +{ int i, len; PyObject* seq; if (obVec == NULL) @@ -161,8 +169,10 @@ static int pybullet_internalSetVector4d(PyObject* obVec, double vector[4]) { seq = PySequence_Fast(obVec, "expected a sequence"); len = PySequence_Size(obVec); - if (len == 4) { - for (i = 0; i < len; i++) { + if (len == 4) + { + for (i = 0; i < len; i++) + { vector[i] = pybullet_internalGetFloatFromSequence(seq, i); } Py_DECREF(seq); @@ -172,203 +182,201 @@ static int pybullet_internalSetVector4d(PyObject* obVec, double vector[4]) { return 0; } - - // Step through one timestep of the simulation -static PyObject* pybullet_stepSimulation(PyObject* self, PyObject* args, PyObject *keywds) +static PyObject* pybullet_stepSimulation(PyObject* self, PyObject* args, PyObject* keywds) { int physicsClientId = 0; - static char *kwlist[] = { "physicsClientId", NULL }; - b3PhysicsClientHandle sm=0; + static char* kwlist[] = {"physicsClientId", NULL}; + b3PhysicsClientHandle sm = 0; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "|i", kwlist,&physicsClientId)) + if (!PyArg_ParseTupleAndKeywords(args, keywds, "|i", kwlist, &physicsClientId)) { return NULL; } sm = getPhysicsClient(physicsClientId); - if (sm == 0) + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - - { - b3SharedMemoryStatusHandle statusHandle; - int statusType; - if (b3CanSubmitCommand(sm)) { - statusHandle = b3SubmitClientCommandAndWaitStatus( - sm, b3InitStepSimulationCommand(sm)); - statusType = b3GetStatusType(statusHandle); - } - } + { + b3SharedMemoryStatusHandle statusHandle; + int statusType; - Py_INCREF(Py_None); - return Py_None; + if (b3CanSubmitCommand(sm)) + { + statusHandle = b3SubmitClientCommandAndWaitStatus( + sm, b3InitStepSimulationCommand(sm)); + statusType = b3GetStatusType(statusHandle); + } + } + + Py_INCREF(Py_None); + return Py_None; } -static PyObject* pybullet_connectPhysicsServer(PyObject* self, PyObject* args, PyObject* keywds) { +static PyObject* pybullet_connectPhysicsServer(PyObject* self, PyObject* args, PyObject* keywds) +{ + int freeIndex = -1; + int method = eCONNECT_GUI; + int i; + b3PhysicsClientHandle sm = 0; - - int freeIndex = -1; - int method = eCONNECT_GUI; - int i; - b3PhysicsClientHandle sm=0; - - if (sNumPhysicsClients>=MAX_PHYSICS_CLIENTS) + if (sNumPhysicsClients >= MAX_PHYSICS_CLIENTS) { - PyErr_SetString(SpamError, - "Exceeding maximum number of physics connections."); - return NULL; + PyErr_SetString(SpamError, + "Exceeding maximum number of physics connections."); + return NULL; } - - { - - int key = SHARED_MEMORY_KEY; - int udpPort = 1234; - int tcpPort = 6667; - - const char* hostName = "localhost"; - - int size = PySequence_Size(args); - if (size == 1) { - if (!PyArg_ParseTuple(args, "i", &method)) { - PyErr_SetString(SpamError, - "connectPhysicsServer expected argument GUI, " - "DIRECT, SHARED_MEMORY, UDP or TCP"); - return NULL; - } - } + int key = SHARED_MEMORY_KEY; + int udpPort = 1234; + int tcpPort = 6667; - //Only one local in-process GUI connection allowed. - if (method == eCONNECT_GUI) - { - int i; - for (i=0;i=0) - { - udpPort = port; - tcpPort = port; - } - } + if (size == 3) + { + int port = -1; + if (!PyArg_ParseTuple(args, "isi", &method, &hostName, &port)) + { + PyErr_SetString(SpamError, + "connectPhysicsServer 3 arguments: method, hostname, port"); + return NULL; + } + if (port >= 0) + { + udpPort = port; + tcpPort = port; + } + } - switch (method) { - case eCONNECT_GUI: { - int argc = 0; - char* argv[1] = {0}; + switch (method) + { + case eCONNECT_GUI: + { + int argc = 0; + char* argv[1] = {0}; #ifdef __APPLE__ - sm = b3CreateInProcessPhysicsServerAndConnectMainThread(argc, argv); + sm = b3CreateInProcessPhysicsServerAndConnectMainThread(argc, argv); #else - sm = b3CreateInProcessPhysicsServerAndConnect(argc, argv); + sm = b3CreateInProcessPhysicsServerAndConnect(argc, argv); #endif - break; - } - case eCONNECT_DIRECT: { - sm = b3ConnectPhysicsDirect(); - break; - } - case eCONNECT_SHARED_MEMORY: { - sm = b3ConnectSharedMemory(key); - break; - } - case eCONNECT_UDP: - { + break; + } + case eCONNECT_DIRECT: + { + sm = b3ConnectPhysicsDirect(); + break; + } + case eCONNECT_SHARED_MEMORY: + { + sm = b3ConnectSharedMemory(key); + break; + } + case eCONNECT_UDP: + { #ifdef BT_ENABLE_ENET - sm = b3ConnectPhysicsUDP(hostName, udpPort); + sm = b3ConnectPhysicsUDP(hostName, udpPort); #else - PyErr_SetString(SpamError, "UDP is not enabled in this pybullet build"); - return NULL; -#endif //BT_ENABLE_ENET + PyErr_SetString(SpamError, "UDP is not enabled in this pybullet build"); + return NULL; +#endif //BT_ENABLE_ENET - break; - } - case eCONNECT_TCP: - { + break; + } + case eCONNECT_TCP: + { #ifdef BT_ENABLE_CLSOCKET - - sm = b3ConnectPhysicsTCP(hostName, tcpPort); + + sm = b3ConnectPhysicsTCP(hostName, tcpPort); #else - PyErr_SetString(SpamError, "TCP is not enabled in this pybullet build"); - return NULL; -#endif //BT_ENABLE_CLSOCKET - - break; - } - - + PyErr_SetString(SpamError, "TCP is not enabled in this pybullet build"); + return NULL; +#endif //BT_ENABLE_CLSOCKET - default: { - PyErr_SetString(SpamError, "connectPhysicsServer unexpected argument"); - return NULL; - } - }; - } + break; + } - if (sm && b3CanSubmitCommand(sm)) - { - for (i=0;i=0) - { - b3SharedMemoryCommandHandle command; + if (sm && b3CanSubmitCommand(sm)) + { + for (i = 0; i < MAX_PHYSICS_CLIENTS; i++) + { + if (sPhysicsClients1[i] == 0) + { + freeIndex = i; + break; + } + } + + if (freeIndex >= 0) + { + b3SharedMemoryCommandHandle command; b3SharedMemoryStatusHandle statusHandle; int statusType; + sPhysicsClients1[freeIndex] = sm; + sPhysicsClientsGUI[freeIndex] = method; + sNumPhysicsClients++; - sPhysicsClients1[freeIndex] = sm; - sPhysicsClientsGUI[freeIndex] = method; - sNumPhysicsClients++; - - command = b3InitSyncBodyInfoCommand(sm); - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + command = b3InitSyncBodyInfoCommand(sm); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); statusType = b3GetStatusType(statusHandle); #if 0 if (statusType != CMD_BODY_INFO_COMPLETED) @@ -377,110 +385,105 @@ static PyObject* pybullet_connectPhysicsServer(PyObject* self, PyObject* args, P return NULL; } #endif - } - - } - return PyInt_FromLong(freeIndex); + } + } + return PyInt_FromLong(freeIndex); } static PyObject* pybullet_disconnectPhysicsServer(PyObject* self, - PyObject* args, - PyObject *keywds) { - + PyObject* args, + PyObject* keywds) +{ int physicsClientId = 0; - b3PhysicsClientHandle sm=0; - static char *kwlist[] = { "physicsClientId", NULL }; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "|i", kwlist,&physicsClientId)) + b3PhysicsClientHandle sm = 0; + static char* kwlist[] = {"physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "|i", kwlist, &physicsClientId)) { return NULL; } sm = getPhysicsClient(physicsClientId); - if (sm == 0) + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - - { - b3DisconnectSharedMemory(sm); - sm = 0; - } + { + b3DisconnectSharedMemory(sm); + sm = 0; + } - sPhysicsClients1[physicsClientId] = 0; - sPhysicsClientsGUI[physicsClientId] = 0; - sNumPhysicsClients--; + sPhysicsClients1[physicsClientId] = 0; + sPhysicsClientsGUI[physicsClientId] = 0; + sNumPhysicsClients--; - Py_INCREF(Py_None); - return Py_None; + Py_INCREF(Py_None); + return Py_None; } -static PyObject* pybullet_saveWorld(PyObject* self, PyObject* args,PyObject *keywds) { +static PyObject* pybullet_saveWorld(PyObject* self, PyObject* args, PyObject* keywds) +{ const char* worldFileName = ""; b3PhysicsClientHandle sm = 0; int physicsClientId = 0; - static char *kwlist[] = { "worldFileName","physicsClientId", NULL }; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|i", kwlist, &worldFileName,&physicsClientId)) + static char* kwlist[] = {"worldFileName", "physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|i", kwlist, &worldFileName, &physicsClientId)) { return NULL; } sm = getPhysicsClient(physicsClientId); - if (sm == 0) + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - { - b3SharedMemoryCommandHandle command; + b3SharedMemoryCommandHandle command; b3SharedMemoryStatusHandle statusHandle; int statusType; command = b3SaveWorldCommandInit(sm, worldFileName); statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); statusType = b3GetStatusType(statusHandle); - if (statusType != CMD_SAVE_WORLD_COMPLETED) { + if (statusType != CMD_SAVE_WORLD_COMPLETED) + { PyErr_SetString(SpamError, "saveWorld command execution failed."); return NULL; } Py_INCREF(Py_None); return Py_None; - - } + } - PyErr_SetString(SpamError, "Cannot execute saveWorld command."); return NULL; - } #define MAX_SDF_BODIES 512 -static PyObject* pybullet_loadBullet(PyObject* self, PyObject* args,PyObject *keywds) +static PyObject* pybullet_loadBullet(PyObject* self, PyObject* args, PyObject* keywds) { const char* bulletFileName = ""; b3SharedMemoryStatusHandle statusHandle; int statusType; b3SharedMemoryCommandHandle command; - int i,numBodies; + int i, numBodies; int bodyIndicesOut[MAX_SDF_BODIES]; PyObject* pylist = 0; b3PhysicsClientHandle sm = 0; - int physicsClientId = 0; - static char *kwlist[] = { "bulletFileName","physicsClientId", NULL }; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|i", kwlist, &bulletFileName,&physicsClientId)) + int physicsClientId = 0; + static char* kwlist[] = {"bulletFileName", "physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|i", kwlist, &bulletFileName, &physicsClientId)) { return NULL; } sm = getPhysicsClient(physicsClientId); - if (sm == 0) + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - command = b3LoadBulletCommandInit(sm, bulletFileName); statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); @@ -492,22 +495,23 @@ static PyObject* pybullet_loadBullet(PyObject* self, PyObject* args,PyObject *ke } numBodies = - b3GetStatusBodyIndices(statusHandle, bodyIndicesOut, MAX_SDF_BODIES); - if (numBodies > MAX_SDF_BODIES) { - PyErr_SetString(SpamError, "loadBullet exceeds body capacity"); - return NULL; - } + b3GetStatusBodyIndices(statusHandle, bodyIndicesOut, MAX_SDF_BODIES); + if (numBodies > MAX_SDF_BODIES) + { + PyErr_SetString(SpamError, "loadBullet exceeds body capacity"); + return NULL; + } - pylist = PyTuple_New(numBodies); + pylist = PyTuple_New(numBodies); - if (numBodies > 0 && numBodies <= MAX_SDF_BODIES) { - for (i = 0; i < numBodies; i++) { - PyTuple_SetItem(pylist, i, PyInt_FromLong(bodyIndicesOut[i])); - } - } - return pylist; - - + if (numBodies > 0 && numBodies <= MAX_SDF_BODIES) + { + for (i = 0; i < numBodies; i++) + { + PyTuple_SetItem(pylist, i, PyInt_FromLong(bodyIndicesOut[i])); + } + } + return pylist; } static PyObject* pybullet_saveBullet(PyObject* self, PyObject* args, PyObject* keywds) @@ -518,20 +522,19 @@ static PyObject* pybullet_saveBullet(PyObject* self, PyObject* args, PyObject* k b3SharedMemoryCommandHandle command; b3PhysicsClientHandle sm = 0; - int physicsClientId = 0; - static char *kwlist[] = { "bulletFileName","physicsClientId", NULL }; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|i", kwlist, &bulletFileName,&physicsClientId)) + int physicsClientId = 0; + static char* kwlist[] = {"bulletFileName", "physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|i", kwlist, &bulletFileName, &physicsClientId)) { return NULL; } sm = getPhysicsClient(physicsClientId); - if (sm == 0) + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - command = b3SaveBulletCommandInit(sm, bulletFileName); statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); statusType = b3GetStatusType(statusHandle); @@ -544,8 +547,6 @@ static PyObject* pybullet_saveBullet(PyObject* self, PyObject* args, PyObject* k return Py_None; } - - static PyObject* pybullet_loadMJCF(PyObject* self, PyObject* args, PyObject* keywds) { const char* mjcfFileName = ""; @@ -558,19 +559,18 @@ static PyObject* pybullet_loadMJCF(PyObject* self, PyObject* args, PyObject* key int bodyIndicesOut[MAX_SDF_BODIES]; PyObject* pylist = 0; int physicsClientId = 0; - static char *kwlist[] = { "mjcfFileName","physicsClientId", NULL }; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|i", kwlist, &mjcfFileName,&physicsClientId)) + static char* kwlist[] = {"mjcfFileName", "physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|i", kwlist, &mjcfFileName, &physicsClientId)) { return NULL; } sm = getPhysicsClient(physicsClientId); - if (sm == 0) + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - command = b3LoadMJCFCommandInit(sm, mjcfFileName); statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); statusType = b3GetStatusType(statusHandle); @@ -580,24 +580,27 @@ static PyObject* pybullet_loadMJCF(PyObject* self, PyObject* args, PyObject* key return NULL; } - numBodies = - b3GetStatusBodyIndices(statusHandle, bodyIndicesOut, MAX_SDF_BODIES); - if (numBodies > MAX_SDF_BODIES) { - PyErr_SetString(SpamError, "SDF exceeds body capacity"); - return NULL; - } + numBodies = + b3GetStatusBodyIndices(statusHandle, bodyIndicesOut, MAX_SDF_BODIES); + if (numBodies > MAX_SDF_BODIES) + { + PyErr_SetString(SpamError, "SDF exceeds body capacity"); + return NULL; + } - pylist = PyTuple_New(numBodies); + pylist = PyTuple_New(numBodies); - if (numBodies > 0 && numBodies <= MAX_SDF_BODIES) { - for (i = 0; i < numBodies; i++) { - PyTuple_SetItem(pylist, i, PyInt_FromLong(bodyIndicesOut[i])); - } - } - return pylist; + if (numBodies > 0 && numBodies <= MAX_SDF_BODIES) + { + for (i = 0; i < numBodies; i++) + { + PyTuple_SetItem(pylist, i, PyInt_FromLong(bodyIndicesOut[i])); + } + } + return pylist; } -static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* args, PyObject *keywds) +static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* args, PyObject* keywds) { double fixedTimeStep = -1; int numSolverIterations = -1; @@ -606,20 +609,20 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar int numSubSteps = -1; int collisionFilterMode = -1; double contactBreakingThreshold = -1; - int maxNumCmdPer1ms = -1; + int maxNumCmdPer1ms = -2; b3PhysicsClientHandle sm = 0; int physicsClientId = 0; - static char *kwlist[] = { "fixedTimeStep", "numSolverIterations","useSplitImpulse","splitImpulsePenetrationThreshold", "numSubSteps","collisionFilterMode", "contactBreakingThreshold", "maxNumCmdPer1ms", "physicsClientId", NULL }; + static char* kwlist[] = {"fixedTimeStep", "numSolverIterations", "useSplitImpulse", "splitImpulsePenetrationThreshold", "numSubSteps", "collisionFilterMode", "contactBreakingThreshold", "maxNumCmdPer1ms", "physicsClientId", NULL}; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diidiidii", kwlist,&fixedTimeStep,&numSolverIterations,&useSplitImpulse,&splitImpulsePenetrationThreshold,&numSubSteps, + if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diidiidii", kwlist, &fixedTimeStep, &numSolverIterations, &useSplitImpulse, &splitImpulsePenetrationThreshold, &numSubSteps, &collisionFilterMode, &contactBreakingThreshold, &maxNumCmdPer1ms, &physicsClientId)) { return NULL; } sm = getPhysicsClient(physicsClientId); - if (sm == 0) + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; @@ -634,7 +637,7 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar b3PhysicsParamSetNumSolverIterations(command, numSolverIterations); } - if (collisionFilterMode>=0) + if (collisionFilterMode >= 0) { b3PhysicsParamSetCollisionFilterMode(command, collisionFilterMode); } @@ -648,19 +651,20 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar } if (useSplitImpulse >= 0) { - b3PhysicsParamSetUseSplitImpulse(command,useSplitImpulse); + b3PhysicsParamSetUseSplitImpulse(command, useSplitImpulse); } if (splitImpulsePenetrationThreshold >= 0) { b3PhysicsParamSetSplitImpulsePenetrationThreshold(command, splitImpulsePenetrationThreshold); } - if (contactBreakingThreshold>=0) + if (contactBreakingThreshold >= 0) { - b3PhysicsParamSetContactBreakingThreshold(command,contactBreakingThreshold); + b3PhysicsParamSetContactBreakingThreshold(command, contactBreakingThreshold); } - if (maxNumCmdPer1ms>=0) + //-1 is disables the maxNumCmdPer1ms feature, allow it + if (maxNumCmdPer1ms >= -1) { - b3PhysicsParamSetMaxNumCommandsPer1ms(command,maxNumCmdPer1ms); + b3PhysicsParamSetMaxNumCommandsPer1ms(command, maxNumCmdPer1ms); } //ret = b3PhysicsParamSetRealTimeSimulation(command, enableRealTimeSimulation); @@ -681,668 +685,701 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar return Py_None; } - // Load a robot from a URDF file (universal robot description format) // function can be called without arguments and will default // to position (0,0,1) with orientation(0,0,0,1) // els(x,y,z) or // loadURDF(pos_x, pos_y, pos_z, orn_x, orn_y, orn_z, orn_w) -static PyObject* pybullet_loadURDF(PyObject* self, PyObject* args, PyObject *keywds) +static PyObject* pybullet_loadURDF(PyObject* self, PyObject* args, PyObject* keywds) { - int physicsClientId = 0; - - static char *kwlist[] = { "fileName", "basePosition", "baseOrientation", "useMaximalCoordinates","useFixedBase","physicsClientId", NULL }; + int physicsClientId = 0; - static char *kwlistBackwardCompatible4[] = { "fileName", "startPosX", "startPosY", "startPosZ", NULL }; - static char *kwlistBackwardCompatible8[] = { "fileName", "startPosX", "startPosY", "startPosZ", "startOrnX", "startOrnY","startOrnZ","startOrnW", NULL }; + static char* kwlist[] = {"fileName", "basePosition", "baseOrientation", "useMaximalCoordinates", "useFixedBase", "physicsClientId", NULL}; + static char* kwlistBackwardCompatible4[] = {"fileName", "startPosX", "startPosY", "startPosZ", NULL}; + static char* kwlistBackwardCompatible8[] = {"fileName", "startPosX", "startPosY", "startPosZ", "startOrnX", "startOrnY", "startOrnZ", "startOrnW", NULL}; - int bodyIndex = -1; - const char* urdfFileName = ""; + int bodyIndex = -1; + const char* urdfFileName = ""; - double startPosX = 0.0; - double startPosY = 0.0; - double startPosZ = 0.0; - double startOrnX = 0.0; - double startOrnY = 0.0; - double startOrnZ = 0.0; - double startOrnW = 1.0; - int useMaximalCoordinates = 0; - int useFixedBase = 0; + double startPosX = 0.0; + double startPosY = 0.0; + double startPosZ = 0.0; + double startOrnX = 0.0; + double startOrnY = 0.0; + double startOrnZ = 0.0; + double startOrnW = 1.0; + int useMaximalCoordinates = 0; + int useFixedBase = 0; - int backwardsCompatibilityArgs = 0; -b3PhysicsClientHandle sm=0; - if (PyArg_ParseTupleAndKeywords(args, keywds, "sddd", kwlistBackwardCompatible4, &urdfFileName, &startPosX, - &startPosY, &startPosZ)) - { - backwardsCompatibilityArgs = 1; - } - else - { - PyErr_Clear(); - } + int backwardsCompatibilityArgs = 0; + b3PhysicsClientHandle sm = 0; + if (PyArg_ParseTupleAndKeywords(args, keywds, "sddd", kwlistBackwardCompatible4, &urdfFileName, &startPosX, + &startPosY, &startPosZ)) + { + backwardsCompatibilityArgs = 1; + } + else + { + PyErr_Clear(); + } + if (PyArg_ParseTupleAndKeywords(args, keywds, "sddddddd", kwlistBackwardCompatible8, &urdfFileName, &startPosX, + &startPosY, &startPosZ, &startOrnX, &startOrnY, &startOrnZ, &startOrnW)) + { + backwardsCompatibilityArgs = 1; + } + else + { + PyErr_Clear(); + } - if (PyArg_ParseTupleAndKeywords(args, keywds, "sddddddd", kwlistBackwardCompatible8,&urdfFileName, &startPosX, - &startPosY, &startPosZ, &startOrnX, &startOrnY,&startOrnZ, &startOrnW)) - { - backwardsCompatibilityArgs = 1; - } - else - { - PyErr_Clear(); - } + if (!backwardsCompatibilityArgs) + { + PyObject* basePosObj = 0; + PyObject* baseOrnObj = 0; + double basePos[3]; + double baseOrn[4]; - - - if (!backwardsCompatibilityArgs) - { - PyObject* basePosObj = 0; - PyObject* baseOrnObj = 0; - double basePos[3]; - double baseOrn[4]; - - - if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|OOiii", kwlist, &urdfFileName, &basePosObj, &baseOrnObj, &useMaximalCoordinates,&useFixedBase,&physicsClientId)) - { - - return NULL; - } - else - { - if (basePosObj) - { - if (!pybullet_internalSetVectord(basePosObj, basePos)) - { - PyErr_SetString(SpamError, "Cannot convert basePosition."); - return NULL; - } - startPosX = basePos[0]; - startPosY = basePos[1]; - startPosZ = basePos[2]; - - } - if (baseOrnObj) - { - if (!pybullet_internalSetVector4d(baseOrnObj, baseOrn)) - { - PyErr_SetString(SpamError, "Cannot convert baseOrientation."); - return NULL; - } - startOrnX = baseOrn[0]; - startOrnY = baseOrn[1]; - startOrnZ = baseOrn[2]; - startOrnW = baseOrn[3]; - } - } - } + if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|OOiii", kwlist, &urdfFileName, &basePosObj, &baseOrnObj, &useMaximalCoordinates, &useFixedBase, &physicsClientId)) + { + return NULL; + } + else + { + if (basePosObj) + { + if (!pybullet_internalSetVectord(basePosObj, basePos)) + { + PyErr_SetString(SpamError, "Cannot convert basePosition."); + return NULL; + } + startPosX = basePos[0]; + startPosY = basePos[1]; + startPosZ = basePos[2]; + } + if (baseOrnObj) + { + if (!pybullet_internalSetVector4d(baseOrnObj, baseOrn)) + { + PyErr_SetString(SpamError, "Cannot convert baseOrientation."); + return NULL; + } + startOrnX = baseOrn[0]; + startOrnY = baseOrn[1]; + startOrnZ = baseOrn[2]; + startOrnW = baseOrn[3]; + } + } + } sm = getPhysicsClient(physicsClientId); - if (sm == 0) + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - if (strlen(urdfFileName)) { - // printf("(%f, %f, %f) (%f, %f, %f, %f)\n", - // startPosX,startPosY,startPosZ,startOrnX, startOrnY,startOrnZ, startOrnW); - - b3SharedMemoryStatusHandle statusHandle; - int statusType; - b3SharedMemoryCommandHandle command = - b3LoadUrdfCommandInit(sm, urdfFileName); - - // setting the initial position, orientation and other arguments are - // optional - b3LoadUrdfCommandSetStartPosition(command, startPosX, startPosY, startPosZ); - b3LoadUrdfCommandSetStartOrientation(command, startOrnX, startOrnY, - startOrnZ, startOrnW); - if (useMaximalCoordinates) + if (strlen(urdfFileName)) { - b3LoadUrdfCommandSetUseMultiBody(command, 0); - } - if (useFixedBase) - { - b3LoadUrdfCommandSetUseFixedBase(command, 1); - } + // printf("(%f, %f, %f) (%f, %f, %f, %f)\n", + // startPosX,startPosY,startPosZ,startOrnX, startOrnY,startOrnZ, startOrnW); - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); - statusType = b3GetStatusType(statusHandle); - if (statusType != CMD_URDF_LOADING_COMPLETED) { - PyErr_SetString(SpamError, "Cannot load URDF file."); - return NULL; - } - bodyIndex = b3GetStatusBodyIndex(statusHandle); - } else { - PyErr_SetString(SpamError, - "Empty filename, method expects 1, 4 or 8 arguments."); - return NULL; - } - return PyLong_FromLong(bodyIndex); + b3SharedMemoryStatusHandle statusHandle; + int statusType; + b3SharedMemoryCommandHandle command = + b3LoadUrdfCommandInit(sm, urdfFileName); + + // setting the initial position, orientation and other arguments are + // optional + b3LoadUrdfCommandSetStartPosition(command, startPosX, startPosY, startPosZ); + b3LoadUrdfCommandSetStartOrientation(command, startOrnX, startOrnY, + startOrnZ, startOrnW); + if (useMaximalCoordinates) + { + b3LoadUrdfCommandSetUseMultiBody(command, 0); + } + if (useFixedBase) + { + b3LoadUrdfCommandSetUseFixedBase(command, 1); + } + + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + statusType = b3GetStatusType(statusHandle); + if (statusType != CMD_URDF_LOADING_COMPLETED) + { + PyErr_SetString(SpamError, "Cannot load URDF file."); + return NULL; + } + bodyIndex = b3GetStatusBodyIndex(statusHandle); + } + else + { + PyErr_SetString(SpamError, + "Empty filename, method expects 1, 4 or 8 arguments."); + return NULL; + } + return PyLong_FromLong(bodyIndex); } +static PyObject* pybullet_loadSDF(PyObject* self, PyObject* args, PyObject* keywds) +{ + const char* sdfFileName = ""; + int numBodies = 0; + int i; + int bodyIndicesOut[MAX_SDF_BODIES]; + PyObject* pylist = 0; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + b3SharedMemoryCommandHandle commandHandle; + b3PhysicsClientHandle sm = 0; - - -static PyObject* pybullet_loadSDF(PyObject* self, PyObject* args, PyObject *keywds) { - const char* sdfFileName = ""; - int numBodies = 0; - int i; - int bodyIndicesOut[MAX_SDF_BODIES]; - PyObject* pylist = 0; - b3SharedMemoryStatusHandle statusHandle; - int statusType; - b3SharedMemoryCommandHandle commandHandle; -b3PhysicsClientHandle sm = 0; - - int physicsClientId = 0; - static char *kwlist[] = { "sdfFileName","physicsClientId", NULL }; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|i", kwlist, &sdfFileName,&physicsClientId)) + int physicsClientId = 0; + static char* kwlist[] = {"sdfFileName", "physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|i", kwlist, &sdfFileName, &physicsClientId)) { return NULL; } sm = getPhysicsClient(physicsClientId); - if (sm == 0) + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - - commandHandle = b3LoadSdfCommandInit(sm, sdfFileName); - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); - statusType = b3GetStatusType(statusHandle); - if (statusType != CMD_SDF_LOADING_COMPLETED) { - PyErr_SetString(SpamError, "Cannot load SDF file."); - return NULL; - } + commandHandle = b3LoadSdfCommandInit(sm, sdfFileName); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + statusType = b3GetStatusType(statusHandle); + if (statusType != CMD_SDF_LOADING_COMPLETED) + { + PyErr_SetString(SpamError, "Cannot load SDF file."); + return NULL; + } - numBodies = - b3GetStatusBodyIndices(statusHandle, bodyIndicesOut, MAX_SDF_BODIES); - if (numBodies > MAX_SDF_BODIES) { - PyErr_SetString(SpamError, "SDF exceeds body capacity"); - return NULL; - } + numBodies = + b3GetStatusBodyIndices(statusHandle, bodyIndicesOut, MAX_SDF_BODIES); + if (numBodies > MAX_SDF_BODIES) + { + PyErr_SetString(SpamError, "SDF exceeds body capacity"); + return NULL; + } - pylist = PyTuple_New(numBodies); + pylist = PyTuple_New(numBodies); - if (numBodies > 0 && numBodies <= MAX_SDF_BODIES) { - for (i = 0; i < numBodies; i++) { - PyTuple_SetItem(pylist, i, PyInt_FromLong(bodyIndicesOut[i])); - } - } - return pylist; + if (numBodies > 0 && numBodies <= MAX_SDF_BODIES) + { + for (i = 0; i < numBodies; i++) + { + PyTuple_SetItem(pylist, i, PyInt_FromLong(bodyIndicesOut[i])); + } + } + return pylist; } // Reset the simulation to remove all loaded objects -static PyObject* pybullet_resetSimulation(PyObject* self, PyObject* args, PyObject* keywds) { - - int physicsClientId = 0; - static char *kwlist[] = { "physicsClientId", NULL }; +static PyObject* pybullet_resetSimulation(PyObject* self, PyObject* args, PyObject* keywds) +{ + int physicsClientId = 0; + static char* kwlist[] = {"physicsClientId", NULL}; b3PhysicsClientHandle sm = 0; - - if (!PyArg_ParseTupleAndKeywords(args, keywds, "|i", kwlist,&physicsClientId)) + + if (!PyArg_ParseTupleAndKeywords(args, keywds, "|i", kwlist, &physicsClientId)) { return NULL; } sm = getPhysicsClient(physicsClientId); - if (sm == 0) + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - { - b3SharedMemoryStatusHandle statusHandle; - statusHandle = b3SubmitClientCommandAndWaitStatus( - sm, b3InitResetSimulationCommand(sm)); - } - Py_INCREF(Py_None); - return Py_None; + { + b3SharedMemoryStatusHandle statusHandle; + statusHandle = b3SubmitClientCommandAndWaitStatus( + sm, b3InitResetSimulationCommand(sm)); + } + Py_INCREF(Py_None); + return Py_None; } //this method is obsolete, use pybullet_setJointMotorControl2 instead -static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args) { - int size; - int bodyIndex, jointIndex, controlMode; +static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args) +{ + int size; + int bodyIndex, jointIndex, controlMode; - double targetPosition = 0.0; - double targetVelocity = 0.0; - double maxForce = 100000.0; - double appliedForce = 0.0; - double kp = 0.1; - double kd = 1.0; - int valid = 0; - int physicsClientId = 0; - b3PhysicsClientHandle sm; + double targetPosition = 0.0; + double targetVelocity = 0.0; + double maxForce = 100000.0; + double appliedForce = 0.0; + double kp = 0.1; + double kd = 1.0; + int valid = 0; + int physicsClientId = 0; + b3PhysicsClientHandle sm; sm = getPhysicsClient(physicsClientId); - if (sm == 0) + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - size = PySequence_Size(args); - if (size == 4) { - double targetValue = 0.0; - // see switch statement below for convertsions dependent on controlMode - if (!PyArg_ParseTuple(args, "iiid", &bodyIndex, &jointIndex, &controlMode, - &targetValue)) { - PyErr_SetString(SpamError, "Error parsing arguments"); - return NULL; - } - valid = 1; - switch (controlMode) { - case CONTROL_MODE_POSITION_VELOCITY_PD: { - targetPosition = targetValue; - break; - } - case CONTROL_MODE_VELOCITY: { - targetVelocity = targetValue; - break; - } - case CONTROL_MODE_TORQUE: { - appliedForce = targetValue; - break; - } - default: { valid = 0; } - } - } - if (size == 5) { - double targetValue = 0.0; - // See switch statement for conversions - if (!PyArg_ParseTuple(args, "iiidd", &bodyIndex, &jointIndex, &controlMode, - &targetValue, &maxForce)) { - PyErr_SetString(SpamError, "Error parsing arguments"); - return NULL; - } - valid = 1; + size = PySequence_Size(args); + if (size == 4) + { + double targetValue = 0.0; + // see switch statement below for convertsions dependent on controlMode + if (!PyArg_ParseTuple(args, "iiid", &bodyIndex, &jointIndex, &controlMode, + &targetValue)) + { + PyErr_SetString(SpamError, "Error parsing arguments"); + return NULL; + } + valid = 1; + switch (controlMode) + { + case CONTROL_MODE_POSITION_VELOCITY_PD: + { + targetPosition = targetValue; + break; + } + case CONTROL_MODE_VELOCITY: + { + targetVelocity = targetValue; + break; + } + case CONTROL_MODE_TORQUE: + { + appliedForce = targetValue; + break; + } + default: + { + valid = 0; + } + } + } + if (size == 5) + { + double targetValue = 0.0; + // See switch statement for conversions + if (!PyArg_ParseTuple(args, "iiidd", &bodyIndex, &jointIndex, &controlMode, + &targetValue, &maxForce)) + { + PyErr_SetString(SpamError, "Error parsing arguments"); + return NULL; + } + valid = 1; - switch (controlMode) { - case CONTROL_MODE_POSITION_VELOCITY_PD: { - targetPosition = targetValue; - break; - } - case CONTROL_MODE_VELOCITY: { - targetVelocity = targetValue; - break; - } - case CONTROL_MODE_TORQUE: { - valid = 0; - break; - } - default: { valid = 0; } - } - } - if (size == 6) { - double gain = 0.0; - double targetValue = 0.0; - if (!PyArg_ParseTuple(args, "iiiddd", &bodyIndex, &jointIndex, &controlMode, - &targetValue, &maxForce, &gain)) { - PyErr_SetString(SpamError, "Error parsing arguments"); - return NULL; - } - valid = 1; + switch (controlMode) + { + case CONTROL_MODE_POSITION_VELOCITY_PD: + { + targetPosition = targetValue; + break; + } + case CONTROL_MODE_VELOCITY: + { + targetVelocity = targetValue; + break; + } + case CONTROL_MODE_TORQUE: + { + valid = 0; + break; + } + default: + { + valid = 0; + } + } + } + if (size == 6) + { + double gain = 0.0; + double targetValue = 0.0; + if (!PyArg_ParseTuple(args, "iiiddd", &bodyIndex, &jointIndex, &controlMode, + &targetValue, &maxForce, &gain)) + { + PyErr_SetString(SpamError, "Error parsing arguments"); + return NULL; + } + valid = 1; - switch (controlMode) { - case CONTROL_MODE_POSITION_VELOCITY_PD: { - targetPosition = targetValue; - kp = gain; - break; - } - case CONTROL_MODE_VELOCITY: { - targetVelocity = targetValue; - kd = gain; - break; - } - case CONTROL_MODE_TORQUE: { - valid = 0; - break; - } - default: { valid = 0; } - } - } - if (size == 8) { - // only applicable for CONTROL_MODE_POSITION_VELOCITY_PD. - if (!PyArg_ParseTuple(args, "iiiddddd", &bodyIndex, &jointIndex, - &controlMode, &targetPosition, &targetVelocity, - &maxForce, &kp, &kd)) { - PyErr_SetString(SpamError, "Error parsing arguments"); - return NULL; - } - valid = 1; - } + switch (controlMode) + { + case CONTROL_MODE_POSITION_VELOCITY_PD: + { + targetPosition = targetValue; + kp = gain; + break; + } + case CONTROL_MODE_VELOCITY: + { + targetVelocity = targetValue; + kd = gain; + break; + } + case CONTROL_MODE_TORQUE: + { + valid = 0; + break; + } + default: + { + valid = 0; + } + } + } + if (size == 8) + { + // only applicable for CONTROL_MODE_POSITION_VELOCITY_PD. + if (!PyArg_ParseTuple(args, "iiiddddd", &bodyIndex, &jointIndex, + &controlMode, &targetPosition, &targetVelocity, + &maxForce, &kp, &kd)) + { + PyErr_SetString(SpamError, "Error parsing arguments"); + return NULL; + } + valid = 1; + } - if (valid) { - int numJoints; - b3SharedMemoryCommandHandle commandHandle; - b3SharedMemoryStatusHandle statusHandle; - struct b3JointInfo info; + if (valid) + { + int numJoints; + b3SharedMemoryCommandHandle commandHandle; + b3SharedMemoryStatusHandle statusHandle; + struct b3JointInfo info; - numJoints = b3GetNumJoints(sm, bodyIndex); - if ((jointIndex >= numJoints) || (jointIndex < 0)) { - PyErr_SetString(SpamError, "Joint index out-of-range."); - return NULL; - } + numJoints = b3GetNumJoints(sm, bodyIndex); + if ((jointIndex >= numJoints) || (jointIndex < 0)) + { + PyErr_SetString(SpamError, "Joint index out-of-range."); + return NULL; + } - if ((controlMode != CONTROL_MODE_VELOCITY) && - (controlMode != CONTROL_MODE_TORQUE) && - (controlMode != CONTROL_MODE_POSITION_VELOCITY_PD)) { - PyErr_SetString(SpamError, "Illegral control mode."); - return NULL; - } + if ((controlMode != CONTROL_MODE_VELOCITY) && + (controlMode != CONTROL_MODE_TORQUE) && + (controlMode != CONTROL_MODE_POSITION_VELOCITY_PD)) + { + PyErr_SetString(SpamError, "Illegral control mode."); + return NULL; + } - commandHandle = b3JointControlCommandInit2(sm, bodyIndex, controlMode); + commandHandle = b3JointControlCommandInit2(sm, bodyIndex, controlMode); - b3GetJointInfo(sm, bodyIndex, jointIndex, &info); + b3GetJointInfo(sm, bodyIndex, jointIndex, &info); - switch (controlMode) { - case CONTROL_MODE_VELOCITY: { - b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex, - targetVelocity); - b3JointControlSetKd(commandHandle, info.m_uIndex, kd); - b3JointControlSetMaximumForce(commandHandle, info.m_uIndex, maxForce); - break; - } + switch (controlMode) + { + case CONTROL_MODE_VELOCITY: + { + b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex, + targetVelocity); + b3JointControlSetKd(commandHandle, info.m_uIndex, kd); + b3JointControlSetMaximumForce(commandHandle, info.m_uIndex, maxForce); + break; + } - case CONTROL_MODE_TORQUE: { - b3JointControlSetDesiredForceTorque(commandHandle, info.m_uIndex, - appliedForce); - break; - } + case CONTROL_MODE_TORQUE: + { + b3JointControlSetDesiredForceTorque(commandHandle, info.m_uIndex, + appliedForce); + break; + } - case CONTROL_MODE_POSITION_VELOCITY_PD: { - b3JointControlSetDesiredPosition(commandHandle, info.m_qIndex, - targetPosition); - b3JointControlSetKp(commandHandle, info.m_uIndex, kp); - b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex, - targetVelocity); - b3JointControlSetKd(commandHandle, info.m_uIndex, kd); - b3JointControlSetMaximumForce(commandHandle, info.m_uIndex, maxForce); - break; - } - default: {} - }; + case CONTROL_MODE_POSITION_VELOCITY_PD: + { + b3JointControlSetDesiredPosition(commandHandle, info.m_qIndex, + targetPosition); + b3JointControlSetKp(commandHandle, info.m_uIndex, kp); + b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex, + targetVelocity); + b3JointControlSetKd(commandHandle, info.m_uIndex, kd); + b3JointControlSetMaximumForce(commandHandle, info.m_uIndex, maxForce); + break; + } + default: + { + } + }; - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); - Py_INCREF(Py_None); - return Py_None; - } - PyErr_SetString(SpamError, "Error parsing arguments in setJointControl."); - return NULL; + Py_INCREF(Py_None); + return Py_None; + } + PyErr_SetString(SpamError, "Error parsing arguments in setJointControl."); + return NULL; } - -static PyObject* pybullet_setJointMotorControl2(PyObject* self, PyObject* args, PyObject* keywds) +static PyObject* pybullet_setJointMotorControl2(PyObject* self, PyObject* args, PyObject* keywds) { - int bodyIndex, jointIndex, controlMode; + int bodyIndex, jointIndex, controlMode; - double targetPosition = 0.0; - double targetVelocity = 0.0; - double force = 100000.0; - double kp = 0.1; - double kd = 1.0; -b3PhysicsClientHandle sm = 0; + double targetPosition = 0.0; + double targetVelocity = 0.0; + double force = 100000.0; + double kp = 0.1; + double kd = 1.0; + b3PhysicsClientHandle sm = 0; - int physicsClientId = 0; - static char *kwlist[] = { "bodyIndex", "jointIndex", "controlMode", "targetPosition", "targetVelocity" - , "force", "positionGain", "velocityGain", "physicsClientId", NULL }; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "iii|dddddi", kwlist,&bodyIndex, &jointIndex, &controlMode, - &targetPosition, &targetVelocity,&force, &kp, &kd, &physicsClientId)) + int physicsClientId = 0; + static char* kwlist[] = {"bodyIndex", "jointIndex", "controlMode", "targetPosition", "targetVelocity", "force", "positionGain", "velocityGain", "physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "iii|dddddi", kwlist, &bodyIndex, &jointIndex, &controlMode, + &targetPosition, &targetVelocity, &force, &kp, &kd, &physicsClientId)) { return NULL; } sm = getPhysicsClient(physicsClientId); - if (sm == 0) + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - { - int numJoints; - b3SharedMemoryCommandHandle commandHandle; - b3SharedMemoryStatusHandle statusHandle; - struct b3JointInfo info; + { + int numJoints; + b3SharedMemoryCommandHandle commandHandle; + b3SharedMemoryStatusHandle statusHandle; + struct b3JointInfo info; - numJoints = b3GetNumJoints(sm, bodyIndex); - if ((jointIndex >= numJoints) || (jointIndex < 0)) { - PyErr_SetString(SpamError, "Joint index out-of-range."); - return NULL; - } + numJoints = b3GetNumJoints(sm, bodyIndex); + if ((jointIndex >= numJoints) || (jointIndex < 0)) + { + PyErr_SetString(SpamError, "Joint index out-of-range."); + return NULL; + } - if ((controlMode != CONTROL_MODE_VELOCITY) && - (controlMode != CONTROL_MODE_TORQUE) && - (controlMode != CONTROL_MODE_POSITION_VELOCITY_PD)) { - PyErr_SetString(SpamError, "Illegral control mode."); - return NULL; - } + if ((controlMode != CONTROL_MODE_VELOCITY) && + (controlMode != CONTROL_MODE_TORQUE) && + (controlMode != CONTROL_MODE_POSITION_VELOCITY_PD)) + { + PyErr_SetString(SpamError, "Illegral control mode."); + return NULL; + } - commandHandle = b3JointControlCommandInit2(sm, bodyIndex, controlMode); + commandHandle = b3JointControlCommandInit2(sm, bodyIndex, controlMode); - b3GetJointInfo(sm, bodyIndex, jointIndex, &info); + b3GetJointInfo(sm, bodyIndex, jointIndex, &info); - switch (controlMode) { - case CONTROL_MODE_VELOCITY: { - b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex, - targetVelocity); - b3JointControlSetKd(commandHandle, info.m_uIndex, kd); - b3JointControlSetMaximumForce(commandHandle, info.m_uIndex, force); - break; - } + switch (controlMode) + { + case CONTROL_MODE_VELOCITY: + { + b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex, + targetVelocity); + b3JointControlSetKd(commandHandle, info.m_uIndex, kd); + b3JointControlSetMaximumForce(commandHandle, info.m_uIndex, force); + break; + } - case CONTROL_MODE_TORQUE: { - b3JointControlSetDesiredForceTorque(commandHandle, info.m_uIndex, - force); - break; - } + case CONTROL_MODE_TORQUE: + { + b3JointControlSetDesiredForceTorque(commandHandle, info.m_uIndex, + force); + break; + } - case CONTROL_MODE_POSITION_VELOCITY_PD: { - b3JointControlSetDesiredPosition(commandHandle, info.m_qIndex, - targetPosition); - b3JointControlSetKp(commandHandle, info.m_uIndex, kp); - b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex, - targetVelocity); - b3JointControlSetKd(commandHandle, info.m_uIndex, kd); - b3JointControlSetMaximumForce(commandHandle, info.m_uIndex, force); - break; - } - default: {} - }; + case CONTROL_MODE_POSITION_VELOCITY_PD: + { + b3JointControlSetDesiredPosition(commandHandle, info.m_qIndex, + targetPosition); + b3JointControlSetKp(commandHandle, info.m_uIndex, kp); + b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex, + targetVelocity); + b3JointControlSetKd(commandHandle, info.m_uIndex, kd); + b3JointControlSetMaximumForce(commandHandle, info.m_uIndex, force); + break; + } + default: + { + } + }; - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); - Py_INCREF(Py_None); - return Py_None; - } -// PyErr_SetString(SpamError, "Error parsing arguments in setJointControl."); -// return NULL; + Py_INCREF(Py_None); + return Py_None; + } + // PyErr_SetString(SpamError, "Error parsing arguments in setJointControl."); + // return NULL; } static PyObject* pybullet_setRealTimeSimulation(PyObject* self, - PyObject* args, - PyObject* keywds) { - - int enableRealTimeSimulation = 0; - int ret; + PyObject* args, + PyObject* keywds) +{ + int enableRealTimeSimulation = 0; + int ret; b3PhysicsClientHandle sm = 0; - int physicsClientId = 0; - static char *kwlist[] = { "enableRealTimeSimulation", "physicsClientId", NULL }; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist,&enableRealTimeSimulation, &physicsClientId)) + int physicsClientId = 0; + static char* kwlist[] = {"enableRealTimeSimulation", "physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist, &enableRealTimeSimulation, &physicsClientId)) { return NULL; } sm = getPhysicsClient(physicsClientId); - if (sm == 0) + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - - { - + { + b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm); + b3SharedMemoryStatusHandle statusHandle; - b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm); - b3SharedMemoryStatusHandle statusHandle; + ret = + b3PhysicsParamSetRealTimeSimulation(command, enableRealTimeSimulation); - ret = - b3PhysicsParamSetRealTimeSimulation(command, enableRealTimeSimulation); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + // ASSERT_EQ(b3GetStatusType(statusHandle), CMD_CLIENT_COMMAND_COMPLETED); + } - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); - // ASSERT_EQ(b3GetStatusType(statusHandle), CMD_CLIENT_COMMAND_COMPLETED); - } - - Py_INCREF(Py_None); - return Py_None; + Py_INCREF(Py_None); + return Py_None; } - - static PyObject* pybullet_setInternalSimFlags(PyObject* self, - PyObject* args, PyObject* keywds) { - - int flags = 0; - int ret; - int physicsClientId = 0; -b3PhysicsClientHandle sm = 0; + PyObject* args, PyObject* keywds) +{ + int flags = 0; + int ret; + int physicsClientId = 0; + b3PhysicsClientHandle sm = 0; - static char *kwlist[] = { "flags", "physicsClientId", NULL }; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist,&flags, &physicsClientId)) + static char* kwlist[] = {"flags", "physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist, &flags, &physicsClientId)) { return NULL; } sm = getPhysicsClient(physicsClientId); - if (sm == 0) + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - - { - - b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm); - b3SharedMemoryStatusHandle statusHandle; + { + b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm); + b3SharedMemoryStatusHandle statusHandle; - ret = - b3PhysicsParamSetInternalSimFlags(command, flags); + ret = + b3PhysicsParamSetInternalSimFlags(command, flags); - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); - // ASSERT_EQ(b3GetStatusType(statusHandle), CMD_CLIENT_COMMAND_COMPLETED); - } + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + // ASSERT_EQ(b3GetStatusType(statusHandle), CMD_CLIENT_COMMAND_COMPLETED); + } - Py_INCREF(Py_None); - return Py_None; + Py_INCREF(Py_None); + return Py_None; } // Set the gravity of the world with (x, y, z) arguments -static PyObject* pybullet_setGravity(PyObject* self, PyObject* args, PyObject* keywds) { - - { - double gravX = 0.0; - double gravY = 0.0; - double gravZ = -10.0; - int ret; - b3PhysicsClientHandle sm = 0; - b3SharedMemoryCommandHandle command; - b3SharedMemoryStatusHandle statusHandle; - - int physicsClientId = 0; - static char *kwlist[] = { "gravX", "gravY", "gravZ", "physicsClientId", NULL }; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "ddd|i", kwlist,&gravX, &gravY, &gravZ, &physicsClientId)) +static PyObject* pybullet_setGravity(PyObject* self, PyObject* args, PyObject* keywds) +{ { - return NULL; - } - sm = getPhysicsClient(physicsClientId); - if (sm == 0) - { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } - + double gravX = 0.0; + double gravY = 0.0; + double gravZ = -10.0; + int ret; + b3PhysicsClientHandle sm = 0; + b3SharedMemoryCommandHandle command; + b3SharedMemoryStatusHandle statusHandle; - command = b3InitPhysicsParamCommand(sm); - - ret = b3PhysicsParamSetGravity(command, gravX, gravY, gravZ); - // ret = b3PhysicsParamSetTimeStep(command, timeStep); - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); - // ASSERT_EQ(b3GetStatusType(statusHandle), CMD_CLIENT_COMMAND_COMPLETED); - } + int physicsClientId = 0; + static char* kwlist[] = {"gravX", "gravY", "gravZ", "physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "ddd|i", kwlist, &gravX, &gravY, &gravZ, &physicsClientId)) + { + return NULL; + } + sm = getPhysicsClient(physicsClientId); + if (sm == 0) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } - Py_INCREF(Py_None); - return Py_None; + command = b3InitPhysicsParamCommand(sm); + + ret = b3PhysicsParamSetGravity(command, gravX, gravY, gravZ); + // ret = b3PhysicsParamSetTimeStep(command, timeStep); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + // ASSERT_EQ(b3GetStatusType(statusHandle), CMD_CLIENT_COMMAND_COMPLETED); + } + + Py_INCREF(Py_None); + return Py_None; } -static PyObject* pybullet_setTimeStep(PyObject* self, PyObject* args, PyObject* keywds) +static PyObject* pybullet_setTimeStep(PyObject* self, PyObject* args, PyObject* keywds) { double timeStep = 0.001; int ret; int physicsClientId = 0; b3PhysicsClientHandle sm = 0; - static char *kwlist[] = { "timeStep", "physicsClientId", NULL }; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "d|i", kwlist,&timeStep, &physicsClientId)) + static char* kwlist[] = {"timeStep", "physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "d|i", kwlist, &timeStep, &physicsClientId)) { return NULL; } sm = getPhysicsClient(physicsClientId); - if (sm == 0) + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } { + b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm); + b3SharedMemoryStatusHandle statusHandle; - b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm); - b3SharedMemoryStatusHandle statusHandle; + ret = b3PhysicsParamSetTimeStep(command, timeStep); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); - ret = b3PhysicsParamSetTimeStep(command, timeStep); - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); - + Py_INCREF(Py_None); + return Py_None; + } +} + +static PyObject* +pybullet_setDefaultContactERP(PyObject* self, PyObject* args, PyObject* keywds) +{ + double defaultContactERP = 0.005; + int physicsClientId = 0; + b3PhysicsClientHandle sm = 0; + + static char* kwlist[] = {"defaultContactERP", "physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "d|i", kwlist, &defaultContactERP, &physicsClientId)) + { + return NULL; + } + sm = getPhysicsClient(physicsClientId); + if (sm == 0) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + { + int ret; + + b3SharedMemoryStatusHandle statusHandle; + + b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm); + ret = b3PhysicsParamSetDefaultContactERP(command, defaultContactERP); + + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + } Py_INCREF(Py_None); return Py_None; } -} - -static PyObject * -pybullet_setDefaultContactERP(PyObject* self, PyObject* args,PyObject* keywds) -{ - double defaultContactERP=0.005; - int physicsClientId = 0; - b3PhysicsClientHandle sm = 0; - - static char *kwlist[] = { "defaultContactERP", "physicsClientId", NULL }; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "d|i", kwlist,&defaultContactERP, &physicsClientId)) - { - return NULL; - } - sm = getPhysicsClient(physicsClientId); - if (sm == 0) - { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } - { - int ret; - - b3SharedMemoryStatusHandle statusHandle; - - b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm); - ret = b3PhysicsParamSetDefaultContactERP(command, defaultContactERP); - - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); - } - - Py_INCREF(Py_None); - return Py_None; -} static int pybullet_internalGetBaseVelocity( - int bodyIndex, double baseLinearVelocity[3], double baseAngularVelocity[3],b3PhysicsClientHandle sm) { + int bodyIndex, double baseLinearVelocity[3], double baseAngularVelocity[3], b3PhysicsClientHandle sm) +{ baseLinearVelocity[0] = 0.; baseLinearVelocity[1] = 0.; baseLinearVelocity[2] = 0.; @@ -1351,7 +1388,8 @@ static int pybullet_internalGetBaseVelocity( baseAngularVelocity[1] = 0.; baseAngularVelocity[2] = 0.; - if (0 == sm) { + if (0 == sm) + { PyErr_SetString(SpamError, "Not connected to physics server."); return 0; } @@ -1367,8 +1405,8 @@ static int pybullet_internalGetBaseVelocity( const double* actualStateQdot; // const double* jointReactionForces[]; - - if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED) { + if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED) + { PyErr_SetString(SpamError, "getBaseVelocity failed."); return 0; } @@ -1393,7 +1431,6 @@ static int pybullet_internalGetBaseVelocity( baseAngularVelocity[0] = actualStateQdot[3]; baseAngularVelocity[1] = actualStateQdot[4]; baseAngularVelocity[2] = actualStateQdot[5]; - } } return 1; @@ -1402,62 +1439,64 @@ static int pybullet_internalGetBaseVelocity( // Internal function used to get the base position and orientation // Orientation is returned in quaternions static int pybullet_internalGetBasePositionAndOrientation( - int bodyIndex, double basePosition[3], double baseOrientation[4],b3PhysicsClientHandle sm) { - basePosition[0] = 0.; - basePosition[1] = 0.; - basePosition[2] = 0.; + int bodyIndex, double basePosition[3], double baseOrientation[4], b3PhysicsClientHandle sm) +{ + basePosition[0] = 0.; + basePosition[1] = 0.; + basePosition[2] = 0.; - baseOrientation[0] = 0.; - baseOrientation[1] = 0.; - baseOrientation[2] = 0.; - baseOrientation[3] = 1.; + baseOrientation[0] = 0.; + baseOrientation[1] = 0.; + baseOrientation[2] = 0.; + baseOrientation[3] = 1.; - if (0 == sm) { - PyErr_SetString(SpamError, "Not connected to physics server."); - return 0; - } + if (0 == sm) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return 0; + } - { - { - b3SharedMemoryCommandHandle cmd_handle = - b3RequestActualStateCommandInit(sm, bodyIndex); - b3SharedMemoryStatusHandle status_handle = - b3SubmitClientCommandAndWaitStatus(sm, cmd_handle); + { + { + b3SharedMemoryCommandHandle cmd_handle = + b3RequestActualStateCommandInit(sm, bodyIndex); + b3SharedMemoryStatusHandle status_handle = + b3SubmitClientCommandAndWaitStatus(sm, cmd_handle); - const int status_type = b3GetStatusType(status_handle); - const double* actualStateQ; - // const double* jointReactionForces[]; - + const int status_type = b3GetStatusType(status_handle); + const double* actualStateQ; + // const double* jointReactionForces[]; - if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED) { - PyErr_SetString(SpamError, "getBasePositionAndOrientation failed."); - return 0; - } + if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED) + { + PyErr_SetString(SpamError, "getBasePositionAndOrientation failed."); + return 0; + } - b3GetStatusActualState( - status_handle, 0 /* body_unique_id */, - 0 /* num_degree_of_freedom_q */, 0 /* num_degree_of_freedom_u */, - 0 /*root_local_inertial_frame*/, &actualStateQ, - 0 /* actual_state_q_dot */, 0 /* joint_reaction_forces */); + b3GetStatusActualState( + status_handle, 0 /* body_unique_id */, + 0 /* num_degree_of_freedom_q */, 0 /* num_degree_of_freedom_u */, + 0 /*root_local_inertial_frame*/, &actualStateQ, + 0 /* actual_state_q_dot */, 0 /* joint_reaction_forces */); - // printf("joint reaction forces="); - // for (i=0; i < (sizeof(jointReactionForces)/sizeof(double)); i++) { - // printf("%f ", jointReactionForces[i]); - // } - // now, position x,y,z = actualStateQ[0],actualStateQ[1],actualStateQ[2] - // and orientation x,y,z,w = - // actualStateQ[3],actualStateQ[4],actualStateQ[5],actualStateQ[6] - basePosition[0] = actualStateQ[0]; - basePosition[1] = actualStateQ[1]; - basePosition[2] = actualStateQ[2]; + // printf("joint reaction forces="); + // for (i=0; i < (sizeof(jointReactionForces)/sizeof(double)); i++) { + // printf("%f ", jointReactionForces[i]); + // } + // now, position x,y,z = actualStateQ[0],actualStateQ[1],actualStateQ[2] + // and orientation x,y,z,w = + // actualStateQ[3],actualStateQ[4],actualStateQ[5],actualStateQ[6] + basePosition[0] = actualStateQ[0]; + basePosition[1] = actualStateQ[1]; + basePosition[2] = actualStateQ[2]; - baseOrientation[0] = actualStateQ[3]; - baseOrientation[1] = actualStateQ[4]; - baseOrientation[2] = actualStateQ[5]; - baseOrientation[3] = actualStateQ[6]; - } - } - return 1; + baseOrientation[0] = actualStateQ[3]; + baseOrientation[1] = actualStateQ[4]; + baseOrientation[2] = actualStateQ[5]; + baseOrientation[3] = actualStateQ[6]; + } + } + return 1; } // Get the positions (x,y,z) and orientation (x,y,z,w) in quaternion @@ -1465,92 +1504,97 @@ static int pybullet_internalGetBasePositionAndOrientation( // Object is retrieved based on body index, which is the order // the object was loaded into the simulation (0-based) static PyObject* pybullet_getBasePositionAndOrientation(PyObject* self, - PyObject* args, PyObject* keywds) { - int bodyUniqueId = -1; - double basePosition[3]; - double baseOrientation[4]; - PyObject* pylistPos; - PyObject* pylistOrientation; - b3PhysicsClientHandle sm = 0; + PyObject* args, PyObject* keywds) +{ + int bodyUniqueId = -1; + double basePosition[3]; + double baseOrientation[4]; + PyObject* pylistPos; + PyObject* pylistOrientation; + b3PhysicsClientHandle sm = 0; - int physicsClientId = 0; - static char *kwlist[] = { "bodyUniqueId", "physicsClientId", NULL }; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist,&bodyUniqueId, &physicsClientId)) + int physicsClientId = 0; + static char* kwlist[] = {"bodyUniqueId", "physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist, &bodyUniqueId, &physicsClientId)) { return NULL; } sm = getPhysicsClient(physicsClientId); - if (sm == 0) + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - - if (0 == pybullet_internalGetBasePositionAndOrientation( - bodyUniqueId, basePosition, baseOrientation,sm)) { - PyErr_SetString(SpamError, - "GetBasePositionAndOrientation failed."); - return NULL; - } - { - PyObject* item; - int i; - int num = 3; - pylistPos = PyTuple_New(num); - for (i = 0; i < num; i++) { - item = PyFloat_FromDouble(basePosition[i]); - PyTuple_SetItem(pylistPos, i, item); - } - } + if (0 == pybullet_internalGetBasePositionAndOrientation( + bodyUniqueId, basePosition, baseOrientation, sm)) + { + PyErr_SetString(SpamError, + "GetBasePositionAndOrientation failed."); + return NULL; + } - { - PyObject* item; - int i; - int num = 4; - pylistOrientation = PyTuple_New(num); - for (i = 0; i < num; i++) { - item = PyFloat_FromDouble(baseOrientation[i]); - PyTuple_SetItem(pylistOrientation, i, item); - } - } + { + PyObject* item; + int i; + int num = 3; + pylistPos = PyTuple_New(num); + for (i = 0; i < num; i++) + { + item = PyFloat_FromDouble(basePosition[i]); + PyTuple_SetItem(pylistPos, i, item); + } + } - { - PyObject* pylist; - pylist = PyTuple_New(2); - PyTuple_SetItem(pylist, 0, pylistPos); - PyTuple_SetItem(pylist, 1, pylistOrientation); - return pylist; - } + { + PyObject* item; + int i; + int num = 4; + pylistOrientation = PyTuple_New(num); + for (i = 0; i < num; i++) + { + item = PyFloat_FromDouble(baseOrientation[i]); + PyTuple_SetItem(pylistOrientation, i, item); + } + } + + { + PyObject* pylist; + pylist = PyTuple_New(2); + PyTuple_SetItem(pylist, 0, pylistPos); + PyTuple_SetItem(pylist, 1, pylistOrientation); + return pylist; + } } - static PyObject* pybullet_getBaseVelocity(PyObject* self, - PyObject* args, PyObject* keywds) { + PyObject* args, PyObject* keywds) +{ int bodyUniqueId = -1; double baseLinearVelocity[3]; double baseAngularVelocity[3]; - PyObject* pylistLinVel=0; - PyObject* pylistAngVel=0; + PyObject* pylistLinVel = 0; + PyObject* pylistAngVel = 0; int physicsClientId = 0; b3PhysicsClientHandle sm = 0; - static char *kwlist[] = { "bodyUniqueId", "physicsClientId", NULL }; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist,&bodyUniqueId, &physicsClientId)) + static char* kwlist[] = {"bodyUniqueId", "physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist, &bodyUniqueId, &physicsClientId)) { return NULL; } sm = getPhysicsClient(physicsClientId); - if (sm == 0) + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - + if (0 == pybullet_internalGetBaseVelocity( - bodyUniqueId, baseLinearVelocity, baseAngularVelocity,sm)) { + bodyUniqueId, baseLinearVelocity, baseAngularVelocity, sm)) + { PyErr_SetString(SpamError, - "getBaseVelocity failed."); + "getBaseVelocity failed."); return NULL; } @@ -1559,7 +1603,8 @@ static PyObject* pybullet_getBaseVelocity(PyObject* self, int i; int num = 3; pylistLinVel = PyTuple_New(num); - for (i = 0; i < num; i++) { + for (i = 0; i < num; i++) + { item = PyFloat_FromDouble(baseLinearVelocity[i]); PyTuple_SetItem(pylistLinVel, i, item); } @@ -1570,7 +1615,8 @@ static PyObject* pybullet_getBaseVelocity(PyObject* self, int i; int num = 3; pylistAngVel = PyTuple_New(num); - for (i = 0; i < num; i++) { + for (i = 0; i < num; i++) + { item = PyFloat_FromDouble(baseAngularVelocity[i]); PyTuple_SetItem(pylistAngVel, i, item); } @@ -1589,331 +1635,301 @@ static PyObject* pybullet_getNumBodies(PyObject* self, PyObject* args, PyObject* int physicsClientId = 0; b3PhysicsClientHandle sm = 0; - static char *kwlist[] = { "physicsClientId", NULL }; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "|i", kwlist,&physicsClientId)) + static char* kwlist[] = {"physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "|i", kwlist, &physicsClientId)) { return NULL; } sm = getPhysicsClient(physicsClientId); - if (sm == 0) + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - - { - int numBodies = b3GetNumBodies(sm); + { + int numBodies = b3GetNumBodies(sm); #if PY_MAJOR_VERSION >= 3 - return PyLong_FromLong(numBodies); + return PyLong_FromLong(numBodies); #else - return PyInt_FromLong(numBodies); + return PyInt_FromLong(numBodies); #endif - } + } } - static PyObject* pybullet_getBodyUniqueId(PyObject* self, PyObject* args, PyObject* keywds) { -int physicsClientId = 0; -int serialIndex=-1; -b3PhysicsClientHandle sm = 0; + int physicsClientId = 0; + int serialIndex = -1; + b3PhysicsClientHandle sm = 0; - static char *kwlist[] = { "serialIndex", "physicsClientId", NULL }; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist,&serialIndex, &physicsClientId)) + static char* kwlist[] = {"serialIndex", "physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist, &serialIndex, &physicsClientId)) { return NULL; } sm = getPhysicsClient(physicsClientId); - if (sm == 0) + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - { - - int bodyUniqueId = -1; - bodyUniqueId = b3GetBodyUniqueId(sm, serialIndex); + int bodyUniqueId = -1; + bodyUniqueId = b3GetBodyUniqueId(sm, serialIndex); #if PY_MAJOR_VERSION >= 3 - return PyLong_FromLong(bodyUniqueId); + return PyLong_FromLong(bodyUniqueId); #else - return PyInt_FromLong(bodyUniqueId); + return PyInt_FromLong(bodyUniqueId); #endif - } + } } static PyObject* pybullet_getBodyInfo(PyObject* self, PyObject* args, PyObject* keywds) { - { - int bodyUniqueId= -1; - b3PhysicsClientHandle sm = 0; + int bodyUniqueId = -1; + b3PhysicsClientHandle sm = 0; - int physicsClientId = 0; - static char *kwlist[] = { "bodyUniqueId", "physicsClientId", NULL }; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist,&bodyUniqueId, &physicsClientId)) + int physicsClientId = 0; + static char* kwlist[] = {"bodyUniqueId", "physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist, &bodyUniqueId, &physicsClientId)) + { + return NULL; + } + sm = getPhysicsClient(physicsClientId); + if (sm == 0) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + { + struct b3BodyInfo info; + if (b3GetBodyInfo(sm, bodyUniqueId, &info)) + { + PyObject* pyListJointInfo = PyTuple_New(1); + PyTuple_SetItem(pyListJointInfo, 0, PyString_FromString(info.m_baseName)); + return pyListJointInfo; + } + } + } + PyErr_SetString(SpamError, "Couldn't get body info"); + return NULL; +} + +static PyObject* pybullet_getConstraintInfo(PyObject* self, PyObject* args, PyObject* keywds) +{ + { + int constraintUniqueId = -1; + b3PhysicsClientHandle sm = 0; + + int physicsClientId = 0; + static char* kwlist[] = {"constraintUniqueId", "physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist, &constraintUniqueId, &physicsClientId)) + { + return NULL; + } + sm = getPhysicsClient(physicsClientId); + if (sm == 0) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + { + struct b3UserConstraint constraintInfo; + + if (b3GetUserConstraintInfo(sm, constraintUniqueId, &constraintInfo)) + { + PyObject* pyListConstraintInfo = PyTuple_New(11); + + PyTuple_SetItem(pyListConstraintInfo, 0, PyLong_FromLong(constraintInfo.m_parentBodyIndex)); + PyTuple_SetItem(pyListConstraintInfo, 1, PyLong_FromLong(constraintInfo.m_parentJointIndex)); + PyTuple_SetItem(pyListConstraintInfo, 2, PyLong_FromLong(constraintInfo.m_childBodyIndex)); + PyTuple_SetItem(pyListConstraintInfo, 3, PyLong_FromLong(constraintInfo.m_childJointIndex)); + PyTuple_SetItem(pyListConstraintInfo, 4, PyLong_FromLong(constraintInfo.m_jointType)); + + { + PyObject* axisObj = PyTuple_New(3); + PyTuple_SetItem(axisObj, 0, PyFloat_FromDouble(constraintInfo.m_jointAxis[0])); + PyTuple_SetItem(axisObj, 1, PyFloat_FromDouble(constraintInfo.m_jointAxis[1])); + PyTuple_SetItem(axisObj, 2, PyFloat_FromDouble(constraintInfo.m_jointAxis[2])); + PyTuple_SetItem(pyListConstraintInfo, 5, axisObj); + } + { + PyObject* parentFramePositionObj = PyTuple_New(3); + PyTuple_SetItem(parentFramePositionObj, 0, PyFloat_FromDouble(constraintInfo.m_parentFrame[0])); + PyTuple_SetItem(parentFramePositionObj, 1, PyFloat_FromDouble(constraintInfo.m_parentFrame[1])); + PyTuple_SetItem(parentFramePositionObj, 2, PyFloat_FromDouble(constraintInfo.m_parentFrame[2])); + PyTuple_SetItem(pyListConstraintInfo, 6, parentFramePositionObj); + } + { + PyObject* childFramePositionObj = PyTuple_New(3); + PyTuple_SetItem(childFramePositionObj, 0, PyFloat_FromDouble(constraintInfo.m_childFrame[0])); + PyTuple_SetItem(childFramePositionObj, 1, PyFloat_FromDouble(constraintInfo.m_childFrame[1])); + PyTuple_SetItem(childFramePositionObj, 2, PyFloat_FromDouble(constraintInfo.m_childFrame[2])); + PyTuple_SetItem(pyListConstraintInfo, 7, childFramePositionObj); + } + { + PyObject* parentFrameOrientationObj = PyTuple_New(4); + PyTuple_SetItem(parentFrameOrientationObj, 0, PyFloat_FromDouble(constraintInfo.m_parentFrame[3])); + PyTuple_SetItem(parentFrameOrientationObj, 1, PyFloat_FromDouble(constraintInfo.m_parentFrame[4])); + PyTuple_SetItem(parentFrameOrientationObj, 2, PyFloat_FromDouble(constraintInfo.m_parentFrame[5])); + PyTuple_SetItem(parentFrameOrientationObj, 3, PyFloat_FromDouble(constraintInfo.m_parentFrame[6])); + PyTuple_SetItem(pyListConstraintInfo, 8, parentFrameOrientationObj); + } + { + PyObject* childFrameOrientation = PyTuple_New(4); + PyTuple_SetItem(childFrameOrientation, 0, PyFloat_FromDouble(constraintInfo.m_childFrame[3])); + PyTuple_SetItem(childFrameOrientation, 1, PyFloat_FromDouble(constraintInfo.m_childFrame[4])); + PyTuple_SetItem(childFrameOrientation, 2, PyFloat_FromDouble(constraintInfo.m_childFrame[5])); + PyTuple_SetItem(childFrameOrientation, 3, PyFloat_FromDouble(constraintInfo.m_childFrame[6])); + PyTuple_SetItem(pyListConstraintInfo, 9, childFrameOrientation); + } + PyTuple_SetItem(pyListConstraintInfo, 10, PyFloat_FromDouble(constraintInfo.m_maxAppliedForce)); + + return pyListConstraintInfo; + } + } + } + + PyErr_SetString(SpamError, "Couldn't get user constraint info"); + return NULL; +} + +static PyObject* pybullet_getNumConstraints(PyObject* self, PyObject* args, PyObject* keywds) +{ + int numConstraints = 0; + int physicsClientId = 0; + b3PhysicsClientHandle sm = 0; + static char* kwlist[] = {"physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "|i", kwlist, &physicsClientId)) { return NULL; } sm = getPhysicsClient(physicsClientId); - if (sm == 0) + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - { - struct b3BodyInfo info; - if (b3GetBodyInfo(sm,bodyUniqueId,&info)) - { - PyObject* pyListJointInfo = PyTuple_New(1); - PyTuple_SetItem(pyListJointInfo, 0, PyString_FromString(info.m_baseName)); - return pyListJointInfo; - } else - { - PyErr_SetString(SpamError, "Couldn't get body info"); - return NULL; - } - } - } + numConstraints = b3GetNumUserConstraints(sm); - PyErr_SetString(SpamError, "error in getBodyInfo."); - return NULL; -} - - -static PyObject* pybullet_getConstraintInfo(PyObject* self, PyObject* args, PyObject* keywds) -{ - - { - int constraintUniqueId= -1; - b3PhysicsClientHandle sm = 0; - - int physicsClientId = 0; - static char *kwlist[] = { "constraintUniqueId", "physicsClientId", NULL }; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist,&constraintUniqueId, &physicsClientId)) - { - return NULL; - } - sm = getPhysicsClient(physicsClientId); - if (sm == 0) - { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } - - { - struct b3UserConstraint constraintInfo; - - if (b3GetUserConstraintInfo(sm,constraintUniqueId, &constraintInfo)) - { - PyObject* pyListConstraintInfo = PyTuple_New(11); - - PyTuple_SetItem(pyListConstraintInfo,0,PyLong_FromLong(constraintInfo.m_parentBodyIndex)); - PyTuple_SetItem(pyListConstraintInfo,1,PyLong_FromLong(constraintInfo.m_parentJointIndex)); - PyTuple_SetItem(pyListConstraintInfo,2,PyLong_FromLong(constraintInfo.m_childBodyIndex)); - PyTuple_SetItem(pyListConstraintInfo,3,PyLong_FromLong(constraintInfo.m_childJointIndex)); - PyTuple_SetItem(pyListConstraintInfo,4,PyLong_FromLong(constraintInfo.m_jointType)); - - { - PyObject* axisObj = PyTuple_New(3); - PyTuple_SetItem(axisObj,0,PyFloat_FromDouble(constraintInfo.m_jointAxis[0])); - PyTuple_SetItem(axisObj,1,PyFloat_FromDouble(constraintInfo.m_jointAxis[1])); - PyTuple_SetItem(axisObj,2,PyFloat_FromDouble(constraintInfo.m_jointAxis[2])); - PyTuple_SetItem(pyListConstraintInfo,5,axisObj); - } - { - PyObject* parentFramePositionObj = PyTuple_New(3); - PyTuple_SetItem(parentFramePositionObj,0,PyFloat_FromDouble(constraintInfo.m_parentFrame[0])); - PyTuple_SetItem(parentFramePositionObj,1,PyFloat_FromDouble(constraintInfo.m_parentFrame[1])); - PyTuple_SetItem(parentFramePositionObj,2,PyFloat_FromDouble(constraintInfo.m_parentFrame[2])); - PyTuple_SetItem(pyListConstraintInfo,6,parentFramePositionObj); - } - { - PyObject* childFramePositionObj = PyTuple_New(3); - PyTuple_SetItem(childFramePositionObj,0,PyFloat_FromDouble(constraintInfo.m_childFrame[0])); - PyTuple_SetItem(childFramePositionObj,1,PyFloat_FromDouble(constraintInfo.m_childFrame[1])); - PyTuple_SetItem(childFramePositionObj,2,PyFloat_FromDouble(constraintInfo.m_childFrame[2])); - PyTuple_SetItem(pyListConstraintInfo,7,childFramePositionObj); - } - { - PyObject* parentFrameOrientationObj = PyTuple_New(4); - PyTuple_SetItem(parentFrameOrientationObj,0,PyFloat_FromDouble(constraintInfo.m_parentFrame[3])); - PyTuple_SetItem(parentFrameOrientationObj,1,PyFloat_FromDouble(constraintInfo.m_parentFrame[4])); - PyTuple_SetItem(parentFrameOrientationObj,2,PyFloat_FromDouble(constraintInfo.m_parentFrame[5])); - PyTuple_SetItem(parentFrameOrientationObj,3,PyFloat_FromDouble(constraintInfo.m_parentFrame[6])); - PyTuple_SetItem(pyListConstraintInfo,8,parentFrameOrientationObj); - } - { - PyObject* childFrameOrientation = PyTuple_New(4); - PyTuple_SetItem(childFrameOrientation,0,PyFloat_FromDouble(constraintInfo.m_childFrame[3])); - PyTuple_SetItem(childFrameOrientation,1,PyFloat_FromDouble(constraintInfo.m_childFrame[4])); - PyTuple_SetItem(childFrameOrientation,2,PyFloat_FromDouble(constraintInfo.m_childFrame[5])); - PyTuple_SetItem(childFrameOrientation,3,PyFloat_FromDouble(constraintInfo.m_childFrame[6])); - PyTuple_SetItem(pyListConstraintInfo,9,childFrameOrientation); - } - PyTuple_SetItem(pyListConstraintInfo,10,PyFloat_FromDouble(constraintInfo.m_maxAppliedForce)); - - return pyListConstraintInfo; - } else - { - PyErr_SetString(SpamError, "Couldn't get user constraint info"); - return NULL; - } - } - } - - PyErr_SetString(SpamError, "error in getConstraintInfo."); - return NULL; -} - - -static PyObject* pybullet_getNumConstraints(PyObject* self, PyObject* args, PyObject* keywds) -{ - int numConstraints = 0; - int physicsClientId = 0; - b3PhysicsClientHandle sm = 0; - static char *kwlist[] = { "physicsClientId", NULL }; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "|i", kwlist, &physicsClientId)) - { - return NULL; - } - sm = getPhysicsClient(physicsClientId); - if (sm == 0) - { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } - - numConstraints = b3GetNumUserConstraints(sm); - #if PY_MAJOR_VERSION >= 3 - return PyLong_FromLong(numConstraints); + return PyLong_FromLong(numConstraints); #else - return PyInt_FromLong(numConstraints); + return PyInt_FromLong(numConstraints); #endif - - } - - // Return the number of joints in an object based on // body index; body index is based on order of sequence // the object is loaded into simulation -static PyObject* pybullet_getNumJoints(PyObject* self, PyObject* args, PyObject* keywds) +static PyObject* pybullet_getNumJoints(PyObject* self, PyObject* args, PyObject* keywds) { int bodyUniqueId = -1; - int numJoints = 0; + int numJoints = 0; int physicsClientId = 0; b3PhysicsClientHandle sm = 0; - static char *kwlist[] = { "bodyUniqueId", "physicsClientId", NULL }; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist,&bodyUniqueId, &physicsClientId)) + static char* kwlist[] = {"bodyUniqueId", "physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist, &bodyUniqueId, &physicsClientId)) { return NULL; } sm = getPhysicsClient(physicsClientId); - if (sm == 0) + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - - numJoints = b3GetNumJoints(sm, bodyUniqueId); + + numJoints = b3GetNumJoints(sm, bodyUniqueId); #if PY_MAJOR_VERSION >= 3 - return PyLong_FromLong(numJoints); + return PyLong_FromLong(numJoints); #else - return PyInt_FromLong(numJoints); + return PyInt_FromLong(numJoints); #endif - } // Initalize all joint positions given a list of values -static PyObject* pybullet_resetJointState(PyObject* self, PyObject* args, PyObject* keywds) { - +static PyObject* pybullet_resetJointState(PyObject* self, PyObject* args, PyObject* keywds) { - int bodyUniqueId; - int jointIndex; - double targetValue; - b3PhysicsClientHandle sm = 0; - - int physicsClientId = 0; - static char *kwlist[] = { "bodyUniqueId", "jointIndex", "targetValue","physicsClientId", NULL }; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "iid|i", kwlist,&bodyUniqueId, &jointIndex, &targetValue, &physicsClientId)) { - return NULL; - } - sm = getPhysicsClient(physicsClientId); - if (sm == 0) - { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } - - - { - b3SharedMemoryCommandHandle commandHandle; - b3SharedMemoryStatusHandle statusHandle; - int numJoints; - - numJoints = b3GetNumJoints(sm, bodyUniqueId); - if ((jointIndex >= numJoints) || (jointIndex < 0)) { - PyErr_SetString(SpamError, "Joint index out-of-range."); - return NULL; - } - - commandHandle = b3CreatePoseCommandInit(sm, bodyUniqueId); - - b3CreatePoseCommandSetJointPosition(sm, commandHandle, jointIndex, - targetValue); - - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); - Py_INCREF(Py_None); - return Py_None; - } - } - PyErr_SetString(SpamError, "error in resetJointState."); - return NULL; -} - - - - -static PyObject* pybullet_resetBaseVelocity(PyObject* self, PyObject* args, PyObject *keywds) -{ - static char *kwlist[] = { "objectUniqueId", "linearVelocity", "angularVelocity","physicsClientId", NULL }; - - - { - int bodyIndex=0; - PyObject* linVelObj=0; - PyObject* angVelObj=0; - double linVel[3] = { 0, 0, 0 }; - double angVel[3] = { 0, 0, 0 }; - int physicsClientId = 0; + int bodyUniqueId; + int jointIndex; + double targetValue; b3PhysicsClientHandle sm = 0; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|OOi", kwlist, &bodyIndex, &linVelObj, &angVelObj,&physicsClientId)) + int physicsClientId = 0; + static char* kwlist[] = {"bodyUniqueId", "jointIndex", "targetValue", "physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "iid|i", kwlist, &bodyUniqueId, &jointIndex, &targetValue, &physicsClientId)) + { + return NULL; + } + sm = getPhysicsClient(physicsClientId); + if (sm == 0) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + { + b3SharedMemoryCommandHandle commandHandle; + b3SharedMemoryStatusHandle statusHandle; + int numJoints; + + numJoints = b3GetNumJoints(sm, bodyUniqueId); + if ((jointIndex >= numJoints) || (jointIndex < 0)) + { + PyErr_SetString(SpamError, "Joint index out-of-range."); + return NULL; + } + + commandHandle = b3CreatePoseCommandInit(sm, bodyUniqueId); + + b3CreatePoseCommandSetJointPosition(sm, commandHandle, jointIndex, + targetValue); + + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + } + } + Py_INCREF(Py_None); + return Py_None; +} + +static PyObject* pybullet_resetBaseVelocity(PyObject* self, PyObject* args, PyObject* keywds) +{ + static char* kwlist[] = {"objectUniqueId", "linearVelocity", "angularVelocity", "physicsClientId", NULL}; + + { + int bodyIndex = 0; + PyObject* linVelObj = 0; + PyObject* angVelObj = 0; + double linVel[3] = {0, 0, 0}; + double angVel[3] = {0, 0, 0}; + int physicsClientId = 0; + b3PhysicsClientHandle sm = 0; + + if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|OOi", kwlist, &bodyIndex, &linVelObj, &angVelObj, &physicsClientId)) { return NULL; } sm = getPhysicsClient(physicsClientId); - if (sm == 0) - { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } - + if (sm == 0) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } if (linVelObj || angVelObj) { - b3SharedMemoryCommandHandle commandHandle; b3SharedMemoryStatusHandle statusHandle; @@ -1945,87 +1961,91 @@ static PyObject* pybullet_resetBaseVelocity(PyObject* self, PyObject* args, PyOb return NULL; } - - // Reset the position and orientation of the base/root link, position [x,y,z] // and orientation quaternion [x,y,z,w] static PyObject* pybullet_resetBasePositionAndOrientation(PyObject* self, - PyObject* args, PyObject* keywds) { - - { - int bodyUniqueId; - PyObject* posObj; - PyObject* ornObj; - double pos[3]; - double orn[4]; // as a quaternion - b3PhysicsClientHandle sm = 0; - - int physicsClientId = 0; - static char *kwlist[] = { "bodyUniqueId", "posObj", "ornObj", "physicsClientId", NULL }; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "iOO|i", kwlist,&bodyUniqueId, &posObj, &ornObj, &physicsClientId)) + PyObject* args, PyObject* keywds) +{ { - return NULL; + int bodyUniqueId; + PyObject* posObj; + PyObject* ornObj; + double pos[3]; + double orn[4]; // as a quaternion + b3PhysicsClientHandle sm = 0; + + int physicsClientId = 0; + static char* kwlist[] = {"bodyUniqueId", "posObj", "ornObj", "physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "iOO|i", kwlist, &bodyUniqueId, &posObj, &ornObj, &physicsClientId)) + { + return NULL; + } + sm = getPhysicsClient(physicsClientId); + if (sm == 0) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + { + b3SharedMemoryCommandHandle commandHandle; + b3SharedMemoryStatusHandle statusHandle; + + { + PyObject* seq; + int len, i; + seq = PySequence_Fast(posObj, "expected a sequence"); + len = PySequence_Size(posObj); + if (len == 3) + { + for (i = 0; i < 3; i++) + { + pos[i] = pybullet_internalGetFloatFromSequence(seq, i); + } + } + else + { + PyErr_SetString(SpamError, "position needs a 3 coordinates [x,y,z]."); + Py_DECREF(seq); + return NULL; + } + Py_DECREF(seq); + } + + { + PyObject* seq; + int len, i; + seq = PySequence_Fast(ornObj, "expected a sequence"); + len = PySequence_Size(ornObj); + if (len == 4) + { + for (i = 0; i < 4; i++) + { + orn[i] = pybullet_internalGetFloatFromSequence(seq, i); + } + } + else + { + PyErr_SetString( + SpamError, + "orientation needs a 4 coordinates, quaternion [x,y,z,w]."); + Py_DECREF(seq); + return NULL; + } + Py_DECREF(seq); + } + + commandHandle = b3CreatePoseCommandInit(sm, bodyUniqueId); + + b3CreatePoseCommandSetBasePosition(commandHandle, pos[0], pos[1], pos[2]); + b3CreatePoseCommandSetBaseOrientation(commandHandle, orn[0], orn[1], + orn[2], orn[3]); + + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + } } - sm = getPhysicsClient(physicsClientId); - if (sm == 0) - { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } - - { - b3SharedMemoryCommandHandle commandHandle; - b3SharedMemoryStatusHandle statusHandle; - - { - PyObject* seq; - int len, i; - seq = PySequence_Fast(posObj, "expected a sequence"); - len = PySequence_Size(posObj); - if (len == 3) { - for (i = 0; i < 3; i++) { - pos[i] = pybullet_internalGetFloatFromSequence(seq, i); - } - } else { - PyErr_SetString(SpamError, "position needs a 3 coordinates [x,y,z]."); - Py_DECREF(seq); - return NULL; - } - Py_DECREF(seq); - } - - { - PyObject* seq; - int len, i; - seq = PySequence_Fast(ornObj, "expected a sequence"); - len = PySequence_Size(ornObj); - if (len == 4) { - for (i = 0; i < 4; i++) { - orn[i] = pybullet_internalGetFloatFromSequence(seq, i); - } - } else { - PyErr_SetString( - SpamError, - "orientation needs a 4 coordinates, quaternion [x,y,z,w]."); - Py_DECREF(seq); - return NULL; - } - Py_DECREF(seq); - } - - commandHandle = b3CreatePoseCommandInit(sm, bodyUniqueId); - - b3CreatePoseCommandSetBasePosition(commandHandle, pos[0], pos[1], pos[2]); - b3CreatePoseCommandSetBaseOrientation(commandHandle, orn[0], orn[1], - orn[2], orn[3]); - - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); - Py_INCREF(Py_None); - return Py_None; - } - } - PyErr_SetString(SpamError, "error in resetJointState."); - return NULL; + Py_INCREF(Py_None); + return Py_None; } // Get the a single joint info for a specific bodyIndex @@ -2042,67 +2062,70 @@ static PyObject* pybullet_resetBasePositionAndOrientation(PyObject* self, // [int, str, int, int, int, int, float, float] // // TODO(hellojas): get joint positions for a body -static PyObject* pybullet_getJointInfo(PyObject* self, PyObject* args,PyObject* keywds) { - PyObject* pyListJointInfo; +static PyObject* pybullet_getJointInfo(PyObject* self, PyObject* args, PyObject* keywds) +{ + PyObject* pyListJointInfo; - struct b3JointInfo info; + struct b3JointInfo info; - int bodyUniqueId = -1; - int jointIndex = -1; - int jointInfoSize = 8; // size of struct b3JointInfo -b3PhysicsClientHandle sm = 0; -int physicsClientId = 0; - static char *kwlist[] = { "bodyUniqueId", "jointIndex", "physicsClientId", NULL }; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|i", kwlist,&bodyUniqueId, &jointIndex, &physicsClientId)) + int bodyUniqueId = -1; + int jointIndex = -1; + int jointInfoSize = 8; // size of struct b3JointInfo + b3PhysicsClientHandle sm = 0; + int physicsClientId = 0; + static char* kwlist[] = {"bodyUniqueId", "jointIndex", "physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|i", kwlist, &bodyUniqueId, &jointIndex, &physicsClientId)) { return NULL; } sm = getPhysicsClient(physicsClientId); - if (sm == 0) + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - - { { - // printf("body index = %d, joint index =%d\n", bodyIndex, jointIndex); + { + // printf("body index = %d, joint index =%d\n", bodyIndex, jointIndex); - pyListJointInfo = PyTuple_New(jointInfoSize); + pyListJointInfo = PyTuple_New(jointInfoSize); - if (b3GetJointInfo(sm, bodyUniqueId, jointIndex, &info)) { - // printf("Joint%d %s, type %d, at q-index %d and u-index %d\n", - // info.m_jointIndex, - // info.m_jointName, - // info.m_jointType, - // info.m_qIndex, - // info.m_uIndex); - // printf(" flags=%d jointDamping=%f jointFriction=%f\n", - // info.m_flags, - // info.m_jointDamping, - // info.m_jointFriction); - PyTuple_SetItem(pyListJointInfo, 0, PyInt_FromLong(info.m_jointIndex)); - PyTuple_SetItem(pyListJointInfo, 1, - PyString_FromString(info.m_jointName)); - PyTuple_SetItem(pyListJointInfo, 2, PyInt_FromLong(info.m_jointType)); - PyTuple_SetItem(pyListJointInfo, 3, PyInt_FromLong(info.m_qIndex)); - PyTuple_SetItem(pyListJointInfo, 4, PyInt_FromLong(info.m_uIndex)); - PyTuple_SetItem(pyListJointInfo, 5, PyInt_FromLong(info.m_flags)); - PyTuple_SetItem(pyListJointInfo, 6, - PyFloat_FromDouble(info.m_jointDamping)); - PyTuple_SetItem(pyListJointInfo, 7, - PyFloat_FromDouble(info.m_jointFriction)); - return pyListJointInfo; - } else { - PyErr_SetString(SpamError, "GetJointInfo failed."); - return NULL; - } - } - } + if (b3GetJointInfo(sm, bodyUniqueId, jointIndex, &info)) + { + // printf("Joint%d %s, type %d, at q-index %d and u-index %d\n", + // info.m_jointIndex, + // info.m_jointName, + // info.m_jointType, + // info.m_qIndex, + // info.m_uIndex); + // printf(" flags=%d jointDamping=%f jointFriction=%f\n", + // info.m_flags, + // info.m_jointDamping, + // info.m_jointFriction); + PyTuple_SetItem(pyListJointInfo, 0, PyInt_FromLong(info.m_jointIndex)); + PyTuple_SetItem(pyListJointInfo, 1, + PyString_FromString(info.m_jointName)); + PyTuple_SetItem(pyListJointInfo, 2, PyInt_FromLong(info.m_jointType)); + PyTuple_SetItem(pyListJointInfo, 3, PyInt_FromLong(info.m_qIndex)); + PyTuple_SetItem(pyListJointInfo, 4, PyInt_FromLong(info.m_uIndex)); + PyTuple_SetItem(pyListJointInfo, 5, PyInt_FromLong(info.m_flags)); + PyTuple_SetItem(pyListJointInfo, 6, + PyFloat_FromDouble(info.m_jointDamping)); + PyTuple_SetItem(pyListJointInfo, 7, + PyFloat_FromDouble(info.m_jointFriction)); + return pyListJointInfo; + } + else + { + PyErr_SetString(SpamError, "GetJointInfo failed."); + return NULL; + } + } + } - Py_INCREF(Py_None); - return Py_None; + Py_INCREF(Py_None); + return Py_None; } // Returns the state of a specific joint in a given bodyIndex @@ -2118,212 +2141,219 @@ int physicsClientId = 0; // TODO(hellojas): check accuracy of position and velocity // TODO(hellojas): check force torque values -static PyObject* pybullet_getJointState(PyObject* self, PyObject* args,PyObject* keywds) { - PyObject* pyListJointForceTorque; - PyObject* pyListJointState; +static PyObject* pybullet_getJointState(PyObject* self, PyObject* args, PyObject* keywds) +{ + PyObject* pyListJointForceTorque; + PyObject* pyListJointState; + struct b3JointSensorState sensorState; - struct b3JointSensorState sensorState; - - int bodyUniqueId = -1; - int jointIndex = -1; - int sensorStateSize = 4; // size of struct b3JointSensorState - int forceTorqueSize = 6; // size of force torque list from b3JointSensorState - int j; + int bodyUniqueId = -1; + int jointIndex = -1; + int sensorStateSize = 4; // size of struct b3JointSensorState + int forceTorqueSize = 6; // size of force torque list from b3JointSensorState + int j; b3PhysicsClientHandle sm = 0; - int physicsClientId = 0; - static char *kwlist[] = { "bodyUniqueId", "jointIndex","physicsClientId", NULL }; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|i", kwlist,&bodyUniqueId, &jointIndex, &physicsClientId)) + int physicsClientId = 0; + static char* kwlist[] = {"bodyUniqueId", "jointIndex", "physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|i", kwlist, &bodyUniqueId, &jointIndex, &physicsClientId)) { return NULL; } sm = getPhysicsClient(physicsClientId); - if (sm == 0) + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - - { - { - int status_type = 0; - b3SharedMemoryCommandHandle cmd_handle; - b3SharedMemoryStatusHandle status_handle; + { + int status_type = 0; + b3SharedMemoryCommandHandle cmd_handle; + b3SharedMemoryStatusHandle status_handle; + if (bodyUniqueId < 0) + { + PyErr_SetString(SpamError, "getJointState failed; invalid bodyIndex"); + return NULL; + } + if (jointIndex < 0) + { + PyErr_SetString(SpamError, "getJointState failed; invalid jointIndex"); + return NULL; + } - if (bodyUniqueId < 0) { - PyErr_SetString(SpamError, "getJointState failed; invalid bodyIndex"); - return NULL; - } - if (jointIndex < 0) { - PyErr_SetString(SpamError, "getJointState failed; invalid jointIndex"); - return NULL; - } + cmd_handle = + b3RequestActualStateCommandInit(sm, bodyUniqueId); + status_handle = + b3SubmitClientCommandAndWaitStatus(sm, cmd_handle); - - cmd_handle = - b3RequestActualStateCommandInit(sm, bodyUniqueId); - status_handle = - b3SubmitClientCommandAndWaitStatus(sm, cmd_handle); + status_type = b3GetStatusType(status_handle); + if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED) + { + PyErr_SetString(SpamError, "getJointState failed."); + return NULL; + } - status_type = b3GetStatusType(status_handle); - if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED) { - PyErr_SetString(SpamError, "getJointState failed."); - return NULL; - } + pyListJointState = PyTuple_New(sensorStateSize); + pyListJointForceTorque = PyTuple_New(forceTorqueSize); - pyListJointState = PyTuple_New(sensorStateSize); - pyListJointForceTorque = PyTuple_New(forceTorqueSize); + if (b3GetJointState(sm, status_handle, jointIndex, &sensorState)) + { + PyTuple_SetItem(pyListJointState, 0, + PyFloat_FromDouble(sensorState.m_jointPosition)); + PyTuple_SetItem(pyListJointState, 1, + PyFloat_FromDouble(sensorState.m_jointVelocity)); - if (b3GetJointState(sm, status_handle, jointIndex, &sensorState)) - { - PyTuple_SetItem(pyListJointState, 0, - PyFloat_FromDouble(sensorState.m_jointPosition)); - PyTuple_SetItem(pyListJointState, 1, - PyFloat_FromDouble(sensorState.m_jointVelocity)); + for (j = 0; j < forceTorqueSize; j++) + { + PyTuple_SetItem(pyListJointForceTorque, j, + PyFloat_FromDouble(sensorState.m_jointForceTorque[j])); + } - for (j = 0; j < forceTorqueSize; j++) { - PyTuple_SetItem(pyListJointForceTorque, j, - PyFloat_FromDouble(sensorState.m_jointForceTorque[j])); - } + PyTuple_SetItem(pyListJointState, 2, pyListJointForceTorque); - PyTuple_SetItem(pyListJointState, 2, pyListJointForceTorque); + PyTuple_SetItem(pyListJointState, 3, + PyFloat_FromDouble(sensorState.m_jointMotorTorque)); - PyTuple_SetItem(pyListJointState, 3, - PyFloat_FromDouble(sensorState.m_jointMotorTorque)); + return pyListJointState; + } + else + { + PyErr_SetString(SpamError, "getJointState failed (2)."); + return NULL; + } + } + } - return pyListJointState; - } else - { - PyErr_SetString(SpamError, "getJointState failed (2)."); - return NULL; - } - } - } - - Py_INCREF(Py_None); - return Py_None; + Py_INCREF(Py_None); + return Py_None; } -static PyObject* pybullet_getLinkState(PyObject* self, PyObject* args,PyObject* keywds) { - PyObject* pyLinkState; - PyObject* pyLinkStateWorldPosition; - PyObject* pyLinkStateWorldOrientation; - PyObject* pyLinkStateLocalInertialPosition; - PyObject* pyLinkStateLocalInertialOrientation; - PyObject* pyLinkStateWorldLinkFramePosition; - PyObject* pyLinkStateWorldLinkFrameOrientation; +static PyObject* pybullet_getLinkState(PyObject* self, PyObject* args, PyObject* keywds) +{ + PyObject* pyLinkState; + PyObject* pyLinkStateWorldPosition; + PyObject* pyLinkStateWorldOrientation; + PyObject* pyLinkStateLocalInertialPosition; + PyObject* pyLinkStateLocalInertialOrientation; + PyObject* pyLinkStateWorldLinkFramePosition; + PyObject* pyLinkStateWorldLinkFrameOrientation; - struct b3LinkState linkState; + struct b3LinkState linkState; - int bodyUniqueId = -1; - int linkIndex = -1; - int i; -b3PhysicsClientHandle sm = 0; + int bodyUniqueId = -1; + int linkIndex = -1; + int i; + b3PhysicsClientHandle sm = 0; - int physicsClientId = 0; - static char *kwlist[] = { "bodyUniqueId", "linkIndex", "physicsClientId", NULL }; + int physicsClientId = 0; + static char* kwlist[] = {"bodyUniqueId", "linkIndex", "physicsClientId", NULL}; if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|i", kwlist, &bodyUniqueId, &linkIndex, &physicsClientId)) { return NULL; } sm = getPhysicsClient(physicsClientId); - if (sm == 0) + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - - { + { - int status_type = 0; - b3SharedMemoryCommandHandle cmd_handle; - b3SharedMemoryStatusHandle status_handle; + { + int status_type = 0; + b3SharedMemoryCommandHandle cmd_handle; + b3SharedMemoryStatusHandle status_handle; - if (bodyUniqueId < 0) { - PyErr_SetString(SpamError, "getLinkState failed; invalid bodyIndex"); - return NULL; - } - if (linkIndex < 0) { - PyErr_SetString(SpamError, "getLinkState failed; invalid linkIndex"); - return NULL; - } + if (bodyUniqueId < 0) + { + PyErr_SetString(SpamError, "getLinkState failed; invalid bodyIndex"); + return NULL; + } + if (linkIndex < 0) + { + PyErr_SetString(SpamError, "getLinkState failed; invalid linkIndex"); + return NULL; + } + cmd_handle = + b3RequestActualStateCommandInit(sm, bodyUniqueId); + status_handle = + b3SubmitClientCommandAndWaitStatus(sm, cmd_handle); - cmd_handle = - b3RequestActualStateCommandInit(sm, bodyUniqueId); - status_handle = - b3SubmitClientCommandAndWaitStatus(sm, cmd_handle); + status_type = b3GetStatusType(status_handle); + if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED) + { + PyErr_SetString(SpamError, "getLinkState failed."); + return NULL; + } - status_type = b3GetStatusType(status_handle); - if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED) { - PyErr_SetString(SpamError, "getLinkState failed."); - return NULL; - } + if (b3GetLinkState(sm, status_handle, linkIndex, &linkState)) + { + pyLinkStateWorldPosition = PyTuple_New(3); + for (i = 0; i < 3; ++i) + { + PyTuple_SetItem(pyLinkStateWorldPosition, i, + PyFloat_FromDouble(linkState.m_worldPosition[i])); + } - if (b3GetLinkState(sm, status_handle, linkIndex, &linkState)) - { - pyLinkStateWorldPosition = PyTuple_New(3); - for (i = 0; i < 3; ++i) { - PyTuple_SetItem(pyLinkStateWorldPosition, i, - PyFloat_FromDouble(linkState.m_worldPosition[i])); - } + pyLinkStateWorldOrientation = PyTuple_New(4); + for (i = 0; i < 4; ++i) + { + PyTuple_SetItem(pyLinkStateWorldOrientation, i, + PyFloat_FromDouble(linkState.m_worldOrientation[i])); + } - pyLinkStateWorldOrientation = PyTuple_New(4); - for (i = 0; i < 4; ++i) { - PyTuple_SetItem(pyLinkStateWorldOrientation, i, - PyFloat_FromDouble(linkState.m_worldOrientation[i])); - } + pyLinkStateLocalInertialPosition = PyTuple_New(3); + for (i = 0; i < 3; ++i) + { + PyTuple_SetItem(pyLinkStateLocalInertialPosition, i, + PyFloat_FromDouble(linkState.m_localInertialPosition[i])); + } - pyLinkStateLocalInertialPosition = PyTuple_New(3); - for (i = 0; i < 3; ++i) { - PyTuple_SetItem(pyLinkStateLocalInertialPosition, i, - PyFloat_FromDouble(linkState.m_localInertialPosition[i])); - } + pyLinkStateLocalInertialOrientation = PyTuple_New(4); + for (i = 0; i < 4; ++i) + { + PyTuple_SetItem(pyLinkStateLocalInertialOrientation, i, + PyFloat_FromDouble(linkState.m_localInertialOrientation[i])); + } - pyLinkStateLocalInertialOrientation = PyTuple_New(4); - for (i = 0; i < 4; ++i) { - PyTuple_SetItem(pyLinkStateLocalInertialOrientation, i, - PyFloat_FromDouble(linkState.m_localInertialOrientation[i])); - } + pyLinkStateWorldLinkFramePosition = PyTuple_New(3); + for (i = 0; i < 3; ++i) + { + PyTuple_SetItem(pyLinkStateWorldLinkFramePosition, i, + PyFloat_FromDouble(linkState.m_worldLinkFramePosition[i])); + } - pyLinkStateWorldLinkFramePosition = PyTuple_New(3); - for (i = 0; i < 3; ++i) { - PyTuple_SetItem(pyLinkStateWorldLinkFramePosition , i, - PyFloat_FromDouble(linkState.m_worldLinkFramePosition[i])); - } + pyLinkStateWorldLinkFrameOrientation = PyTuple_New(4); + for (i = 0; i < 4; ++i) + { + PyTuple_SetItem(pyLinkStateWorldLinkFrameOrientation, i, + PyFloat_FromDouble(linkState.m_worldLinkFrameOrientation[i])); + } - pyLinkStateWorldLinkFrameOrientation = PyTuple_New(4); - for (i = 0; i < 4; ++i) { - PyTuple_SetItem(pyLinkStateWorldLinkFrameOrientation, i, - PyFloat_FromDouble(linkState.m_worldLinkFrameOrientation[i])); - } + pyLinkState = PyTuple_New(6); + PyTuple_SetItem(pyLinkState, 0, pyLinkStateWorldPosition); + PyTuple_SetItem(pyLinkState, 1, pyLinkStateWorldOrientation); + PyTuple_SetItem(pyLinkState, 2, pyLinkStateLocalInertialPosition); + PyTuple_SetItem(pyLinkState, 3, pyLinkStateLocalInertialOrientation); + PyTuple_SetItem(pyLinkState, 4, pyLinkStateWorldLinkFramePosition); + PyTuple_SetItem(pyLinkState, 5, pyLinkStateWorldLinkFrameOrientation); + return pyLinkState; + } + } + } - pyLinkState = PyTuple_New(6); - PyTuple_SetItem(pyLinkState, 0, pyLinkStateWorldPosition); - PyTuple_SetItem(pyLinkState, 1, pyLinkStateWorldOrientation); - PyTuple_SetItem(pyLinkState, 2, pyLinkStateLocalInertialPosition); - PyTuple_SetItem(pyLinkState, 3, pyLinkStateLocalInertialOrientation); - PyTuple_SetItem(pyLinkState, 4, pyLinkStateWorldLinkFramePosition ); - PyTuple_SetItem(pyLinkState, 5, pyLinkStateWorldLinkFrameOrientation); - - return pyLinkState; - } - } - } - - Py_INCREF(Py_None); - return Py_None; + Py_INCREF(Py_None); + return Py_None; } - -static PyObject* pybullet_readUserDebugParameter(PyObject* self, PyObject* args, PyObject *keywds) +static PyObject* pybullet_readUserDebugParameter(PyObject* self, PyObject* args, PyObject* keywds) { - b3SharedMemoryCommandHandle commandHandle; b3SharedMemoryStatusHandle statusHandle; int statusType; @@ -2331,28 +2361,28 @@ static PyObject* pybullet_readUserDebugParameter(PyObject* self, PyObject* args, int physicsClientId = 0; b3PhysicsClientHandle sm = 0; - static char *kwlist[] = { "itemUniqueId", "physicsClientId", NULL }; + static char* kwlist[] = {"itemUniqueId", "physicsClientId", NULL}; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist,&itemUniqueId, &physicsClientId)) + if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist, &itemUniqueId, &physicsClientId)) { return NULL; } sm = getPhysicsClient(physicsClientId); - if (sm == 0) + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - commandHandle = b3InitUserDebugReadParameter(sm,itemUniqueId); + commandHandle = b3InitUserDebugReadParameter(sm, itemUniqueId); statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); statusType = b3GetStatusType(statusHandle); - if (statusType == CMD_USER_DEBUG_DRAW_PARAMETER_COMPLETED) + if (statusType == CMD_USER_DEBUG_DRAW_PARAMETER_COMPLETED) { - double paramValue=0.f; - int ok = b3GetStatusDebugParameterValue(statusHandle,¶mValue); + double paramValue = 0.f; + int ok = b3GetStatusDebugParameterValue(statusHandle, ¶mValue); if (ok) { PyObject* item = PyFloat_FromDouble(paramValue); @@ -2362,220 +2392,200 @@ static PyObject* pybullet_readUserDebugParameter(PyObject* self, PyObject* args, PyErr_SetString(SpamError, "Failed to read parameter."); return NULL; - } - - -static PyObject* pybullet_addUserDebugParameter(PyObject* self, PyObject* args, PyObject *keywds) +static PyObject* pybullet_addUserDebugParameter(PyObject* self, PyObject* args, PyObject* keywds) { - - b3SharedMemoryCommandHandle commandHandle; - b3SharedMemoryStatusHandle statusHandle; - int statusType; + b3SharedMemoryCommandHandle commandHandle; + b3SharedMemoryStatusHandle statusHandle; + int statusType; - char* text; + char* text; - - double rangeMin = 0.f; - double rangeMax = 1.f; - double startValue = 0.f; - int physicsClientId = 0; - b3PhysicsClientHandle sm=0; - static char *kwlist[] = { "paramName", "rangeMin", "rangeMax","startValue", "physicsClientId", NULL }; + double rangeMin = 0.f; + double rangeMax = 1.f; + double startValue = 0.f; + int physicsClientId = 0; + b3PhysicsClientHandle sm = 0; + static char* kwlist[] = {"paramName", "rangeMin", "rangeMax", "startValue", "physicsClientId", NULL}; - - if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|dddi", kwlist, &text, &rangeMin, &rangeMax,&startValue, &physicsClientId)) - { - return NULL; - } + if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|dddi", kwlist, &text, &rangeMin, &rangeMax, &startValue, &physicsClientId)) + { + return NULL; + } sm = getPhysicsClient(physicsClientId); - if (sm == 0) + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - - commandHandle = b3InitUserDebugAddParameter(sm,text,rangeMin,rangeMax, startValue); - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); - statusType = b3GetStatusType(statusHandle); - - if (statusType == CMD_USER_DEBUG_DRAW_COMPLETED) + commandHandle = b3InitUserDebugAddParameter(sm, text, rangeMin, rangeMax, startValue); + + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + statusType = b3GetStatusType(statusHandle); + + if (statusType == CMD_USER_DEBUG_DRAW_COMPLETED) { int debugItemUniqueId = b3GetDebugItemUniqueId(statusHandle); PyObject* item = PyInt_FromLong(debugItemUniqueId); return item; } - - PyErr_SetString(SpamError, "Error in addUserDebugParameter."); - return NULL; + PyErr_SetString(SpamError, "Error in addUserDebugParameter."); + return NULL; } - -static PyObject* pybullet_addUserDebugText(PyObject* self, PyObject* args, PyObject *keywds) +static PyObject* pybullet_addUserDebugText(PyObject* self, PyObject* args, PyObject* keywds) { - - b3SharedMemoryCommandHandle commandHandle; - b3SharedMemoryStatusHandle statusHandle; - int statusType; - int res = 0; + b3SharedMemoryCommandHandle commandHandle; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + int res = 0; -char* text; - double posXYZ[3]; - double colorRGB[3]={1,1,1}; + char* text; + double posXYZ[3]; + double colorRGB[3] = {1, 1, 1}; - - PyObject* textPositionObj=0; - PyObject* textColorRGBObj=0; - double textSize = 1.f; - double lifeTime = 0.f; - int physicsClientId = 0; - b3PhysicsClientHandle sm=0; - static char *kwlist[] = { "text", "textPosition", "textColorRGB", "textSize", "lifeTime","physicsClientId", NULL }; + PyObject* textPositionObj = 0; + PyObject* textColorRGBObj = 0; + double textSize = 1.f; + double lifeTime = 0.f; + int physicsClientId = 0; + b3PhysicsClientHandle sm = 0; + static char* kwlist[] = {"text", "textPosition", "textColorRGB", "textSize", "lifeTime", "physicsClientId", NULL}; - - if (!PyArg_ParseTupleAndKeywords(args, keywds, "sO|Oddi", kwlist, &text, &textPositionObj, &textColorRGBObj,&textSize, &lifeTime,&physicsClientId)) - { - return NULL; - } + if (!PyArg_ParseTupleAndKeywords(args, keywds, "sO|Oddi", kwlist, &text, &textPositionObj, &textColorRGBObj, &textSize, &lifeTime, &physicsClientId)) + { + return NULL; + } sm = getPhysicsClient(physicsClientId); - if (sm == 0) + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } + res = pybullet_internalSetVectord(textPositionObj, posXYZ); + if (!res) + { + PyErr_SetString(SpamError, "Error converting textPositionObj[3]"); + return NULL; + } - res = pybullet_internalSetVectord(textPositionObj,posXYZ); - if (!res) - { - PyErr_SetString(SpamError, "Error converting textPositionObj[3]"); - return NULL; - } + if (textColorRGBObj) + { + res = pybullet_internalSetVectord(textColorRGBObj, colorRGB); + if (!res) + { + PyErr_SetString(SpamError, "Error converting textColorRGBObj[3]"); + return NULL; + } + } - if (textColorRGBObj) - { - res = pybullet_internalSetVectord(textColorRGBObj,colorRGB); - if (!res) - { - PyErr_SetString(SpamError, "Error converting textColorRGBObj[3]"); - return NULL; - } - } + commandHandle = b3InitUserDebugDrawAddText3D(sm, text, posXYZ, colorRGB, textSize, lifeTime); - - commandHandle = b3InitUserDebugDrawAddText3D(sm,text,posXYZ,colorRGB,textSize,lifeTime); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + statusType = b3GetStatusType(statusHandle); + if (statusType == CMD_USER_DEBUG_DRAW_COMPLETED) + { + int debugItemUniqueId = b3GetDebugItemUniqueId(statusHandle); + PyObject* item = PyInt_FromLong(debugItemUniqueId); + return item; + } - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); - statusType = b3GetStatusType(statusHandle); - if (statusType == CMD_USER_DEBUG_DRAW_COMPLETED) - { - int debugItemUniqueId = b3GetDebugItemUniqueId(statusHandle); - PyObject* item = PyInt_FromLong(debugItemUniqueId); - return item; - } - - PyErr_SetString(SpamError, "Error in addUserDebugText."); - return NULL; + PyErr_SetString(SpamError, "Error in addUserDebugText."); + return NULL; } - -static PyObject* pybullet_addUserDebugLine(PyObject* self, PyObject* args, PyObject *keywds) +static PyObject* pybullet_addUserDebugLine(PyObject* self, PyObject* args, PyObject* keywds) { - - b3SharedMemoryCommandHandle commandHandle; - b3SharedMemoryStatusHandle statusHandle; - int statusType; - int res = 0; + b3SharedMemoryCommandHandle commandHandle; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + int res = 0; - - double fromXYZ[3]; - double toXYZ[3]; - double colorRGB[3]={1,1,1}; + double fromXYZ[3]; + double toXYZ[3]; + double colorRGB[3] = {1, 1, 1}; - PyObject* lineFromObj=0; - PyObject* lineToObj=0; - PyObject* lineColorRGBObj=0; - double lineWidth = 1.f; - double lifeTime = 0.f; - int physicsClientId = 0; - b3PhysicsClientHandle sm=0; - static char *kwlist[] = { "lineFromXYZ", "lineToXYZ", "lineColorRGB", "lineWidth", "lifeTime","physicsClientId", NULL }; + PyObject* lineFromObj = 0; + PyObject* lineToObj = 0; + PyObject* lineColorRGBObj = 0; + double lineWidth = 1.f; + double lifeTime = 0.f; + int physicsClientId = 0; + b3PhysicsClientHandle sm = 0; + static char* kwlist[] = {"lineFromXYZ", "lineToXYZ", "lineColorRGB", "lineWidth", "lifeTime", "physicsClientId", NULL}; - - if (!PyArg_ParseTupleAndKeywords(args, keywds, "OO|Oddi", kwlist, &lineFromObj, &lineToObj, &lineColorRGBObj,&lineWidth, &lifeTime,&physicsClientId)) - { - return NULL; - } + if (!PyArg_ParseTupleAndKeywords(args, keywds, "OO|Oddi", kwlist, &lineFromObj, &lineToObj, &lineColorRGBObj, &lineWidth, &lifeTime, &physicsClientId)) + { + return NULL; + } sm = getPhysicsClient(physicsClientId); - if (sm == 0) + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - res = pybullet_internalSetVectord(lineFromObj,fromXYZ); - if (!res) - { - PyErr_SetString(SpamError, "Error converting lineFrom[3]"); - return NULL; - } + res = pybullet_internalSetVectord(lineFromObj, fromXYZ); + if (!res) + { + PyErr_SetString(SpamError, "Error converting lineFrom[3]"); + return NULL; + } - res = pybullet_internalSetVectord(lineToObj,toXYZ); - if (!res) - { - PyErr_SetString(SpamError, "Error converting lineTo[3]"); - return NULL; - } - if (lineColorRGBObj) - { - res = pybullet_internalSetVectord(lineColorRGBObj,colorRGB); - } - - commandHandle = b3InitUserDebugDrawAddLine3D(sm,fromXYZ,toXYZ,colorRGB,lineWidth,lifeTime); + res = pybullet_internalSetVectord(lineToObj, toXYZ); + if (!res) + { + PyErr_SetString(SpamError, "Error converting lineTo[3]"); + return NULL; + } + if (lineColorRGBObj) + { + res = pybullet_internalSetVectord(lineColorRGBObj, colorRGB); + } - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); - statusType = b3GetStatusType(statusHandle); - if (statusType == CMD_USER_DEBUG_DRAW_COMPLETED) - { - int debugItemUniqueId = b3GetDebugItemUniqueId(statusHandle); - PyObject* item = PyInt_FromLong(debugItemUniqueId); - return item; - } + commandHandle = b3InitUserDebugDrawAddLine3D(sm, fromXYZ, toXYZ, colorRGB, lineWidth, lifeTime); - PyErr_SetString(SpamError, "Error in addUserDebugLine."); - return NULL; + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + statusType = b3GetStatusType(statusHandle); + if (statusType == CMD_USER_DEBUG_DRAW_COMPLETED) + { + int debugItemUniqueId = b3GetDebugItemUniqueId(statusHandle); + PyObject* item = PyInt_FromLong(debugItemUniqueId); + return item; + } + + PyErr_SetString(SpamError, "Error in addUserDebugLine."); + return NULL; } - - - -static PyObject* pybullet_removeUserDebugItem(PyObject* self, PyObject* args, PyObject *keywds) +static PyObject* pybullet_removeUserDebugItem(PyObject* self, PyObject* args, PyObject* keywds) { - b3SharedMemoryCommandHandle commandHandle; - b3SharedMemoryStatusHandle statusHandle; - int statusType; - int itemUniqueId; - int physicsClientId = 0; - b3PhysicsClientHandle sm = 0; + b3SharedMemoryCommandHandle commandHandle; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + int itemUniqueId; + int physicsClientId = 0; + b3PhysicsClientHandle sm = 0; - static char *kwlist[] = { "itemUniqueId", "physicsClientId", NULL }; + static char* kwlist[] = {"itemUniqueId", "physicsClientId", NULL}; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist,&itemUniqueId, &physicsClientId)) + if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist, &itemUniqueId, &physicsClientId)) { return NULL; } sm = getPhysicsClient(physicsClientId); - if (sm == 0) + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - commandHandle = b3InitUserDebugDrawRemove(sm,itemUniqueId); + commandHandle = b3InitUserDebugDrawRemove(sm, itemUniqueId); statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); statusType = b3GetStatusType(statusHandle); @@ -2584,38 +2594,36 @@ static PyObject* pybullet_removeUserDebugItem(PyObject* self, PyObject* args, Py return Py_None; } -static PyObject* pybullet_removeAllUserDebugItems(PyObject* self, PyObject* args, PyObject *keywds) +static PyObject* pybullet_removeAllUserDebugItems(PyObject* self, PyObject* args, PyObject* keywds) { - b3SharedMemoryCommandHandle commandHandle; - b3SharedMemoryStatusHandle statusHandle; - int statusType; - int physicsClientId = 0; - b3PhysicsClientHandle sm = 0; - static char *kwlist[] = { "physicsClientId", NULL }; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "|i", kwlist,&physicsClientId)) + b3SharedMemoryCommandHandle commandHandle; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + int physicsClientId = 0; + b3PhysicsClientHandle sm = 0; + static char* kwlist[] = {"physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "|i", kwlist, &physicsClientId)) { return NULL; } sm = getPhysicsClient(physicsClientId); - if (sm == 0) + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - commandHandle = b3InitUserDebugDrawRemoveAll(sm); statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); statusType = b3GetStatusType(statusHandle); - Py_INCREF(Py_None); return Py_None; } -static PyObject* pybullet_startStateLogging(PyObject* self, PyObject* args, PyObject *keywds) +static PyObject* pybullet_startStateLogging(PyObject* self, PyObject* args, PyObject* keywds) { b3SharedMemoryStatusHandle statusHandle; int statusType; @@ -2624,16 +2632,17 @@ static PyObject* pybullet_startStateLogging(PyObject* self, PyObject* args, PyOb int loggingType = -1; char* fileName = 0; PyObject* objectUniqueIdsObj = 0; + int maxLogDof = -1; - static char *kwlist[] = { "loggingType", "fileName", "objectUniqueIds", "physicsClientId", NULL }; + static char* kwlist[] = {"loggingType", "fileName", "objectUniqueIds", "maxLogDof", "physicsClientId", NULL}; int physicsClientId = 0; - - if (!PyArg_ParseTupleAndKeywords(args, keywds, "is|Oi", kwlist, - &loggingType, &fileName, &objectUniqueIdsObj,&physicsClientId)) + + if (!PyArg_ParseTupleAndKeywords(args, keywds, "is|Oii", kwlist, + &loggingType, &fileName, &objectUniqueIdsObj, &maxLogDof, &physicsClientId)) return NULL; sm = getPhysicsClient(physicsClientId); - if (sm == 0) + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; @@ -2641,8 +2650,8 @@ static PyObject* pybullet_startStateLogging(PyObject* self, PyObject* args, PyOb { b3SharedMemoryCommandHandle commandHandle; commandHandle = b3StateLoggingCommandInit(sm); - - b3StateLoggingStart(commandHandle,loggingType,fileName); + + b3StateLoggingStart(commandHandle, loggingType, fileName); if (objectUniqueIdsObj) { @@ -2651,19 +2660,23 @@ static PyObject* pybullet_startStateLogging(PyObject* self, PyObject* args, PyOb { int len = PySequence_Size(objectUniqueIdsObj); int i; - for ( i=0;i 0) + { + b3StateLoggingSetMaxLogDof(commandHandle, maxLogDof); + } + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); statusType = b3GetStatusType(statusHandle); - if (statusType==CMD_STATE_LOGGING_START_COMPLETED) + if (statusType == CMD_STATE_LOGGING_START_COMPLETED) { int loggingUniqueId = b3GetStatusLoggingUniqueId(statusHandle); PyObject* loggingUidObj = PyInt_FromLong(loggingUniqueId); @@ -2675,140 +2688,139 @@ static PyObject* pybullet_startStateLogging(PyObject* self, PyObject* args, PyOb return Py_None; } -static PyObject* pybullet_stopStateLogging(PyObject* self, PyObject* args, PyObject *keywds) +static PyObject* pybullet_stopStateLogging(PyObject* self, PyObject* args, PyObject* keywds) { b3SharedMemoryStatusHandle statusHandle; int statusType; - int loggingId=-1; + int loggingId = -1; b3PhysicsClientHandle sm = 0; - static char *kwlist[] = { "loggingId", "physicsClientId", NULL }; - int physicsClientId = 0; - + static char* kwlist[] = {"loggingId", "physicsClientId", NULL}; + int physicsClientId = 0; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist, - &loggingId,&physicsClientId)) + &loggingId, &physicsClientId)) return NULL; sm = getPhysicsClient(physicsClientId); - if (sm == 0) + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - if (loggingId>=0) + if (loggingId >= 0) { b3SharedMemoryCommandHandle commandHandle; commandHandle = b3StateLoggingCommandInit(sm); b3StateLoggingStop(commandHandle, loggingId); statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); statusType = b3GetStatusType(statusHandle); - } Py_INCREF(Py_None); return Py_None; } -static PyObject* pybullet_setTimeOut(PyObject* self, PyObject* args, PyObject *keywds) +static PyObject* pybullet_setTimeOut(PyObject* self, PyObject* args, PyObject* keywds) { - static char *kwlist[] = { "timeOutInSeconds", "physicsClientId", NULL }; + static char* kwlist[] = {"timeOutInSeconds", "physicsClientId", NULL}; double timeOutInSeconds = -1; int physicsClientId = 0; b3PhysicsClientHandle sm = 0; if (!PyArg_ParseTupleAndKeywords(args, keywds, "d|i", kwlist, - &timeOutInSeconds, &physicsClientId)) + &timeOutInSeconds, &physicsClientId)) return NULL; - if (timeOutInSeconds>=0) + if (timeOutInSeconds >= 0) { sm = getPhysicsClient(physicsClientId); - if (sm == 0) + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - b3SetTimeOut(sm,timeOutInSeconds); + b3SetTimeOut(sm, timeOutInSeconds); } Py_INCREF(Py_None); return Py_None; } -static PyObject* pybullet_rayTest(PyObject* self, PyObject* args, PyObject *keywds) +static PyObject* pybullet_rayTest(PyObject* self, PyObject* args, PyObject* keywds) { b3SharedMemoryCommandHandle commandHandle; b3SharedMemoryStatusHandle statusHandle; int statusType; - PyObject* rayFromObj=0; - PyObject* rayToObj=0; + PyObject* rayFromObj = 0; + PyObject* rayToObj = 0; double from[3]; double to[3]; b3PhysicsClientHandle sm = 0; - static char *kwlist[] = { "rayFromPosition", "rayToPosition", "physicsClientId", NULL }; - int physicsClientId = 0; - + static char* kwlist[] = {"rayFromPosition", "rayToPosition", "physicsClientId", NULL}; + int physicsClientId = 0; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "OO|i", kwlist, - &rayFromObj, &rayToObj,&physicsClientId)) + &rayFromObj, &rayToObj, &physicsClientId)) return NULL; sm = getPhysicsClient(physicsClientId); - if (sm == 0) + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - pybullet_internalSetVectord(rayFromObj,from); - pybullet_internalSetVectord(rayToObj,to); + pybullet_internalSetVectord(rayFromObj, from); + pybullet_internalSetVectord(rayToObj, to); - commandHandle = b3CreateRaycastCommandInit(sm, from[0],from[1],from[2], - to[0],to[1],to[2]); + commandHandle = b3CreateRaycastCommandInit(sm, from[0], from[1], from[2], + to[0], to[1], to[2]); statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); statusType = b3GetStatusType(statusHandle); - if (statusType==CMD_REQUEST_RAY_CAST_INTERSECTIONS_COMPLETED) + if (statusType == CMD_REQUEST_RAY_CAST_INTERSECTIONS_COMPLETED) { struct b3RaycastInformation raycastInfo; PyObject* rayHitsObj = 0; int i; b3GetRaycastInformation(sm, &raycastInfo); - + rayHitsObj = PyTuple_New(raycastInfo.m_numRayHits); - for (i=0;i=-1) + if (trackObjectUid >= -1) { - b3SetVRCameraTrackingObject(commandHandle,trackObjectUid); + b3SetVRCameraTrackingObject(commandHandle, trackObjectUid); } statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); @@ -2896,10 +2907,9 @@ static PyObject* pybullet_setVRCameraState(PyObject* self, PyObject* args, PyObj Py_INCREF(Py_None); return Py_None; - } -static PyObject* pybullet_getKeyboardEvents(PyObject* self, PyObject* args, PyObject *keywds) +static PyObject* pybullet_getKeyboardEvents(PyObject* self, PyObject* args, PyObject* keywds) { b3SharedMemoryCommandHandle commandHandle; b3SharedMemoryStatusHandle statusHandle; @@ -2908,16 +2918,16 @@ static PyObject* pybullet_getKeyboardEvents(PyObject* self, PyObject* args, PyOb b3PhysicsClientHandle sm = 0; struct b3KeyboardEventsData keyboardEventsData; PyObject* keyEventsObj = 0; - int i=0; + int i = 0; - static char *kwlist[] = { "physicsClientId", NULL }; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "|i", kwlist,&physicsClientId)) + static char* kwlist[] = {"physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "|i", kwlist, &physicsClientId)) { return NULL; } - + sm = getPhysicsClient(physicsClientId); - if (sm == 0) + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; @@ -2925,166 +2935,161 @@ static PyObject* pybullet_getKeyboardEvents(PyObject* self, PyObject* args, PyOb commandHandle = b3RequestKeyboardEventsCommandInit(sm); b3SubmitClientCommandAndWaitStatus(sm, commandHandle); - b3GetKeyboardEventsData(sm,&keyboardEventsData); + b3GetKeyboardEventsData(sm, &keyboardEventsData); keyEventsObj = PyDict_New(); - - for (i=0;i=0) && (cameraYaw>=0) && (cameraPitch>=0)) + int physicsClientId = 0; + b3PhysicsClientHandle sm = 0; + static char* kwlist[] = {"cameraDistance", "cameraYaw", "cameraPitch", "cameraTargetPosition", "physicsClientId", NULL}; + + if (!PyArg_ParseTupleAndKeywords(args, keywds, "fffO|i", kwlist, + &cameraDistance, &cameraYaw, &cameraPitch, &cameraTargetPosObj, &physicsClientId)) + return NULL; + + sm = getPhysicsClient(physicsClientId); + if (sm == 0) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + { + b3SharedMemoryCommandHandle commandHandle = b3InitConfigureOpenGLVisualizer(sm); + if ((cameraDistance >= 0)) { float cameraTargetPosition[3]; - if (pybullet_internalSetVector(cameraTargetPosObj,cameraTargetPosition)) + if (pybullet_internalSetVector(cameraTargetPosObj, cameraTargetPosition)) { - b3ConfigureOpenGLVisualizerSetViewMatrix(commandHandle,cameraDistance,cameraPitch, cameraYaw, cameraTargetPosition); + b3ConfigureOpenGLVisualizerSetViewMatrix(commandHandle, cameraDistance, cameraPitch, cameraYaw, cameraTargetPosition); } } - b3SubmitClientCommandAndWaitStatus(sm, commandHandle); - } - Py_INCREF(Py_None); - return Py_None; + b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + } + Py_INCREF(Py_None); + return Py_None; } -static PyObject* pybullet_setDebugObjectColor(PyObject* self, PyObject* args, PyObject *keywds) +static PyObject* pybullet_setDebugObjectColor(PyObject* self, PyObject* args, PyObject* keywds) { PyObject* objectColorRGBObj = 0; double objectColorRGB[3]; @@ -3092,15 +3097,15 @@ static PyObject* pybullet_setDebugObjectColor(PyObject* self, PyObject* args, Py int objectUniqueId = -1; int linkIndex = -2; int physicsClientId = 0; - b3PhysicsClientHandle sm=0; - static char *kwlist[] = { "objectUniqueId", "linkIndex","objectDebugColorRGB", "physicsClientId", NULL }; + b3PhysicsClientHandle sm = 0; + static char* kwlist[] = {"objectUniqueId", "linkIndex", "objectDebugColorRGB", "physicsClientId", NULL}; if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|Oi", kwlist, - &objectUniqueId, &linkIndex, &objectColorRGBObj,&physicsClientId)) + &objectUniqueId, &linkIndex, &objectColorRGBObj, &physicsClientId)) return NULL; sm = getPhysicsClient(physicsClientId); - if (sm == 0) + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; @@ -3124,9 +3129,8 @@ static PyObject* pybullet_setDebugObjectColor(PyObject* self, PyObject* args, Py Py_INCREF(Py_None); return Py_None; } - -static PyObject* pybullet_getVisualShapeData(PyObject* self, PyObject* args, PyObject* keywds) +static PyObject* pybullet_getVisualShapeData(PyObject* self, PyObject* args, PyObject* keywds) { int objectUniqueId = -1; b3SharedMemoryCommandHandle commandHandle; @@ -3137,22 +3141,19 @@ static PyObject* pybullet_getVisualShapeData(PyObject* self, PyObject* args, PyO PyObject* pyResultList = 0; int physicsClientId = 0; b3PhysicsClientHandle sm = 0; - static char *kwlist[] = { "objectUniqueId", "physicsClientId", NULL }; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist,&objectUniqueId,&physicsClientId)) + static char* kwlist[] = {"objectUniqueId", "physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist, &objectUniqueId, &physicsClientId)) { return NULL; } sm = getPhysicsClient(physicsClientId); - if (sm == 0) + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - { - - commandHandle = b3InitRequestVisualShapeInformation(sm, objectUniqueId); statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); statusType = b3GetStatusType(statusHandle); @@ -3166,10 +3167,10 @@ static PyObject* pybullet_getVisualShapeData(PyObject* self, PyObject* args, PyO PyObject* item; item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_objectUniqueId); PyTuple_SetItem(visualShapeObList, 0, item); - + item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_linkIndex); PyTuple_SetItem(visualShapeObList, 1, item); - + item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_visualGeometryType); PyTuple_SetItem(visualShapeObList, 2, item); @@ -3183,7 +3184,7 @@ static PyObject* pybullet_getVisualShapeData(PyObject* self, PyObject* args, PyO PyTuple_SetItem(vec, 2, item); PyTuple_SetItem(visualShapeObList, 3, vec); } - + item = PyString_FromString(visualShapeInfo.m_visualShapeData[i].m_meshAssetFileName); PyTuple_SetItem(visualShapeObList, 4, item); @@ -3211,19 +3212,18 @@ static PyObject* pybullet_getVisualShapeData(PyObject* self, PyObject* args, PyO PyTuple_SetItem(visualShapeObList, 6, vec); } - { - PyObject* rgba = PyTuple_New(4); - item = PyFloat_FromDouble(visualShapeInfo.m_visualShapeData[i].m_rgbaColor[0]); - PyTuple_SetItem(rgba, 0, item); - item = PyFloat_FromDouble(visualShapeInfo.m_visualShapeData[i].m_rgbaColor[1]); - PyTuple_SetItem(rgba, 1, item); - item = PyFloat_FromDouble(visualShapeInfo.m_visualShapeData[i].m_rgbaColor[2]); - PyTuple_SetItem(rgba, 2, item); - item = PyFloat_FromDouble(visualShapeInfo.m_visualShapeData[i].m_rgbaColor[3]); - PyTuple_SetItem(rgba, 3, item); - PyTuple_SetItem(visualShapeObList, 7, rgba); - } - + { + PyObject* rgba = PyTuple_New(4); + item = PyFloat_FromDouble(visualShapeInfo.m_visualShapeData[i].m_rgbaColor[0]); + PyTuple_SetItem(rgba, 0, item); + item = PyFloat_FromDouble(visualShapeInfo.m_visualShapeData[i].m_rgbaColor[1]); + PyTuple_SetItem(rgba, 1, item); + item = PyFloat_FromDouble(visualShapeInfo.m_visualShapeData[i].m_rgbaColor[2]); + PyTuple_SetItem(rgba, 2, item); + item = PyFloat_FromDouble(visualShapeInfo.m_visualShapeData[i].m_rgbaColor[3]); + PyTuple_SetItem(rgba, 3, item); + PyTuple_SetItem(visualShapeObList, 7, rgba); + } PyTuple_SetItem(pyResultList, i, visualShapeObList); } @@ -3240,94 +3240,89 @@ static PyObject* pybullet_getVisualShapeData(PyObject* self, PyObject* args, PyO return Py_None; } -static PyObject* pybullet_resetVisualShapeData(PyObject* self, PyObject* args,PyObject* keywds) +static PyObject* pybullet_resetVisualShapeData(PyObject* self, PyObject* args, PyObject* keywds) { - - int objectUniqueId = -1; - int jointIndex = -1; - int shapeIndex = -1; - int textureUniqueId = -1; - b3SharedMemoryCommandHandle commandHandle; - b3SharedMemoryStatusHandle statusHandle; - int statusType; - int physicsClientId = 0; + int objectUniqueId = -1; + int jointIndex = -1; + int shapeIndex = -1; + int textureUniqueId = -1; + b3SharedMemoryCommandHandle commandHandle; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + int physicsClientId = 0; b3PhysicsClientHandle sm = 0; - static char *kwlist[] = { "objectUniqueId", "jointIndex", "shapeIndex", "textureUniqueId", "physicsClientId", NULL }; + static char* kwlist[] = {"objectUniqueId", "jointIndex", "shapeIndex", "textureUniqueId", "physicsClientId", NULL}; if (!PyArg_ParseTupleAndKeywords(args, keywds, "iiii|i", kwlist, &objectUniqueId, &jointIndex, &shapeIndex, &textureUniqueId, &physicsClientId)) { return NULL; } sm = getPhysicsClient(physicsClientId); - if (sm == 0) + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - - { - - commandHandle = b3InitUpdateVisualShape(sm, objectUniqueId, jointIndex, shapeIndex, textureUniqueId); - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); - statusType = b3GetStatusType(statusHandle); - if (statusType == CMD_VISUAL_SHAPE_UPDATE_COMPLETED) - { - } - else - { - PyErr_SetString(SpamError, "Error resetting visual shape info"); - return NULL; - } - } - - Py_INCREF(Py_None); - return Py_None; + { + commandHandle = b3InitUpdateVisualShape(sm, objectUniqueId, jointIndex, shapeIndex, textureUniqueId); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + statusType = b3GetStatusType(statusHandle); + if (statusType == CMD_VISUAL_SHAPE_UPDATE_COMPLETED) + { + } + else + { + PyErr_SetString(SpamError, "Error resetting visual shape info"); + return NULL; + } + } + + Py_INCREF(Py_None); + return Py_None; } static PyObject* pybullet_loadTexture(PyObject* self, PyObject* args, PyObject* keywds) { - const char* filename = 0; - b3SharedMemoryCommandHandle commandHandle; - b3SharedMemoryStatusHandle statusHandle; - int statusType; - + const char* filename = 0; + b3SharedMemoryCommandHandle commandHandle; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + int physicsClientId = 0; b3PhysicsClientHandle sm = 0; - static char *kwlist[] = { "textureFilename", "physicsClientId", NULL }; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|i", kwlist,&filename, &physicsClientId)) + static char* kwlist[] = {"textureFilename", "physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|i", kwlist, &filename, &physicsClientId)) { return NULL; } sm = getPhysicsClient(physicsClientId); - if (sm == 0) + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - - { - - commandHandle = b3InitLoadTexture(sm, filename); - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); - statusType = b3GetStatusType(statusHandle); - if (statusType == CMD_LOAD_TEXTURE_COMPLETED) - { - } - else - { - PyErr_SetString(SpamError, "Error loading texture"); - return NULL; - } - } - - Py_INCREF(Py_None); - return Py_None; + + { + commandHandle = b3InitLoadTexture(sm, filename); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + statusType = b3GetStatusType(statusHandle); + if (statusType == CMD_LOAD_TEXTURE_COMPLETED) + { + } + else + { + PyErr_SetString(SpamError, "Error loading texture"); + return NULL; + } + } + + Py_INCREF(Py_None); + return Py_None; } - -static PyObject* MyConvertContactPoint( struct b3ContactInformation* contactPointPtr) +static PyObject* MyConvertContactPoint(struct b3ContactInformation* contactPointPtr) { - /* + /* 0 int m_contactFlags; 1 int m_bodyUniqueIdA; 2 int m_bodyUniqueIdB; @@ -3347,88 +3342,87 @@ static PyObject* MyConvertContactPoint( struct b3ContactInformation* contactPoin int i; PyObject* pyResultList = PyTuple_New(contactPointPtr->m_numContactPoints); - for (i = 0; i < contactPointPtr->m_numContactPoints; i++) { - PyObject* contactObList = PyTuple_New(10); // see above 10 fields - PyObject* item; - item = - PyInt_FromLong(contactPointPtr->m_contactPointData[i].m_contactFlags); - PyTuple_SetItem(contactObList, 0, item); - item = PyInt_FromLong( - contactPointPtr->m_contactPointData[i].m_bodyUniqueIdA); - PyTuple_SetItem(contactObList, 1, item); - item = PyInt_FromLong( - contactPointPtr->m_contactPointData[i].m_bodyUniqueIdB); - PyTuple_SetItem(contactObList, 2, item); - item = - PyInt_FromLong(contactPointPtr->m_contactPointData[i].m_linkIndexA); - PyTuple_SetItem(contactObList, 3, item); - item = - PyInt_FromLong(contactPointPtr->m_contactPointData[i].m_linkIndexB); - PyTuple_SetItem(contactObList, 4, item); + for (i = 0; i < contactPointPtr->m_numContactPoints; i++) + { + PyObject* contactObList = PyTuple_New(10); // see above 10 fields + PyObject* item; + item = + PyInt_FromLong(contactPointPtr->m_contactPointData[i].m_contactFlags); + PyTuple_SetItem(contactObList, 0, item); + item = PyInt_FromLong( + contactPointPtr->m_contactPointData[i].m_bodyUniqueIdA); + PyTuple_SetItem(contactObList, 1, item); + item = PyInt_FromLong( + contactPointPtr->m_contactPointData[i].m_bodyUniqueIdB); + PyTuple_SetItem(contactObList, 2, item); + item = + PyInt_FromLong(contactPointPtr->m_contactPointData[i].m_linkIndexA); + PyTuple_SetItem(contactObList, 3, item); + item = + PyInt_FromLong(contactPointPtr->m_contactPointData[i].m_linkIndexB); + PyTuple_SetItem(contactObList, 4, item); - { - PyObject* posAObj = PyTuple_New(3); + { + PyObject* posAObj = PyTuple_New(3); - item = PyFloat_FromDouble( - contactPointPtr->m_contactPointData[i].m_positionOnAInWS[0]); - PyTuple_SetItem(posAObj, 0, item); - item = PyFloat_FromDouble( - contactPointPtr->m_contactPointData[i].m_positionOnAInWS[1]); - PyTuple_SetItem(posAObj, 1, item); - item = PyFloat_FromDouble( - contactPointPtr->m_contactPointData[i].m_positionOnAInWS[2]); - PyTuple_SetItem(posAObj, 2, item); + item = PyFloat_FromDouble( + contactPointPtr->m_contactPointData[i].m_positionOnAInWS[0]); + PyTuple_SetItem(posAObj, 0, item); + item = PyFloat_FromDouble( + contactPointPtr->m_contactPointData[i].m_positionOnAInWS[1]); + PyTuple_SetItem(posAObj, 1, item); + item = PyFloat_FromDouble( + contactPointPtr->m_contactPointData[i].m_positionOnAInWS[2]); + PyTuple_SetItem(posAObj, 2, item); - PyTuple_SetItem(contactObList, 5, posAObj); - } + PyTuple_SetItem(contactObList, 5, posAObj); + } - { - PyObject* posBObj = PyTuple_New(3); + { + PyObject* posBObj = PyTuple_New(3); + item = PyFloat_FromDouble( + contactPointPtr->m_contactPointData[i].m_positionOnBInWS[0]); + PyTuple_SetItem(posBObj, 0, item); + item = PyFloat_FromDouble( + contactPointPtr->m_contactPointData[i].m_positionOnBInWS[1]); + PyTuple_SetItem(posBObj, 1, item); + item = PyFloat_FromDouble( + contactPointPtr->m_contactPointData[i].m_positionOnBInWS[2]); + PyTuple_SetItem(posBObj, 2, item); - item = PyFloat_FromDouble( - contactPointPtr->m_contactPointData[i].m_positionOnBInWS[0]); - PyTuple_SetItem(posBObj, 0, item); - item = PyFloat_FromDouble( - contactPointPtr->m_contactPointData[i].m_positionOnBInWS[1]); - PyTuple_SetItem(posBObj, 1, item); - item = PyFloat_FromDouble( - contactPointPtr->m_contactPointData[i].m_positionOnBInWS[2]); - PyTuple_SetItem(posBObj, 2, item); + PyTuple_SetItem(contactObList, 6, posBObj); + } - PyTuple_SetItem(contactObList, 6, posBObj); + { + PyObject* normalOnB = PyTuple_New(3); + item = PyFloat_FromDouble( + contactPointPtr->m_contactPointData[i].m_contactNormalOnBInWS[0]); + PyTuple_SetItem(normalOnB, 0, item); + item = PyFloat_FromDouble( + contactPointPtr->m_contactPointData[i].m_contactNormalOnBInWS[1]); + PyTuple_SetItem(normalOnB, 1, item); + item = PyFloat_FromDouble( + contactPointPtr->m_contactPointData[i].m_contactNormalOnBInWS[2]); + PyTuple_SetItem(normalOnB, 2, item); + PyTuple_SetItem(contactObList, 7, normalOnB); + } - } + item = PyFloat_FromDouble( + contactPointPtr->m_contactPointData[i].m_contactDistance); + PyTuple_SetItem(contactObList, 8, item); + item = PyFloat_FromDouble( + contactPointPtr->m_contactPointData[i].m_normalForce); + PyTuple_SetItem(contactObList, 9, item); - { - PyObject* normalOnB = PyTuple_New(3); - item = PyFloat_FromDouble( - contactPointPtr->m_contactPointData[i].m_contactNormalOnBInWS[0]); - PyTuple_SetItem(normalOnB, 0, item); - item = PyFloat_FromDouble( - contactPointPtr->m_contactPointData[i].m_contactNormalOnBInWS[1]); - PyTuple_SetItem(normalOnB, 1, item); - item = PyFloat_FromDouble( - contactPointPtr->m_contactPointData[i].m_contactNormalOnBInWS[2]); - PyTuple_SetItem(normalOnB, 2, item); - PyTuple_SetItem(contactObList, 7, normalOnB); - } - - item = PyFloat_FromDouble( - contactPointPtr->m_contactPointData[i].m_contactDistance); - PyTuple_SetItem(contactObList, 8, item); - item = PyFloat_FromDouble( - contactPointPtr->m_contactPointData[i].m_normalForce); - PyTuple_SetItem(contactObList, 9, item); - - PyTuple_SetItem(pyResultList, i, contactObList); - } - return pyResultList; + PyTuple_SetItem(pyResultList, i, contactObList); + } + return pyResultList; } -static PyObject* pybullet_getOverlappingObjects(PyObject* self, PyObject* args, PyObject *keywds) +static PyObject* pybullet_getOverlappingObjects(PyObject* self, PyObject* args, PyObject* keywds) { - PyObject* aabbMinOb=0, *aabbMaxOb=0; + PyObject *aabbMinOb = 0, *aabbMaxOb = 0; double aabbMin[3]; double aabbMax[3]; b3SharedMemoryCommandHandle commandHandle; @@ -3437,21 +3431,19 @@ static PyObject* pybullet_getOverlappingObjects(PyObject* self, PyObject* args, int i; int physicsClientId = 0; b3PhysicsClientHandle sm = 0; - static char *kwlist[] = { "aabbMin", "aabbMax", "physicsClientId",NULL }; - + static char* kwlist[] = {"aabbMin", "aabbMax", "physicsClientId", NULL}; if (!PyArg_ParseTupleAndKeywords(args, keywds, "OO|i", kwlist, - &aabbMinOb, &aabbMaxOb,&physicsClientId)) + &aabbMinOb, &aabbMaxOb, &physicsClientId)) return NULL; sm = getPhysicsClient(physicsClientId); - if (sm == 0) + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - pybullet_internalSetVectord(aabbMinOb, aabbMin); pybullet_internalSetVectord(aabbMaxOb, aabbMax); @@ -3465,8 +3457,9 @@ static PyObject* pybullet_getOverlappingObjects(PyObject* self, PyObject* args, //For huge amount of overlap, we could use numpy instead (see camera pixel data) //What would Python do with huge amount of data? Pass it onto TensorFlow! - for (i = 0; i < overlapData.m_numOverlappingObjects; i++) { - PyObject* overlap = PyTuple_New(2);//body unique id and link index + for (i = 0; i < overlapData.m_numOverlappingObjects; i++) + { + PyObject* overlap = PyTuple_New(2); //body unique id and link index PyObject* item; item = @@ -3485,163 +3478,155 @@ static PyObject* pybullet_getOverlappingObjects(PyObject* self, PyObject* args, return Py_None; } - -static PyObject* pybullet_getClosestPointData(PyObject* self, PyObject* args, PyObject *keywds) +static PyObject* pybullet_getClosestPointData(PyObject* self, PyObject* args, PyObject* keywds) { - int bodyUniqueIdA = -1; - int bodyUniqueIdB = -1; - int linkIndexA = -2; - int linkIndexB = -2; + int bodyUniqueIdA = -1; + int bodyUniqueIdB = -1; + int linkIndexA = -2; + int linkIndexB = -2; - double distanceThreshold = 0.f; + double distanceThreshold = 0.f; - b3SharedMemoryCommandHandle commandHandle; - struct b3ContactInformation contactPointData; - b3SharedMemoryStatusHandle statusHandle; - int statusType; - int physicsClientId = 0; - b3PhysicsClientHandle sm = 0; - static char *kwlist[] = { "bodyA", "bodyB", "distance", "linkIndexA","linkIndexB","physicsClientId", NULL }; + b3SharedMemoryCommandHandle commandHandle; + struct b3ContactInformation contactPointData; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + int physicsClientId = 0; + b3PhysicsClientHandle sm = 0; + static char* kwlist[] = {"bodyA", "bodyB", "distance", "linkIndexA", "linkIndexB", "physicsClientId", NULL}; - - - if (!PyArg_ParseTupleAndKeywords(args, keywds, "iid|iii", kwlist, - &bodyUniqueIdA, &bodyUniqueIdB, &distanceThreshold, &linkIndexA, &linkIndexB,&physicsClientId)) - return NULL; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "iid|iii", kwlist, + &bodyUniqueIdA, &bodyUniqueIdB, &distanceThreshold, &linkIndexA, &linkIndexB, &physicsClientId)) + return NULL; sm = getPhysicsClient(physicsClientId); - if (sm == 0) + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - commandHandle = b3InitClosestDistanceQuery(sm); - b3SetClosestDistanceFilterBodyA(commandHandle, bodyUniqueIdA); - b3SetClosestDistanceFilterBodyB(commandHandle, bodyUniqueIdB); - b3SetClosestDistanceThreshold(commandHandle, distanceThreshold); - if (linkIndexA >= -1) - { - b3SetClosestDistanceFilterLinkA(commandHandle, linkIndexA); - } - if (linkIndexB >= -1) - { - b3SetClosestDistanceFilterLinkB(commandHandle, linkIndexB); - } + commandHandle = b3InitClosestDistanceQuery(sm); + b3SetClosestDistanceFilterBodyA(commandHandle, bodyUniqueIdA); + b3SetClosestDistanceFilterBodyB(commandHandle, bodyUniqueIdB); + b3SetClosestDistanceThreshold(commandHandle, distanceThreshold); + if (linkIndexA >= -1) + { + b3SetClosestDistanceFilterLinkA(commandHandle, linkIndexA); + } + if (linkIndexB >= -1) + { + b3SetClosestDistanceFilterLinkB(commandHandle, linkIndexB); + } - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); - statusType = b3GetStatusType(statusHandle); - if (statusType == CMD_CONTACT_POINT_INFORMATION_COMPLETED) { - + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + statusType = b3GetStatusType(statusHandle); + if (statusType == CMD_CONTACT_POINT_INFORMATION_COMPLETED) + { + b3GetContactPointInformation(sm, &contactPointData); - b3GetContactPointInformation(sm, &contactPointData); + return MyConvertContactPoint(&contactPointData); + } - return MyConvertContactPoint(&contactPointData); - - } - - Py_INCREF(Py_None); - return Py_None; + Py_INCREF(Py_None); + return Py_None; } - -static PyObject* pybullet_changeUserConstraint(PyObject* self, PyObject* args, PyObject *keywds) +static PyObject* pybullet_changeUserConstraint(PyObject* self, PyObject* args, PyObject* keywds) { - static char *kwlist[] = { "userConstraintUniqueId","jointChildPivot", "jointChildFrameOrientation","maxForce", "physicsClientId", NULL}; - int userConstraintUniqueId=-1; + static char* kwlist[] = {"userConstraintUniqueId", "jointChildPivot", "jointChildFrameOrientation", "maxForce", "physicsClientId", NULL}; + int userConstraintUniqueId = -1; b3SharedMemoryCommandHandle commandHandle; b3SharedMemoryStatusHandle statusHandle; int statusType; int physicsClientId = 0; b3PhysicsClientHandle sm = 0; - PyObject* jointChildPivotObj=0; - PyObject* jointChildFrameOrnObj=0; + PyObject* jointChildPivotObj = 0; + PyObject* jointChildFrameOrnObj = 0; double jointChildPivot[3]; double jointChildFrameOrn[4]; double maxForce = -1; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|OOdi", kwlist,&userConstraintUniqueId,&jointChildPivotObj, &jointChildFrameOrnObj,&maxForce, &physicsClientId)) - { - return NULL; - } - - sm = getPhysicsClient(physicsClientId); - if (sm == 0) - { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } - - commandHandle = b3InitChangeUserConstraintCommand(sm,userConstraintUniqueId); - - if (pybullet_internalSetVectord(jointChildPivotObj,jointChildPivot)) - { - b3InitChangeUserConstraintSetPivotInB(commandHandle, jointChildPivot); - } - if (pybullet_internalSetVector4d(jointChildFrameOrnObj,jointChildFrameOrn)) - { - b3InitChangeUserConstraintSetFrameInB(commandHandle, jointChildFrameOrn); - } - if (maxForce>=0) - { - b3InitChangeUserConstraintSetMaxForce(commandHandle,maxForce); - } - - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); - statusType = b3GetStatusType(statusHandle); - Py_INCREF(Py_None); - return Py_None; -}; - - -static PyObject* pybullet_removeUserConstraint(PyObject* self, PyObject* args, PyObject *keywds) -{ - static char *kwlist[] = { "userConstraintUniqueId","physicsClientId", NULL}; - int userConstraintUniqueId=-1; - b3SharedMemoryCommandHandle commandHandle; - b3SharedMemoryStatusHandle statusHandle; - int statusType; - int physicsClientId = 0; - b3PhysicsClientHandle sm = 0; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist,&userConstraintUniqueId,&physicsClientId)) - { - return NULL; - } - - sm = getPhysicsClient(physicsClientId); - if (sm == 0) - { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } - - commandHandle = b3InitRemoveUserConstraintCommand(sm,userConstraintUniqueId); - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); - statusType = b3GetStatusType(statusHandle); - Py_INCREF(Py_None); - return Py_None; -}; - - -/* -static PyObject* pybullet_updateUserConstraint(PyObject* self, PyObject* args, PyObject *keywds) -{ - return NULL; -} -*/ - -static PyObject* pybullet_enableJointForceTorqueSensor(PyObject* self, PyObject* args, PyObject *keywds) -{ - int bodyUniqueId = -1; - int jointIndex = -1; - int enableSensor = 1; - int physicsClientId = 0; - b3PhysicsClientHandle sm = 0; - int numJoints = -1; - - static char *kwlist[] = {"bodyUniqueId", "jointIndex" ,"enableSensor", "physicsClientId" }; - - if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|ii", kwlist, &bodyUniqueId, &jointIndex, &enableSensor,&physicsClientId)) + if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|OOdi", kwlist, &userConstraintUniqueId, &jointChildPivotObj, &jointChildFrameOrnObj, &maxForce, &physicsClientId)) + { + return NULL; + } + + sm = getPhysicsClient(physicsClientId); + if (sm == 0) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + commandHandle = b3InitChangeUserConstraintCommand(sm, userConstraintUniqueId); + + if (pybullet_internalSetVectord(jointChildPivotObj, jointChildPivot)) + { + b3InitChangeUserConstraintSetPivotInB(commandHandle, jointChildPivot); + } + if (pybullet_internalSetVector4d(jointChildFrameOrnObj, jointChildFrameOrn)) + { + b3InitChangeUserConstraintSetFrameInB(commandHandle, jointChildFrameOrn); + } + if (maxForce >= 0) + { + b3InitChangeUserConstraintSetMaxForce(commandHandle, maxForce); + } + + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + statusType = b3GetStatusType(statusHandle); + Py_INCREF(Py_None); + return Py_None; +}; + +static PyObject* pybullet_removeUserConstraint(PyObject* self, PyObject* args, PyObject* keywds) +{ + static char* kwlist[] = {"userConstraintUniqueId", "physicsClientId", NULL}; + int userConstraintUniqueId = -1; + b3SharedMemoryCommandHandle commandHandle; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + int physicsClientId = 0; + b3PhysicsClientHandle sm = 0; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist, &userConstraintUniqueId, &physicsClientId)) + { + return NULL; + } + + sm = getPhysicsClient(physicsClientId); + if (sm == 0) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + commandHandle = b3InitRemoveUserConstraintCommand(sm, userConstraintUniqueId); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + statusType = b3GetStatusType(statusHandle); + Py_INCREF(Py_None); + return Py_None; +}; + +/* +static PyObject* pybullet_updateUserConstraint(PyObject* self, PyObject* args, PyObject *keywds) +{ + return NULL; +} +*/ + +static PyObject* pybullet_enableJointForceTorqueSensor(PyObject* self, PyObject* args, PyObject* keywds) +{ + int bodyUniqueId = -1; + int jointIndex = -1; + int enableSensor = 1; + int physicsClientId = 0; + b3PhysicsClientHandle sm = 0; + int numJoints = -1; + + static char* kwlist[] = {"bodyUniqueId", "jointIndex", "enableSensor", "physicsClientId"}; + + if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|ii", kwlist, &bodyUniqueId, &jointIndex, &enableSensor, &physicsClientId)) { return NULL; } @@ -3653,7 +3638,6 @@ static PyObject* pybullet_enableJointForceTorqueSensor(PyObject* self, PyObject* return NULL; } - if (bodyUniqueId < 0) { PyErr_SetString(SpamError, "Error: invalid bodyUniqueId"); @@ -3686,71 +3670,66 @@ static PyObject* pybullet_enableJointForceTorqueSensor(PyObject* self, PyObject* return NULL; } - -static PyObject* pybullet_createUserConstraint(PyObject* self, PyObject* args, PyObject *keywds) +static PyObject* pybullet_createUserConstraint(PyObject* self, PyObject* args, PyObject* keywds) { - b3SharedMemoryCommandHandle commandHandle; - int parentBodyUniqueId=-1; - int parentLinkIndex=-1; - int childBodyUniqueId=-1; - int childLinkIndex=-1; - int jointType=ePoint2PointType; - PyObject* jointAxisObj=0; - double jointAxis[3]={0,0,0}; + int parentBodyUniqueId = -1; + int parentLinkIndex = -1; + int childBodyUniqueId = -1; + int childLinkIndex = -1; + int jointType = ePoint2PointType; + PyObject* jointAxisObj = 0; + double jointAxis[3] = {0, 0, 0}; PyObject* parentFramePositionObj = 0; - double parentFramePosition[3]={0,0,0}; + double parentFramePosition[3] = {0, 0, 0}; PyObject* childFramePositionObj = 0; - double childFramePosition[3]={0,0,0}; + double childFramePosition[3] = {0, 0, 0}; PyObject* parentFrameOrientationObj = 0; - double parentFrameOrientation[4]={0,0,0,1}; + double parentFrameOrientation[4] = {0, 0, 0, 1}; PyObject* childFrameOrientationObj = 0; - double childFrameOrientation[4]={0,0,0,1}; + double childFrameOrientation[4] = {0, 0, 0, 1}; struct b3JointInfo jointInfo; b3SharedMemoryStatusHandle statusHandle; int statusType; int physicsClientId = 0; b3PhysicsClientHandle sm = 0; - static char *kwlist[] = { "parentBodyUniqueId", "parentLinkIndex", - "childBodyUniqueId", "childLinkIndex", - "jointType", - "jointAxis", - "parentFramePosition", - "childFramePosition", - "parentFrameOrientation", - "childFrameOrientation", - "physicsClientId", - NULL }; + static char* kwlist[] = {"parentBodyUniqueId", "parentLinkIndex", + "childBodyUniqueId", "childLinkIndex", + "jointType", + "jointAxis", + "parentFramePosition", + "childFramePosition", + "parentFrameOrientation", + "childFrameOrientation", + "physicsClientId", + NULL}; - - - if (!PyArg_ParseTupleAndKeywords(args, keywds, "iiiiiOOO|OOi", kwlist,&parentBodyUniqueId,&parentLinkIndex, - &childBodyUniqueId,&childLinkIndex, - &jointType,&jointAxisObj, - &parentFramePositionObj, - &childFramePositionObj, - &parentFrameOrientationObj, - &childFrameOrientationObj, - &physicsClientId - )) + if (!PyArg_ParseTupleAndKeywords(args, keywds, "iiiiiOOO|OOi", kwlist, &parentBodyUniqueId, &parentLinkIndex, + &childBodyUniqueId, &childLinkIndex, + &jointType, &jointAxisObj, + &parentFramePositionObj, + &childFramePositionObj, + &parentFrameOrientationObj, + &childFrameOrientationObj, + &physicsClientId)) { return NULL; } - + sm = getPhysicsClient(physicsClientId); - if (sm == 0) + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - pybullet_internalSetVectord(jointAxisObj,jointAxis); - pybullet_internalSetVectord(parentFramePositionObj,parentFramePosition); - pybullet_internalSetVectord(childFramePositionObj,childFramePosition); - pybullet_internalSetVector4d(parentFrameOrientationObj,parentFrameOrientation); - pybullet_internalSetVector4d(childFrameOrientationObj,childFrameOrientation); - + pybullet_internalSetVectord(jointAxisObj, jointAxis); + pybullet_internalSetVectord(parentFramePositionObj, parentFramePosition); + pybullet_internalSetVectord(childFramePositionObj, childFramePosition); + pybullet_internalSetVector4d(parentFrameOrientationObj, parentFrameOrientation); + pybullet_internalSetVector4d(childFrameOrientationObj, childFrameOrientation); + jointInfo.m_jointType = jointType; jointInfo.m_parentFrame[0] = parentFramePosition[0]; jointInfo.m_parentFrame[1] = parentFramePosition[1]; @@ -3767,103 +3746,99 @@ static PyObject* pybullet_createUserConstraint(PyObject* self, PyObject* args, P jointInfo.m_childFrame[4] = childFrameOrientation[1]; jointInfo.m_childFrame[5] = childFrameOrientation[2]; jointInfo.m_childFrame[6] = childFrameOrientation[3]; - + jointInfo.m_jointAxis[0] = jointAxis[0]; jointInfo.m_jointAxis[1] = jointAxis[1]; jointInfo.m_jointAxis[2] = jointAxis[2]; - commandHandle = b3InitCreateUserConstraintCommand(sm, parentBodyUniqueId, parentLinkIndex, childBodyUniqueId, childLinkIndex, &jointInfo); + commandHandle = b3InitCreateUserConstraintCommand(sm, parentBodyUniqueId, parentLinkIndex, childBodyUniqueId, childLinkIndex, &jointInfo); statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); statusType = b3GetStatusType(statusHandle); - if (statusType==CMD_USER_CONSTRAINT_COMPLETED) + if (statusType == CMD_USER_CONSTRAINT_COMPLETED) { int userConstraintUid = b3GetStatusUserConstraintUniqueId(statusHandle); PyObject* ob = PyLong_FromLong(userConstraintUid); return ob; } - PyErr_SetString(SpamError, "createConstraint failed."); return NULL; } -static PyObject* pybullet_getContactPointData(PyObject* self, PyObject* args, PyObject *keywds) { - int bodyUniqueIdA = -1; - int bodyUniqueIdB = -1; - - b3SharedMemoryCommandHandle commandHandle; - struct b3ContactInformation contactPointData; - b3SharedMemoryStatusHandle statusHandle; - int statusType; +static PyObject* pybullet_getContactPointData(PyObject* self, PyObject* args, PyObject* keywds) +{ + int bodyUniqueIdA = -1; + int bodyUniqueIdB = -1; + b3SharedMemoryCommandHandle commandHandle; + struct b3ContactInformation contactPointData; + b3SharedMemoryStatusHandle statusHandle; + int statusType; - static char *kwlist[] = { "bodyA", "bodyB","physicsClientId", NULL }; + static char* kwlist[] = {"bodyA", "bodyB", "physicsClientId", NULL}; - int physicsClientId = 0; - b3PhysicsClientHandle sm = 0; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "|iii", kwlist, - &bodyUniqueIdA, &bodyUniqueIdB,&physicsClientId)) - return NULL; + int physicsClientId = 0; + b3PhysicsClientHandle sm = 0; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "|iii", kwlist, + &bodyUniqueIdA, &bodyUniqueIdB, &physicsClientId)) + return NULL; sm = getPhysicsClient(physicsClientId); - if (sm == 0) + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - commandHandle = b3InitRequestContactPointInformation(sm); - b3SetContactFilterBodyA(commandHandle, bodyUniqueIdA); - b3SetContactFilterBodyB(commandHandle, bodyUniqueIdB); - //b3SetContactQueryMode(commandHandle, mode); + commandHandle = b3InitRequestContactPointInformation(sm); + b3SetContactFilterBodyA(commandHandle, bodyUniqueIdA); + b3SetContactFilterBodyB(commandHandle, bodyUniqueIdB); + //b3SetContactQueryMode(commandHandle, mode); - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); - statusType = b3GetStatusType(statusHandle); - if (statusType == CMD_CONTACT_POINT_INFORMATION_COMPLETED) { - - b3GetContactPointInformation(sm, &contactPointData); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + statusType = b3GetStatusType(statusHandle); + if (statusType == CMD_CONTACT_POINT_INFORMATION_COMPLETED) + { + b3GetContactPointInformation(sm, &contactPointData); - return MyConvertContactPoint(&contactPointData); - - } + return MyConvertContactPoint(&contactPointData); + } - Py_INCREF(Py_None); - return Py_None; + Py_INCREF(Py_None); + return Py_None; } - - /// Render an image from the current timestep of the simulation, width, height are required, other args are optional // getCameraImage(w, h, view[16], projection[16], lightDir[3], lightColor[3], lightDist, hasShadow, lightAmbientCoeff, lightDiffuseCoeff, lightSpecularCoeff, renderer) -static PyObject* pybullet_getCameraImage(PyObject* self, PyObject* args, PyObject *keywds) +static PyObject* pybullet_getCameraImage(PyObject* self, PyObject* args, PyObject* keywds) { /// request an image from a simulated camera, using software or hardware renderer. struct b3CameraImageData imageData; - PyObject* objViewMat = 0, *objProjMat = 0, *lightDirObj = 0, *lightColorObj = 0; + PyObject *objViewMat = 0, *objProjMat = 0, *lightDirObj = 0, *lightColorObj = 0; int width, height; float viewMatrix[16]; float projectionMatrix[16]; float lightDir[3]; - float lightColor[3]; - float lightDist = 10.0; - int hasShadow = 0; - float lightAmbientCoeff = 0.6; - float lightDiffuseCoeff = 0.35; - float lightSpecularCoeff = 0.05; - int renderer = 0; + float lightColor[3]; + float lightDist = 10.0; + int hasShadow = 0; + float lightAmbientCoeff = 0.6; + float lightDiffuseCoeff = 0.35; + float lightSpecularCoeff = 0.05; + int renderer = 0; // inialize cmd b3SharedMemoryCommandHandle command; int physicsClientId = 0; b3PhysicsClientHandle sm = 0; // set camera resolution, optionally view, projection matrix, light direction, light color, light distance, shadow - static char *kwlist[] = { "width", "height", "viewMatrix", "projectionMatrix", "lightDirection", "lightColor", "lightDistance", "shadow", "lightAmbientCoeff", "lightDiffuseCoeff", "lightSpecularCoeff", "renderer", "physicsClientId", NULL }; + static char* kwlist[] = {"width", "height", "viewMatrix", "projectionMatrix", "lightDirection", "lightColor", "lightDistance", "shadow", "lightAmbientCoeff", "lightDiffuseCoeff", "lightSpecularCoeff", "renderer", "physicsClientId", NULL}; if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|OOOOfifffii", kwlist, &width, &height, &objViewMat, &objProjMat, &lightDirObj, &lightColorObj, &lightDist, &hasShadow, &lightAmbientCoeff, &lightDiffuseCoeff, &lightSpecularCoeff, &renderer, &physicsClientId)) { return NULL; } sm = getPhysicsClient(physicsClientId); - if (sm == 0) + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; @@ -3882,21 +3857,21 @@ static PyObject* pybullet_getCameraImage(PyObject* self, PyObject* args, PyObjec { b3RequestCameraImageSetLightDirection(command, lightDir); } - //set light color only if function succeeds - if (pybullet_internalSetVector(lightColorObj, lightColor)) - { - b3RequestCameraImageSetLightColor(command, lightColor); - } + //set light color only if function succeeds + if (pybullet_internalSetVector(lightColorObj, lightColor)) + { + b3RequestCameraImageSetLightColor(command, lightColor); + } - b3RequestCameraImageSetLightDistance(command, lightDist); - - b3RequestCameraImageSetShadow(command, hasShadow); - - b3RequestCameraImageSetLightAmbientCoeff(command, lightAmbientCoeff); - b3RequestCameraImageSetLightDiffuseCoeff(command, lightDiffuseCoeff); - b3RequestCameraImageSetLightSpecularCoeff(command, lightSpecularCoeff); - - b3RequestCameraImageSelectRenderer(command, renderer); + b3RequestCameraImageSetLightDistance(command, lightDist); + + b3RequestCameraImageSetShadow(command, hasShadow); + + b3RequestCameraImageSetLightAmbientCoeff(command, lightAmbientCoeff); + b3RequestCameraImageSetLightDiffuseCoeff(command, lightDiffuseCoeff); + b3RequestCameraImageSetLightSpecularCoeff(command, lightSpecularCoeff); + + b3RequestCameraImageSelectRenderer(command, renderer); if (b3CanSubmitCommand(sm)) { @@ -3907,7 +3882,8 @@ static PyObject* pybullet_getCameraImage(PyObject* self, PyObject* args, PyObjec statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); statusType = b3GetStatusType(statusHandle); - if (statusType == CMD_CAMERA_IMAGE_COMPLETED) { + if (statusType == CMD_CAMERA_IMAGE_COMPLETED) + { PyObject* item2; PyObject* pyResultList; // store 4 elements in this result: width, // height, rgbData, depth @@ -3927,26 +3903,26 @@ static PyObject* pybullet_getCameraImage(PyObject* self, PyObject* args, PyObjec int bytesPerPixel = 4; // Red, Green, Blue, and Alpha each 8 bit values - npy_intp rgb_dims[3] = { imageData.m_pixelHeight, imageData.m_pixelWidth, - bytesPerPixel }; - npy_intp dep_dims[2] = { imageData.m_pixelHeight, imageData.m_pixelWidth }; - npy_intp seg_dims[2] = { imageData.m_pixelHeight, imageData.m_pixelWidth }; + npy_intp rgb_dims[3] = {imageData.m_pixelHeight, imageData.m_pixelWidth, + bytesPerPixel}; + npy_intp dep_dims[2] = {imageData.m_pixelHeight, imageData.m_pixelWidth}; + npy_intp seg_dims[2] = {imageData.m_pixelHeight, imageData.m_pixelWidth}; pyRGB = PyArray_SimpleNew(3, rgb_dims, NPY_UINT8); pyDep = PyArray_SimpleNew(2, dep_dims, NPY_FLOAT32); pySeg = PyArray_SimpleNew(2, seg_dims, NPY_INT32); memcpy(PyArray_DATA(pyRGB), imageData.m_rgbColorData, - imageData.m_pixelHeight * imageData.m_pixelWidth * bytesPerPixel); + imageData.m_pixelHeight * imageData.m_pixelWidth * bytesPerPixel); memcpy(PyArray_DATA(pyDep), imageData.m_depthValues, - imageData.m_pixelHeight * imageData.m_pixelWidth); + imageData.m_pixelHeight * imageData.m_pixelWidth); memcpy(PyArray_DATA(pySeg), imageData.m_segmentationMaskValues, - imageData.m_pixelHeight * imageData.m_pixelWidth); + imageData.m_pixelHeight * imageData.m_pixelWidth); PyTuple_SetItem(pyResultList, 2, pyRGB); PyTuple_SetItem(pyResultList, 3, pyDep); PyTuple_SetItem(pyResultList, 4, pySeg); -#else//PYBULLET_USE_NUMPY +#else //PYBULLET_USE_NUMPY PyObject* pylistRGB; PyObject* pylistDep; PyObject* pylistSeg; @@ -3969,8 +3945,10 @@ static PyObject* pybullet_getCameraImage(PyObject* self, PyObject* args, PyObjec PyTuple_New(imageData.m_pixelWidth * imageData.m_pixelHeight); pylistSeg = PyTuple_New(imageData.m_pixelWidth * imageData.m_pixelHeight); - for (i = 0; i < imageData.m_pixelWidth; i++) { - for (j = 0; j < imageData.m_pixelHeight; j++) { + for (i = 0; i < imageData.m_pixelWidth; i++) + { + for (j = 0; j < imageData.m_pixelHeight; j++) + { // TODO(hellojas): validate depth values make sense int depIndex = i + j * imageData.m_pixelWidth; { @@ -3983,7 +3961,8 @@ static PyObject* pybullet_getCameraImage(PyObject* self, PyObject* args, PyObjec PyTuple_SetItem(pylistSeg, depIndex, item2); } - for (p = 0; p < bytesPerPixel; p++) { + for (p = 0; p < bytesPerPixel; p++) + { int pixelIndex = bytesPerPixel * (i + j * imageData.m_pixelWidth) + p; item = PyInt_FromLong(imageData.m_rgbColorData[pixelIndex]); @@ -3997,7 +3976,7 @@ static PyObject* pybullet_getCameraImage(PyObject* self, PyObject* args, PyObjec PyTuple_SetItem(pyResultList, 3, pylistDep); PyTuple_SetItem(pyResultList, 4, pylistSeg); return pyResultList; -#endif//PYBULLET_USE_NUMPY +#endif //PYBULLET_USE_NUMPY return pyResultList; } @@ -4005,12 +3984,9 @@ static PyObject* pybullet_getCameraImage(PyObject* self, PyObject* args, PyObjec Py_INCREF(Py_None); return Py_None; - } - - -static PyObject* pybullet_computeViewMatrix(PyObject* self, PyObject* args, PyObject *keywds) +static PyObject* pybullet_computeViewMatrix(PyObject* self, PyObject* args, PyObject* keywds) { PyObject* camEyeObj = 0; PyObject* camTargetPositionObj = 0; @@ -4020,7 +3996,7 @@ static PyObject* pybullet_computeViewMatrix(PyObject* self, PyObject* args, PyOb float camUpVector[3]; // set camera resolution, optionally view, projection matrix, light position - static char *kwlist[] = { "cameraEyePosition", "cameraTargetPosition", "cameraUpVector",NULL }; + static char* kwlist[] = {"cameraEyePosition", "cameraTargetPosition", "cameraUpVector", NULL}; if (!PyArg_ParseTupleAndKeywords(args, keywds, "OOO", kwlist, &camEyeObj, &camTargetPositionObj, &camUpVectorObj)) { @@ -4032,7 +4008,7 @@ static PyObject* pybullet_computeViewMatrix(PyObject* self, PyObject* args, PyOb pybullet_internalSetVector(camUpVectorObj, camUpVector)) { float viewMatrix[16]; - PyObject* pyResultList=0; + PyObject* pyResultList = 0; int i; b3ComputeViewMatrixFromPositions(camEye, camTargetPosition, camUpVector, viewMatrix); @@ -4050,7 +4026,7 @@ static PyObject* pybullet_computeViewMatrix(PyObject* self, PyObject* args, PyOb } ///compute a view matrix, helper function for b3RequestCameraImageSetCameraMatrices -static PyObject* pybullet_computeViewMatrixFromYawPitchRoll(PyObject* self, PyObject* args, PyObject *keywds) +static PyObject* pybullet_computeViewMatrixFromYawPitchRoll(PyObject* self, PyObject* args, PyObject* keywds) { PyObject* cameraTargetPositionObj = 0; float cameraTargetPosition[3]; @@ -4061,9 +4037,9 @@ static PyObject* pybullet_computeViewMatrixFromYawPitchRoll(PyObject* self, PyOb int i; // set camera resolution, optionally view, projection matrix, light position - static char *kwlist[] = { "cameraTargetPosition", "distance", "yaw", "pitch", "roll", "upAxisIndex" ,NULL }; - - if (!PyArg_ParseTupleAndKeywords(args, keywds, "Offffi", kwlist, &cameraTargetPositionObj, &distance,&yaw,&pitch,&roll, &upAxisIndex)) + static char* kwlist[] = {"cameraTargetPosition", "distance", "yaw", "pitch", "roll", "upAxisIndex", NULL}; + + if (!PyArg_ParseTupleAndKeywords(args, keywds, "Offffi", kwlist, &cameraTargetPositionObj, &distance, &yaw, &pitch, &roll, &upAxisIndex)) { return NULL; } @@ -4083,12 +4059,10 @@ static PyObject* pybullet_computeViewMatrixFromYawPitchRoll(PyObject* self, PyOb PyTuple_SetItem(pyResultList, i, item); } return pyResultList; - - } ///compute a projection matrix, helper function for b3RequestCameraImageSetCameraMatrices -static PyObject* pybullet_computeProjectionMatrix(PyObject* self, PyObject* args, PyObject *keywds) +static PyObject* pybullet_computeProjectionMatrix(PyObject* self, PyObject* args, PyObject* keywds) { PyObject* pyResultList = 0; float left; @@ -4101,14 +4075,13 @@ static PyObject* pybullet_computeProjectionMatrix(PyObject* self, PyObject* args int i; // set camera resolution, optionally view, projection matrix, light position - static char *kwlist[] = { "left", "right", "bottom", "top", "nearVal", "farVal" ,NULL }; + static char* kwlist[] = {"left", "right", "bottom", "top", "nearVal", "farVal", NULL}; if (!PyArg_ParseTupleAndKeywords(args, keywds, "ffffff", kwlist, &left, &right, &bottom, &top, &nearVal, &farVal)) { return NULL; } - b3ComputeProjectionMatrix(left, right, bottom, top, nearVal, farVal, projectionMatrix); pyResultList = PyTuple_New(16); @@ -4118,23 +4091,22 @@ static PyObject* pybullet_computeProjectionMatrix(PyObject* self, PyObject* args PyTuple_SetItem(pyResultList, i, item); } return pyResultList; - } -static PyObject* pybullet_computeProjectionMatrixFOV(PyObject* self, PyObject* args, PyObject *keywds) +static PyObject* pybullet_computeProjectionMatrixFOV(PyObject* self, PyObject* args, PyObject* keywds) { - float fov, aspect, nearVal, farVal; + float fov, aspect, nearVal, farVal; PyObject* pyResultList = 0; float projectionMatrix[16]; int i; - static char *kwlist[] = { "fov","aspect","nearVal","farVal",NULL }; + static char* kwlist[] = {"fov", "aspect", "nearVal", "farVal", NULL}; if (!PyArg_ParseTupleAndKeywords(args, keywds, "ffff", kwlist, &fov, &aspect, &nearVal, &farVal)) { return NULL; } - + b3ComputeProjectionMatrixFOV(fov, aspect, nearVal, farVal, projectionMatrix); pyResultList = PyTuple_New(16); @@ -4144,10 +4116,8 @@ static PyObject* pybullet_computeProjectionMatrixFOV(PyObject* self, PyObject* a PyTuple_SetItem(pyResultList, i, item); } return pyResultList; - } - // Render an image from the current timestep of the simulation // // Examples: @@ -4171,561 +4141,620 @@ static PyObject* pybullet_computeProjectionMatrixFOV(PyObject* self, PyObject* a // TODO(hellojas): fix image is cut off at head // TODO(hellojas): should we add check to give minimum image resolution // to see object based on camera position? -static PyObject* pybullet_renderImageObsolete(PyObject* self, PyObject* args) { - /// request an image from a simulated camera, using a software renderer. - struct b3CameraImageData imageData; - PyObject* objViewMat, *objProjMat; - PyObject* objCameraPos, *objTargetPos, *objCameraUp; +static PyObject* pybullet_renderImageObsolete(PyObject* self, PyObject* args) +{ + /// request an image from a simulated camera, using a software renderer. + struct b3CameraImageData imageData; + PyObject *objViewMat, *objProjMat; + PyObject *objCameraPos, *objTargetPos, *objCameraUp; - int width, height; - int size = PySequence_Size(args); - float viewMatrix[16]; - float projectionMatrix[16]; + int width, height; + int size = PySequence_Size(args); + float viewMatrix[16]; + float projectionMatrix[16]; - float cameraPos[3]; - float targetPos[3]; - float cameraUp[3]; + float cameraPos[3]; + float targetPos[3]; + float cameraUp[3]; - float left, right, bottom, top, aspect; - float nearVal, farVal; - float fov; + float left, right, bottom, top, aspect; + float nearVal, farVal; + float fov; - // inialize cmd - b3SharedMemoryCommandHandle command; - b3PhysicsClientHandle sm; - int physicsClientId = 0; + // inialize cmd + b3SharedMemoryCommandHandle command; + b3PhysicsClientHandle sm; + int physicsClientId = 0; - sm = getPhysicsClient(physicsClientId); - if (sm == 0) + sm = getPhysicsClient(physicsClientId); + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - command = b3InitRequestCameraImage(sm); + command = b3InitRequestCameraImage(sm); - if (size == 2) // only set camera resolution - { - if (PyArg_ParseTuple(args, "ii", &width, &height)) { - b3RequestCameraImageSetPixelResolution(command, width, height); - } - } else if (size == 4) // set camera resolution and view and projection matrix - { - if (PyArg_ParseTuple(args, "iiOO", &width, &height, &objViewMat, - &objProjMat)) { - b3RequestCameraImageSetPixelResolution(command, width, height); + if (size == 2) // only set camera resolution + { + if (PyArg_ParseTuple(args, "ii", &width, &height)) + { + b3RequestCameraImageSetPixelResolution(command, width, height); + } + } + else if (size == 4) // set camera resolution and view and projection matrix + { + if (PyArg_ParseTuple(args, "iiOO", &width, &height, &objViewMat, + &objProjMat)) + { + b3RequestCameraImageSetPixelResolution(command, width, height); - // set camera matrices only if set matrix function succeeds - if (pybullet_internalSetMatrix(objViewMat, viewMatrix) && - (pybullet_internalSetMatrix(objProjMat, projectionMatrix))) { - b3RequestCameraImageSetCameraMatrices(command, viewMatrix, - projectionMatrix); - } else { - PyErr_SetString(SpamError, "Error parsing view or projection matrix."); - return NULL; - } - } - } else if (size == 7) // set camera resolution, camera positions and - // calculate projection using near/far values. - { - if (PyArg_ParseTuple(args, "iiOOOff", &width, &height, &objCameraPos, - &objTargetPos, &objCameraUp, &nearVal, &farVal)) { - b3RequestCameraImageSetPixelResolution(command, width, height); - if (pybullet_internalSetVector(objCameraPos, cameraPos) && - pybullet_internalSetVector(objTargetPos, targetPos) && - pybullet_internalSetVector(objCameraUp, cameraUp)) - { - b3RequestCameraImageSetViewMatrix(command, cameraPos, targetPos, - cameraUp); - } else { - PyErr_SetString(SpamError, - "Error parsing camera position, target or up."); - return NULL; - } + // set camera matrices only if set matrix function succeeds + if (pybullet_internalSetMatrix(objViewMat, viewMatrix) && + (pybullet_internalSetMatrix(objProjMat, projectionMatrix))) + { + b3RequestCameraImageSetCameraMatrices(command, viewMatrix, + projectionMatrix); + } + else + { + PyErr_SetString(SpamError, "Error parsing view or projection matrix."); + return NULL; + } + } + } + else if (size == 7) // set camera resolution, camera positions and + // calculate projection using near/far values. + { + if (PyArg_ParseTuple(args, "iiOOOff", &width, &height, &objCameraPos, + &objTargetPos, &objCameraUp, &nearVal, &farVal)) + { + b3RequestCameraImageSetPixelResolution(command, width, height); + if (pybullet_internalSetVector(objCameraPos, cameraPos) && + pybullet_internalSetVector(objTargetPos, targetPos) && + pybullet_internalSetVector(objCameraUp, cameraUp)) + { + b3RequestCameraImageSetViewMatrix(command, cameraPos, targetPos, + cameraUp); + } + else + { + PyErr_SetString(SpamError, + "Error parsing camera position, target or up."); + return NULL; + } - aspect = width / height; - left = -aspect * nearVal; - right = aspect * nearVal; - bottom = -nearVal; - top = nearVal; - b3RequestCameraImageSetProjectionMatrix(command, left, right, bottom, top, - nearVal, farVal); - } - } else if (size == 8) // set camera resolution, camera positions and - // calculate projection using near/far values & field - // of view - { - if (PyArg_ParseTuple(args, "iiOOOfff", &width, &height, &objCameraPos, - &objTargetPos, &objCameraUp, &nearVal, &farVal, - &fov)) { - b3RequestCameraImageSetPixelResolution(command, width, height); - if (pybullet_internalSetVector(objCameraPos, cameraPos) && - pybullet_internalSetVector(objTargetPos, targetPos) && - pybullet_internalSetVector(objCameraUp, cameraUp)) { - b3RequestCameraImageSetViewMatrix(command, cameraPos, targetPos, - cameraUp); - } else { - PyErr_SetString(SpamError, - "Error parsing camera position, target or up."); - return NULL; - } + aspect = width / height; + left = -aspect * nearVal; + right = aspect * nearVal; + bottom = -nearVal; + top = nearVal; + b3RequestCameraImageSetProjectionMatrix(command, left, right, bottom, top, + nearVal, farVal); + } + } + else if (size == 8) // set camera resolution, camera positions and + // calculate projection using near/far values & field + // of view + { + if (PyArg_ParseTuple(args, "iiOOOfff", &width, &height, &objCameraPos, + &objTargetPos, &objCameraUp, &nearVal, &farVal, + &fov)) + { + b3RequestCameraImageSetPixelResolution(command, width, height); + if (pybullet_internalSetVector(objCameraPos, cameraPos) && + pybullet_internalSetVector(objTargetPos, targetPos) && + pybullet_internalSetVector(objCameraUp, cameraUp)) + { + b3RequestCameraImageSetViewMatrix(command, cameraPos, targetPos, + cameraUp); + } + else + { + PyErr_SetString(SpamError, + "Error parsing camera position, target or up."); + return NULL; + } - aspect = width / height; - b3RequestCameraImageSetFOVProjectionMatrix(command, fov, aspect, nearVal, - farVal); - } - } else if (size == 11) { - int upAxisIndex = 1; - float camDistance, yaw, pitch, roll; + aspect = width / height; + b3RequestCameraImageSetFOVProjectionMatrix(command, fov, aspect, nearVal, + farVal); + } + } + else if (size == 11) + { + int upAxisIndex = 1; + float camDistance, yaw, pitch, roll; - // sometimes more arguments are better :-) - if (PyArg_ParseTuple(args, "iiOffffifff", &width, &height, &objTargetPos, - &camDistance, &yaw, &pitch, &roll, &upAxisIndex, - &nearVal, &farVal, &fov)) { - b3RequestCameraImageSetPixelResolution(command, width, height); - if (pybullet_internalSetVector(objTargetPos, targetPos)) { - // printf("width = %d, height = %d, targetPos = %f,%f,%f, distance = %f, - // yaw = %f, pitch = %f, upAxisIndex = %d, near=%f, far=%f, - // fov=%f\n",width,height,targetPos[0],targetPos[1],targetPos[2],camDistance,yaw,pitch,upAxisIndex,nearVal,farVal,fov); + // sometimes more arguments are better :-) + if (PyArg_ParseTuple(args, "iiOffffifff", &width, &height, &objTargetPos, + &camDistance, &yaw, &pitch, &roll, &upAxisIndex, + &nearVal, &farVal, &fov)) + { + b3RequestCameraImageSetPixelResolution(command, width, height); + if (pybullet_internalSetVector(objTargetPos, targetPos)) + { + // printf("width = %d, height = %d, targetPos = %f,%f,%f, distance = %f, + // yaw = %f, pitch = %f, upAxisIndex = %d, near=%f, far=%f, + // fov=%f\n",width,height,targetPos[0],targetPos[1],targetPos[2],camDistance,yaw,pitch,upAxisIndex,nearVal,farVal,fov); - b3RequestCameraImageSetViewMatrix2(command, targetPos, camDistance, yaw, - pitch, roll, upAxisIndex); - aspect = width / height; - b3RequestCameraImageSetFOVProjectionMatrix(command, fov, aspect, - nearVal, farVal); - } else { - PyErr_SetString(SpamError, "Error parsing camera target pos"); - } - } else { - PyErr_SetString(SpamError, "Error parsing arguments"); - } + b3RequestCameraImageSetViewMatrix2(command, targetPos, camDistance, yaw, + pitch, roll, upAxisIndex); + aspect = width / height; + b3RequestCameraImageSetFOVProjectionMatrix(command, fov, aspect, + nearVal, farVal); + } + else + { + PyErr_SetString(SpamError, "Error parsing camera target pos"); + } + } + else + { + PyErr_SetString(SpamError, "Error parsing arguments"); + } + } + else + { + PyErr_SetString(SpamError, "Invalid number of args passed to renderImage."); + return NULL; + } - } else { - PyErr_SetString(SpamError, "Invalid number of args passed to renderImage."); - return NULL; - } + if (b3CanSubmitCommand(sm)) + { + b3SharedMemoryStatusHandle statusHandle; + int statusType; - if (b3CanSubmitCommand(sm)) { - b3SharedMemoryStatusHandle statusHandle; - int statusType; + // b3RequestCameraImageSelectRenderer(command,ER_BULLET_HARDWARE_OPENGL); - // b3RequestCameraImageSelectRenderer(command,ER_BULLET_HARDWARE_OPENGL); - - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); - statusType = b3GetStatusType(statusHandle); - if (statusType == CMD_CAMERA_IMAGE_COMPLETED) { - PyObject* item2; - PyObject* pyResultList; // store 4 elements in this result: width, - // height, rgbData, depth + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + statusType = b3GetStatusType(statusHandle); + if (statusType == CMD_CAMERA_IMAGE_COMPLETED) + { + PyObject* item2; + PyObject* pyResultList; // store 4 elements in this result: width, + // height, rgbData, depth #ifdef PYBULLET_USE_NUMPY - PyObject* pyRGB; - PyObject* pyDep; - PyObject* pySeg; + PyObject* pyRGB; + PyObject* pyDep; + PyObject* pySeg; - int i, j, p; + int i, j, p; - b3GetCameraImageData(sm, &imageData); - // TODO(hellojas): error handling if image size is 0 - pyResultList = PyTuple_New(5); - PyTuple_SetItem(pyResultList, 0, PyInt_FromLong(imageData.m_pixelWidth)); - PyTuple_SetItem(pyResultList, 1, PyInt_FromLong(imageData.m_pixelHeight)); + b3GetCameraImageData(sm, &imageData); + // TODO(hellojas): error handling if image size is 0 + pyResultList = PyTuple_New(5); + PyTuple_SetItem(pyResultList, 0, PyInt_FromLong(imageData.m_pixelWidth)); + PyTuple_SetItem(pyResultList, 1, PyInt_FromLong(imageData.m_pixelHeight)); - int bytesPerPixel = 4; // Red, Green, Blue, and Alpha each 8 bit values + int bytesPerPixel = 4; // Red, Green, Blue, and Alpha each 8 bit values - npy_intp rgb_dims[3] = {imageData.m_pixelHeight, imageData.m_pixelWidth, - bytesPerPixel}; - npy_intp dep_dims[2] = {imageData.m_pixelHeight, imageData.m_pixelWidth}; - npy_intp seg_dims[2] = {imageData.m_pixelHeight, imageData.m_pixelWidth}; + npy_intp rgb_dims[3] = {imageData.m_pixelHeight, imageData.m_pixelWidth, + bytesPerPixel}; + npy_intp dep_dims[2] = {imageData.m_pixelHeight, imageData.m_pixelWidth}; + npy_intp seg_dims[2] = {imageData.m_pixelHeight, imageData.m_pixelWidth}; - pyRGB = PyArray_SimpleNew(3, rgb_dims, NPY_UINT8); - pyDep = PyArray_SimpleNew(2, dep_dims, NPY_FLOAT32); - pySeg = PyArray_SimpleNew(2, seg_dims, NPY_INT32); + pyRGB = PyArray_SimpleNew(3, rgb_dims, NPY_UINT8); + pyDep = PyArray_SimpleNew(2, dep_dims, NPY_FLOAT32); + pySeg = PyArray_SimpleNew(2, seg_dims, NPY_INT32); - memcpy(PyArray_DATA(pyRGB), imageData.m_rgbColorData, - imageData.m_pixelHeight * imageData.m_pixelWidth * bytesPerPixel); - memcpy(PyArray_DATA(pyDep), imageData.m_depthValues, - imageData.m_pixelHeight * imageData.m_pixelWidth); - memcpy(PyArray_DATA(pySeg), imageData.m_segmentationMaskValues, - imageData.m_pixelHeight * imageData.m_pixelWidth); - - PyTuple_SetItem(pyResultList, 2, pyRGB); - PyTuple_SetItem(pyResultList, 3, pyDep); - PyTuple_SetItem(pyResultList, 4, pySeg); -#else//PYBULLET_USE_NUMPY - PyObject* pylistRGB; - PyObject* pylistDep; - PyObject* pylistSeg; + memcpy(PyArray_DATA(pyRGB), imageData.m_rgbColorData, + imageData.m_pixelHeight * imageData.m_pixelWidth * bytesPerPixel); + memcpy(PyArray_DATA(pyDep), imageData.m_depthValues, + imageData.m_pixelHeight * imageData.m_pixelWidth); + memcpy(PyArray_DATA(pySeg), imageData.m_segmentationMaskValues, + imageData.m_pixelHeight * imageData.m_pixelWidth); - int i, j, p; + PyTuple_SetItem(pyResultList, 2, pyRGB); + PyTuple_SetItem(pyResultList, 3, pyDep); + PyTuple_SetItem(pyResultList, 4, pySeg); +#else //PYBULLET_USE_NUMPY + PyObject* pylistRGB; + PyObject* pylistDep; + PyObject* pylistSeg; - b3GetCameraImageData(sm, &imageData); - // TODO(hellojas): error handling if image size is 0 - pyResultList = PyTuple_New(5); - PyTuple_SetItem(pyResultList, 0, PyInt_FromLong(imageData.m_pixelWidth)); - PyTuple_SetItem(pyResultList, 1, PyInt_FromLong(imageData.m_pixelHeight)); + int i, j, p; - { - PyObject* item; - int bytesPerPixel = 4; // Red, Green, Blue, and Alpha each 8 bit values - int num = - bytesPerPixel * imageData.m_pixelWidth * imageData.m_pixelHeight; - pylistRGB = PyTuple_New(num); - pylistDep = - PyTuple_New(imageData.m_pixelWidth * imageData.m_pixelHeight); - pylistSeg = - PyTuple_New(imageData.m_pixelWidth * imageData.m_pixelHeight); - for (i = 0; i < imageData.m_pixelWidth; i++) { - for (j = 0; j < imageData.m_pixelHeight; j++) { - // TODO(hellojas): validate depth values make sense - int depIndex = i + j * imageData.m_pixelWidth; - { - item = PyFloat_FromDouble(imageData.m_depthValues[depIndex]); - PyTuple_SetItem(pylistDep, depIndex, item); - } - { - item2 = - PyLong_FromLong(imageData.m_segmentationMaskValues[depIndex]); - PyTuple_SetItem(pylistSeg, depIndex, item2); - } + b3GetCameraImageData(sm, &imageData); + // TODO(hellojas): error handling if image size is 0 + pyResultList = PyTuple_New(5); + PyTuple_SetItem(pyResultList, 0, PyInt_FromLong(imageData.m_pixelWidth)); + PyTuple_SetItem(pyResultList, 1, PyInt_FromLong(imageData.m_pixelHeight)); - for (p = 0; p < bytesPerPixel; p++) { - int pixelIndex = - bytesPerPixel * (i + j * imageData.m_pixelWidth) + p; - item = PyInt_FromLong(imageData.m_rgbColorData[pixelIndex]); - PyTuple_SetItem(pylistRGB, pixelIndex, item); - } - } - } - } + { + PyObject* item; + int bytesPerPixel = 4; // Red, Green, Blue, and Alpha each 8 bit values + int num = + bytesPerPixel * imageData.m_pixelWidth * imageData.m_pixelHeight; + pylistRGB = PyTuple_New(num); + pylistDep = + PyTuple_New(imageData.m_pixelWidth * imageData.m_pixelHeight); + pylistSeg = + PyTuple_New(imageData.m_pixelWidth * imageData.m_pixelHeight); + for (i = 0; i < imageData.m_pixelWidth; i++) + { + for (j = 0; j < imageData.m_pixelHeight; j++) + { + // TODO(hellojas): validate depth values make sense + int depIndex = i + j * imageData.m_pixelWidth; + { + item = PyFloat_FromDouble(imageData.m_depthValues[depIndex]); + PyTuple_SetItem(pylistDep, depIndex, item); + } + { + item2 = + PyLong_FromLong(imageData.m_segmentationMaskValues[depIndex]); + PyTuple_SetItem(pylistSeg, depIndex, item2); + } - PyTuple_SetItem(pyResultList, 2, pylistRGB); - PyTuple_SetItem(pyResultList, 3, pylistDep); - PyTuple_SetItem(pyResultList, 4, pylistSeg); - return pyResultList; -#endif//PYBULLET_USE_NUMPY + for (p = 0; p < bytesPerPixel; p++) + { + int pixelIndex = + bytesPerPixel * (i + j * imageData.m_pixelWidth) + p; + item = PyInt_FromLong(imageData.m_rgbColorData[pixelIndex]); + PyTuple_SetItem(pylistRGB, pixelIndex, item); + } + } + } + } - return pyResultList; - } - } + PyTuple_SetItem(pyResultList, 2, pylistRGB); + PyTuple_SetItem(pyResultList, 3, pylistDep); + PyTuple_SetItem(pyResultList, 4, pylistSeg); + return pyResultList; +#endif //PYBULLET_USE_NUMPY - Py_INCREF(Py_None); - return Py_None; + return pyResultList; + } + } + + Py_INCREF(Py_None); + return Py_None; } -static PyObject* pybullet_applyExternalForce(PyObject* self, PyObject* args,PyObject* keywds) { - - { - int objectUniqueId=-1, linkIndex=-1, flags; - double force[3]; - double position[3] = {0.0, 0.0, 0.0}; - PyObject* forceObj=0, *posObj=0; - - b3SharedMemoryCommandHandle command; - b3SharedMemoryStatusHandle statusHandle; - int physicsClientId = 0; - b3PhysicsClientHandle sm = 0; - static char *kwlist[] = { "objectUniqueId", "linkIndex", - "forceObj", "posObj", "flags", "physicsClientId", NULL }; - - if (!PyArg_ParseTupleAndKeywords(args, keywds, "iiOOi|i", kwlist,&objectUniqueId, &linkIndex, - &forceObj, &posObj, &flags, &physicsClientId)) +static PyObject* pybullet_applyExternalForce(PyObject* self, PyObject* args, PyObject* keywds) +{ { - return NULL; + int objectUniqueId = -1, linkIndex = -1, flags; + double force[3]; + double position[3] = {0.0, 0.0, 0.0}; + PyObject *forceObj = 0, *posObj = 0; + + b3SharedMemoryCommandHandle command; + b3SharedMemoryStatusHandle statusHandle; + int physicsClientId = 0; + b3PhysicsClientHandle sm = 0; + static char* kwlist[] = {"objectUniqueId", "linkIndex", + "forceObj", "posObj", "flags", "physicsClientId", NULL}; + + if (!PyArg_ParseTupleAndKeywords(args, keywds, "iiOOi|i", kwlist, &objectUniqueId, &linkIndex, + &forceObj, &posObj, &flags, &physicsClientId)) + { + return NULL; + } + sm = getPhysicsClient(physicsClientId); + if (sm == 0) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + { + PyObject* seq; + int len, i; + seq = PySequence_Fast(forceObj, "expected a sequence"); + len = PySequence_Size(forceObj); + if (len == 3) + { + for (i = 0; i < 3; i++) + { + force[i] = pybullet_internalGetFloatFromSequence(seq, i); + } + } + else + { + PyErr_SetString(SpamError, "force needs a 3 coordinates [x,y,z]."); + Py_DECREF(seq); + return NULL; + } + Py_DECREF(seq); + } + { + PyObject* seq; + int len, i; + seq = PySequence_Fast(posObj, "expected a sequence"); + len = PySequence_Size(posObj); + if (len == 3) + { + for (i = 0; i < 3; i++) + { + position[i] = pybullet_internalGetFloatFromSequence(seq, i); + } + } + else + { + PyErr_SetString(SpamError, "position needs a 3 coordinates [x,y,z]."); + Py_DECREF(seq); + return NULL; + } + Py_DECREF(seq); + } + if ((flags != EF_WORLD_FRAME) && (flags != EF_LINK_FRAME)) + { + PyErr_SetString(SpamError, + "flag has to be either WORLD_FRAME or LINK_FRAME"); + return NULL; + } + command = b3ApplyExternalForceCommandInit(sm); + b3ApplyExternalForce(command, objectUniqueId, linkIndex, force, position, + flags); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); } - sm = getPhysicsClient(physicsClientId); - if (sm == 0) - { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } - { - PyObject* seq; - int len, i; - seq = PySequence_Fast(forceObj, "expected a sequence"); - len = PySequence_Size(forceObj); - if (len == 3) { - for (i = 0; i < 3; i++) { - force[i] = pybullet_internalGetFloatFromSequence(seq, i); - } - } else { - PyErr_SetString(SpamError, "force needs a 3 coordinates [x,y,z]."); - Py_DECREF(seq); - return NULL; - } - Py_DECREF(seq); - } - { - PyObject* seq; - int len, i; - seq = PySequence_Fast(posObj, "expected a sequence"); - len = PySequence_Size(posObj); - if (len == 3) { - for (i = 0; i < 3; i++) { - position[i] = pybullet_internalGetFloatFromSequence(seq, i); - } - } else { - PyErr_SetString(SpamError, "position needs a 3 coordinates [x,y,z]."); - Py_DECREF(seq); - return NULL; - } - Py_DECREF(seq); - } - if ((flags != EF_WORLD_FRAME) && (flags != EF_LINK_FRAME)) { - PyErr_SetString(SpamError, - "flag has to be either WORLD_FRAME or LINK_FRAME"); - return NULL; - } - command = b3ApplyExternalForceCommandInit(sm); - b3ApplyExternalForce(command, objectUniqueId, linkIndex, force, position, - flags); - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); - } - Py_INCREF(Py_None); - return Py_None; + Py_INCREF(Py_None); + return Py_None; } -static PyObject* pybullet_applyExternalTorque(PyObject* self, PyObject* args,PyObject* keywds) { - - { - int objectUniqueId, linkIndex, flags; - double torque[3]; - PyObject* torqueObj; - int physicsClientId = 0; - b3PhysicsClientHandle sm = 0; - static char *kwlist[] = { "objectUniqueId", "linkIndex", "torqueObj", - "flags", "physicsClientId", NULL }; - - if (!PyArg_ParseTupleAndKeywords(args, keywds, "iiOi|i", kwlist,&objectUniqueId, &linkIndex, &torqueObj, - &flags, &physicsClientId)) +static PyObject* pybullet_applyExternalTorque(PyObject* self, PyObject* args, PyObject* keywds) +{ { - return NULL; - } - sm = getPhysicsClient(physicsClientId); - if (sm == 0) - { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } - - { - PyObject* seq; - int len, i; - seq = PySequence_Fast(torqueObj, "expected a sequence"); - len = PySequence_Size(torqueObj); - if (len == 3) { - for (i = 0; i < 3; i++) { - torque[i] = pybullet_internalGetFloatFromSequence(seq, i); - } - } else { - PyErr_SetString(SpamError, "torque needs a 3 coordinates [x,y,z]."); - Py_DECREF(seq); - return NULL; - } - Py_DECREF(seq); + int objectUniqueId, linkIndex, flags; + double torque[3]; + PyObject* torqueObj; + int physicsClientId = 0; + b3PhysicsClientHandle sm = 0; + static char* kwlist[] = {"objectUniqueId", "linkIndex", "torqueObj", + "flags", "physicsClientId", NULL}; - if (linkIndex < -1) { - PyErr_SetString(SpamError, - "Invalid link index, has to be -1 or larger"); - return NULL; - } - if ((flags != EF_WORLD_FRAME) && (flags != EF_LINK_FRAME)) { - PyErr_SetString(SpamError, - "flag has to be either WORLD_FRAME or LINK_FRAME"); - return NULL; - } - { - b3SharedMemoryStatusHandle statusHandle; - b3SharedMemoryCommandHandle command = - b3ApplyExternalForceCommandInit(sm); - b3ApplyExternalTorque(command, objectUniqueId, -1, torque, flags); - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); - } - } - } + if (!PyArg_ParseTupleAndKeywords(args, keywds, "iiOi|i", kwlist, &objectUniqueId, &linkIndex, &torqueObj, + &flags, &physicsClientId)) + { + return NULL; + } + sm = getPhysicsClient(physicsClientId); + if (sm == 0) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } - Py_INCREF(Py_None); - return Py_None; + { + PyObject* seq; + int len, i; + seq = PySequence_Fast(torqueObj, "expected a sequence"); + len = PySequence_Size(torqueObj); + if (len == 3) + { + for (i = 0; i < 3; i++) + { + torque[i] = pybullet_internalGetFloatFromSequence(seq, i); + } + } + else + { + PyErr_SetString(SpamError, "torque needs a 3 coordinates [x,y,z]."); + Py_DECREF(seq); + return NULL; + } + Py_DECREF(seq); + + if (linkIndex < -1) + { + PyErr_SetString(SpamError, + "Invalid link index, has to be -1 or larger"); + return NULL; + } + if ((flags != EF_WORLD_FRAME) && (flags != EF_LINK_FRAME)) + { + PyErr_SetString(SpamError, + "flag has to be either WORLD_FRAME or LINK_FRAME"); + return NULL; + } + { + b3SharedMemoryStatusHandle statusHandle; + b3SharedMemoryCommandHandle command = + b3ApplyExternalForceCommandInit(sm); + b3ApplyExternalTorque(command, objectUniqueId, -1, torque, flags); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + } + } + } + + Py_INCREF(Py_None); + return Py_None; } static PyObject* pybullet_getQuaternionFromEuler(PyObject* self, - PyObject* args) { - double rpy[3]; + PyObject* args) +{ + double rpy[3]; - PyObject* eulerObj; + PyObject* eulerObj; - if (PyArg_ParseTuple(args, "O", &eulerObj)) { - PyObject* seq; - int len, i; - seq = PySequence_Fast(eulerObj, "expected a sequence"); - len = PySequence_Size(eulerObj); - if (len == 3) { - for (i = 0; i < 3; i++) { - rpy[i] = pybullet_internalGetFloatFromSequence(seq, i); - } - } else { - PyErr_SetString(SpamError, - "Euler angles need a 3 coordinates [roll, pitch, yaw]."); - Py_DECREF(seq); - return NULL; - } - Py_DECREF(seq); - } else { - PyErr_SetString(SpamError, - "Euler angles need a 3 coordinates [roll, pitch, yaw]."); - return NULL; - } + if (PyArg_ParseTuple(args, "O", &eulerObj)) + { + PyObject* seq; + int len, i; + seq = PySequence_Fast(eulerObj, "expected a sequence"); + len = PySequence_Size(eulerObj); + if (len == 3) + { + for (i = 0; i < 3; i++) + { + rpy[i] = pybullet_internalGetFloatFromSequence(seq, i); + } + } + else + { + PyErr_SetString(SpamError, + "Euler angles need a 3 coordinates [roll, pitch, yaw]."); + Py_DECREF(seq); + return NULL; + } + Py_DECREF(seq); + } + else + { + PyErr_SetString(SpamError, + "Euler angles need a 3 coordinates [roll, pitch, yaw]."); + return NULL; + } - { - double phi, the, psi; - double roll = rpy[0]; - double pitch = rpy[1]; - double yaw = rpy[2]; - phi = roll / 2.0; - the = pitch / 2.0; - psi = yaw / 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)}; + { + double phi, the, psi; + double roll = rpy[0]; + double pitch = rpy[1]; + double yaw = rpy[2]; + phi = roll / 2.0; + the = pitch / 2.0; + psi = yaw / 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; - { - PyObject* pylist; - int i; - pylist = PyTuple_New(4); - for (i = 0; i < 4; i++) - PyTuple_SetItem(pylist, i, PyFloat_FromDouble(quat[i])); - return pylist; - } - } - } - Py_INCREF(Py_None); - return Py_None; + // 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; + { + PyObject* pylist; + int i; + pylist = PyTuple_New(4); + for (i = 0; i < 4; i++) + PyTuple_SetItem(pylist, i, PyFloat_FromDouble(quat[i])); + return pylist; + } + } + } + Py_INCREF(Py_None); + return Py_None; } /// quaternion <-> euler yaw/pitch/roll convention from URDF/SDF, see Gazebo /// https://github.com/arpg/Gazebo/blob/master/gazebo/math/Quaternion.cc static PyObject* pybullet_getEulerFromQuaternion(PyObject* self, - PyObject* args) { - double squ; - double sqx; - double sqy; - double sqz; + PyObject* args) +{ + double squ; + double sqx; + double sqy; + double sqz; - double quat[4]; + double quat[4]; - PyObject* quatObj; + PyObject* quatObj; - if (PyArg_ParseTuple(args, "O", &quatObj)) { - PyObject* seq; - int len, i; - seq = PySequence_Fast(quatObj, "expected a sequence"); - len = PySequence_Size(quatObj); - if (len == 4) { - for (i = 0; i < 4; i++) { - quat[i] = pybullet_internalGetFloatFromSequence(seq, i); - } - } else { - PyErr_SetString(SpamError, "Quaternion need a 4 components [x,y,z,w]."); - Py_DECREF(seq); - return NULL; - } - Py_DECREF(seq); - } else { - PyErr_SetString(SpamError, "Quaternion need a 4 components [x,y,z,w]."); - return NULL; - } + if (PyArg_ParseTuple(args, "O", &quatObj)) + { + PyObject* seq; + int len, i; + seq = PySequence_Fast(quatObj, "expected a sequence"); + len = PySequence_Size(quatObj); + if (len == 4) + { + for (i = 0; i < 4; i++) + { + quat[i] = pybullet_internalGetFloatFromSequence(seq, i); + } + } + else + { + PyErr_SetString(SpamError, "Quaternion need a 4 components [x,y,z,w]."); + Py_DECREF(seq); + return NULL; + } + Py_DECREF(seq); + } + else + { + PyErr_SetString(SpamError, "Quaternion need a 4 components [x,y,z,w]."); + return NULL; + } - { - double rpy[3]; - double sarg; - 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); - { - PyObject* pylist; - int i; - pylist = PyTuple_New(3); - for (i = 0; i < 3; i++) - PyTuple_SetItem(pylist, i, PyFloat_FromDouble(rpy[i])); - return pylist; - } - } - Py_INCREF(Py_None); - return Py_None; + { + double rpy[3]; + double sarg; + 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); + { + PyObject* pylist; + int i; + pylist = PyTuple_New(3); + for (i = 0; i < 3; i++) + PyTuple_SetItem(pylist, i, PyFloat_FromDouble(rpy[i])); + return pylist; + } + } + Py_INCREF(Py_None); + return Py_None; } - ///Inverse Kinematics binding static PyObject* pybullet_calculateInverseKinematics(PyObject* self, - PyObject* args, PyObject* keywds) + PyObject* args, PyObject* keywds) - { - int bodyIndex; - int endEffectorLinkIndex; - - PyObject* targetPosObj=0; - PyObject* targetOrnObj=0; +{ + int bodyIndex; + int endEffectorLinkIndex; - int physicsClientId = 0; - b3PhysicsClientHandle sm = 0; - PyObject* lowerLimitsObj = 0; - PyObject* upperLimitsObj = 0; - PyObject* jointRangesObj = 0; - PyObject* restPosesObj = 0; - PyObject* jointDampingObj = 0; + PyObject* targetPosObj = 0; + PyObject* targetOrnObj = 0; - static char *kwlist[] = { "bodyIndex", "endEffectorLinkIndex", "targetPosition", "targetOrientation","lowerLimits", "upperLimits", "jointRanges", "restPoses", "physicsClientId", "jointDamping", NULL }; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "iiO|OOOOOiO", kwlist, &bodyIndex, &endEffectorLinkIndex, &targetPosObj,&targetOrnObj,&lowerLimitsObj, &upperLimitsObj, &jointRangesObj, &restPosesObj, &physicsClientId, & jointDampingObj)) + int physicsClientId = 0; + b3PhysicsClientHandle sm = 0; + PyObject* lowerLimitsObj = 0; + PyObject* upperLimitsObj = 0; + PyObject* jointRangesObj = 0; + PyObject* restPosesObj = 0; + PyObject* jointDampingObj = 0; + + static char* kwlist[] = {"bodyIndex", "endEffectorLinkIndex", "targetPosition", "targetOrientation", "lowerLimits", "upperLimits", "jointRanges", "restPoses", "physicsClientId", "jointDamping", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "iiO|OOOOOiO", kwlist, &bodyIndex, &endEffectorLinkIndex, &targetPosObj, &targetOrnObj, &lowerLimitsObj, &upperLimitsObj, &jointRangesObj, &restPosesObj, &physicsClientId, &jointDampingObj)) { return NULL; } sm = getPhysicsClient(physicsClientId); - if (sm == 0) + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } { double pos[3]; - double ori[4]={0,1.0,0,0}; - int hasPos =pybullet_internalSetVectord(targetPosObj,pos); - int hasOrn = pybullet_internalSetVector4d(targetOrnObj,ori); + double ori[4] = {0, 1.0, 0, 0}; + int hasPos = pybullet_internalSetVectord(targetPosObj, pos); + int hasOrn = pybullet_internalSetVector4d(targetOrnObj, ori); int szLowerLimits = lowerLimitsObj ? PySequence_Size(lowerLimitsObj) : 0; - int szUpperLimits = upperLimitsObj? PySequence_Size(upperLimitsObj): 0; - int szJointRanges = jointRangesObj? PySequence_Size(jointRangesObj):0; - int szRestPoses = restPosesObj? PySequence_Size(restPosesObj):0; - int szJointDamping = jointDampingObj? PySequence_Size(jointDampingObj):0; + int szUpperLimits = upperLimitsObj ? PySequence_Size(upperLimitsObj) : 0; + int szJointRanges = jointRangesObj ? PySequence_Size(jointRangesObj) : 0; + int szRestPoses = restPosesObj ? PySequence_Size(restPosesObj) : 0; + int szJointDamping = jointDampingObj ? PySequence_Size(jointDampingObj) : 0; int numJoints = b3GetNumJoints(sm, bodyIndex); - int hasNullSpace = 0; - int hasJointDamping = 0; + int hasJointDamping = 0; double* lowerLimits = 0; double* upperLimits = 0; double* jointRanges = 0; double* restPoses = 0; - double* jointDamping = 0; - + double* jointDamping = 0; + if (numJoints && (szLowerLimits == numJoints) && (szUpperLimits == numJoints) && - (szJointRanges == numJoints) && (szRestPoses == numJoints)) + (szJointRanges == numJoints) && (szRestPoses == numJoints)) { int szInBytes = sizeof(double) * numJoints; int i; @@ -4734,77 +4763,80 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self, jointRanges = (double*)malloc(szInBytes); restPoses = (double*)malloc(szInBytes); - for (i = 0; i < numJoints; i++) + for (i = 0; i < numJoints; i++) { - lowerLimits[i] = - pybullet_internalGetFloatFromSequence(lowerLimitsObj, i); - upperLimits[i] = - pybullet_internalGetFloatFromSequence(upperLimitsObj, i); - jointRanges[i] = - pybullet_internalGetFloatFromSequence(jointRangesObj, i); - restPoses[i] = - pybullet_internalGetFloatFromSequence(restPosesObj, i); + lowerLimits[i] = + pybullet_internalGetFloatFromSequence(lowerLimitsObj, i); + upperLimits[i] = + pybullet_internalGetFloatFromSequence(upperLimitsObj, i); + jointRanges[i] = + pybullet_internalGetFloatFromSequence(jointRangesObj, i); + restPoses[i] = + pybullet_internalGetFloatFromSequence(restPosesObj, i); } hasNullSpace = 1; } - - if (numJoints && (szJointDamping == numJoints)) - { - int szInBytes = sizeof(double) * numJoints; - int i; - jointDamping = (double*)malloc(szInBytes); - for (i = 0; i < numJoints; i++) - { - jointDamping[i] = pybullet_internalGetFloatFromSequence(jointDampingObj, i); - } - hasJointDamping = 1; - } + + if (numJoints && (szJointDamping == numJoints)) + { + int szInBytes = sizeof(double) * numJoints; + int i; + jointDamping = (double*)malloc(szInBytes); + for (i = 0; i < numJoints; i++) + { + jointDamping[i] = pybullet_internalGetFloatFromSequence(jointDampingObj, i); + } + hasJointDamping = 1; + } if (hasPos) { b3SharedMemoryStatusHandle statusHandle; - int numPos=0; + int numPos = 0; int resultBodyIndex; int result; - b3SharedMemoryCommandHandle command = b3CalculateInverseKinematicsCommandInit(sm,bodyIndex); + b3SharedMemoryCommandHandle command = b3CalculateInverseKinematicsCommandInit(sm, bodyIndex); if (hasNullSpace) { if (hasOrn) { b3CalculateInverseKinematicsPosOrnWithNullSpaceVel(command, numJoints, endEffectorLinkIndex, pos, ori, lowerLimits, upperLimits, jointRanges, restPoses); - } else - { - b3CalculateInverseKinematicsPosWithNullSpaceVel(command,numJoints, endEffectorLinkIndex, pos, lowerLimits, upperLimits, jointRanges, restPoses); } - } else + else + { + b3CalculateInverseKinematicsPosWithNullSpaceVel(command, numJoints, endEffectorLinkIndex, pos, lowerLimits, upperLimits, jointRanges, restPoses); + } + } + else { if (hasOrn) { - b3CalculateInverseKinematicsAddTargetPositionWithOrientation(command,endEffectorLinkIndex,pos,ori); - } else + b3CalculateInverseKinematicsAddTargetPositionWithOrientation(command, endEffectorLinkIndex, pos, ori); + } + else { - b3CalculateInverseKinematicsAddTargetPurePosition(command,endEffectorLinkIndex,pos); + b3CalculateInverseKinematicsAddTargetPurePosition(command, endEffectorLinkIndex, pos); } } - - if (hasJointDamping) - { - b3CalculateInverseKinematicsSetJointDamping(command, numJoints, jointDamping); - } + + if (hasJointDamping) + { + b3CalculateInverseKinematicsSetJointDamping(command, numJoints, jointDamping); + } statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); - + result = b3GetStatusInverseKinematicsJointPositions(statusHandle, - &resultBodyIndex, - &numPos, - 0); + &resultBodyIndex, + &numPos, + 0); if (result && numPos) { int i; PyObject* pylist; - double* ikOutPutJointPos = (double*)malloc(numPos*sizeof(double)); + double* ikOutPutJointPos = (double*)malloc(numPos * sizeof(double)); result = b3GetStatusInverseKinematicsJointPositions(statusHandle, &resultBodyIndex, &numPos, @@ -4815,7 +4847,7 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self, PyTuple_SetItem(pylist, i, PyFloat_FromDouble(ikOutPutJointPos[i])); } - + free(ikOutPutJointPos); return pylist; } @@ -4825,440 +4857,408 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self, "Error in calculateInverseKinematics"); return NULL; } - } else + } + else { PyErr_SetString(SpamError, - "calculateInverseKinematics couldn't extract position vector3"); + "calculateInverseKinematics couldn't extract position vector3"); return NULL; } } - - Py_INCREF(Py_None); - return Py_None; + + Py_INCREF(Py_None); + return Py_None; } - - /// Given an object id, joint positions, joint velocities and joint /// accelerations, /// compute the joint forces using Inverse Dynamics static PyObject* pybullet_calculateInverseDynamics(PyObject* self, - PyObject* args,PyObject* keywds) + PyObject* args, PyObject* keywds) { - - - { - int bodyIndex; - PyObject* objPositionsQ; - PyObject* objVelocitiesQdot; - PyObject* objAccelerations; - int physicsClientId = 0; - b3PhysicsClientHandle sm = 0; - static char *kwlist[] = { "bodyIndex", "objPositions", "objVelocities", "objAccelerations","physicsClientId", NULL }; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "iOOO|i", kwlist, &bodyIndex, &objPositionsQ, - &objVelocitiesQdot, &objAccelerations,&physicsClientId)) { - return NULL; + int bodyIndex; + PyObject* objPositionsQ; + PyObject* objVelocitiesQdot; + PyObject* objAccelerations; + int physicsClientId = 0; + b3PhysicsClientHandle sm = 0; + static char* kwlist[] = {"bodyIndex", "objPositions", "objVelocities", "objAccelerations", "physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "iOOO|i", kwlist, &bodyIndex, &objPositionsQ, + &objVelocitiesQdot, &objAccelerations, &physicsClientId)) + { + return NULL; + } + sm = getPhysicsClient(physicsClientId); + if (sm == 0) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + { + int szObPos = PySequence_Size(objPositionsQ); + int szObVel = PySequence_Size(objVelocitiesQdot); + int szObAcc = PySequence_Size(objAccelerations); + int numJoints = b3GetNumJoints(sm, bodyIndex); + if (numJoints && (szObPos == numJoints) && (szObVel == numJoints) && + (szObAcc == numJoints)) + { + int szInBytes = sizeof(double) * numJoints; + int i; + PyObject* pylist = 0; + double* jointPositionsQ = (double*)malloc(szInBytes); + double* jointVelocitiesQdot = (double*)malloc(szInBytes); + double* jointAccelerations = (double*)malloc(szInBytes); + double* jointForcesOutput = (double*)malloc(szInBytes); + + for (i = 0; i < numJoints; i++) + { + jointPositionsQ[i] = + pybullet_internalGetFloatFromSequence(objPositionsQ, i); + jointVelocitiesQdot[i] = + pybullet_internalGetFloatFromSequence(objVelocitiesQdot, i); + jointAccelerations[i] = + pybullet_internalGetFloatFromSequence(objAccelerations, i); + } + + { + b3SharedMemoryStatusHandle statusHandle; + int statusType; + b3SharedMemoryCommandHandle commandHandle = + b3CalculateInverseDynamicsCommandInit( + sm, bodyIndex, jointPositionsQ, jointVelocitiesQdot, + jointAccelerations); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + + statusType = b3GetStatusType(statusHandle); + + if (statusType == CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED) + { + int bodyUniqueId; + int dofCount; + + b3GetStatusInverseDynamicsJointForces(statusHandle, &bodyUniqueId, + &dofCount, 0); + + if (dofCount) + { + b3GetStatusInverseDynamicsJointForces(statusHandle, 0, 0, + jointForcesOutput); + { + { + int i; + pylist = PyTuple_New(dofCount); + for (i = 0; i < dofCount; i++) + PyTuple_SetItem(pylist, i, + PyFloat_FromDouble(jointForcesOutput[i])); + } + } + } + } + else + { + PyErr_SetString(SpamError, + "Internal error in calculateInverseDynamics"); + } + } + free(jointPositionsQ); + free(jointVelocitiesQdot); + free(jointAccelerations); + free(jointForcesOutput); + if (pylist) return pylist; + } + else + { + PyErr_SetString(SpamError, + "calculateInverseDynamics numJoints needs to be " + "positive and [joint positions], [joint velocities], " + "[joint accelerations] need to match the number of " + "joints."); + return NULL; + } + } } - sm = getPhysicsClient(physicsClientId); - if (sm == 0) - { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } - - { - int szObPos = PySequence_Size(objPositionsQ); - int szObVel = PySequence_Size(objVelocitiesQdot); - int szObAcc = PySequence_Size(objAccelerations); - int numJoints = b3GetNumJoints(sm, bodyIndex); - if (numJoints && (szObPos == numJoints) && (szObVel == numJoints) && - (szObAcc == numJoints)) { - int szInBytes = sizeof(double) * numJoints; - int i; - PyObject* pylist = 0; - double* jointPositionsQ = (double*)malloc(szInBytes); - double* jointVelocitiesQdot = (double*)malloc(szInBytes); - double* jointAccelerations = (double*)malloc(szInBytes); - double* jointForcesOutput = (double*)malloc(szInBytes); - - for (i = 0; i < numJoints; i++) { - jointPositionsQ[i] = - pybullet_internalGetFloatFromSequence(objPositionsQ, i); - jointVelocitiesQdot[i] = - pybullet_internalGetFloatFromSequence(objVelocitiesQdot, i); - jointAccelerations[i] = - pybullet_internalGetFloatFromSequence(objAccelerations, i); - } - - { - b3SharedMemoryStatusHandle statusHandle; - int statusType; - b3SharedMemoryCommandHandle commandHandle = - b3CalculateInverseDynamicsCommandInit( - sm, bodyIndex, jointPositionsQ, jointVelocitiesQdot, - jointAccelerations); - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); - - statusType = b3GetStatusType(statusHandle); - - if (statusType == CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED) { - int bodyUniqueId; - int dofCount; - - b3GetStatusInverseDynamicsJointForces(statusHandle, &bodyUniqueId, - &dofCount, 0); - - if (dofCount) { - b3GetStatusInverseDynamicsJointForces(statusHandle, 0, 0, - jointForcesOutput); - { - { - int i; - pylist = PyTuple_New(dofCount); - for (i = 0; i < dofCount; i++) - PyTuple_SetItem(pylist, i, - PyFloat_FromDouble(jointForcesOutput[i])); - } - } - } - - } else { - PyErr_SetString(SpamError, - "Internal error in calculateInverseDynamics"); - } - } - free(jointPositionsQ); - free(jointVelocitiesQdot); - free(jointAccelerations); - free(jointForcesOutput); - if (pylist) return pylist; - } else { - PyErr_SetString(SpamError, - "calculateInverseDynamics numJoints needs to be " - "positive and [joint positions], [joint velocities], " - "[joint accelerations] need to match the number of " - "joints."); - return NULL; - } - - } - } - Py_INCREF(Py_None); - return Py_None; + Py_INCREF(Py_None); + return Py_None; } static PyMethodDef SpamMethods[] = { - {"connect",(PyCFunction)pybullet_connectPhysicsServer, METH_VARARGS|METH_KEYWORDS, - "Connect to an existing physics server (using shared memory by default)."}, + {"connect", (PyCFunction)pybullet_connectPhysicsServer, METH_VARARGS | METH_KEYWORDS, + "Connect to an existing physics server (using shared memory by default)."}, - {"disconnect", (PyCFunction)pybullet_disconnectPhysicsServer, METH_VARARGS|METH_KEYWORDS, - "Disconnect from the physics server."}, + {"disconnect", (PyCFunction)pybullet_disconnectPhysicsServer, METH_VARARGS | METH_KEYWORDS, + "Disconnect from the physics server."}, - {"resetSimulation", (PyCFunction)pybullet_resetSimulation, METH_VARARGS|METH_KEYWORDS, - "Reset the simulation: remove all objects and start from an empty world."}, + {"resetSimulation", (PyCFunction)pybullet_resetSimulation, METH_VARARGS | METH_KEYWORDS, + "Reset the simulation: remove all objects and start from an empty world."}, - {"stepSimulation", (PyCFunction)pybullet_stepSimulation, METH_VARARGS|METH_KEYWORDS, - "Step the simulation using forward dynamics."}, + {"stepSimulation", (PyCFunction)pybullet_stepSimulation, METH_VARARGS | METH_KEYWORDS, + "Step the simulation using forward dynamics."}, - {"setGravity", (PyCFunction)pybullet_setGravity, METH_VARARGS|METH_KEYWORDS, - "Set the gravity acceleration (x,y,z)."}, + {"setGravity", (PyCFunction)pybullet_setGravity, METH_VARARGS | METH_KEYWORDS, + "Set the gravity acceleration (x,y,z)."}, - {"setTimeStep",(PyCFunction) pybullet_setTimeStep, METH_VARARGS|METH_KEYWORDS, - "Set the amount of time to proceed at each call to stepSimulation. (unit " - "is seconds, typically range is 0.01 or 0.001)"}, + {"setTimeStep", (PyCFunction)pybullet_setTimeStep, METH_VARARGS | METH_KEYWORDS, + "Set the amount of time to proceed at each call to stepSimulation. (unit " + "is seconds, typically range is 0.01 or 0.001)"}, - - - {"setDefaultContactERP", (PyCFunction) pybullet_setDefaultContactERP, METH_VARARGS| METH_KEYWORDS, - "Set the amount of contact penetration Error Recovery Paramater " - "(ERP) in each time step. \ + {"setDefaultContactERP", (PyCFunction)pybullet_setDefaultContactERP, METH_VARARGS | METH_KEYWORDS, + "Set the amount of contact penetration Error Recovery Paramater " + "(ERP) in each time step. \ This is an tuning parameter to control resting contact stability. " - "This value depends on the time step."}, - - { "setRealTimeSimulation",(PyCFunction) pybullet_setRealTimeSimulation, METH_VARARGS| METH_KEYWORDS, - "Enable or disable real time simulation (using the real time clock," - " RTC) in the physics server. Expects one integer argument, 0 or 1" }, + "This value depends on the time step."}, - { "setPhysicsEngineParameter", (PyCFunction)pybullet_setPhysicsEngineParameter, METH_VARARGS | METH_KEYWORDS, - "Set some internal physics engine parameter, such as cfm or erp etc." }, + {"setRealTimeSimulation", (PyCFunction)pybullet_setRealTimeSimulation, METH_VARARGS | METH_KEYWORDS, + "Enable or disable real time simulation (using the real time clock," + " RTC) in the physics server. Expects one integer argument, 0 or 1"}, - { "setInternalSimFlags", (PyCFunction)pybullet_setInternalSimFlags, METH_VARARGS| METH_KEYWORDS, - "This is for experimental purposes, use at own risk, magic may or not happen"}, - - {"loadURDF", (PyCFunction) pybullet_loadURDF, METH_VARARGS | METH_KEYWORDS, - "Create a multibody by loading a URDF file."}, + {"setPhysicsEngineParameter", (PyCFunction)pybullet_setPhysicsEngineParameter, METH_VARARGS | METH_KEYWORDS, + "Set some internal physics engine parameter, such as cfm or erp etc."}, - {"loadSDF", (PyCFunction)pybullet_loadSDF, METH_VARARGS| METH_KEYWORDS, - "Load multibodies from an SDF file."}, + {"setInternalSimFlags", (PyCFunction)pybullet_setInternalSimFlags, METH_VARARGS | METH_KEYWORDS, + "This is for experimental purposes, use at own risk, magic may or not happen"}, - { "loadBullet", (PyCFunction)pybullet_loadBullet, METH_VARARGS| METH_KEYWORDS, - "Restore the full state of the world from a .bullet file." }, + {"loadURDF", (PyCFunction)pybullet_loadURDF, METH_VARARGS | METH_KEYWORDS, + "Create a multibody by loading a URDF file."}, - { "saveBullet", (PyCFunction)pybullet_saveBullet, METH_VARARGS| METH_KEYWORDS, - "Save the full state of the world to a .bullet file." }, + {"loadSDF", (PyCFunction)pybullet_loadSDF, METH_VARARGS | METH_KEYWORDS, + "Load multibodies from an SDF file."}, - { "loadMJCF",(PyCFunction) pybullet_loadMJCF, METH_VARARGS| METH_KEYWORDS, - "Load multibodies from an MJCF file." }, + {"loadBullet", (PyCFunction)pybullet_loadBullet, METH_VARARGS | METH_KEYWORDS, + "Restore the full state of the world from a .bullet file."}, - {"createConstraint", (PyCFunction)pybullet_createUserConstraint, METH_VARARGS | METH_KEYWORDS, - "Create a constraint between two bodies. Returns a (int) unique id, if successfull." - }, + {"saveBullet", (PyCFunction)pybullet_saveBullet, METH_VARARGS | METH_KEYWORDS, + "Save the full state of the world to a .bullet file."}, + + {"loadMJCF", (PyCFunction)pybullet_loadMJCF, METH_VARARGS | METH_KEYWORDS, + "Load multibodies from an MJCF file."}, + + {"createConstraint", (PyCFunction)pybullet_createUserConstraint, METH_VARARGS | METH_KEYWORDS, + "Create a constraint between two bodies. Returns a (int) unique id, if successfull."}, {"changeConstraint", (PyCFunction)pybullet_changeUserConstraint, METH_VARARGS | METH_KEYWORDS, - "Change some parameters of an existing constraint, such as the child pivot or child frame orientation, using its unique id." - }, - + "Change some parameters of an existing constraint, such as the child pivot or child frame orientation, using its unique id."}, {"removeConstraint", (PyCFunction)pybullet_removeUserConstraint, METH_VARARGS | METH_KEYWORDS, - "Remove a constraint using its unique id." - }, - - { "enableJointForceTorqueSensor", (PyCFunction)pybullet_enableJointForceTorqueSensor, METH_VARARGS | METH_KEYWORDS, - "Enable or disable a joint force/torque sensor measuring the joint reaction forces." - }, + "Remove a constraint using its unique id."}, - {"saveWorld", (PyCFunction)pybullet_saveWorld, METH_VARARGS| METH_KEYWORDS, - "Save a approximate Python file to reproduce the current state of the world: saveWorld" - "(filename). (very preliminary and approximately)"}, + {"enableJointForceTorqueSensor", (PyCFunction)pybullet_enableJointForceTorqueSensor, METH_VARARGS | METH_KEYWORDS, + "Enable or disable a joint force/torque sensor measuring the joint reaction forces."}, - {"getNumBodies", (PyCFunction)pybullet_getNumBodies, METH_VARARGS| METH_KEYWORDS, - "Get the number of bodies in the simulation."}, + {"saveWorld", (PyCFunction)pybullet_saveWorld, METH_VARARGS | METH_KEYWORDS, + "Save a approximate Python file to reproduce the current state of the world: saveWorld" + "(filename). (very preliminary and approximately)"}, - {"getBodyUniqueId", (PyCFunction)pybullet_getBodyUniqueId, METH_VARARGS| METH_KEYWORDS, - "getBodyUniqueId is used after connecting to server with existing bodies." + {"getNumBodies", (PyCFunction)pybullet_getNumBodies, METH_VARARGS | METH_KEYWORDS, + "Get the number of bodies in the simulation."}, + + {"getBodyUniqueId", (PyCFunction)pybullet_getBodyUniqueId, METH_VARARGS | METH_KEYWORDS, + "getBodyUniqueId is used after connecting to server with existing bodies." "Get the unique id of the body, given a integer range [0.. number of bodies)."}, - {"getBodyInfo",(PyCFunction) pybullet_getBodyInfo, METH_VARARGS | METH_KEYWORDS, - "Get the body info, given a body unique id."}, - - {"getNumConstraints", (PyCFunction)pybullet_getNumConstraints, METH_VARARGS| METH_KEYWORDS, - "Get the number of user-created constraints in the simulation."}, + {"getBodyInfo", (PyCFunction)pybullet_getBodyInfo, METH_VARARGS | METH_KEYWORDS, + "Get the body info, given a body unique id."}, - {"getConstraintInfo",(PyCFunction) pybullet_getConstraintInfo, METH_VARARGS | METH_KEYWORDS, - "Get the user-created constraint info, given a constraint unique id."}, - - {"getConstraintUniqueId", (PyCFunction)pybullet_getBodyUniqueId, METH_VARARGS| METH_KEYWORDS, - "Get the unique id of the constraint, given a integer index in range [0.. number of constraints)."}, - - {"getBasePositionAndOrientation",(PyCFunction) pybullet_getBasePositionAndOrientation, - METH_VARARGS | METH_KEYWORDS, - "Get the world position and orientation of the base of the object. " - "(x,y,z) position vector and (x,y,z,w) quaternion orientation."}, + {"getNumConstraints", (PyCFunction)pybullet_getNumConstraints, METH_VARARGS | METH_KEYWORDS, + "Get the number of user-created constraints in the simulation."}, - {"resetBasePositionAndOrientation", - (PyCFunction) pybullet_resetBasePositionAndOrientation, METH_VARARGS| METH_KEYWORDS, - "Reset the world position and orientation of the base of the object " - "instantaneously, not through physics simulation. (x,y,z) position vector " - "and (x,y,z,w) quaternion orientation."}, + {"getConstraintInfo", (PyCFunction)pybullet_getConstraintInfo, METH_VARARGS | METH_KEYWORDS, + "Get the user-created constraint info, given a constraint unique id."}, - { "getBaseVelocity", (PyCFunction)pybullet_getBaseVelocity, - METH_VARARGS| METH_KEYWORDS, - "Get the linear and angular velocity of the base of the object " - " in world space coordinates. " - "(x,y,z) linear velocity vector and (x,y,z) angular velocity vector." }, + {"getConstraintUniqueId", (PyCFunction)pybullet_getBodyUniqueId, METH_VARARGS | METH_KEYWORDS, + "Get the unique id of the constraint, given a integer index in range [0.. number of constraints)."}, - { "resetBaseVelocity", (PyCFunction)pybullet_resetBaseVelocity, METH_VARARGS | METH_KEYWORDS, - "Reset the linear and/or angular velocity of the base of the object " - " in world space coordinates. " - "linearVelocity (x,y,z) and angularVelocity (x,y,z)." }, + {"getBasePositionAndOrientation", (PyCFunction)pybullet_getBasePositionAndOrientation, + METH_VARARGS | METH_KEYWORDS, + "Get the world position and orientation of the base of the object. " + "(x,y,z) position vector and (x,y,z,w) quaternion orientation."}, - {"getNumJoints", (PyCFunction)pybullet_getNumJoints, METH_VARARGS| METH_KEYWORDS, - "Get the number of joints for an object."}, + {"resetBasePositionAndOrientation", + (PyCFunction)pybullet_resetBasePositionAndOrientation, METH_VARARGS | METH_KEYWORDS, + "Reset the world position and orientation of the base of the object " + "instantaneously, not through physics simulation. (x,y,z) position vector " + "and (x,y,z,w) quaternion orientation."}, - {"getJointInfo", (PyCFunction)pybullet_getJointInfo, METH_VARARGS| METH_KEYWORDS, - "Get the name and type info for a joint on a body."}, + {"getBaseVelocity", (PyCFunction)pybullet_getBaseVelocity, + METH_VARARGS | METH_KEYWORDS, + "Get the linear and angular velocity of the base of the object " + " in world space coordinates. " + "(x,y,z) linear velocity vector and (x,y,z) angular velocity vector."}, - {"getJointState",(PyCFunction) pybullet_getJointState, METH_VARARGS| METH_KEYWORDS, - "Get the state (position, velocity etc) for a joint on a body."}, + {"resetBaseVelocity", (PyCFunction)pybullet_resetBaseVelocity, METH_VARARGS | METH_KEYWORDS, + "Reset the linear and/or angular velocity of the base of the object " + " in world space coordinates. " + "linearVelocity (x,y,z) and angularVelocity (x,y,z)."}, - {"getLinkState", (PyCFunction)pybullet_getLinkState, METH_VARARGS| METH_KEYWORDS, - "Provides extra information such as the Cartesian world coordinates" - " center of mass (COM) of the link, relative to the world reference" - " frame."}, + {"getNumJoints", (PyCFunction)pybullet_getNumJoints, METH_VARARGS | METH_KEYWORDS, + "Get the number of joints for an object."}, - {"resetJointState",(PyCFunction) pybullet_resetJointState, METH_VARARGS| METH_KEYWORDS, - "Reset the state (position, velocity etc) for a joint on a body " - "instantaneously, not through physics simulation."}, + {"getJointInfo", (PyCFunction)pybullet_getJointInfo, METH_VARARGS | METH_KEYWORDS, + "Get the name and type info for a joint on a body."}, - {"setJointMotorControl",(PyCFunction) pybullet_setJointMotorControl, METH_VARARGS, - "This (obsolete) method cannot select non-zero physicsClientId, use setJointMotorControl2 instead." + {"getJointState", (PyCFunction)pybullet_getJointState, METH_VARARGS | METH_KEYWORDS, + "Get the state (position, velocity etc) for a joint on a body."}, + + {"getLinkState", (PyCFunction)pybullet_getLinkState, METH_VARARGS | METH_KEYWORDS, + "Provides extra information such as the Cartesian world coordinates" + " center of mass (COM) of the link, relative to the world reference" + " frame."}, + + {"resetJointState", (PyCFunction)pybullet_resetJointState, METH_VARARGS | METH_KEYWORDS, + "Reset the state (position, velocity etc) for a joint on a body " + "instantaneously, not through physics simulation."}, + + {"setJointMotorControl", (PyCFunction)pybullet_setJointMotorControl, METH_VARARGS, + "This (obsolete) method cannot select non-zero physicsClientId, use setJointMotorControl2 instead." "Set a single joint motor control mode and desired target value. There is " - "no immediate state change, stepSimulation will process the motors."}, + "no immediate state change, stepSimulation will process the motors."}, - {"setJointMotorControl2",(PyCFunction) pybullet_setJointMotorControl2, METH_VARARGS| METH_KEYWORDS, - "Set a single joint motor control mode and desired target value. There is " - "no immediate state change, stepSimulation will process the motors."}, + {"setJointMotorControl2", (PyCFunction)pybullet_setJointMotorControl2, METH_VARARGS | METH_KEYWORDS, + "Set a single joint motor control mode and desired target value. There is " + "no immediate state change, stepSimulation will process the motors."}, - {"applyExternalForce",(PyCFunction) pybullet_applyExternalForce, METH_VARARGS| METH_KEYWORDS, - "for objectUniqueId, linkIndex (-1 for base/root link), apply a force " - "[x,y,z] at the a position [x,y,z], flag to select FORCE_IN_LINK_FRAME or " - "WORLD_FRAME coordinates"}, + {"applyExternalForce", (PyCFunction)pybullet_applyExternalForce, METH_VARARGS | METH_KEYWORDS, + "for objectUniqueId, linkIndex (-1 for base/root link), apply a force " + "[x,y,z] at the a position [x,y,z], flag to select FORCE_IN_LINK_FRAME or " + "WORLD_FRAME coordinates"}, - {"applyExternalTorque", (PyCFunction)pybullet_applyExternalTorque, METH_VARARGS| METH_KEYWORDS, - "for objectUniqueId, linkIndex (-1 for base/root link) apply a torque " - "[x,y,z] in Cartesian coordinates, flag to select TORQUE_IN_LINK_FRAME or " - "WORLD_FRAME coordinates"}, + {"applyExternalTorque", (PyCFunction)pybullet_applyExternalTorque, METH_VARARGS | METH_KEYWORDS, + "for objectUniqueId, linkIndex (-1 for base/root link) apply a torque " + "[x,y,z] in Cartesian coordinates, flag to select TORQUE_IN_LINK_FRAME or " + "WORLD_FRAME coordinates"}, - {"renderImage", pybullet_renderImageObsolete, METH_VARARGS, - "obsolete, please use getCameraImage and getViewProjectionMatrices instead" - }, + {"renderImage", pybullet_renderImageObsolete, METH_VARARGS, + "obsolete, please use getCameraImage and getViewProjectionMatrices instead"}, - { "getCameraImage",(PyCFunction)pybullet_getCameraImage, METH_VARARGS| METH_KEYWORDS, - "Render an image (given the pixel resolution width, height, camera viewMatrix " - ", projectionMatrix, lightDirection, lightColor, lightDistance, shadow, lightAmbientCoeff, lightDiffuseCoeff, lightSpecularCoeff, and renderer), and return the " - "8-8-8bit RGB pixel data and floating point depth values" + {"getCameraImage", (PyCFunction)pybullet_getCameraImage, METH_VARARGS | METH_KEYWORDS, + "Render an image (given the pixel resolution width, height, camera viewMatrix " + ", projectionMatrix, lightDirection, lightColor, lightDistance, shadow, lightAmbientCoeff, lightDiffuseCoeff, lightSpecularCoeff, and renderer), and return the " + "8-8-8bit RGB pixel data and floating point depth values" #ifdef PYBULLET_USE_NUMPY - " as NumPy arrays" + " as NumPy arrays" #endif - }, - - { "computeViewMatrix", (PyCFunction)pybullet_computeViewMatrix, METH_VARARGS | METH_KEYWORDS, - "Compute a camera viewmatrix from camera eye, target position and up vector " - }, - - { "computeViewMatrixFromYawPitchRoll",(PyCFunction)pybullet_computeViewMatrixFromYawPitchRoll, METH_VARARGS | METH_KEYWORDS, - "Compute a camera viewmatrix from camera eye, target position and up vector " - }, - - { "computeProjectionMatrix", (PyCFunction)pybullet_computeProjectionMatrix, METH_VARARGS | METH_KEYWORDS, - "Compute a camera projection matrix from screen left/right/bottom/top/near/far values" }, - { "computeProjectionMatrixFOV", (PyCFunction)pybullet_computeProjectionMatrixFOV, METH_VARARGS | METH_KEYWORDS, - "Compute a camera projection matrix from fov, aspect ratio, near, far values" - }, + {"computeViewMatrix", (PyCFunction)pybullet_computeViewMatrix, METH_VARARGS | METH_KEYWORDS, + "Compute a camera viewmatrix from camera eye, target position and up vector "}, - {"getContactPoints", (PyCFunction)pybullet_getContactPointData, METH_VARARGS | METH_KEYWORDS, - "Return existing contact points after the stepSimulation command. " - "Optional arguments one or two object unique " - "ids, that need to be involved in the contact."}, + {"computeViewMatrixFromYawPitchRoll", (PyCFunction)pybullet_computeViewMatrixFromYawPitchRoll, METH_VARARGS | METH_KEYWORDS, + "Compute a camera viewmatrix from camera eye, target position and up vector "}, - {"getClosestPoints", (PyCFunction)pybullet_getClosestPointData, METH_VARARGS | METH_KEYWORDS, - "Compute the closest points between two objects, if the distance is below a given threshold." - "Input is two objects unique ids and distance threshold." - }, + {"computeProjectionMatrix", (PyCFunction)pybullet_computeProjectionMatrix, METH_VARARGS | METH_KEYWORDS, + "Compute a camera projection matrix from screen left/right/bottom/top/near/far values"}, - { "getOverlappingObjects", (PyCFunction)pybullet_getOverlappingObjects, METH_VARARGS | METH_KEYWORDS, - "Return all the objects that have overlap with a given " - "axis-aligned bounding box volume (AABB)." - "Input are two vectors defining the AABB in world space [min_x,min_y,min_z],[max_x,max_y,max_z]." - }, + {"computeProjectionMatrixFOV", (PyCFunction)pybullet_computeProjectionMatrixFOV, METH_VARARGS | METH_KEYWORDS, + "Compute a camera projection matrix from fov, aspect ratio, near, far values"}, + {"getContactPoints", (PyCFunction)pybullet_getContactPointData, METH_VARARGS | METH_KEYWORDS, + "Return existing contact points after the stepSimulation command. " + "Optional arguments one or two object unique " + "ids, that need to be involved in the contact."}, - { "addUserDebugLine", (PyCFunction)pybullet_addUserDebugLine, METH_VARARGS | METH_KEYWORDS, + {"getClosestPoints", (PyCFunction)pybullet_getClosestPointData, METH_VARARGS | METH_KEYWORDS, + "Compute the closest points between two objects, if the distance is below a given threshold." + "Input is two objects unique ids and distance threshold."}, + + {"getOverlappingObjects", (PyCFunction)pybullet_getOverlappingObjects, METH_VARARGS | METH_KEYWORDS, + "Return all the objects that have overlap with a given " + "axis-aligned bounding box volume (AABB)." + "Input are two vectors defining the AABB in world space [min_x,min_y,min_z],[max_x,max_y,max_z]."}, + + {"addUserDebugLine", (PyCFunction)pybullet_addUserDebugLine, METH_VARARGS | METH_KEYWORDS, "Add a user debug draw line with lineFrom[3], lineTo[3], lineColorRGB[3], lineWidth, lifeTime. " - "A lifeTime of 0 means permanent until removed. Returns a unique id for the user debug item." - }, + "A lifeTime of 0 means permanent until removed. Returns a unique id for the user debug item."}, - - { "addUserDebugText", (PyCFunction)pybullet_addUserDebugText, METH_VARARGS | METH_KEYWORDS, + {"addUserDebugText", (PyCFunction)pybullet_addUserDebugText, METH_VARARGS | METH_KEYWORDS, "Add a user debug draw line with text, textPosition[3], textSize and lifeTime in seconds " - "A lifeTime of 0 means permanent until removed. Returns a unique id for the user debug item." - }, + "A lifeTime of 0 means permanent until removed. Returns a unique id for the user debug item."}, - { "addUserDebugParameter", (PyCFunction)pybullet_addUserDebugParameter, METH_VARARGS | METH_KEYWORDS, - "Add a user debug parameter, such as a slider, that can be controlled using a GUI." - }, - { "readUserDebugParameter", (PyCFunction)pybullet_readUserDebugParameter, METH_VARARGS | METH_KEYWORDS, - "Read the current value of a user debug parameter, given the user debug item unique id." - }, + {"addUserDebugParameter", (PyCFunction)pybullet_addUserDebugParameter, METH_VARARGS | METH_KEYWORDS, + "Add a user debug parameter, such as a slider, that can be controlled using a GUI."}, + {"readUserDebugParameter", (PyCFunction)pybullet_readUserDebugParameter, METH_VARARGS | METH_KEYWORDS, + "Read the current value of a user debug parameter, given the user debug item unique id."}, - { "removeUserDebugItem", (PyCFunction)pybullet_removeUserDebugItem, METH_VARARGS | METH_KEYWORDS, - "remove a user debug draw item, giving its unique id" - }, + {"removeUserDebugItem", (PyCFunction)pybullet_removeUserDebugItem, METH_VARARGS | METH_KEYWORDS, + "remove a user debug draw item, giving its unique id"}, + {"removeAllUserDebugItems", (PyCFunction)pybullet_removeAllUserDebugItems, METH_VARARGS | METH_KEYWORDS, + "remove all user debug draw items"}, - { "removeAllUserDebugItems", (PyCFunction)pybullet_removeAllUserDebugItems, METH_VARARGS | METH_KEYWORDS, - "remove all user debug draw items" - }, + {"setDebugObjectColor", (PyCFunction)pybullet_setDebugObjectColor, METH_VARARGS | METH_KEYWORDS, + "Override the wireframe debug drawing color for a particular object unique id / link index." + "If you ommit the color, the custom color will be removed."}, - { "setDebugObjectColor", (PyCFunction)pybullet_setDebugObjectColor, METH_VARARGS | METH_KEYWORDS, - "Override the wireframe debug drawing color for a particular object unique id / link index." - "If you ommit the color, the custom color will be removed." - }, + {"configureDebugVisualizer", (PyCFunction)pybullet_configureDebugVisualizer, METH_VARARGS | METH_KEYWORDS, + "For the 3D OpenGL Visualizer, enable/disable GUI, shadows."}, - { "configureDebugVisualizer", (PyCFunction)pybullet_configureDebugVisualizer, METH_VARARGS | METH_KEYWORDS, - "For the 3D OpenGL Visualizer, enable/disable GUI, shadows." - }, + {"resetDebugVisualizerCamera", (PyCFunction)pybullet_resetDebugVisualizerCamera, METH_VARARGS | METH_KEYWORDS, + "For the 3D OpenGL Visualizer, set the camera distance, yaw, pitch and target position."}, - { "resetDebugVisualizerCamera", (PyCFunction)pybullet_resetDebugVisualizerCamera, METH_VARARGS | METH_KEYWORDS, - "For the 3D OpenGL Visualizer, set the camera distance, yaw, pitch and target position." - }, + {"getVisualShapeData", (PyCFunction)pybullet_getVisualShapeData, METH_VARARGS | METH_KEYWORDS, + "Return the visual shape information for one object."}, - + {"resetVisualShapeData", (PyCFunction)pybullet_resetVisualShapeData, METH_VARARGS | METH_KEYWORDS, + "Reset part of the visual shape information for one object."}, - {"getVisualShapeData", (PyCFunction)pybullet_getVisualShapeData, METH_VARARGS| METH_KEYWORDS, - "Return the visual shape information for one object." }, - - {"resetVisualShapeData", (PyCFunction)pybullet_resetVisualShapeData, METH_VARARGS| METH_KEYWORDS, - "Reset part of the visual shape information for one object." }, - - {"loadTexture", (PyCFunction)pybullet_loadTexture, METH_VARARGS| METH_KEYWORDS, - "Load texture file." }, - - {"getQuaternionFromEuler", pybullet_getQuaternionFromEuler, METH_VARARGS, - "Convert Euler [roll, pitch, yaw] as in URDF/SDF convention, to " - "quaternion [x,y,z,w]"}, + {"loadTexture", (PyCFunction)pybullet_loadTexture, METH_VARARGS | METH_KEYWORDS, + "Load texture file."}, - {"getEulerFromQuaternion", pybullet_getEulerFromQuaternion, METH_VARARGS, - "Convert quaternion [x,y,z,w] to Euler [roll, pitch, yaw] as in URDF/SDF " - "convention"}, + {"getQuaternionFromEuler", pybullet_getQuaternionFromEuler, METH_VARARGS, + "Convert Euler [roll, pitch, yaw] as in URDF/SDF convention, to " + "quaternion [x,y,z,w]"}, - {"getMatrixFromQuaternion", pybullet_getMatrixFromQuaternion,METH_VARARGS, - "Compute the 3x3 matrix from a quaternion, as a list of 9 values (row-major)"}, + {"getEulerFromQuaternion", pybullet_getEulerFromQuaternion, METH_VARARGS, + "Convert quaternion [x,y,z,w] to Euler [roll, pitch, yaw] as in URDF/SDF " + "convention"}, - {"calculateInverseDynamics", (PyCFunction)pybullet_calculateInverseDynamics, METH_VARARGS| METH_KEYWORDS, - "Given an object id, joint positions, joint velocities and joint " - "accelerations, compute the joint forces using Inverse Dynamics"}, - - {"calculateInverseKinematics", (PyCFunction)pybullet_calculateInverseKinematics, - METH_VARARGS| METH_KEYWORDS, - "Inverse Kinematics bindings: Given an object id, " - "current joint positions and target position" - " for the end effector," - "compute the inverse kinematics and return the new joint state"}, - - { "getVREvents", (PyCFunction)pybullet_getVREvents, METH_VARARGS | METH_KEYWORDS, - "Get Virtual Reality events, for example to track VR controllers position/buttons" - }, - { "setVRCameraState", (PyCFunction)pybullet_setVRCameraState, METH_VARARGS | METH_KEYWORDS, - "Set properties of the VR Camera such as its root transform " - "for teleporting or to track objects (camera inside a vehicle for example)." - }, - { "getKeyboardEvents", (PyCFunction)pybullet_getKeyboardEvents, METH_VARARGS | METH_KEYWORDS, - "Get Keyboard events, keycode and state (KEY_IS_DOWN, KEY_WAS_TRIGGER, KEY_WAS_RELEASED)" - }, + {"getMatrixFromQuaternion", pybullet_getMatrixFromQuaternion, METH_VARARGS, + "Compute the 3x3 matrix from a quaternion, as a list of 9 values (row-major)"}, - { "startStateLogging", (PyCFunction)pybullet_startStateLogging, METH_VARARGS | METH_KEYWORDS, - "Start logging of state, such as robot base position, orientation, joint positions etc. " - "Specify loggingType (STATE_LOG_MINITAUR, STATE_LOG_GENERIC_ROBOT, STATE_LOG_VR_CONTROLLERS etc), " - "fileName, optional objectUniqueId. Function returns int loggingUniqueId" - }, - { "stopStateLogging", (PyCFunction)pybullet_stopStateLogging, METH_VARARGS | METH_KEYWORDS, - "Stop logging of robot state, given a loggingUniqueId." - }, - { "rayTest", (PyCFunction)pybullet_rayTest, METH_VARARGS | METH_KEYWORDS, - "Cast a ray and return the first object hit, if any. " - "Takes two arguments (from position [x,y,z] and to position [x,y,z] in Cartesian world coordinates" - }, - { "setTimeOut", (PyCFunction)pybullet_setTimeOut, METH_VARARGS | METH_KEYWORDS, - "Set the timeOut in seconds, used for most of the API calls." - }, - // todo(erwincoumans) - // saveSnapshot - // loadSnapshot - // raycast info - // object names + {"calculateInverseDynamics", (PyCFunction)pybullet_calculateInverseDynamics, METH_VARARGS | METH_KEYWORDS, + "Given an object id, joint positions, joint velocities and joint " + "accelerations, compute the joint forces using Inverse Dynamics"}, - {NULL, NULL, 0, NULL} /* Sentinel */ + {"calculateInverseKinematics", (PyCFunction)pybullet_calculateInverseKinematics, + METH_VARARGS | METH_KEYWORDS, + "Inverse Kinematics bindings: Given an object id, " + "current joint positions and target position" + " for the end effector," + "compute the inverse kinematics and return the new joint state"}, + + {"getVREvents", (PyCFunction)pybullet_getVREvents, METH_VARARGS | METH_KEYWORDS, + "Get Virtual Reality events, for example to track VR controllers position/buttons"}, + {"setVRCameraState", (PyCFunction)pybullet_setVRCameraState, METH_VARARGS | METH_KEYWORDS, + "Set properties of the VR Camera such as its root transform " + "for teleporting or to track objects (camera inside a vehicle for example)."}, + {"getKeyboardEvents", (PyCFunction)pybullet_getKeyboardEvents, METH_VARARGS | METH_KEYWORDS, + "Get Keyboard events, keycode and state (KEY_IS_DOWN, KEY_WAS_TRIGGER, KEY_WAS_RELEASED)"}, + + {"startStateLogging", (PyCFunction)pybullet_startStateLogging, METH_VARARGS | METH_KEYWORDS, + "Start logging of state, such as robot base position, orientation, joint positions etc. " + "Specify loggingType (STATE_LOG_MINITAUR, STATE_LOG_GENERIC_ROBOT, STATE_LOG_VR_CONTROLLERS etc), " + "fileName, optional objectUniqueId. Function returns int loggingUniqueId"}, + {"stopStateLogging", (PyCFunction)pybullet_stopStateLogging, METH_VARARGS | METH_KEYWORDS, + "Stop logging of robot state, given a loggingUniqueId."}, + {"rayTest", (PyCFunction)pybullet_rayTest, METH_VARARGS | METH_KEYWORDS, + "Cast a ray and return the first object hit, if any. " + "Takes two arguments (from position [x,y,z] and to position [x,y,z] in Cartesian world coordinates"}, + {"setTimeOut", (PyCFunction)pybullet_setTimeOut, METH_VARARGS | METH_KEYWORDS, + "Set the timeOut in seconds, used for most of the API calls."}, + // todo(erwincoumans) + // saveSnapshot + // loadSnapshot + // raycast info + // object names + + {NULL, NULL, 0, NULL} /* Sentinel */ }; - ///copied from CommonWindowInterface.h +///copied from CommonWindowInterface.h - -enum { +enum +{ B3G_ESCAPE = 27, B3G_F1 = 0xff00, B3G_F2, @@ -5294,15 +5294,15 @@ enum { #if PY_MAJOR_VERSION >= 3 static struct PyModuleDef moduledef = { - PyModuleDef_HEAD_INIT, "pybullet", /* m_name */ - "Python bindings for Bullet Physics Robotics API (also known as Shared " - "Memory API)", /* m_doc */ - -1, /* m_size */ - SpamMethods, /* m_methods */ - NULL, /* m_reload */ - NULL, /* m_traverse */ - NULL, /* m_clear */ - NULL, /* m_free */ + PyModuleDef_HEAD_INIT, "pybullet", /* m_name */ + "Python bindings for Bullet Physics Robotics API (also known as Shared " + "Memory API)", /* m_doc */ + -1, /* m_size */ + SpamMethods, /* m_methods */ + NULL, /* m_reload */ + NULL, /* m_traverse */ + NULL, /* m_clear */ + NULL, /* m_free */ }; #endif @@ -5313,111 +5313,110 @@ PyInit_pybullet(void) initpybullet(void) #endif { - - PyObject* m; + PyObject* m; #if PY_MAJOR_VERSION >= 3 - m = PyModule_Create(&moduledef); + m = PyModule_Create(&moduledef); #else - m = Py_InitModule3("pybullet", SpamMethods, "Python bindings for Bullet"); + m = Py_InitModule3("pybullet", SpamMethods, "Python bindings for Bullet"); #endif #if PY_MAJOR_VERSION >= 3 - if (m == NULL) return m; + if (m == NULL) return m; #else - if (m == NULL) return; + if (m == NULL) return; #endif - PyModule_AddIntConstant(m, "SHARED_MEMORY", - eCONNECT_SHARED_MEMORY); // user read - PyModule_AddIntConstant(m, "DIRECT", eCONNECT_DIRECT); // user read - PyModule_AddIntConstant(m, "GUI", eCONNECT_GUI); // user read - PyModule_AddIntConstant(m, "UDP", eCONNECT_UDP); // user read - PyModule_AddIntConstant(m, "TCP", eCONNECT_TCP); // user read + PyModule_AddIntConstant(m, "SHARED_MEMORY", + eCONNECT_SHARED_MEMORY); // user read + PyModule_AddIntConstant(m, "DIRECT", eCONNECT_DIRECT); // user read + PyModule_AddIntConstant(m, "GUI", eCONNECT_GUI); // user read + PyModule_AddIntConstant(m, "UDP", eCONNECT_UDP); // user read + PyModule_AddIntConstant(m, "TCP", eCONNECT_TCP); // user read - PyModule_AddIntConstant(m, "JOINT_REVOLUTE", eRevoluteType); // user read - PyModule_AddIntConstant(m, "JOINT_PRISMATIC", ePrismaticType); // user read - PyModule_AddIntConstant(m, "JOINT_SPHERICAL", eSphericalType); // user read - PyModule_AddIntConstant(m, "JOINT_PLANAR", ePlanarType); // user read - PyModule_AddIntConstant(m, "JOINT_FIXED", eFixedType); // user read - PyModule_AddIntConstant(m, "JOINT_POINT2POINT", ePoint2PointType); // user read + PyModule_AddIntConstant(m, "JOINT_REVOLUTE", eRevoluteType); // user read + PyModule_AddIntConstant(m, "JOINT_PRISMATIC", ePrismaticType); // user read + PyModule_AddIntConstant(m, "JOINT_SPHERICAL", eSphericalType); // user read + PyModule_AddIntConstant(m, "JOINT_PLANAR", ePlanarType); // user read + PyModule_AddIntConstant(m, "JOINT_FIXED", eFixedType); // user read + PyModule_AddIntConstant(m, "JOINT_POINT2POINT", ePoint2PointType); // user read - PyModule_AddIntConstant(m, "SENSOR_FORCE_TORQUE", eSensorForceTorqueType); // user read + PyModule_AddIntConstant(m, "SENSOR_FORCE_TORQUE", eSensorForceTorqueType); // user read + PyModule_AddIntConstant(m, "TORQUE_CONTROL", CONTROL_MODE_TORQUE); + PyModule_AddIntConstant(m, "VELOCITY_CONTROL", + CONTROL_MODE_VELOCITY); // user read + PyModule_AddIntConstant(m, "POSITION_CONTROL", + CONTROL_MODE_POSITION_VELOCITY_PD); // user read - PyModule_AddIntConstant(m, "TORQUE_CONTROL", CONTROL_MODE_TORQUE); - PyModule_AddIntConstant(m, "VELOCITY_CONTROL", - CONTROL_MODE_VELOCITY); // user read - PyModule_AddIntConstant(m, "POSITION_CONTROL", - CONTROL_MODE_POSITION_VELOCITY_PD); // user read + PyModule_AddIntConstant(m, "LINK_FRAME", EF_LINK_FRAME); + PyModule_AddIntConstant(m, "WORLD_FRAME", EF_WORLD_FRAME); - PyModule_AddIntConstant(m, "LINK_FRAME", EF_LINK_FRAME); - PyModule_AddIntConstant(m, "WORLD_FRAME", EF_WORLD_FRAME); + PyModule_AddIntConstant(m, "CONTACT_REPORT_EXISTING", CONTACT_QUERY_MODE_REPORT_EXISTING_CONTACT_POINTS); + PyModule_AddIntConstant(m, "CONTACT_RECOMPUTE_CLOSEST", CONTACT_QUERY_MODE_COMPUTE_CLOSEST_POINTS); - PyModule_AddIntConstant(m, "CONTACT_REPORT_EXISTING", CONTACT_QUERY_MODE_REPORT_EXISTING_CONTACT_POINTS); - PyModule_AddIntConstant(m, "CONTACT_RECOMPUTE_CLOSEST", CONTACT_QUERY_MODE_COMPUTE_CLOSEST_POINTS); + PyModule_AddIntConstant(m, "VR_BUTTON_IS_DOWN", eButtonIsDown); + PyModule_AddIntConstant(m, "VR_BUTTON_WAS_TRIGGERED", eButtonTriggered); + PyModule_AddIntConstant(m, "VR_BUTTON_WAS_RELEASED", eButtonReleased); - PyModule_AddIntConstant(m, "VR_BUTTON_IS_DOWN", eButtonIsDown); - PyModule_AddIntConstant(m, "VR_BUTTON_WAS_TRIGGERED", eButtonTriggered); - PyModule_AddIntConstant(m, "VR_BUTTON_WAS_RELEASED", eButtonReleased); - - PyModule_AddIntConstant(m, "VR_MAX_CONTROLLERS", MAX_VR_CONTROLLERS); - PyModule_AddIntConstant(m, "VR_MAX_BUTTONS", MAX_VR_BUTTONS); + PyModule_AddIntConstant(m, "VR_MAX_CONTROLLERS", MAX_VR_CONTROLLERS); + PyModule_AddIntConstant(m, "VR_MAX_BUTTONS", MAX_VR_BUTTONS); - PyModule_AddIntConstant(m, "KEY_IS_DOWN", eButtonIsDown); - PyModule_AddIntConstant(m, "KEY_WAS_TRIGGERED", eButtonTriggered); - PyModule_AddIntConstant(m, "KEY_WAS_RELEASED", eButtonReleased); + PyModule_AddIntConstant(m, "KEY_IS_DOWN", eButtonIsDown); + PyModule_AddIntConstant(m, "KEY_WAS_TRIGGERED", eButtonTriggered); + PyModule_AddIntConstant(m, "KEY_WAS_RELEASED", eButtonReleased); - PyModule_AddIntConstant(m, "STATE_LOGGING_MINITAUR", STATE_LOGGING_MINITAUR); - PyModule_AddIntConstant(m, "STATE_LOGGING_GENERIC_ROBOT", STATE_LOGGING_GENERIC_ROBOT); - PyModule_AddIntConstant(m, "STATE_LOGGING_VR_CONTROLLERS", STATE_LOGGING_VR_CONTROLLERS); + PyModule_AddIntConstant(m, "STATE_LOGGING_MINITAUR", STATE_LOGGING_MINITAUR); + PyModule_AddIntConstant(m, "STATE_LOGGING_GENERIC_ROBOT", STATE_LOGGING_GENERIC_ROBOT); + PyModule_AddIntConstant(m, "STATE_LOGGING_VR_CONTROLLERS", STATE_LOGGING_VR_CONTROLLERS); - PyModule_AddIntConstant(m, "COV_ENABLE_GUI", COV_ENABLE_GUI); - PyModule_AddIntConstant(m, "COV_ENABLE_SHADOWS", COV_ENABLE_SHADOWS); - PyModule_AddIntConstant(m, "COV_ENABLE_WIREFRAME", COV_ENABLE_WIREFRAME); - PyModule_AddIntConstant(m,"B3G_F1",B3G_F1); - PyModule_AddIntConstant(m,"B3G_F2",B3G_F2); - PyModule_AddIntConstant(m,"B3G_F3",B3G_F3); - PyModule_AddIntConstant(m,"B3G_F4",B3G_F4); - PyModule_AddIntConstant(m,"B3G_F5",B3G_F5); - PyModule_AddIntConstant(m,"B3G_F6",B3G_F6); - PyModule_AddIntConstant(m,"B3G_F7",B3G_F7); - PyModule_AddIntConstant(m,"B3G_F8",B3G_F8); - PyModule_AddIntConstant(m,"B3G_F9",B3G_F9); - PyModule_AddIntConstant(m,"B3G_F10",B3G_F10); - PyModule_AddIntConstant(m,"B3G_F11",B3G_F11); - PyModule_AddIntConstant(m,"B3G_F12",B3G_F12); - PyModule_AddIntConstant(m,"B3G_F13",B3G_F13); - PyModule_AddIntConstant(m,"B3G_F14",B3G_F14); - PyModule_AddIntConstant(m,"B3G_F15",B3G_F15); - PyModule_AddIntConstant(m,"B3G_LEFT_ARROW",B3G_LEFT_ARROW); - PyModule_AddIntConstant(m,"B3G_RIGHT_ARROW",B3G_RIGHT_ARROW); - PyModule_AddIntConstant(m,"B3G_UP_ARROW",B3G_UP_ARROW); - PyModule_AddIntConstant(m,"B3G_DOWN_ARROW",B3G_DOWN_ARROW); - PyModule_AddIntConstant(m,"B3G_PAGE_UP",B3G_PAGE_UP); - PyModule_AddIntConstant(m,"B3G_PAGE_DOWN",B3G_PAGE_DOWN); - PyModule_AddIntConstant(m,"B3G_END",B3G_END); - PyModule_AddIntConstant(m,"B3G_HOME",B3G_HOME); - PyModule_AddIntConstant(m,"B3G_INSERT",B3G_INSERT); - PyModule_AddIntConstant(m,"B3G_DELETE",B3G_DELETE); - PyModule_AddIntConstant(m,"B3G_BACKSPACE", B3G_BACKSPACE); - PyModule_AddIntConstant(m,"B3G_SHIFT", B3G_SHIFT); - PyModule_AddIntConstant(m,"B3G_CONTROL", B3G_CONTROL); - PyModule_AddIntConstant(m,"B3G_ALT", B3G_ALT); - PyModule_AddIntConstant(m,"B3G_RETURN", B3G_RETURN); + PyModule_AddIntConstant(m, "COV_ENABLE_GUI", COV_ENABLE_GUI); + PyModule_AddIntConstant(m, "COV_ENABLE_SHADOWS", COV_ENABLE_SHADOWS); + PyModule_AddIntConstant(m, "COV_ENABLE_WIREFRAME", COV_ENABLE_WIREFRAME); + PyModule_AddIntConstant(m, "ER_TINY_RENDERER", ER_TINY_RENDERER); + PyModule_AddIntConstant(m, "ER_BULLET_HARDWARE_OPENGL", ER_BULLET_HARDWARE_OPENGL); - - SpamError = PyErr_NewException("pybullet.error", NULL, NULL); - Py_INCREF(SpamError); - PyModule_AddObject(m, "error", SpamError); + PyModule_AddIntConstant(m, "B3G_F1", B3G_F1); + PyModule_AddIntConstant(m, "B3G_F2", B3G_F2); + PyModule_AddIntConstant(m, "B3G_F3", B3G_F3); + PyModule_AddIntConstant(m, "B3G_F4", B3G_F4); + PyModule_AddIntConstant(m, "B3G_F5", B3G_F5); + PyModule_AddIntConstant(m, "B3G_F6", B3G_F6); + PyModule_AddIntConstant(m, "B3G_F7", B3G_F7); + PyModule_AddIntConstant(m, "B3G_F8", B3G_F8); + PyModule_AddIntConstant(m, "B3G_F9", B3G_F9); + PyModule_AddIntConstant(m, "B3G_F10", B3G_F10); + PyModule_AddIntConstant(m, "B3G_F11", B3G_F11); + PyModule_AddIntConstant(m, "B3G_F12", B3G_F12); + PyModule_AddIntConstant(m, "B3G_F13", B3G_F13); + PyModule_AddIntConstant(m, "B3G_F14", B3G_F14); + PyModule_AddIntConstant(m, "B3G_F15", B3G_F15); + PyModule_AddIntConstant(m, "B3G_LEFT_ARROW", B3G_LEFT_ARROW); + PyModule_AddIntConstant(m, "B3G_RIGHT_ARROW", B3G_RIGHT_ARROW); + PyModule_AddIntConstant(m, "B3G_UP_ARROW", B3G_UP_ARROW); + PyModule_AddIntConstant(m, "B3G_DOWN_ARROW", B3G_DOWN_ARROW); + PyModule_AddIntConstant(m, "B3G_PAGE_UP", B3G_PAGE_UP); + PyModule_AddIntConstant(m, "B3G_PAGE_DOWN", B3G_PAGE_DOWN); + PyModule_AddIntConstant(m, "B3G_END", B3G_END); + PyModule_AddIntConstant(m, "B3G_HOME", B3G_HOME); + PyModule_AddIntConstant(m, "B3G_INSERT", B3G_INSERT); + PyModule_AddIntConstant(m, "B3G_DELETE", B3G_DELETE); + PyModule_AddIntConstant(m, "B3G_BACKSPACE", B3G_BACKSPACE); + PyModule_AddIntConstant(m, "B3G_SHIFT", B3G_SHIFT); + PyModule_AddIntConstant(m, "B3G_CONTROL", B3G_CONTROL); + PyModule_AddIntConstant(m, "B3G_ALT", B3G_ALT); + PyModule_AddIntConstant(m, "B3G_RETURN", B3G_RETURN); + + SpamError = PyErr_NewException("pybullet.error", NULL, NULL); + Py_INCREF(SpamError); + PyModule_AddObject(m, "error", SpamError); #ifdef PYBULLET_USE_NUMPY - // Initialize numpy array. - import_array(); -#endif //PYBULLET_USE_NUMPY - + // Initialize numpy array. + import_array(); +#endif //PYBULLET_USE_NUMPY #if PY_MAJOR_VERSION >= 3 - return m; + return m; #endif } diff --git a/examples/pybullet/quadruped.py b/examples/pybullet/quadruped.py index 0d8f32b47..cf097941e 100644 --- a/examples/pybullet/quadruped.py +++ b/examples/pybullet/quadruped.py @@ -3,104 +3,156 @@ import time import math useRealTime = 0 -fixedTimeStep = 0.001 +fixedTimeStep = 0.01 +speed = 10 +amplitude = 0.8 +jump_amp = 0.5 +maxForce = 3.5 +kneeFrictionForce = 0.00 +kp = 1 +kd = .1 + physId = p.connect(p.SHARED_MEMORY) if (physId<0): p.connect(p.GUI) +#p.resetSimulation() +p.loadURDF("plane.urdf",0,0,0) +p.setPhysicsEngineParameter(numSolverIterations=50) -p.loadURDF("plane.urdf") -p.setGravity(0,0,-1) +p.setTimeOut(4) + +p.setGravity(0,0,0) p.setTimeStep(fixedTimeStep) +orn = p.getQuaternionFromEuler([0,0,0.4]) p.setRealTimeSimulation(0) -quadruped = p.loadURDF("quadruped/quadruped.urdf",1,-2,1) +quadruped = p.loadURDF("quadruped/minitaur.urdf",[1,0,0.2],orn,useFixedBase=False) +nJoints = p.getNumJoints(quadruped) + +jointNameToId = {} +for i in range(nJoints): + jointInfo = p.getJointInfo(quadruped, i) + jointNameToId[jointInfo[1].decode('UTF-8')] = jointInfo[0] + + +motor_front_rightR_joint = jointNameToId['motor_front_rightR_joint'] +hip_front_rightR_link = jointNameToId['hip_front_rightR_link'] +knee_front_rightR_link = jointNameToId['knee_front_rightR_link'] +motor_front_rightL_joint = jointNameToId['motor_front_rightL_joint'] +motor_front_rightL_link = jointNameToId['motor_front_rightL_link'] +knee_front_rightL_link = jointNameToId['knee_front_rightL_link'] +motor_front_leftR_joint = jointNameToId['motor_front_leftR_joint'] +hip_front_leftR_link = jointNameToId['hip_front_leftR_link'] +knee_front_leftR_link = jointNameToId['knee_front_leftR_link'] +motor_front_leftL_joint = jointNameToId['motor_front_leftL_joint'] +motor_front_leftL_link = jointNameToId['motor_front_leftL_link'] +knee_front_leftL_link = jointNameToId['knee_front_leftL_link'] +motor_back_rightR_joint = jointNameToId['motor_back_rightR_joint'] +hip_rightR_link = jointNameToId['hip_rightR_link'] +knee_back_rightR_link = jointNameToId['knee_back_rightR_link'] +motor_back_rightL_joint = jointNameToId['motor_back_rightL_joint'] +motor_back_rightL_link = jointNameToId['motor_back_rightL_link'] +knee_back_rightL_link = jointNameToId['knee_back_rightL_link'] +motor_back_leftR_joint = jointNameToId['motor_back_leftR_joint'] +hip_leftR_link = jointNameToId['hip_leftR_link'] +knee_back_leftR_link = jointNameToId['knee_back_leftR_link'] +motor_back_leftL_joint = jointNameToId['motor_back_leftL_joint'] +motor_back_leftL_link = jointNameToId['motor_back_leftL_link'] +knee_back_leftL_link = jointNameToId['knee_back_leftL_link'] + +motordir=[-1,-1,-1,-1,1,1,1,1] +halfpi = 1.57079632679 +kneeangle = -2.1834 +p.resetJointState(quadruped,motor_front_leftL_joint,motordir[0]*halfpi) +p.resetJointState(quadruped,knee_front_leftL_link,motordir[0]*kneeangle) +p.resetJointState(quadruped,motor_front_leftR_joint,motordir[1]*halfpi) +p.resetJointState(quadruped,knee_front_leftR_link,motordir[1]*kneeangle) +cid = p.createConstraint(quadruped,knee_front_leftR_link,quadruped,knee_front_leftL_link,p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2]) +p.changeConstraint(cid,maxForce=10000) + +p.setJointMotorControl(quadruped,knee_front_leftL_link,p.VELOCITY_CONTROL,0,kneeFrictionForce) +p.setJointMotorControl(quadruped,knee_front_leftR_link,p.VELOCITY_CONTROL,0,kneeFrictionForce) + +p.resetJointState(quadruped,motor_back_leftL_joint,motordir[2]*halfpi) +p.resetJointState(quadruped,knee_back_leftL_link,motordir[2]*kneeangle) +p.resetJointState(quadruped,motor_back_leftR_joint,motordir[3]*halfpi) +p.resetJointState(quadruped,knee_back_leftR_link,motordir[3]*kneeangle) +cid = p.createConstraint(quadruped,knee_back_leftR_link,quadruped,knee_back_leftL_link,p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2]) +p.changeConstraint(cid,maxForce=10000) +p.setJointMotorControl(quadruped,knee_back_leftL_link,p.VELOCITY_CONTROL,0,kneeFrictionForce) +p.setJointMotorControl(quadruped,knee_back_leftR_link,p.VELOCITY_CONTROL,0,kneeFrictionForce) + + #p.getNumJoints(1) -#right front leg -p.resetJointState(quadruped,0,1.57) -p.resetJointState(quadruped,2,-2.2) -p.resetJointState(quadruped,3,-1.57) -p.resetJointState(quadruped,5,2.2) -p.createConstraint(quadruped,2,quadruped,5,p.JOINT_POINT2POINT,[0,0,0],[0,0.01,0.2],[0,-0.015,0.2]) - -p.setJointMotorControl(quadruped,0,p.POSITION_CONTROL,1.57,1) -p.setJointMotorControl(quadruped,1,p.VELOCITY_CONTROL,0,0) -p.setJointMotorControl(quadruped,2,p.VELOCITY_CONTROL,0,0) -p.setJointMotorControl(quadruped,3,p.POSITION_CONTROL,-1.57,1) -p.setJointMotorControl(quadruped,4,p.VELOCITY_CONTROL,0,0) -p.setJointMotorControl(quadruped,5,p.VELOCITY_CONTROL,0,0) - -#left front leg -p.resetJointState(quadruped,6,1.57) -p.resetJointState(quadruped,8,-2.2) -p.resetJointState(quadruped,9,-1.57) -p.resetJointState(quadruped,11,2.2) -p.createConstraint(quadruped,8,quadruped,11,p.JOINT_POINT2POINT,[0,0,0],[0,-0.01,0.2],[0,0.015,0.2]) - -p.setJointMotorControl(quadruped,6,p.POSITION_CONTROL,1.57,1) -p.setJointMotorControl(quadruped,7,p.VELOCITY_CONTROL,0,0) -p.setJointMotorControl(quadruped,8,p.VELOCITY_CONTROL,0,0) -p.setJointMotorControl(quadruped,9,p.POSITION_CONTROL,-1.57,1) -p.setJointMotorControl(quadruped,10,p.VELOCITY_CONTROL,0,0) -p.setJointMotorControl(quadruped,11,p.VELOCITY_CONTROL,0,0) -#right back leg -p.resetJointState(quadruped,12,1.57) -p.resetJointState(quadruped,14,-2.2) -p.resetJointState(quadruped,15,-1.57) -p.resetJointState(quadruped,17,2.2) -p.createConstraint(quadruped,14,quadruped,17,p.JOINT_POINT2POINT,[0,0,0],[0,0.01,0.2],[0,-0.015,0.2]) +p.resetJointState(quadruped,motor_front_rightL_joint,motordir[4]*halfpi) +p.resetJointState(quadruped,knee_front_rightL_link,motordir[4]*kneeangle) +p.resetJointState(quadruped,motor_front_rightR_joint,motordir[5]*halfpi) +p.resetJointState(quadruped,knee_front_rightR_link,motordir[5]*kneeangle) +cid = p.createConstraint(quadruped,knee_front_rightR_link,quadruped,knee_front_rightL_link,p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2]) +p.changeConstraint(cid,maxForce=10000) -p.setJointMotorControl(quadruped,12,p.POSITION_CONTROL,1.57,1) -p.setJointMotorControl(quadruped,13,p.VELOCITY_CONTROL,0,0) -p.setJointMotorControl(quadruped,14,p.VELOCITY_CONTROL,0,0) -p.setJointMotorControl(quadruped,15,p.POSITION_CONTROL,-1.57,1) -p.setJointMotorControl(quadruped,16,p.VELOCITY_CONTROL,0,0) -p.setJointMotorControl(quadruped,17,p.VELOCITY_CONTROL,0,0) +p.setJointMotorControl(quadruped,knee_front_rightL_link,p.VELOCITY_CONTROL,0,kneeFrictionForce) +p.setJointMotorControl(quadruped,knee_front_rightR_link,p.VELOCITY_CONTROL,0,kneeFrictionForce) -#left back leg -p.resetJointState(quadruped,18,1.57) -p.resetJointState(quadruped,20,-2.2) -p.resetJointState(quadruped,21,-1.57) -p.resetJointState(quadruped,23,2.2) -p.createConstraint(quadruped,20,quadruped,23,p.JOINT_POINT2POINT,[0,0,0],[0,-0.01,0.2],[0,0.015,0.2]) +p.resetJointState(quadruped,motor_back_rightL_joint,motordir[6]*halfpi) +p.resetJointState(quadruped,knee_back_rightL_link,motordir[6]*kneeangle) +p.resetJointState(quadruped,motor_back_rightR_joint,motordir[7]*halfpi) +p.resetJointState(quadruped,knee_back_rightR_link,motordir[7]*kneeangle) +cid = p.createConstraint(quadruped,knee_back_rightR_link,quadruped,knee_back_rightL_link,p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2]) +p.changeConstraint(cid,maxForce=10000) -p.setJointMotorControl(quadruped,18,p.POSITION_CONTROL,1.57,1) -p.setJointMotorControl(quadruped,19,p.VELOCITY_CONTROL,0,0) -p.setJointMotorControl(quadruped,20,p.VELOCITY_CONTROL,0,0) -p.setJointMotorControl(quadruped,21,p.POSITION_CONTROL,-1.57,1) -p.setJointMotorControl(quadruped,22,p.VELOCITY_CONTROL,0,0) -p.setJointMotorControl(quadruped,23,p.VELOCITY_CONTROL,0,0) - - - -p_gain =1 -speed = 10 -amplitude = 1.3 - -#stand still -p.setRealTimeSimulation(useRealTime) - - -t=0.0 -ref_time = time.time() -t_end = t + 4 -while t < t_end: - if (useRealTime==0): - t = t+fixedTimeStep - p.stepSimulation() - else: - t = time.time()-ref_time - p.setGravity(0,0,-1) +p.setJointMotorControl(quadruped,knee_back_rightL_link,p.VELOCITY_CONTROL,0,kneeFrictionForce) +p.setJointMotorControl(quadruped,knee_back_rightR_link,p.VELOCITY_CONTROL,0,kneeFrictionForce) p.setGravity(0,0,-10) -jump_amp = 0.5 + + +legnumbering=[ + motor_front_leftL_joint, + motor_front_leftR_joint, + motor_back_leftL_joint, + motor_back_leftR_joint, + motor_front_rightL_joint, + motor_front_rightR_joint, + motor_back_rightL_joint, + motor_back_rightR_joint] + +for i in range (8): + print (legnumbering[i]) +#use the Minitaur leg numbering +p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[0],controlMode=p.POSITION_CONTROL,targetPosition=motordir[0]*1.57,positionGain=kp, velocityGain=kd, force=maxForce) +p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[1],controlMode=p.POSITION_CONTROL,targetPosition=motordir[1]*1.57,positionGain=kp, velocityGain=kd, force=maxForce) +p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[2],controlMode=p.POSITION_CONTROL,targetPosition=motordir[2]*1.57,positionGain=kp, velocityGain=kd, force=maxForce) +p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[3],controlMode=p.POSITION_CONTROL,targetPosition=motordir[3]*1.57,positionGain=kp, velocityGain=kd, force=maxForce) +p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[4],controlMode=p.POSITION_CONTROL,targetPosition=motordir[4]*1.57,positionGain=kp, velocityGain=kd, force=maxForce) +p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[5],controlMode=p.POSITION_CONTROL,targetPosition=motordir[5]*1.57,positionGain=kp, velocityGain=kd, force=maxForce) +p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[6],controlMode=p.POSITION_CONTROL,targetPosition=motordir[6]*1.57,positionGain=kp, velocityGain=kd, force=maxForce) +p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[7],controlMode=p.POSITION_CONTROL,targetPosition=motordir[7]*1.57,positionGain=kp, velocityGain=kd, force=maxForce) +#stand still +p.setRealTimeSimulation(useRealTime) + +#while (True): +# time.sleep(0.01) +#p.stepSimulation() + + +print("quadruped Id = ") +print(quadruped) +p.saveWorld("quadru.py") +logId = p.startStateLogging(p.STATE_LOGGING_MINITAUR,"quadrupedLog.txt",[quadruped]) + + + + + #jump t = 0.0 -t_end = t + 10 +t_end = t + 100 i=0 ref_time = time.time() @@ -109,56 +161,19 @@ while t < t_end: t = time.time()-ref_time else: t = t+fixedTimeStep - - p.setJointMotorControl(quadruped,0,p.POSITION_CONTROL,math.sin(t*speed)*jump_amp+1.57,p_gain) - p.setJointMotorControl(quadruped,3,p.POSITION_CONTROL,-math.sin(t*speed)*jump_amp-1.57,p_gain) - p.setJointMotorControl(quadruped,6,p.POSITION_CONTROL,math.sin(t*speed)*jump_amp+1.57,p_gain) - p.setJointMotorControl(quadruped,9,p.POSITION_CONTROL,-math.sin(t*speed)*jump_amp-1.57,p_gain) - p.setJointMotorControl(quadruped,12,p.POSITION_CONTROL,math.sin(t*speed)*jump_amp+1.57,p_gain) - p.setJointMotorControl(quadruped,15,p.POSITION_CONTROL,-math.sin(t*speed)*jump_amp-1.57,p_gain) - p.setJointMotorControl(quadruped,18,p.POSITION_CONTROL,math.sin(t*speed)*jump_amp+1.57,p_gain) - p.setJointMotorControl(quadruped,21,p.POSITION_CONTROL,-math.sin(t*speed)*jump_amp-1.57,p_gain) + if (True): + + target = math.sin(t*speed)*jump_amp+1.57; + p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[0],controlMode=p.POSITION_CONTROL,targetPosition=motordir[0]*target,positionGain=kp, velocityGain=kd, force=maxForce) + p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[1],controlMode=p.POSITION_CONTROL,targetPosition=motordir[1]*target,positionGain=kp, velocityGain=kd, force=maxForce) + p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[2],controlMode=p.POSITION_CONTROL,targetPosition=motordir[2]*target,positionGain=kp, velocityGain=kd, force=maxForce) + p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[3],controlMode=p.POSITION_CONTROL,targetPosition=motordir[3]*target,positionGain=kp, velocityGain=kd, force=maxForce) + p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[4],controlMode=p.POSITION_CONTROL,targetPosition=motordir[4]*target,positionGain=kp, velocityGain=kd, force=maxForce) + p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[5],controlMode=p.POSITION_CONTROL,targetPosition=motordir[5]*target,positionGain=kp, velocityGain=kd, force=maxForce) + p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[6],controlMode=p.POSITION_CONTROL,targetPosition=motordir[6]*target,positionGain=kp, velocityGain=kd, force=maxForce) + p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[7],controlMode=p.POSITION_CONTROL,targetPosition=motordir[7]*target,positionGain=kp, velocityGain=kd, force=maxForce) + if (useRealTime==0): p.stepSimulation() + time.sleep(fixedTimeStep) - -#hop forward -t_end = 20 -i=0 -while t < t_end: - if (useRealTime): - t = time.time()-ref_time - else: - t = t+fixedTimeStep - - p.setJointMotorControl(quadruped,0,p.POSITION_CONTROL,math.sin(t*speed)*amplitude+1.57,p_gain) - p.setJointMotorControl(quadruped,3,p.POSITION_CONTROL,-1.57,p_gain) - p.setJointMotorControl(quadruped,6,p.POSITION_CONTROL,math.sin(t*speed)*amplitude+1.57,p_gain) - p.setJointMotorControl(quadruped,9,p.POSITION_CONTROL,-1.57,p_gain) - p.setJointMotorControl(quadruped,12,p.POSITION_CONTROL,math.sin(t*speed+3.14)*amplitude+1.57,p_gain) - p.setJointMotorControl(quadruped,15,p.POSITION_CONTROL,-1.57,p_gain) - p.setJointMotorControl(quadruped,18,p.POSITION_CONTROL,math.sin(t*speed+3.14)*amplitude+1.57,p_gain) - p.setJointMotorControl(quadruped,21,p.POSITION_CONTROL,-1.57,p_gain) - if (useRealTime==0): - p.stepSimulation() - -#walk -t_end = 100 -i=0 -while t < t_end: - if (useRealTime): - t = time.time()-ref_time - else: - t = t+fixedTimeStep - - p.setJointMotorControl(quadruped,0,p.POSITION_CONTROL,math.sin(t*3)*.3+1.57,1) - p.setJointMotorControl(quadruped,3,p.POSITION_CONTROL,-1.57,1) - p.setJointMotorControl(quadruped,6,p.POSITION_CONTROL,math.sin(t*3+0.5*3.14)*.3+1.57,1) - p.setJointMotorControl(quadruped,9,p.POSITION_CONTROL,-1.57,1) - p.setJointMotorControl(quadruped,12,p.POSITION_CONTROL,math.sin(t*3+3.14)*.3+1.57,1) - p.setJointMotorControl(quadruped,15,p.POSITION_CONTROL,-1.57,1) - p.setJointMotorControl(quadruped,18,p.POSITION_CONTROL,math.sin(t*3+1.5*3.14)*.3+1.57,1) - p.setJointMotorControl(quadruped,21,p.POSITION_CONTROL,-1.57,1) - - p.stepSimulation() -p.setRealTimeSimulation(1) diff --git a/examples/pybullet/quadruped_playback.py b/examples/pybullet/quadruped_playback.py new file mode 100644 index 000000000..7aaefda2e --- /dev/null +++ b/examples/pybullet/quadruped_playback.py @@ -0,0 +1,126 @@ +import pybullet as p +import time +import math +from datetime import datetime +from numpy import * +from pylab import * +import struct +import sys +import os, fnmatch +import argparse +from time import sleep + +def readLogFile(filename, verbose = True): + f = open(filename, 'rb') + + print('Opened'), + print(filename) + + keys = f.readline().decode('utf8').rstrip('\n').split(',') + fmt = f.readline().decode('utf8').rstrip('\n') + + # The byte number of one record + sz = struct.calcsize(fmt) + # The type number of one record + ncols = len(fmt) + + if verbose: + print('Keys:'), + print(keys) + print('Format:'), + print(fmt) + print('Size:'), + print(sz) + print('Columns:'), + print(ncols) + + # Read data + wholeFile = f.read() + # split by alignment word + chunks = wholeFile.split(b'\xaa\xbb') + print ("num chunks") + print (len(chunks)) + + log = list() + for chunk in chunks: + if len(chunk) == sz: + values = struct.unpack(fmt, chunk) + record = list() + for i in range(ncols): + record.append(values[i]) + log.append(record) + + return log + +clid = p.connect(p.SHARED_MEMORY) + +log = readLogFile("LOG00076.TXT"); + +recordNum = len(log) +print('record num:'), +print(recordNum) +itemNum = len(log[0]) +print('item num:'), +print(itemNum) + +useRealTime = 0 +fixedTimeStep = 0.001 +speed = 10 +amplitude = 0.8 +jump_amp = 0.5 +maxForce = 3.5 +kp = .05 +kd = .5 + + + +quadruped = 1 +nJoints = p.getNumJoints(quadruped) +jointNameToId = {} +for i in range(nJoints): + jointInfo = p.getJointInfo(quadruped, i) + jointNameToId[jointInfo[1].decode('UTF-8')] = jointInfo[0] + +motor_front_rightR_joint = jointNameToId['motor_front_rightR_joint'] +hip_front_rightR_link = jointNameToId['hip_front_rightR_link'] +knee_front_rightR_link = jointNameToId['knee_front_rightR_link'] +motor_front_rightL_joint = jointNameToId['motor_front_rightL_joint'] +motor_front_rightL_link = jointNameToId['motor_front_rightL_link'] +knee_front_rightL_link = jointNameToId['knee_front_rightL_link'] +motor_front_leftR_joint = jointNameToId['motor_front_leftR_joint'] +hip_front_leftR_link = jointNameToId['hip_front_leftR_link'] +knee_front_leftR_link = jointNameToId['knee_front_leftR_link'] +motor_front_leftL_joint = jointNameToId['motor_front_leftL_joint'] +motor_front_leftL_link = jointNameToId['motor_front_leftL_link'] +knee_front_leftL_link = jointNameToId['knee_front_leftL_link'] +motor_back_rightR_joint = jointNameToId['motor_back_rightR_joint'] +hip_rightR_link = jointNameToId['hip_rightR_link'] +knee_back_rightR_link = jointNameToId['knee_back_rightR_link'] +motor_back_rightL_joint = jointNameToId['motor_back_rightL_joint'] +motor_back_rightL_link = jointNameToId['motor_back_rightL_link'] +knee_back_rightL_link = jointNameToId['knee_back_rightL_link'] +motor_back_leftR_joint = jointNameToId['motor_back_leftR_joint'] +hip_leftR_link = jointNameToId['hip_leftR_link'] +knee_back_leftR_link = jointNameToId['knee_back_leftR_link'] +motor_back_leftL_joint = jointNameToId['motor_back_leftL_joint'] +motor_back_leftL_link = jointNameToId['motor_back_leftL_link'] +knee_back_leftL_link = jointNameToId['knee_back_leftL_link'] + +motorDir = [1, 1, 1, 1, 1, 1, 1, 1]; +legnumbering=[motor_front_leftR_joint,motor_front_leftL_joint,motor_back_leftR_joint,motor_back_leftL_joint,motor_front_rightR_joint,motor_front_rightL_joint,motor_back_rightR_joint,motor_back_rightL_joint] + + +for record in log: + p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=legnumbering[0], controlMode=p.POSITION_CONTROL, targetPosition=motorDir[0]*record[7], positionGain=kp, velocityGain=kd, force=maxForce) + p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=legnumbering[1], controlMode=p.POSITION_CONTROL, targetPosition=motorDir[1]*record[8], positionGain=kp, velocityGain=kd, force=maxForce) + p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=legnumbering[2], controlMode=p.POSITION_CONTROL, targetPosition=motorDir[2]*record[9], positionGain=kp, velocityGain=kd, force=maxForce) + p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=legnumbering[3], controlMode=p.POSITION_CONTROL, targetPosition=motorDir[3]*record[10], positionGain=kp, velocityGain=kd, force=maxForce) + p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=legnumbering[4], controlMode=p.POSITION_CONTROL, targetPosition=motorDir[4]*record[11], positionGain=kp, velocityGain=kd, force=maxForce) + p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=legnumbering[5], controlMode=p.POSITION_CONTROL, targetPosition=motorDir[5]*record[12], positionGain=kp, velocityGain=kd, force=maxForce) + p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=legnumbering[6], controlMode=p.POSITION_CONTROL, targetPosition=motorDir[6]*record[13], positionGain=kp, velocityGain=kd, force=maxForce) + p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=legnumbering[7], controlMode=p.POSITION_CONTROL, targetPosition=motorDir[7]*record[14], positionGain=kp, velocityGain=kd, force=maxForce) + p.setGravity(0.000000,0.000000,-10.000000) + p.stepSimulation() + p.stepSimulation() + sleep(0.01) + \ No newline at end of file diff --git a/examples/pybullet/quadruped_setup_playback.py b/examples/pybullet/quadruped_setup_playback.py new file mode 100644 index 000000000..a2b041888 --- /dev/null +++ b/examples/pybullet/quadruped_setup_playback.py @@ -0,0 +1,21 @@ +import pybullet as p +p.connect(p.SHARED_MEMORY) +objects = [p.loadURDF("plane.urdf", 0.000000,0.000000,-.300000,0.000000,0.000000,0.000000,1.000000)] +objects = [p.loadURDF("quadruped/minitaur.urdf", [-0.000046,-0.000068,0.200774],[-0.000701,0.000387,-0.000252,1.000000],useFixedBase=False)] +ob = objects[0] +jointPositions=[ 0.000000, 1.531256, 0.000000, -2.240112, 1.527979, 0.000000, -2.240646, 1.533105, 0.000000, -2.238254, 1.530335, 0.000000, -2.238298, 0.000000, -1.528038, 0.000000, 2.242656, -1.525193, 0.000000, 2.244008, -1.530011, 0.000000, 2.240683, -1.528687, 0.000000, 2.240517 ] +for ji in range (p.getNumJoints(ob)): + p.resetJointState(ob,ji,jointPositions[ji]) + p.setJointMotorControl2(bodyIndex=ob, jointIndex=ji, controlMode=p.VELOCITY_CONTROL, force=0) + +cid0 = p.createConstraint(1,3,1,6,p.JOINT_POINT2POINT,[0.000000,0.000000,0.000000],[0.000000,0.005000,0.200000],[0.000000,0.010000,0.200000],[0.000000,0.000000,0.000000,1.000000],[0.000000,0.000000,0.000000,1.000000]) +p.changeConstraint(cid0,maxForce=500.000000) +cid1 = p.createConstraint(1,16,1,19,p.JOINT_POINT2POINT,[0.000000,0.000000,0.000000],[0.000000,0.005000,0.200000],[0.000000,0.010000,0.200000],[0.000000,0.000000,0.000000,1.000000],[0.000000,0.000000,0.000000,1.000000]) +p.changeConstraint(cid1,maxForce=500.000000) +cid2 = p.createConstraint(1,9,1,12,p.JOINT_POINT2POINT,[0.000000,0.000000,0.000000],[0.000000,0.005000,0.200000],[0.000000,0.010000,0.200000],[0.000000,0.000000,0.000000,1.000000],[0.000000,0.000000,0.000000,1.000000]) +p.changeConstraint(cid2,maxForce=500.000000) +cid3 = p.createConstraint(1,22,1,25,p.JOINT_POINT2POINT,[0.000000,0.000000,0.000000],[0.000000,0.005000,0.200000],[0.000000,0.010000,0.200000],[0.000000,0.000000,0.000000,1.000000],[0.000000,0.000000,0.000000,1.000000]) +p.changeConstraint(cid3,maxForce=500.000000) +p.setGravity(0.000000,0.000000,0.000000) +p.stepSimulation() +p.disconnect() diff --git a/examples/pybullet/testrender.py b/examples/pybullet/testrender.py index 0d3475c84..72fa9e27e 100644 --- a/examples/pybullet/testrender.py +++ b/examples/pybullet/testrender.py @@ -29,7 +29,8 @@ for pitch in range (0,360,10) : viewMatrix = pybullet.computeViewMatrixFromYawPitchRoll(camTargetPos, camDistance, yaw, pitch, roll, upAxisIndex) aspect = pixelWidth / pixelHeight; projectionMatrix = pybullet.computeProjectionMatrixFOV(fov, aspect, nearPlane, farPlane); - img_arr = pybullet.getCameraImage(pixelWidth, pixelHeight, viewMatrix,projectionMatrix, lightDirection,lightColor) + #getCameraImage can also use renderer=pybullet.ER_BULLET_HARDWARE_OPENGL + img_arr = pybullet.getCameraImage(pixelWidth, pixelHeight, viewMatrix,projectionMatrix, lightDirection,lightColor,renderer=pybullet.ER_TINY_RENDERER) w=img_arr[0] h=img_arr[1] rgb=img_arr[2] diff --git a/examples/pybullet/testrender_np.py b/examples/pybullet/testrender_np.py index b8fca10eb..9f84fbed9 100644 --- a/examples/pybullet/testrender_np.py +++ b/examples/pybullet/testrender_np.py @@ -6,7 +6,7 @@ import matplotlib.pyplot as plt import pybullet import time -pybullet.connect(pybullet.DIRECT) +pybullet.connect(pybullet.GUI) pybullet.loadURDF("r2d2.urdf") camTargetPos = [0,0,0] @@ -18,8 +18,8 @@ pitch = 10.0 roll=0 upAxisIndex = 2 camDistance = 4 -pixelWidth = 1920 -pixelHeight = 1080 +pixelWidth = 1024 +pixelHeight = 768 nearPlane = 0.01 farPlane = 1000 @@ -31,7 +31,7 @@ for pitch in range (0,360,10) : viewMatrix = pybullet.computeViewMatrixFromYawPitchRoll(camTargetPos, camDistance, yaw, pitch, roll, upAxisIndex) aspect = pixelWidth / pixelHeight; projectionMatrix = pybullet.computeProjectionMatrixFOV(fov, aspect, nearPlane, farPlane); - img_arr = pybullet.getCameraImage(pixelWidth, pixelHeight, viewMatrix,projectionMatrix, [0,1,0]) + img_arr = pybullet.getCameraImage(pixelWidth, pixelHeight, viewMatrix,projectionMatrix, [0,1,0],renderer=pybullet.ER_BULLET_HARDWARE_OPENGL) stop = time.time() print ("renderImage %f" % (stop - start)) @@ -40,13 +40,12 @@ for pitch in range (0,360,10) : rgb=img_arr[2] #color data RGB dep=img_arr[3] #depth data - print 'width = %d height = %d' % (w,h) + print ('width = %d height = %d' % (w,h)) #note that sending the data to matplotlib is really slow - #show + plt.imshow(rgb,interpolation='none') - #plt.show() - plt.pause(0.01) + plt.pause(0.001) main_stop = time.time() diff --git a/examples/pybullet/vr_kuka_control.py b/examples/pybullet/vr_kuka_control.py new file mode 100644 index 000000000..08f71d336 --- /dev/null +++ b/examples/pybullet/vr_kuka_control.py @@ -0,0 +1,99 @@ +## Assume you have run vr_kuka_setup and have default scene set up +# Require p.setInternalSimFlags(0) in kuka_setup +import pybullet as p +import math +# import numpy as np + +p.connect(p.SHARED_MEMORY) + +kuka = 3 +kuka_gripper = 7 +POSITION = 1 +ORIENTATION = 2 +BUTTONS = 6 + +THRESHOLD = 1.3 +LOWER_LIMITS = [-.967, -2.0, -2.96, 0.19, -2.96, -2.09, -3.05] +UPPER_LIMITS = [.96, 2.0, 2.96, 2.29, 2.96, 2.09, 3.05] +JOINT_RANGE = [5.8, 4, 5.8, 4, 5.8, 4, 6] +REST_POSE = [0, 0, 0, math.pi / 2, 0, -math.pi * 0.66, 0] +JOINT_DAMP = [.1, .1, .1, .1, .1, .1, .1] +REST_JOINT_POS = [-0., -0., 0., 1.570793, 0., -1.036725, 0.000001] +MAX_FORCE = 500 +KUKA_GRIPPER_REST_POS = [0., -0.011130, -0.206421, 0.205143, -0.009999, 0., -0.010055, 0.] +KUKA_GRIPPER_CLOZ_POS = [0.0, -0.047564246423083795, 0.6855956234759611, -0.7479294372303137, 0.05054599996976922, 0.0, 0.049838105678835724, 0.0] + +def euc_dist(posA, posB): + dist = 0. + for i in range(len(posA)): + dist += (posA[i] - posB[i]) ** 2 + return dist + +p.setRealTimeSimulation(1) + +controllers = [e[0] for e in p.getVREvents()] + +while True: + + events = p.getVREvents() + for e in (events): + + # Only use one controller + ########################################### + # This is important: make sure there's only one VR Controller! + if e[0] == controllers[0]: + break + + sq_len = euc_dist(p.getLinkState(kuka, 6)[0], e[POSITION]) + + # A simplistic version of gripper control + #@TO-DO: Add slider for the gripper + if e[BUTTONS][33] & p.VR_BUTTON_WAS_TRIGGERED: + # avg = 0. + for i in range(p.getNumJoints(kuka_gripper)): + p.setJointMotorControl2(kuka_gripper, i, p.POSITION_CONTROL, targetPosition=KUKA_GRIPPER_CLOZ_POS[i], force=50) + + if e[BUTTONS][33] & p.VR_BUTTON_WAS_RELEASED: + for i in range(p.getNumJoints(kuka_gripper)): + p.setJointMotorControl2(kuka_gripper, i, p.POSITION_CONTROL, targetPosition=KUKA_GRIPPER_REST_POS[i], force=50) + + if sq_len < THRESHOLD * THRESHOLD: + eef_pos = e[POSITION] + + joint_pos = p.calculateInverseKinematics(kuka, 6, eef_pos, + lowerLimits=LOWER_LIMITS, upperLimits=UPPER_LIMITS, + jointRanges=JOINT_RANGE, restPoses=REST_POSE, jointDamping=JOINT_DAMP) + + # Only need links 1- 4, no need for joint 5-6 with pure position IK + for i in range(len(joint_pos) - 2): + p.setJointMotorControl2(kuka, i, p.POSITION_CONTROL, + targetPosition=joint_pos[i], targetVelocity=0, positionGain=0.05, + velocityGain=1.0, force=MAX_FORCE) + + # Rotate the end effector + targetOrn = e[ORIENTATION] + + _, _, z = p.getEulerFromQuaternion(targetOrn) + # End effector needs protection, done by using triangular tricks + + if LOWER_LIMITS[6] < z < UPPER_LIMITS[6]: + p.setJointMotorControl2(kuka, 6, p.POSITION_CONTROL, + targetPosition=z, targetVelocity=0, positionGain=0.03, velocityGain=1.0, force=MAX_FORCE) + + else: + p.setJointMotorControl2(kuka, 6, p.POSITION_CONTROL, + targetPosition=joint_pos[6], targetVelocity=0, positionGain=0.05, + velocityGain=1.0, force=MAX_FORCE) + + p.setJointMotorControl2(kuka, 5, p.POSITION_CONTROL, + targetPosition=-1.57, targetVelocity=0, + positionGain=0.03, velocityGain=1.0, force=MAX_FORCE) + + else: + # Set back to original rest pose + for jointIndex in range(p.getNumJoints(kuka)): + p.setJointMotorControl2(kuka, jointIndex, p.POSITION_CONTROL, + REST_JOINT_POS[jointIndex], 0) + + + diff --git a/examples/pybullet/vr_kuka_pr2_move.py b/examples/pybullet/vr_kuka_pr2_move.py index 22e5730c1..53dbe844d 100644 --- a/examples/pybullet/vr_kuka_pr2_move.py +++ b/examples/pybullet/vr_kuka_pr2_move.py @@ -15,14 +15,8 @@ BUTTONS=6 gripper_max_joint = 0.550569 while True: events = p.getVREvents() - - for e in (events): - if e[CONTROLLER_ID] == 3: # To make sure we only get the value for one of the remotes - p.changeConstraint(pr2_cid, e[POSITION], e[ORIENTATION], maxForce=500) - p.setJointMotorControl2(pr2_gripper, 0, controlMode=p.POSITION_CONTROL, - targetPosition=gripper_max_joint - e[ANALOG] * gripper_max_joint, - force=1.0) - p.setJointMotorControl2(pr2_gripper, 2, controlMode=p.POSITION_CONTROL, - targetPosition=gripper_max_joint - e[ANALOG] * gripper_max_joint, - force=1.1) - \ No newline at end of file + for e in (events): + if e[CONTROLLER_ID] == 3: # To make sure we only get the value for one of the remotes + p.changeConstraint(pr2_cid, e[POSITION], e[ORIENTATION], maxForce=500) + p.setJointMotorControl2(pr2_gripper, 0, controlMode=p.POSITION_CONTROL,targetPosition=gripper_max_joint - e[ANALOG] * gripper_max_joint,force=1.0) + p.setJointMotorControl2(pr2_gripper, 2, controlMode=p.POSITION_CONTROL,targetPosition=gripper_max_joint - e[ANALOG] * gripper_max_joint,force=1.1) \ No newline at end of file diff --git a/examples/pybullet/vrhand.py b/examples/pybullet/vrhand.py index 44651c65e..2ab6ab732 100644 --- a/examples/pybullet/vrhand.py +++ b/examples/pybullet/vrhand.py @@ -53,15 +53,28 @@ def convertSensor(x): b = (v-minV)/float(maxV-minV) return (1.0-b) +controllerId = -1 + +serialSteps = 0 +serialStepsUntilCheckVREvents = 3 + + ser = serial.Serial(port='COM9',baudrate=115200,parity=serial.PARITY_ODD,stopbits=serial.STOPBITS_TWO,bytesize=serial.SEVENBITS) if (ser.isOpen()): while True: events = p.getVREvents() for e in (events): if (e[BUTTONS][33]&p.VR_BUTTON_IS_DOWN): + controllerId = e[0] + if (e[0] == controllerId): p.changeConstraint(hand_cid,e[POSITION],e[ORIENTATION], maxForce=50) + serialSteps = 0 while ser.inWaiting() > 0: + serialSteps=serialSteps+1 + if (serialSteps>serialStepsUntilCheckVREvents): + ser.flushInput() + break line = str(ser.readline()) words = line.split(",") if (len(words)==6): diff --git a/examples/pybullet/vrminitaur.py b/examples/pybullet/vrminitaur.py new file mode 100644 index 000000000..c6e1daac8 --- /dev/null +++ b/examples/pybullet/vrminitaur.py @@ -0,0 +1,49 @@ +#script to track a robot with a VR controller attached to it. + +import time +import pybullet as p + +#first try to connect to shared memory (VR), if it fails use local GUI +c = p.connect(p.SHARED_MEMORY) +if (c<0): + c = p.connect(p.GUI) +p.resetSimulation() +p.setGravity(0,0,0) +print(c) +if (c<0): + p.connect(p.GUI) + +#load the MuJoCo MJCF hand +minitaur = p.loadURDF("quadruped/minitaur.urdf") +robot_cid = -1 + +POSITION=1 +ORIENTATION=2 +BUTTONS=6 + +p.setRealTimeSimulation(1) + +controllerId = -1 + +while True: + events = p.getVREvents() + for e in (events): + #print (e[BUTTONS]) + if (e[BUTTONS][33]&p.VR_BUTTON_WAS_TRIGGERED ): + if (controllerId >= 0): + controllerId = -1 + else: + controllerId = e[0] + if (e[0] == controllerId): + if (robot_cid>=0): + p.changeConstraint(robot_cid,e[POSITION],e[ORIENTATION], maxForce=5000) + if (e[BUTTONS][32]&p.VR_BUTTON_WAS_TRIGGERED ): + if (robot_cid>=0): + p.removeConstraint(robot_cid) + + #q = [0,0,0,1] + euler = p.getEulerFromQuaternion(e[ORIENTATION]) + q = p.getQuaternionFromEuler([euler[0],euler[1],0]) #-euler[0],-euler[1],-euler[2]]) + robot_cid = p.createConstraint(minitaur,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0.1,0,0],e[POSITION],q,e[ORIENTATION]) + + \ No newline at end of file diff --git a/src/Bullet3Common/b3Quaternion.h b/src/Bullet3Common/b3Quaternion.h index c89f2cf39..0b7564dcd 100644 --- a/src/Bullet3Common/b3Quaternion.h +++ b/src/Bullet3Common/b3Quaternion.h @@ -126,15 +126,16 @@ public: sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw, cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw); } - /**@brief Set the quaternion using euler angles + + /**@brief Set the quaternion using euler angles * @param yaw Angle around Z * @param pitch Angle around Y * @param roll Angle around X */ - void setEulerZYX(const b3Scalar& yaw, const b3Scalar& pitch, const b3Scalar& roll) + void setEulerZYX(const b3Scalar& yawZ, const b3Scalar& pitchY, const b3Scalar& rollX) { - b3Scalar halfYaw = b3Scalar(yaw) * b3Scalar(0.5); - b3Scalar halfPitch = b3Scalar(pitch) * b3Scalar(0.5); - b3Scalar halfRoll = b3Scalar(roll) * b3Scalar(0.5); + b3Scalar halfYaw = b3Scalar(yawZ) * b3Scalar(0.5); + b3Scalar halfPitch = b3Scalar(pitchY) * b3Scalar(0.5); + b3Scalar halfRoll = b3Scalar(rollX) * b3Scalar(0.5); b3Scalar cosYaw = b3Cos(halfYaw); b3Scalar sinYaw = b3Sin(halfYaw); b3Scalar cosPitch = b3Cos(halfPitch); @@ -145,7 +146,30 @@ public: cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw, //y cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw, //z cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw); //formerly yzx + normalize(); } + + /**@brief Get the euler angles from this quaternion + * @param yaw Angle around Z + * @param pitch Angle around Y + * @param roll Angle around X */ + void getEulerZYX(b3Scalar& yawZ, b3Scalar& pitchY, b3Scalar& rollX) const + { + b3Scalar squ; + b3Scalar sqx; + b3Scalar sqy; + b3Scalar sqz; + b3Scalar sarg; + sqx = m_floats[0] * m_floats[0]; + sqy = m_floats[1] * m_floats[1]; + sqz = m_floats[2] * m_floats[2]; + squ = m_floats[3] * m_floats[3]; + rollX = b3Atan2(2 * (m_floats[1] * m_floats[2] + m_floats[3] * m_floats[0]), squ - sqx - sqy + sqz); + sarg = b3Scalar(-2.) * (m_floats[0] * m_floats[2] - m_floats[3] * m_floats[1]); + pitchY = sarg <= b3Scalar(-1.0) ? b3Scalar(-0.5) * B3_PI: (sarg >= b3Scalar(1.0) ? b3Scalar(0.5) * B3_PI : b3Asin(sarg)); + yawZ = b3Atan2(2 * (m_floats[0] * m_floats[1] + m_floats[3] * m_floats[2]), squ + sqx - sqy - sqz); + } + /**@brief Add two quaternions * @param q The quaternion to add to this one */ B3_FORCE_INLINE b3Quaternion& operator+=(const b3Quaternion& q) diff --git a/src/Bullet3Common/b3Scalar.h b/src/Bullet3Common/b3Scalar.h index 16a9ac7a7..dbc7fea39 100644 --- a/src/Bullet3Common/b3Scalar.h +++ b/src/Bullet3Common/b3Scalar.h @@ -71,6 +71,7 @@ inline int b3GetVersion() #else #if (defined (_WIN32) && (_MSC_VER) && _MSC_VER >= 1400) && (!defined (B3_USE_DOUBLE_PRECISION)) + #if (defined (_M_IX86) || defined (_M_X64)) #define B3_USE_SSE #ifdef B3_USE_SSE //B3_USE_SSE_IN_API is disabled under Windows by default, because @@ -82,6 +83,7 @@ inline int b3GetVersion() //#define B3_USE_SSE_IN_API #endif //B3_USE_SSE #include + #endif #endif #endif//_XBOX diff --git a/src/BulletCollision/BroadphaseCollision/btDbvt.h b/src/BulletCollision/BroadphaseCollision/btDbvt.h index 1ca175723..b5a001458 100644 --- a/src/BulletCollision/BroadphaseCollision/btDbvt.h +++ b/src/BulletCollision/BroadphaseCollision/btDbvt.h @@ -942,7 +942,13 @@ inline void btDbvt::collideTV( const btDbvtNode* root, ATTRIBUTE_ALIGNED16(btDbvtVolume) volume(vol); btAlignedObjectArray stack; stack.resize(0); +#ifndef BT_DISABLE_STACK_TEMP_MEMORY + char tempmemory[SIMPLE_STACKSIZE*sizeof(const btDbvtNode*)]; + stack.initializeFromBuffer(tempmemory, 0, SIMPLE_STACKSIZE); +#else stack.reserve(SIMPLE_STACKSIZE); +#endif //BT_DISABLE_STACK_TEMP_MEMORY + stack.push_back(root); do { const btDbvtNode* n=stack[stack.size()-1]; @@ -1078,7 +1084,12 @@ inline void btDbvt::rayTest( const btDbvtNode* root, int depth=1; int treshold=DOUBLE_STACKSIZE-2; + char tempmemory[DOUBLE_STACKSIZE * sizeof(const btDbvtNode*)]; +#ifndef BT_DISABLE_STACK_TEMP_MEMORY + stack.initializeFromBuffer(tempmemory, DOUBLE_STACKSIZE, DOUBLE_STACKSIZE); +#else//BT_DISABLE_STACK_TEMP_MEMORY stack.resize(DOUBLE_STACKSIZE); +#endif //BT_DISABLE_STACK_TEMP_MEMORY stack[0]=root; btVector3 bounds[2]; do { diff --git a/src/BulletCollision/CollisionDispatch/btCollisionObject.h b/src/BulletCollision/CollisionDispatch/btCollisionObject.h index 0cae21000..598ad2229 100644 --- a/src/BulletCollision/CollisionDispatch/btCollisionObject.h +++ b/src/BulletCollision/CollisionDispatch/btCollisionObject.h @@ -139,6 +139,7 @@ public: CF_DISABLE_SPU_COLLISION_PROCESSING = 64,//disable parallel/SPU processing CF_HAS_CONTACT_STIFFNESS_DAMPING = 128, CF_HAS_CUSTOM_DEBUG_RENDERING_COLOR = 256, + CF_HAS_FRICTION_ANCHOR = 512, }; enum CollisionObjectTypes diff --git a/src/BulletCollision/CollisionDispatch/btCollisionWorldImporter.h b/src/BulletCollision/CollisionDispatch/btCollisionWorldImporter.h index ecb7a8a68..81c614272 100644 --- a/src/BulletCollision/CollisionDispatch/btCollisionWorldImporter.h +++ b/src/BulletCollision/CollisionDispatch/btCollisionWorldImporter.h @@ -124,7 +124,6 @@ public: btCollisionShape* getCollisionShapeByIndex(int index); int getNumRigidBodies() const; btCollisionObject* getRigidBodyByIndex(int index) const; - //int getNumConstraints() const; int getNumBvhs() const; btOptimizedBvh* getBvhByIndex(int index) const; diff --git a/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp b/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp index c1041e2ca..aa3d159f8 100644 --- a/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp +++ b/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp @@ -146,7 +146,13 @@ void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld,const b newPt.m_combinedContactStiffness1 = calculateCombinedContactStiffness(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject()); newPt.m_contactPointFlags |= BT_CONTACT_FLAG_CONTACT_STIFFNESS_DAMPING; } - + + if ( (m_body0Wrap->getCollisionObject()->getCollisionFlags()& btCollisionObject::CF_HAS_FRICTION_ANCHOR) || + (m_body1Wrap->getCollisionObject()->getCollisionFlags()& btCollisionObject::CF_HAS_FRICTION_ANCHOR)) + { + newPt.m_contactPointFlags |= BT_CONTACT_FLAG_FRICTION_ANCHOR; + } + btPlaneSpace1(newPt.m_normalWorldOnB,newPt.m_lateralFrictionDir1,newPt.m_lateralFrictionDir2); diff --git a/src/BulletCollision/CollisionShapes/btBox2dShape.h b/src/BulletCollision/CollisionShapes/btBox2dShape.h index ce333783e..22bee4f2c 100644 --- a/src/BulletCollision/CollisionShapes/btBox2dShape.h +++ b/src/BulletCollision/CollisionShapes/btBox2dShape.h @@ -103,11 +103,12 @@ public: btScalar minDimension = boxHalfExtents.getX(); if (minDimension>boxHalfExtents.getY()) minDimension = boxHalfExtents.getY(); - setSafeMargin(minDimension); m_shapeType = BOX_2D_SHAPE_PROXYTYPE; btVector3 margin(getMargin(),getMargin(),getMargin()); m_implicitShapeDimensions = (boxHalfExtents * m_localScaling) - margin; + + setSafeMargin(minDimension); }; virtual void setMargin(btScalar collisionMargin) diff --git a/src/BulletCollision/CollisionShapes/btBoxShape.cpp b/src/BulletCollision/CollisionShapes/btBoxShape.cpp index 3859138f1..72eeb3891 100644 --- a/src/BulletCollision/CollisionShapes/btBoxShape.cpp +++ b/src/BulletCollision/CollisionShapes/btBoxShape.cpp @@ -19,10 +19,10 @@ btBoxShape::btBoxShape( const btVector3& boxHalfExtents) { m_shapeType = BOX_SHAPE_PROXYTYPE; - setSafeMargin(boxHalfExtents); - btVector3 margin(getMargin(),getMargin(),getMargin()); m_implicitShapeDimensions = (boxHalfExtents * m_localScaling) - margin; + + setSafeMargin(boxHalfExtents); }; diff --git a/src/BulletCollision/CollisionShapes/btCylinderShape.cpp b/src/BulletCollision/CollisionShapes/btCylinderShape.cpp index 6cfe43be4..604b3fc77 100644 --- a/src/BulletCollision/CollisionShapes/btCylinderShape.cpp +++ b/src/BulletCollision/CollisionShapes/btCylinderShape.cpp @@ -19,10 +19,11 @@ btCylinderShape::btCylinderShape (const btVector3& halfExtents) :btConvexInternalShape(), m_upAxis(1) { - setSafeMargin(halfExtents); - btVector3 margin(getMargin(),getMargin(),getMargin()); m_implicitShapeDimensions = (halfExtents * m_localScaling) - margin; + + setSafeMargin(halfExtents); + m_shapeType = CYLINDER_SHAPE_PROXYTYPE; } diff --git a/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h b/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h index 04ab54ed9..571ad2c5f 100644 --- a/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h +++ b/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h @@ -41,6 +41,7 @@ enum btContactPointFlags BT_CONTACT_FLAG_HAS_CONTACT_CFM=2, BT_CONTACT_FLAG_HAS_CONTACT_ERP=4, BT_CONTACT_FLAG_CONTACT_STIFFNESS_DAMPING = 8, + BT_CONTACT_FLAG_FRICTION_ANCHOR = 16, }; /// ManifoldContactPoint collects and maintains persistent contactpoints. diff --git a/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp b/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp index 79647356b..23aaece22 100644 --- a/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp +++ b/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp @@ -281,6 +281,7 @@ void btPersistentManifold::refreshContactPoints(const btTransform& trA,const btT removeContactPoint(i); } else { + //todo: friction anchor may require the contact to be around a bit longer //contact also becomes invalid when relative movement orthogonal to normal exceeds margin projectedPoint = manifoldPoint.m_positionWorldOnA - manifoldPoint.m_normalWorldOnB * manifoldPoint.m_distance1; projectedDifference = manifoldPoint.m_positionWorldOnB - projectedPoint; diff --git a/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h b/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h index e97172154..f872c8e1c 100644 --- a/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h +++ b/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h @@ -185,39 +185,54 @@ public: gContactEndedCallback(this); } } - void replaceContactPoint(const btManifoldPoint& newPoint,int insertIndex) + void replaceContactPoint(const btManifoldPoint& newPoint, int insertIndex) { btAssert(validContactDistance(newPoint)); #define MAINTAIN_PERSISTENCY 1 #ifdef MAINTAIN_PERSISTENCY - int lifeTime = m_pointCache[insertIndex].getLifeTime(); - btScalar appliedImpulse = m_pointCache[insertIndex].m_appliedImpulse; - btScalar appliedLateralImpulse1 = m_pointCache[insertIndex].m_appliedImpulseLateral1; - btScalar appliedLateralImpulse2 = m_pointCache[insertIndex].m_appliedImpulseLateral2; -// bool isLateralFrictionInitialized = m_pointCache[insertIndex].m_lateralFrictionInitialized; - - - - btAssert(lifeTime>=0); - void* cache = m_pointCache[insertIndex].m_userPersistentData; - - m_pointCache[insertIndex] = newPoint; - m_pointCache[insertIndex].m_userPersistentData = cache; - m_pointCache[insertIndex].m_appliedImpulse = appliedImpulse; - m_pointCache[insertIndex].m_appliedImpulseLateral1 = appliedLateralImpulse1; - m_pointCache[insertIndex].m_appliedImpulseLateral2 = appliedLateralImpulse2; - + int lifeTime = m_pointCache[insertIndex].getLifeTime(); + btScalar appliedImpulse = m_pointCache[insertIndex].m_appliedImpulse; + btScalar appliedLateralImpulse1 = m_pointCache[insertIndex].m_appliedImpulseLateral1; + btScalar appliedLateralImpulse2 = m_pointCache[insertIndex].m_appliedImpulseLateral2; + + bool replacePoint = true; + ///we keep existing contact points for friction anchors + ///if the friction force is within the Coulomb friction cone + if (newPoint.m_contactPointFlags & BT_CONTACT_FLAG_FRICTION_ANCHOR) + { + // printf("appliedImpulse=%f\n", appliedImpulse); + // printf("appliedLateralImpulse1=%f\n", appliedLateralImpulse1); + // printf("appliedLateralImpulse2=%f\n", appliedLateralImpulse2); + // printf("mu = %f\n", m_pointCache[insertIndex].m_combinedFriction); + btScalar mu = m_pointCache[insertIndex].m_combinedFriction; + btScalar eps = 0; //we could allow to enlarge or shrink the tolerance to check against the friction cone a bit, say 1e-7 + btScalar a = appliedLateralImpulse1 * appliedLateralImpulse1 + appliedLateralImpulse2 * appliedLateralImpulse2; + btScalar b = eps + mu * appliedImpulse; + b = b * b; + replacePoint = (a) > (b); + } + + if (replacePoint) + { + btAssert(lifeTime >= 0); + void* cache = m_pointCache[insertIndex].m_userPersistentData; + + m_pointCache[insertIndex] = newPoint; + m_pointCache[insertIndex].m_userPersistentData = cache; + m_pointCache[insertIndex].m_appliedImpulse = appliedImpulse; + m_pointCache[insertIndex].m_appliedImpulseLateral1 = appliedLateralImpulse1; + m_pointCache[insertIndex].m_appliedImpulseLateral2 = appliedLateralImpulse2; + } m_pointCache[insertIndex].m_lifeTime = lifeTime; #else clearUserCache(m_pointCache[insertIndex]); m_pointCache[insertIndex] = newPoint; - + #endif } - bool validContactDistance(const btManifoldPoint& pt) const { return pt.m_distance1 <= getContactBreakingThreshold(); diff --git a/src/BulletDynamics/Character/btKinematicCharacterController.cpp b/src/BulletDynamics/Character/btKinematicCharacterController.cpp index 339bb6fc1..cb1aa71a1 100644 --- a/src/BulletDynamics/Character/btKinematicCharacterController.cpp +++ b/src/BulletDynamics/Character/btKinematicCharacterController.cpp @@ -137,8 +137,6 @@ btKinematicCharacterController::btKinematicCharacterController (btPairCachingGho m_ghostObject = ghostObject; m_up.setValue(0.0f, 0.0f, 1.0f); m_jumpAxis.setValue(0.0f, 0.0f, 1.0f); - setUp(up); - setStepHeight(stepHeight); m_addedMargin = 0.02; m_walkDirection.setValue(0.0,0.0,0.0); m_AngVel.setValue(0.0, 0.0, 0.0); @@ -156,13 +154,16 @@ btKinematicCharacterController::btKinematicCharacterController (btPairCachingGho m_wasOnGround = false; m_wasJumping = false; m_interpolateUp = true; - setMaxSlope(btRadians(45.0)); m_currentStepOffset = 0.0; m_maxPenetrationDepth = 0.2; full_drop = false; bounce_fix = false; m_linearDamping = btScalar(0.0); m_angularDamping = btScalar(0.0); + + setUp(up); + setStepHeight(stepHeight); + setMaxSlope(btRadians(45.0)); } btKinematicCharacterController::~btKinematicCharacterController () @@ -657,7 +658,7 @@ void btKinematicCharacterController::setLinearVelocity(const btVector3& velocity if (c != 0) { //there is a component in walkdirection for vertical velocity - btVector3 upComponent = m_up * (sinf(SIMD_HALF_PI - acosf(c)) * m_walkDirection.length()); + btVector3 upComponent = m_up * (btSin(SIMD_HALF_PI - btAcos(c)) * m_walkDirection.length()); m_walkDirection -= upComponent; m_verticalVelocity = (c < 0.0f ? -1 : 1) * upComponent.length(); diff --git a/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp index 09b7388b6..0572256f7 100644 --- a/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp @@ -642,7 +642,7 @@ void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTr btTransform trDeltaAB = trB * trPose * trA.inverse(); btQuaternion qDeltaAB = trDeltaAB.getRotation(); btVector3 swingAxis = btVector3(qDeltaAB.x(), qDeltaAB.y(), qDeltaAB.z()); - float swingAxisLen2 = swingAxis.length2(); + btScalar swingAxisLen2 = swingAxis.length2(); if(btFuzzyZero(swingAxisLen2)) { return; @@ -903,7 +903,7 @@ btVector3 btConeTwistConstraint::GetPointForAngle(btScalar fAngleInRadians, btSc // a^2 b^2 // Do the math and it should be clear. - float swingLimit = m_swingSpan1; // if xEllipse == 0, just use axis b (1) + btScalar swingLimit = m_swingSpan1; // if xEllipse == 0, just use axis b (1) if (fabs(xEllipse) > SIMD_EPSILON) { btScalar surfaceSlope2 = (yEllipse*yEllipse)/(xEllipse*xEllipse); diff --git a/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h b/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h index 739b066fe..f3d4d45af 100644 --- a/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h +++ b/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h @@ -43,10 +43,13 @@ struct btContactSolverInfoData btScalar m_restitution; int m_numIterations; btScalar m_maxErrorReduction; - btScalar m_sor; - btScalar m_erp;//used as Baumgarte factor - btScalar m_erp2;//used in Split Impulse - btScalar m_globalCfm;//constraint force mixing + btScalar m_sor;//successive over-relaxation term + btScalar m_erp;//error reduction for non-contact constraints + btScalar m_erp2;//error reduction for contact constraints + btScalar m_globalCfm;//constraint force mixing for contacts and non-contacts + btScalar m_frictionERP;//error reduction for friction constraints + btScalar m_frictionCFM;//constraint force mixing for friction constraints + int m_splitImpulse; btScalar m_splitImpulsePenetrationThreshold; btScalar m_splitImpulseTurnErp; @@ -79,6 +82,8 @@ struct btContactSolverInfo : public btContactSolverInfoData m_erp = btScalar(0.2); m_erp2 = btScalar(0.2); m_globalCfm = btScalar(0.); + m_frictionERP = btScalar(0.2);//positional friction 'anchors' are disabled by default + m_frictionCFM = btScalar(0.); m_sor = btScalar(1.); m_splitImpulse = true; m_splitImpulsePenetrationThreshold = -.04f; diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp index 982650da2..7a0ed924d 100644 --- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp +++ b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp @@ -534,7 +534,7 @@ void btSequentialImpulseConstraintSolver::applyAnisotropicFriction(btCollisionOb -void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstraint& solverConstraint, const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip) +void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstraint& solverConstraint, const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip) { @@ -612,7 +612,17 @@ void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstr btScalar velocityError = desiredVelocity - rel_vel; btScalar velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv; - solverConstraint.m_rhs = velocityImpulse; + + btScalar penetrationImpulse = btScalar(0); + + if (cp.m_contactPointFlags & BT_CONTACT_FLAG_FRICTION_ANCHOR) + { + btScalar distance = (cp.getPositionWorldOnA() - cp.getPositionWorldOnB()).dot(normalAxis); + btScalar positionalError = -distance * infoGlobal.m_frictionERP/infoGlobal.m_timeStep; + penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv; + } + + solverConstraint.m_rhs = penetrationImpulse + velocityImpulse; solverConstraint.m_rhsPenetration = 0.f; solverConstraint.m_cfm = cfmSlip; solverConstraint.m_lowerLimit = -solverConstraint.m_friction; @@ -621,12 +631,12 @@ void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstr } } -btSolverConstraint& btSequentialImpulseConstraintSolver::addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip) +btSolverConstraint& btSequentialImpulseConstraintSolver::addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip) { btSolverConstraint& solverConstraint = m_tmpSolverContactFrictionConstraintPool.expandNonInitializing(); solverConstraint.m_frictionIndex = frictionIndex; setupFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, - colObj0, colObj1, relaxation, desiredVelocity, cfmSlip); + colObj0, colObj1, relaxation, infoGlobal, desiredVelocity, cfmSlip); return solverConstraint; } @@ -1168,6 +1178,7 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m ///In that case, you can set the target relative motion in each friction direction (cp.m_contactMotion1 and cp.m_contactMotion2) ///this will give a conveyor belt effect /// + if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !(cp.m_contactPointFlags&BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED)) { cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel; @@ -1177,7 +1188,7 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m cp.m_lateralFrictionDir1 *= 1.f/btSqrt(lat_rel_vel); applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION); applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION); - addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); + addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation,infoGlobal); if((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) { @@ -1185,7 +1196,7 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m cp.m_lateralFrictionDir2.normalize();//?? applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION); applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION); - addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); + addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, infoGlobal); } } else @@ -1194,13 +1205,13 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION); applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION); - addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); + addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, infoGlobal); if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) { applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION); applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION); - addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); + addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, infoGlobal); } @@ -1212,10 +1223,10 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m } else { - addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation,cp.m_contactMotion1, cp.m_frictionCFM); + addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, infoGlobal, cp.m_contactMotion1, cp.m_frictionCFM); if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) - addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, cp.m_contactMotion2, cp.m_frictionCFM); + addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, infoGlobal, cp.m_contactMotion2, cp.m_frictionCFM); } setFrictionConstraintImpulse( solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal); @@ -1526,7 +1537,8 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal); btScalar fsum = btFabs(sum); btAssert(fsum > SIMD_EPSILON); - solverConstraint.m_jacDiagABInv = fsum>SIMD_EPSILON?btScalar(1.)/sum : 0.f; + btScalar sorRelaxation = 1.f;//todo: get from globalInfo? + solverConstraint.m_jacDiagABInv = fsum>SIMD_EPSILON?sorRelaxation/sum : 0.f; } diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h index 0dd31d142..b81537138 100644 --- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h +++ b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h @@ -62,6 +62,7 @@ protected: void setupFrictionConstraint( btSolverConstraint& solverConstraint, const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB, btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2, btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, + const btContactSolverInfo& infoGlobal, btScalar desiredVelocity=0., btScalar cfmSlip=0.); void setupTorsionalFrictionConstraint( btSolverConstraint& solverConstraint, const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB, @@ -69,7 +70,7 @@ protected: btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity=0., btScalar cfmSlip=0.); - btSolverConstraint& addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity=0., btScalar cfmSlip=0.); + btSolverConstraint& addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity=0., btScalar cfmSlip=0.); btSolverConstraint& addTorsionalFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,btScalar torsionalFriction, const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity=0, btScalar cfmSlip=0.f); diff --git a/src/BulletDynamics/Featherstone/btMultiBody.cpp b/src/BulletDynamics/Featherstone/btMultiBody.cpp index dd5f253b6..c14aa2aea 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBody.cpp @@ -1238,7 +1238,7 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar -void btMultiBody::solveImatrix(const btVector3& rhs_top, const btVector3& rhs_bot, float result[6]) const +void btMultiBody::solveImatrix(const btVector3& rhs_top, const btVector3& rhs_bot, btScalar result[6]) const { int num_links = getNumLinks(); ///solve I * x = rhs, so the result = invI * rhs diff --git a/src/BulletDynamics/Featherstone/btMultiBody.h b/src/BulletDynamics/Featherstone/btMultiBody.h index c3a4bb8c1..b36d0e4e8 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.h +++ b/src/BulletDynamics/Featherstone/btMultiBody.h @@ -616,7 +616,7 @@ private: void operator=(const btMultiBody &); // not implemented - void solveImatrix(const btVector3& rhs_top, const btVector3& rhs_bot, float result[6]) const; + void solveImatrix(const btVector3& rhs_top, const btVector3& rhs_bot, btScalar result[6]) const; void solveImatrix(const btSpatialForceVector &rhs, btSpatialMotionVector &result) const; void updateLinksDofOffsets() diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp index 903ef8d6d..dac631c08 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp @@ -48,9 +48,11 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl } //solve featherstone normal contact - for (int j=0;jm_multiBodyFrictionContactConstraints.size();j++) + for (int j1=0;j1m_multiBodyFrictionContactConstraints.size();j1++) { if (iteration < infoGlobal.m_numIterations) { - btMultiBodySolverConstraint& frictionConstraint = m_multiBodyFrictionContactConstraints[j]; + int index = j1;//iteration&1? j1 : m_multiBodyFrictionContactConstraints.size()-1-j1; + + btMultiBodySolverConstraint& frictionConstraint = m_multiBodyFrictionContactConstraints[index]; btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse; //adjust friction limits here if (totalImpulse>btScalar(0)) @@ -240,32 +244,39 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol //cfm = 1 / ( dt * kp + kd ) //erp = dt * kp / ( dt * kp + kd ) - btScalar cfm = infoGlobal.m_globalCfm; - btScalar erp = infoGlobal.m_erp2; - - if ((cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_CFM) || (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_ERP)) - { - if (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_CFM) - cfm = cp.m_contactCFM; - if (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_ERP) - erp = cp.m_contactERP; - } else - { - if (cp.m_contactPointFlags & BT_CONTACT_FLAG_CONTACT_STIFFNESS_DAMPING) - { - btScalar denom = ( infoGlobal.m_timeStep * cp.m_combinedContactStiffness1 + cp.m_combinedContactDamping1 ); - if (denom < SIMD_EPSILON) - { - denom = SIMD_EPSILON; - } - cfm = btScalar(1) / denom; - erp = (infoGlobal.m_timeStep * cp.m_combinedContactStiffness1) / denom; - } - } - - cfm *= invTimeStep; + btScalar cfm; + btScalar erp; + if (isFriction) + { + cfm = infoGlobal.m_frictionCFM; + erp = infoGlobal.m_frictionERP; + } else + { + cfm = infoGlobal.m_globalCfm; + erp = infoGlobal.m_erp2; + if ((cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_CFM) || (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_ERP)) + { + if (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_CFM) + cfm = cp.m_contactCFM; + if (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_ERP) + erp = cp.m_contactERP; + } else + { + if (cp.m_contactPointFlags & BT_CONTACT_FLAG_CONTACT_STIFFNESS_DAMPING) + { + btScalar denom = ( infoGlobal.m_timeStep * cp.m_combinedContactStiffness1 + cp.m_combinedContactDamping1 ); + if (denom < SIMD_EPSILON) + { + denom = SIMD_EPSILON; + } + cfm = btScalar(1) / denom; + erp = (infoGlobal.m_timeStep * cp.m_combinedContactStiffness1) / denom; + } + } + } + cfm *= invTimeStep; if (multiBodyA) { @@ -425,8 +436,19 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol btScalar restitution = 0.f; - btScalar penetration = isFriction? 0 : cp.getDistance()+infoGlobal.m_linearSlop; - + btScalar distance = 0; + if (!isFriction) + { + distance = cp.getDistance()+infoGlobal.m_linearSlop; + } else + { + if (cp.m_contactPointFlags & BT_CONTACT_FLAG_FRICTION_ANCHOR) + { + distance = (cp.getPositionWorldOnA() - cp.getPositionWorldOnB()).dot(contactNormal); + } + } + + btScalar rel_vel = 0.f; int ndofA = 0; int ndofB = 0; @@ -521,15 +543,20 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol btScalar positionalError = 0.f; btScalar velocityError = restitution - rel_vel;// * damping; //note for friction restitution is always set to 0 (check above) so it is acutally velocityError = -rel_vel for friction - - if (penetration>0) + if (isFriction) { - positionalError = 0; - velocityError -= penetration / infoGlobal.m_timeStep; - + positionalError = -distance * erp/infoGlobal.m_timeStep; } else { - positionalError = -penetration * erp/infoGlobal.m_timeStep; + if (distance>0) + { + positionalError = 0; + velocityError -= distance / infoGlobal.m_timeStep; + + } else + { + positionalError = -distance * erp/infoGlobal.m_timeStep; + } } btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv; @@ -556,7 +583,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol } else { - solverConstraint.m_rhs = velocityImpulse; + solverConstraint.m_rhs = penetrationImpulse+velocityImpulse; solverConstraint.m_rhsPenetration = 0.f; solverConstraint.m_lowerLimit = -solverConstraint.m_friction; solverConstraint.m_upperLimit = solverConstraint.m_friction; @@ -1024,12 +1051,18 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold* applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION); addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal); - if (rollingFriction > 0) + if (rollingFriction > 0 ) { - addMultiBodyTorsionalFrictionConstraint(cp.m_normalWorldOnB,manifold,frictionIndex,cp,cp.m_combinedSpinningFriction, colObj0,colObj1, relaxation,infoGlobal); - addMultiBodyTorsionalFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,cp.m_combinedRollingFriction, colObj0,colObj1, relaxation,infoGlobal); - addMultiBodyTorsionalFrictionConstraint(cp.m_lateralFrictionDir2,manifold,frictionIndex,cp,cp.m_combinedRollingFriction, colObj0,colObj1, relaxation,infoGlobal); - + if (cp.m_combinedSpinningFriction>0) + { + addMultiBodyTorsionalFrictionConstraint(cp.m_normalWorldOnB,manifold,frictionIndex,cp,cp.m_combinedSpinningFriction, colObj0,colObj1, relaxation,infoGlobal); + } + if (cp.m_combinedRollingFriction>0) + { + + addMultiBodyTorsionalFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,cp.m_combinedRollingFriction, colObj0,colObj1, relaxation,infoGlobal); + addMultiBodyTorsionalFrictionConstraint(cp.m_lateralFrictionDir2,manifold,frictionIndex,cp,cp.m_combinedRollingFriction, colObj0,colObj1, relaxation,infoGlobal); + } rollingFriction--; } diff --git a/src/BulletSoftBody/btDefaultSoftBodySolver.cpp b/src/BulletSoftBody/btDefaultSoftBodySolver.cpp index e90d24e6e..9c2040307 100644 --- a/src/BulletSoftBody/btDefaultSoftBodySolver.cpp +++ b/src/BulletSoftBody/btDefaultSoftBodySolver.cpp @@ -100,9 +100,9 @@ void btDefaultSoftBodySolver::copySoftBodyToVertexBuffer( const btSoftBody *cons for( int vertexIndex = 0; vertexIndex < numVertices; ++vertexIndex ) { btVector3 position = clothVertices[vertexIndex].m_x; - *(vertexPointer + 0) = position.getX(); - *(vertexPointer + 1) = position.getY(); - *(vertexPointer + 2) = position.getZ(); + *(vertexPointer + 0) = (float)position.getX(); + *(vertexPointer + 1) = (float)position.getY(); + *(vertexPointer + 2) = (float)position.getZ(); vertexPointer += vertexStride; } } @@ -115,9 +115,9 @@ void btDefaultSoftBodySolver::copySoftBodyToVertexBuffer( const btSoftBody *cons for( int vertexIndex = 0; vertexIndex < numVertices; ++vertexIndex ) { btVector3 normal = clothVertices[vertexIndex].m_n; - *(normalPointer + 0) = normal.getX(); - *(normalPointer + 1) = normal.getY(); - *(normalPointer + 2) = normal.getZ(); + *(normalPointer + 0) = (float)normal.getX(); + *(normalPointer + 1) = (float)normal.getY(); + *(normalPointer + 2) = (float)normal.getZ(); normalPointer += normalStride; } } diff --git a/src/LinearMath/btQuickprof.cpp b/src/LinearMath/btQuickprof.cpp index 16ca88cdc..b419495c0 100644 --- a/src/LinearMath/btQuickprof.cpp +++ b/src/LinearMath/btQuickprof.cpp @@ -218,7 +218,7 @@ unsigned long long int btClock::getTimeNanoseconds() QueryPerformanceCounter(¤tTime); elapsedTime.QuadPart = currentTime.QuadPart - m_data->mStartTime.QuadPart; - elapsedTime.QuadPart *= 1e9; + elapsedTime.QuadPart *= 1000000000; elapsedTime.QuadPart /= m_data->mClockFrequency.QuadPart; return (unsigned long long) elapsedTime.QuadPart; @@ -287,7 +287,7 @@ static btClock gProfileClock; inline void Profile_Get_Ticks(unsigned long int * ticks) { - *ticks = gProfileClock.getTimeMicroseconds(); + *ticks = (unsigned long int)gProfileClock.getTimeMicroseconds(); } inline float Profile_Get_Tick_Rate(void) @@ -687,7 +687,11 @@ unsigned int btQuickprofGetCurrentThreadIndex2() { const unsigned int kNullIndex = ~0U; #ifdef _WIN32 - __declspec( thread ) static unsigned int sThreadIndex = kNullIndex; + #if defined(__MINGW32__) || defined(__MINGW64__) + static __thread unsigned int sThreadIndex = kNullIndex; + #else + __declspec( thread ) static unsigned int sThreadIndex = kNullIndex; + #endif #else #ifdef __APPLE__ #if TARGET_OS_IPHONE diff --git a/src/LinearMath/btScalar.h b/src/LinearMath/btScalar.h index 00754567b..054652255 100644 --- a/src/LinearMath/btScalar.h +++ b/src/LinearMath/btScalar.h @@ -12,59 +12,54 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - - #ifndef BT_SCALAR_H #define BT_SCALAR_H - #ifdef BT_MANAGED_CODE //Aligned data types not supported in managed code #pragma unmanaged #endif - #include -#include //size_t for MSVC 6.0 +#include //size_t for MSVC 6.0 #include /* SVN $Revision$ on $Date$ from http://bullet.googlecode.com*/ #define BT_BULLET_VERSION 286 -inline int btGetVersion() +inline int btGetVersion() { return BT_BULLET_VERSION; } +// clang and most formatting tools don't support indentation of preprocessor guards, so turn it off +// clang-format off #if defined(DEBUG) || defined (_DEBUG) -#define BT_DEBUG + #define BT_DEBUG #endif - #ifdef _WIN32 - - #if defined(__MINGW32__) || defined(__CYGWIN__) || (defined (_MSC_VER) && _MSC_VER < 1300) - - #define SIMD_FORCE_INLINE inline - #define ATTRIBUTE_ALIGNED16(a) a - #define ATTRIBUTE_ALIGNED64(a) a - #define ATTRIBUTE_ALIGNED128(a) a - #elif (_M_ARM) - #define SIMD_FORCE_INLINE __forceinline - #define ATTRIBUTE_ALIGNED16(a) __declspec() a - #define ATTRIBUTE_ALIGNED64(a) __declspec() a - #define ATTRIBUTE_ALIGNED128(a) __declspec () a - #else - //#define BT_HAS_ALIGNED_ALLOCATOR - #pragma warning(disable : 4324) // disable padding warning + #if defined(__MINGW32__) || defined(__CYGWIN__) || (defined (_MSC_VER) && _MSC_VER < 1300) + #define SIMD_FORCE_INLINE inline + #define ATTRIBUTE_ALIGNED16(a) a + #define ATTRIBUTE_ALIGNED64(a) a + #define ATTRIBUTE_ALIGNED128(a) a + #elif defined(_M_ARM) + #define SIMD_FORCE_INLINE __forceinline + #define ATTRIBUTE_ALIGNED16(a) __declspec() a + #define ATTRIBUTE_ALIGNED64(a) __declspec() a + #define ATTRIBUTE_ALIGNED128(a) __declspec () a + #else//__MINGW32__ + //#define BT_HAS_ALIGNED_ALLOCATOR + #pragma warning(disable : 4324) // disable padding warning // #pragma warning(disable:4530) // Disable the exception disable but used in MSCV Stl warning. - #pragma warning(disable:4996) //Turn off warnings about deprecated C routines + #pragma warning(disable:4996) //Turn off warnings about deprecated C routines // #pragma warning(disable:4786) // Disable the "debug name too long" warning - #define SIMD_FORCE_INLINE __forceinline - #define ATTRIBUTE_ALIGNED16(a) __declspec(align(16)) a - #define ATTRIBUTE_ALIGNED64(a) __declspec(align(64)) a - #define ATTRIBUTE_ALIGNED128(a) __declspec (align(128)) a + #define SIMD_FORCE_INLINE __forceinline + #define ATTRIBUTE_ALIGNED16(a) __declspec(align(16)) a + #define ATTRIBUTE_ALIGNED64(a) __declspec(align(64)) a + #define ATTRIBUTE_ALIGNED128(a) __declspec (align(128)) a #ifdef _XBOX #define BT_USE_VMX128 @@ -100,28 +95,28 @@ inline int btGetVersion() #endif//_XBOX - #endif //__MINGW32__ + #endif //__MINGW32__ -#ifdef BT_DEBUG - #ifdef _MSC_VER - #include - #define btAssert(x) { if(!(x)){printf("Assert "__FILE__ ":%u (%s)\n", __LINE__, #x);__debugbreak(); }} - #else//_MSC_VER - #include - #define btAssert assert - #endif//_MSC_VER -#else + #ifdef BT_DEBUG + #ifdef _MSC_VER + #include + #define btAssert(x) { if(!(x)){printf("Assert "__FILE__ ":%u (%s)\n", __LINE__, #x);__debugbreak(); }} + #else//_MSC_VER + #include + #define btAssert assert + #endif//_MSC_VER + #else #define btAssert(x) -#endif + #endif //btFullAssert is optional, slows down a lot #define btFullAssert(x) #define btLikely(_c) _c #define btUnlikely(_c) _c -#else +#else//_WIN32 -#if defined (__CELLOS_LV2__) + #if defined (__CELLOS_LV2__) #define SIMD_FORCE_INLINE inline __attribute__((always_inline)) #define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16))) #define ATTRIBUTE_ALIGNED64(a) a __attribute__ ((aligned (64))) @@ -129,217 +124,249 @@ inline int btGetVersion() #ifndef assert #include #endif -#ifdef BT_DEBUG -#ifdef __SPU__ -#include -#define printf spu_printf - #define btAssert(x) {if(!(x)){printf("Assert "__FILE__ ":%u ("#x")\n", __LINE__);spu_hcmpeq(0,0);}} -#else - #define btAssert assert -#endif + #ifdef BT_DEBUG + #ifdef __SPU__ + #include + #define printf spu_printf + #define btAssert(x) {if(!(x)){printf("Assert "__FILE__ ":%u ("#x")\n", __LINE__);spu_hcmpeq(0,0);}} + #else + #define btAssert assert + #endif -#else - #define btAssert(x) -#endif + #else//BT_DEBUG + #define btAssert(x) + #endif//BT_DEBUG //btFullAssert is optional, slows down a lot #define btFullAssert(x) #define btLikely(_c) _c #define btUnlikely(_c) _c -#else + #else//defined (__CELLOS_LV2__) -#ifdef USE_LIBSPE2 + #ifdef USE_LIBSPE2 - #define SIMD_FORCE_INLINE __inline - #define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16))) - #define ATTRIBUTE_ALIGNED64(a) a __attribute__ ((aligned (64))) - #define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128))) - #ifndef assert - #include - #endif -#ifdef BT_DEBUG - #define btAssert assert -#else - #define btAssert(x) -#endif - //btFullAssert is optional, slows down a lot - #define btFullAssert(x) + #define SIMD_FORCE_INLINE __inline + #define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16))) + #define ATTRIBUTE_ALIGNED64(a) a __attribute__ ((aligned (64))) + #define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128))) + #ifndef assert + #include + #endif + #ifdef BT_DEBUG + #define btAssert assert + #else + #define btAssert(x) + #endif + //btFullAssert is optional, slows down a lot + #define btFullAssert(x) - #define btLikely(_c) __builtin_expect((_c), 1) - #define btUnlikely(_c) __builtin_expect((_c), 0) + #define btLikely(_c) __builtin_expect((_c), 1) + #define btUnlikely(_c) __builtin_expect((_c), 0) -#else + #else//USE_LIBSPE2 //non-windows systems -#if (defined (__APPLE__) && (!defined (BT_USE_DOUBLE_PRECISION))) - #if defined (__i386__) || defined (__x86_64__) - #define BT_USE_SIMD_VECTOR3 - #define BT_USE_SSE - //BT_USE_SSE_IN_API is enabled on Mac OSX by default, because memory is automatically aligned on 16-byte boundaries - //if apps run into issues, we will disable the next line - #define BT_USE_SSE_IN_API - #ifdef BT_USE_SSE - // include appropriate SSE level - #if defined (__SSE4_1__) - #include - #elif defined (__SSSE3__) - #include - #elif defined (__SSE3__) - #include - #else - #include - #endif - #endif //BT_USE_SSE - #elif defined( __ARM_NEON__ ) - #ifdef __clang__ - #define BT_USE_NEON 1 - #define BT_USE_SIMD_VECTOR3 + #if (defined (__APPLE__) && (!defined (BT_USE_DOUBLE_PRECISION))) + #if defined (__i386__) || defined (__x86_64__) + #define BT_USE_SIMD_VECTOR3 + #define BT_USE_SSE + //BT_USE_SSE_IN_API is enabled on Mac OSX by default, because memory is automatically aligned on 16-byte boundaries + //if apps run into issues, we will disable the next line + #define BT_USE_SSE_IN_API + #ifdef BT_USE_SSE + // include appropriate SSE level + #if defined (__SSE4_1__) + #include + #elif defined (__SSSE3__) + #include + #elif defined (__SSE3__) + #include + #else + #include + #endif + #endif //BT_USE_SSE + #elif defined( __ARM_NEON__ ) + #ifdef __clang__ + #define BT_USE_NEON 1 + #define BT_USE_SIMD_VECTOR3 - #if defined BT_USE_NEON && defined (__clang__) - #include - #endif//BT_USE_NEON - #endif //__clang__ - #endif//__arm__ + #if defined BT_USE_NEON && defined (__clang__) + #include + #endif//BT_USE_NEON + #endif //__clang__ + #endif//__arm__ - #define SIMD_FORCE_INLINE inline __attribute__ ((always_inline)) -///@todo: check out alignment methods for other platforms/compilers - #define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16))) - #define ATTRIBUTE_ALIGNED64(a) a __attribute__ ((aligned (64))) - #define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128))) - #ifndef assert - #include - #endif + #define SIMD_FORCE_INLINE inline __attribute__ ((always_inline)) + ///@todo: check out alignment methods for other platforms/compilers + #define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16))) + #define ATTRIBUTE_ALIGNED64(a) a __attribute__ ((aligned (64))) + #define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128))) + #ifndef assert + #include + #endif - #if defined(DEBUG) || defined (_DEBUG) - #if defined (__i386__) || defined (__x86_64__) - #include - #define btAssert(x)\ - {\ - if(!(x))\ - {\ - printf("Assert %s in line %d, file %s\n",#x, __LINE__, __FILE__);\ - asm volatile ("int3");\ - }\ - } - #else//defined (__i386__) || defined (__x86_64__) - #define btAssert assert - #endif//defined (__i386__) || defined (__x86_64__) - #else//defined(DEBUG) || defined (_DEBUG) - #define btAssert(x) - #endif//defined(DEBUG) || defined (_DEBUG) + #if defined(DEBUG) || defined (_DEBUG) + #if defined (__i386__) || defined (__x86_64__) + #include + #define btAssert(x)\ + {\ + if(!(x))\ + {\ + printf("Assert %s in line %d, file %s\n",#x, __LINE__, __FILE__);\ + asm volatile ("int3");\ + }\ + } + #else//defined (__i386__) || defined (__x86_64__) + #define btAssert assert + #endif//defined (__i386__) || defined (__x86_64__) + #else//defined(DEBUG) || defined (_DEBUG) + #define btAssert(x) + #endif//defined(DEBUG) || defined (_DEBUG) - //btFullAssert is optional, slows down a lot - #define btFullAssert(x) - #define btLikely(_c) _c - #define btUnlikely(_c) _c + //btFullAssert is optional, slows down a lot + #define btFullAssert(x) + #define btLikely(_c) _c + #define btUnlikely(_c) _c -#else + #else//__APPLE__ - #define SIMD_FORCE_INLINE inline - ///@todo: check out alignment methods for other platforms/compilers - ///#define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16))) - ///#define ATTRIBUTE_ALIGNED64(a) a __attribute__ ((aligned (64))) - ///#define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128))) - #define ATTRIBUTE_ALIGNED16(a) a - #define ATTRIBUTE_ALIGNED64(a) a - #define ATTRIBUTE_ALIGNED128(a) a - #ifndef assert - #include - #endif + #define SIMD_FORCE_INLINE inline + ///@todo: check out alignment methods for other platforms/compilers + ///#define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16))) + ///#define ATTRIBUTE_ALIGNED64(a) a __attribute__ ((aligned (64))) + ///#define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128))) + #define ATTRIBUTE_ALIGNED16(a) a + #define ATTRIBUTE_ALIGNED64(a) a + #define ATTRIBUTE_ALIGNED128(a) a + #ifndef assert + #include + #endif -#if defined(DEBUG) || defined (_DEBUG) - #define btAssert assert -#else - #define btAssert(x) -#endif + #if defined(DEBUG) || defined (_DEBUG) + #define btAssert assert + #else + #define btAssert(x) + #endif - //btFullAssert is optional, slows down a lot - #define btFullAssert(x) - #define btLikely(_c) _c - #define btUnlikely(_c) _c -#endif //__APPLE__ - -#endif // LIBSPE2 - -#endif //__CELLOS_LV2__ -#endif + //btFullAssert is optional, slows down a lot + #define btFullAssert(x) + #define btLikely(_c) _c + #define btUnlikely(_c) _c + #endif //__APPLE__ + #endif // LIBSPE2 + #endif //__CELLOS_LV2__ +#endif//_WIN32 ///The btScalar type abstracts floating point numbers, to easily switch between double and single floating point precision. #if defined(BT_USE_DOUBLE_PRECISION) - -typedef double btScalar; -//this number could be bigger in double precision -#define BT_LARGE_FLOAT 1e30 + typedef double btScalar; + //this number could be bigger in double precision + #define BT_LARGE_FLOAT 1e30 #else - -typedef float btScalar; -//keep BT_LARGE_FLOAT*BT_LARGE_FLOAT < FLT_MAX -#define BT_LARGE_FLOAT 1e18f + typedef float btScalar; + //keep BT_LARGE_FLOAT*BT_LARGE_FLOAT < FLT_MAX + #define BT_LARGE_FLOAT 1e18f #endif #ifdef BT_USE_SSE -typedef __m128 btSimdFloat4; -#endif//BT_USE_SSE + typedef __m128 btSimdFloat4; +#endif //BT_USE_SSE -#if defined (BT_USE_SSE) -//#if defined BT_USE_SSE_IN_API && defined (BT_USE_SSE) -#ifdef _WIN32 +#if defined(BT_USE_SSE) + //#if defined BT_USE_SSE_IN_API && defined (BT_USE_SSE) + #ifdef _WIN32 -#ifndef BT_NAN -static int btNanMask = 0x7F800001; -#define BT_NAN (*(float*)&btNanMask) -#endif + #ifndef BT_NAN + static int btNanMask = 0x7F800001; + #define BT_NAN (*(float *)&btNanMask) + #endif -#ifndef BT_INFINITY -static int btInfinityMask = 0x7F800000; -#define BT_INFINITY (*(float*)&btInfinityMask) -inline int btGetInfinityMask()//suppress stupid compiler warning -{ - return btInfinityMask; -} -#endif + #ifndef BT_INFINITY + static int btInfinityMask = 0x7F800000; + #define BT_INFINITY (*(float *)&btInfinityMask) + inline int btGetInfinityMask() //suppress stupid compiler warning + { + return btInfinityMask; + } + #endif -//use this, in case there are clashes (such as xnamath.h) -#ifndef BT_NO_SIMD_OPERATOR_OVERLOADS -inline __m128 operator + (const __m128 A, const __m128 B) -{ - return _mm_add_ps(A, B); -} -inline __m128 operator - (const __m128 A, const __m128 B) -{ - return _mm_sub_ps(A, B); -} -inline __m128 operator * (const __m128 A, const __m128 B) -{ - return _mm_mul_ps(A, B); -} -#endif //BT_NO_SIMD_OPERATOR_OVERLOADS + //use this, in case there are clashes (such as xnamath.h) + #ifndef BT_NO_SIMD_OPERATOR_OVERLOADS + inline __m128 operator+(const __m128 A, const __m128 B) + { + return _mm_add_ps(A, B); + } -#define btCastfTo128i(a) (_mm_castps_si128(a)) -#define btCastfTo128d(a) (_mm_castps_pd(a)) -#define btCastiTo128f(a) (_mm_castsi128_ps(a)) -#define btCastdTo128f(a) (_mm_castpd_ps(a)) -#define btCastdTo128i(a) (_mm_castpd_si128(a)) -#define btAssign128(r0,r1,r2,r3) _mm_setr_ps(r0,r1,r2,r3) + inline __m128 operator-(const __m128 A, const __m128 B) + { + return _mm_sub_ps(A, B); + } -#else//_WIN32 + inline __m128 operator*(const __m128 A, const __m128 B) + { + return _mm_mul_ps(A, B); + } + #endif //BT_NO_SIMD_OPERATOR_OVERLOADS -#define btCastfTo128i(a) ((__m128i)(a)) -#define btCastfTo128d(a) ((__m128d)(a)) -#define btCastiTo128f(a) ((__m128) (a)) -#define btCastdTo128f(a) ((__m128) (a)) -#define btCastdTo128i(a) ((__m128i)(a)) -#define btAssign128(r0,r1,r2,r3) (__m128){r0,r1,r2,r3} -#define BT_INFINITY INFINITY -#define BT_NAN NAN -#endif//_WIN32 -#else + #define btCastfTo128i(a) (_mm_castps_si128(a)) + #define btCastfTo128d(a) (_mm_castps_pd(a)) + #define btCastiTo128f(a) (_mm_castsi128_ps(a)) + #define btCastdTo128f(a) (_mm_castpd_ps(a)) + #define btCastdTo128i(a) (_mm_castpd_si128(a)) + #define btAssign128(r0, r1, r2, r3) _mm_setr_ps(r0, r1, r2, r3) + + #else //_WIN32 + + #define btCastfTo128i(a) ((__m128i)(a)) + #define btCastfTo128d(a) ((__m128d)(a)) + #define btCastiTo128f(a) ((__m128)(a)) + #define btCastdTo128f(a) ((__m128)(a)) + #define btCastdTo128i(a) ((__m128i)(a)) + #define btAssign128(r0, r1, r2, r3) \ + (__m128) { r0, r1, r2, r3 } + #define BT_INFINITY INFINITY + #define BT_NAN NAN + #endif //_WIN32 +#else//BT_USE_SSE + + #ifdef BT_USE_NEON + #include + + typedef float32x4_t btSimdFloat4; + #define BT_INFINITY INFINITY + #define BT_NAN NAN + #define btAssign128(r0, r1, r2, r3) \ + (float32x4_t) { r0, r1, r2, r3 } + #else //BT_USE_NEON + + #ifndef BT_INFINITY + struct btInfMaskConverter + { + union { + float mask; + int intmask; + }; + btInfMaskConverter(int mask = 0x7F800000) + : intmask(mask) + { + } + }; + static btInfMaskConverter btInfinityMask = 0x7F800000; + #define BT_INFINITY (btInfinityMask.mask) + inline int btGetInfinityMask() //suppress stupid compiler warning + { + return btInfinityMask.intmask; + } + #endif + #endif //BT_USE_NEON + +#endif //BT_USE_SSE #ifdef BT_USE_NEON #include @@ -347,193 +374,181 @@ inline __m128 operator * (const __m128 A, const __m128 B) typedef float32x4_t btSimdFloat4; #define BT_INFINITY INFINITY #define BT_NAN NAN - #define btAssign128(r0,r1,r2,r3) (float32x4_t){r0,r1,r2,r3} -#else//BT_USE_NEON - - #ifndef BT_INFINITY - struct btInfMaskConverter - { - union { - float mask; - int intmask; - }; - btInfMaskConverter(int mask=0x7F800000) - :intmask(mask) - { - } - }; - static btInfMaskConverter btInfinityMask = 0x7F800000; - #define BT_INFINITY (btInfinityMask.mask) - inline int btGetInfinityMask()//suppress stupid compiler warning - { - return btInfinityMask.intmask; - } - #endif + #define btAssign128(r0, r1, r2, r3) \ + (float32x4_t) { r0, r1, r2, r3 } #endif//BT_USE_NEON -#endif //BT_USE_SSE - -#ifdef BT_USE_NEON -#include - -typedef float32x4_t btSimdFloat4; -#define BT_INFINITY INFINITY -#define BT_NAN NAN -#define btAssign128(r0,r1,r2,r3) (float32x4_t){r0,r1,r2,r3} -#endif - - - - - -#define BT_DECLARE_ALIGNED_ALLOCATOR() \ - SIMD_FORCE_INLINE void* operator new(size_t sizeInBytes) { return btAlignedAlloc(sizeInBytes,16); } \ - SIMD_FORCE_INLINE void operator delete(void* ptr) { btAlignedFree(ptr); } \ - SIMD_FORCE_INLINE void* operator new(size_t, void* ptr) { return ptr; } \ - SIMD_FORCE_INLINE void operator delete(void*, void*) { } \ - SIMD_FORCE_INLINE void* operator new[](size_t sizeInBytes) { return btAlignedAlloc(sizeInBytes,16); } \ - SIMD_FORCE_INLINE void operator delete[](void* ptr) { btAlignedFree(ptr); } \ - SIMD_FORCE_INLINE void* operator new[](size_t, void* ptr) { return ptr; } \ - SIMD_FORCE_INLINE void operator delete[](void*, void*) { } \ - - +#define BT_DECLARE_ALIGNED_ALLOCATOR() \ + SIMD_FORCE_INLINE void *operator new(size_t sizeInBytes) { return btAlignedAlloc(sizeInBytes, 16); } \ + SIMD_FORCE_INLINE void operator delete(void *ptr) { btAlignedFree(ptr); } \ + SIMD_FORCE_INLINE void *operator new(size_t, void *ptr) { return ptr; } \ + SIMD_FORCE_INLINE void operator delete(void *, void *) {} \ + SIMD_FORCE_INLINE void *operator new[](size_t sizeInBytes) { return btAlignedAlloc(sizeInBytes, 16); } \ + SIMD_FORCE_INLINE void operator delete[](void *ptr) { btAlignedFree(ptr); } \ + SIMD_FORCE_INLINE void *operator new[](size_t, void *ptr) { return ptr; } \ + SIMD_FORCE_INLINE void operator delete[](void *, void *) {} #if defined(BT_USE_DOUBLE_PRECISION) || defined(BT_FORCE_DOUBLE_FUNCTIONS) - -SIMD_FORCE_INLINE btScalar btSqrt(btScalar x) { return sqrt(x); } -SIMD_FORCE_INLINE btScalar btFabs(btScalar x) { return fabs(x); } -SIMD_FORCE_INLINE btScalar btCos(btScalar x) { return cos(x); } -SIMD_FORCE_INLINE btScalar btSin(btScalar x) { return sin(x); } -SIMD_FORCE_INLINE btScalar btTan(btScalar x) { return tan(x); } -SIMD_FORCE_INLINE btScalar btAcos(btScalar x) { if (xbtScalar(1)) x=btScalar(1); return acos(x); } -SIMD_FORCE_INLINE btScalar btAsin(btScalar x) { if (xbtScalar(1)) x=btScalar(1); return asin(x); } -SIMD_FORCE_INLINE btScalar btAtan(btScalar x) { return atan(x); } -SIMD_FORCE_INLINE btScalar btAtan2(btScalar x, btScalar y) { return atan2(x, y); } -SIMD_FORCE_INLINE btScalar btExp(btScalar x) { return exp(x); } -SIMD_FORCE_INLINE btScalar btLog(btScalar x) { return log(x); } -SIMD_FORCE_INLINE btScalar btPow(btScalar x,btScalar y) { return pow(x,y); } -SIMD_FORCE_INLINE btScalar btFmod(btScalar x,btScalar y) { return fmod(x,y); } -#else - -SIMD_FORCE_INLINE btScalar btSqrt(btScalar y) -{ -#ifdef USE_APPROXIMATION -#ifdef __LP64__ - float xhalf = 0.5f*y; - int i = *(int*)&y; - i = 0x5f375a86 - (i>>1); - y = *(float*)&i; - y = y*(1.5f - xhalf*y*y); - y = y*(1.5f - xhalf*y*y); - y = y*(1.5f - xhalf*y*y); - y=1/y; - return y; -#else - double x, z, tempf; - unsigned long *tfptr = ((unsigned long *)&tempf) + 1; - tempf = y; - *tfptr = (0xbfcdd90a - *tfptr)>>1; /* estimate of 1/sqrt(y) */ - x = tempf; - z = y*btScalar(0.5); - x = (btScalar(1.5)*x)-(x*x)*(x*z); /* iteration formula */ - x = (btScalar(1.5)*x)-(x*x)*(x*z); - x = (btScalar(1.5)*x)-(x*x)*(x*z); - x = (btScalar(1.5)*x)-(x*x)*(x*z); - x = (btScalar(1.5)*x)-(x*x)*(x*z); - return x*y; -#endif -#else - return sqrtf(y); -#endif -} -SIMD_FORCE_INLINE btScalar btFabs(btScalar x) { return fabsf(x); } -SIMD_FORCE_INLINE btScalar btCos(btScalar x) { return cosf(x); } -SIMD_FORCE_INLINE btScalar btSin(btScalar x) { return sinf(x); } -SIMD_FORCE_INLINE btScalar btTan(btScalar x) { return tanf(x); } -SIMD_FORCE_INLINE btScalar btAcos(btScalar x) { - if (xbtScalar(1)) - x=btScalar(1); - return acosf(x); -} -SIMD_FORCE_INLINE btScalar btAsin(btScalar x) { - if (xbtScalar(1)) - x=btScalar(1); - return asinf(x); -} -SIMD_FORCE_INLINE btScalar btAtan(btScalar x) { return atanf(x); } -SIMD_FORCE_INLINE btScalar btAtan2(btScalar x, btScalar y) { return atan2f(x, y); } -SIMD_FORCE_INLINE btScalar btExp(btScalar x) { return expf(x); } -SIMD_FORCE_INLINE btScalar btLog(btScalar x) { return logf(x); } -SIMD_FORCE_INLINE btScalar btPow(btScalar x,btScalar y) { return powf(x,y); } -SIMD_FORCE_INLINE btScalar btFmod(btScalar x,btScalar y) { return fmodf(x,y); } - -#endif + SIMD_FORCE_INLINE btScalar btSqrt(btScalar x) + { + return sqrt(x); + } + SIMD_FORCE_INLINE btScalar btFabs(btScalar x) { return fabs(x); } + SIMD_FORCE_INLINE btScalar btCos(btScalar x) { return cos(x); } + SIMD_FORCE_INLINE btScalar btSin(btScalar x) { return sin(x); } + SIMD_FORCE_INLINE btScalar btTan(btScalar x) { return tan(x); } + SIMD_FORCE_INLINE btScalar btAcos(btScalar x) + { + if (x < btScalar(-1)) x = btScalar(-1); + if (x > btScalar(1)) x = btScalar(1); + return acos(x); + } + SIMD_FORCE_INLINE btScalar btAsin(btScalar x) + { + if (x < btScalar(-1)) x = btScalar(-1); + if (x > btScalar(1)) x = btScalar(1); + return asin(x); + } + SIMD_FORCE_INLINE btScalar btAtan(btScalar x) { return atan(x); } + SIMD_FORCE_INLINE btScalar btAtan2(btScalar x, btScalar y) { return atan2(x, y); } + SIMD_FORCE_INLINE btScalar btExp(btScalar x) { return exp(x); } + SIMD_FORCE_INLINE btScalar btLog(btScalar x) { return log(x); } + SIMD_FORCE_INLINE btScalar btPow(btScalar x, btScalar y) { return pow(x, y); } + SIMD_FORCE_INLINE btScalar btFmod(btScalar x, btScalar y) { return fmod(x, y); } -#define SIMD_PI btScalar(3.1415926535897932384626433832795029) -#define SIMD_2_PI (btScalar(2.0) * SIMD_PI) -#define SIMD_HALF_PI (SIMD_PI * btScalar(0.5)) +#else//BT_USE_DOUBLE_PRECISION + + SIMD_FORCE_INLINE btScalar btSqrt(btScalar y) + { + #ifdef USE_APPROXIMATION + #ifdef __LP64__ + float xhalf = 0.5f * y; + int i = *(int *)&y; + i = 0x5f375a86 - (i >> 1); + y = *(float *)&i; + y = y * (1.5f - xhalf * y * y); + y = y * (1.5f - xhalf * y * y); + y = y * (1.5f - xhalf * y * y); + y = 1 / y; + return y; + #else + double x, z, tempf; + unsigned long *tfptr = ((unsigned long *)&tempf) + 1; + tempf = y; + *tfptr = (0xbfcdd90a - *tfptr) >> 1; /* estimate of 1/sqrt(y) */ + x = tempf; + z = y * btScalar(0.5); + x = (btScalar(1.5) * x) - (x * x) * (x * z); /* iteration formula */ + x = (btScalar(1.5) * x) - (x * x) * (x * z); + x = (btScalar(1.5) * x) - (x * x) * (x * z); + x = (btScalar(1.5) * x) - (x * x) * (x * z); + x = (btScalar(1.5) * x) - (x * x) * (x * z); + return x * y; + #endif + #else + return sqrtf(y); + #endif + } + SIMD_FORCE_INLINE btScalar btFabs(btScalar x) { return fabsf(x); } + SIMD_FORCE_INLINE btScalar btCos(btScalar x) { return cosf(x); } + SIMD_FORCE_INLINE btScalar btSin(btScalar x) { return sinf(x); } + SIMD_FORCE_INLINE btScalar btTan(btScalar x) { return tanf(x); } + SIMD_FORCE_INLINE btScalar btAcos(btScalar x) + { + if (x < btScalar(-1)) + x = btScalar(-1); + if (x > btScalar(1)) + x = btScalar(1); + return acosf(x); + } + SIMD_FORCE_INLINE btScalar btAsin(btScalar x) + { + if (x < btScalar(-1)) + x = btScalar(-1); + if (x > btScalar(1)) + x = btScalar(1); + return asinf(x); + } + SIMD_FORCE_INLINE btScalar btAtan(btScalar x) { return atanf(x); } + SIMD_FORCE_INLINE btScalar btAtan2(btScalar x, btScalar y) { return atan2f(x, y); } + SIMD_FORCE_INLINE btScalar btExp(btScalar x) { return expf(x); } + SIMD_FORCE_INLINE btScalar btLog(btScalar x) { return logf(x); } + SIMD_FORCE_INLINE btScalar btPow(btScalar x, btScalar y) { return powf(x, y); } + SIMD_FORCE_INLINE btScalar btFmod(btScalar x, btScalar y) { return fmodf(x, y); } + +#endif//BT_USE_DOUBLE_PRECISION + +#define SIMD_PI btScalar(3.1415926535897932384626433832795029) +#define SIMD_2_PI (btScalar(2.0) * SIMD_PI) +#define SIMD_HALF_PI (SIMD_PI * btScalar(0.5)) #define SIMD_RADS_PER_DEG (SIMD_2_PI / btScalar(360.0)) -#define SIMD_DEGS_PER_RAD (btScalar(360.0) / SIMD_2_PI) +#define SIMD_DEGS_PER_RAD (btScalar(360.0) / SIMD_2_PI) #define SIMDSQRT12 btScalar(0.7071067811865475244008443621048490) - -#define btRecipSqrt(x) ((btScalar)(btScalar(1.0)/btSqrt(btScalar(x)))) /* reciprocal square root */ -#define btRecip(x) (btScalar(1.0)/btScalar(x)) +#define btRecipSqrt(x) ((btScalar)(btScalar(1.0) / btSqrt(btScalar(x)))) /* reciprocal square root */ +#define btRecip(x) (btScalar(1.0) / btScalar(x)) #ifdef BT_USE_DOUBLE_PRECISION -#define SIMD_EPSILON DBL_EPSILON -#define SIMD_INFINITY DBL_MAX -#define BT_ONE 1.0 -#define BT_ZERO 0.0 -#define BT_TWO 2.0 -#define BT_HALF 0.5 + #define SIMD_EPSILON DBL_EPSILON + #define SIMD_INFINITY DBL_MAX + #define BT_ONE 1.0 + #define BT_ZERO 0.0 + #define BT_TWO 2.0 + #define BT_HALF 0.5 #else -#define SIMD_EPSILON FLT_EPSILON -#define SIMD_INFINITY FLT_MAX -#define BT_ONE 1.0f -#define BT_ZERO 0.0f -#define BT_TWO 2.0f -#define BT_HALF 0.5f + #define SIMD_EPSILON FLT_EPSILON + #define SIMD_INFINITY FLT_MAX + #define BT_ONE 1.0f + #define BT_ZERO 0.0f + #define BT_TWO 2.0f + #define BT_HALF 0.5f #endif -SIMD_FORCE_INLINE btScalar btAtan2Fast(btScalar y, btScalar x) +// clang-format on + +SIMD_FORCE_INLINE btScalar btAtan2Fast(btScalar y, btScalar x) { btScalar coeff_1 = SIMD_PI / 4.0f; btScalar coeff_2 = 3.0f * coeff_1; btScalar abs_y = btFabs(y); btScalar angle; - if (x >= 0.0f) { + if (x >= 0.0f) + { btScalar r = (x - abs_y) / (x + abs_y); angle = coeff_1 - coeff_1 * r; - } else { + } + else + { btScalar r = (x + abs_y) / (abs_y - x); angle = coeff_2 - coeff_1 * r; } return (y < 0.0f) ? -angle : angle; } -SIMD_FORCE_INLINE bool btFuzzyZero(btScalar x) { return btFabs(x) < SIMD_EPSILON; } +SIMD_FORCE_INLINE bool btFuzzyZero(btScalar x) { return btFabs(x) < SIMD_EPSILON; } -SIMD_FORCE_INLINE bool btEqual(btScalar a, btScalar eps) { +SIMD_FORCE_INLINE bool btEqual(btScalar a, btScalar eps) +{ return (((a) <= eps) && !((a) < -eps)); } -SIMD_FORCE_INLINE bool btGreaterEqual (btScalar a, btScalar eps) { +SIMD_FORCE_INLINE bool btGreaterEqual(btScalar a, btScalar eps) +{ return (!((a) <= eps)); } - -SIMD_FORCE_INLINE int btIsNegative(btScalar x) { - return x < btScalar(0.0) ? 1 : 0; +SIMD_FORCE_INLINE int btIsNegative(btScalar x) +{ + return x < btScalar(0.0) ? 1 : 0; } SIMD_FORCE_INLINE btScalar btRadians(btScalar x) { return x * SIMD_RADS_PER_DEG; } SIMD_FORCE_INLINE btScalar btDegrees(btScalar x) { return x * SIMD_DEGS_PER_RAD; } -#define BT_DECLARE_HANDLE(name) typedef struct name##__ { int unused; } *name +#define BT_DECLARE_HANDLE(name) \ + typedef struct name##__ \ + { \ + int unused; \ + } * name #ifndef btFsel SIMD_FORCE_INLINE btScalar btFsel(btScalar a, btScalar b, btScalar c) @@ -541,60 +556,57 @@ SIMD_FORCE_INLINE btScalar btFsel(btScalar a, btScalar b, btScalar c) return a >= 0 ? b : c; } #endif -#define btFsels(a,b,c) (btScalar)btFsel(a,b,c) - +#define btFsels(a, b, c) (btScalar) btFsel(a, b, c) SIMD_FORCE_INLINE bool btMachineIsLittleEndian() { - long int i = 1; - const char *p = (const char *) &i; - if (p[0] == 1) // Lowest address contains the least significant byte - return true; - else - return false; + long int i = 1; + const char *p = (const char *)&i; + if (p[0] == 1) // Lowest address contains the least significant byte + return true; + else + return false; } - - ///btSelect avoids branches, which makes performance much better for consoles like Playstation 3 and XBox 360 ///Thanks Phil Knight. See also http://www.cellperformance.com/articles/2006/04/more_techniques_for_eliminatin_1.html -SIMD_FORCE_INLINE unsigned btSelect(unsigned condition, unsigned valueIfConditionNonZero, unsigned valueIfConditionZero) +SIMD_FORCE_INLINE unsigned btSelect(unsigned condition, unsigned valueIfConditionNonZero, unsigned valueIfConditionZero) { - // Set testNz to 0xFFFFFFFF if condition is nonzero, 0x00000000 if condition is zero - // Rely on positive value or'ed with its negative having sign bit on - // and zero value or'ed with its negative (which is still zero) having sign bit off - // Use arithmetic shift right, shifting the sign bit through all 32 bits - unsigned testNz = (unsigned)(((int)condition | -(int)condition) >> 31); - unsigned testEqz = ~testNz; - return ((valueIfConditionNonZero & testNz) | (valueIfConditionZero & testEqz)); + // Set testNz to 0xFFFFFFFF if condition is nonzero, 0x00000000 if condition is zero + // Rely on positive value or'ed with its negative having sign bit on + // and zero value or'ed with its negative (which is still zero) having sign bit off + // Use arithmetic shift right, shifting the sign bit through all 32 bits + unsigned testNz = (unsigned)(((int)condition | -(int)condition) >> 31); + unsigned testEqz = ~testNz; + return ((valueIfConditionNonZero & testNz) | (valueIfConditionZero & testEqz)); } SIMD_FORCE_INLINE int btSelect(unsigned condition, int valueIfConditionNonZero, int valueIfConditionZero) { - unsigned testNz = (unsigned)(((int)condition | -(int)condition) >> 31); - unsigned testEqz = ~testNz; - return static_cast((valueIfConditionNonZero & testNz) | (valueIfConditionZero & testEqz)); + unsigned testNz = (unsigned)(((int)condition | -(int)condition) >> 31); + unsigned testEqz = ~testNz; + return static_cast((valueIfConditionNonZero & testNz) | (valueIfConditionZero & testEqz)); } SIMD_FORCE_INLINE float btSelect(unsigned condition, float valueIfConditionNonZero, float valueIfConditionZero) { #ifdef BT_HAVE_NATIVE_FSEL - return (float)btFsel((btScalar)condition - btScalar(1.0f), valueIfConditionNonZero, valueIfConditionZero); + return (float)btFsel((btScalar)condition - btScalar(1.0f), valueIfConditionNonZero, valueIfConditionZero); #else - return (condition != 0) ? valueIfConditionNonZero : valueIfConditionZero; + return (condition != 0) ? valueIfConditionNonZero : valueIfConditionZero; #endif } -template SIMD_FORCE_INLINE void btSwap(T& a, T& b) +template +SIMD_FORCE_INLINE void btSwap(T &a, T &b) { T tmp = a; a = b; b = tmp; } - //PCK: endian swapping functions SIMD_FORCE_INLINE unsigned btSwapEndian(unsigned val) { - return (((val & 0xff000000) >> 24) | ((val & 0x00ff0000) >> 8) | ((val & 0x0000ff00) << 8) | ((val & 0x000000ff) << 24)); + return (((val & 0xff000000) >> 24) | ((val & 0x00ff0000) >> 8) | ((val & 0x0000ff00) << 8) | ((val & 0x000000ff) << 24)); } SIMD_FORCE_INLINE unsigned short btSwapEndian(unsigned short val) @@ -609,127 +621,127 @@ SIMD_FORCE_INLINE unsigned btSwapEndian(int val) SIMD_FORCE_INLINE unsigned short btSwapEndian(short val) { - return btSwapEndian((unsigned short) val); + return btSwapEndian((unsigned short)val); } ///btSwapFloat uses using char pointers to swap the endianness ////btSwapFloat/btSwapDouble will NOT return a float, because the machine might 'correct' invalid floating point values -///Not all values of sign/exponent/mantissa are valid floating point numbers according to IEEE 754. -///When a floating point unit is faced with an invalid value, it may actually change the value, or worse, throw an exception. -///In most systems, running user mode code, you wouldn't get an exception, but instead the hardware/os/runtime will 'fix' the number for you. +///Not all values of sign/exponent/mantissa are valid floating point numbers according to IEEE 754. +///When a floating point unit is faced with an invalid value, it may actually change the value, or worse, throw an exception. +///In most systems, running user mode code, you wouldn't get an exception, but instead the hardware/os/runtime will 'fix' the number for you. ///so instead of returning a float/double, we return integer/long long integer -SIMD_FORCE_INLINE unsigned int btSwapEndianFloat(float d) +SIMD_FORCE_INLINE unsigned int btSwapEndianFloat(float d) { - unsigned int a = 0; - unsigned char *dst = (unsigned char *)&a; - unsigned char *src = (unsigned char *)&d; + unsigned int a = 0; + unsigned char *dst = (unsigned char *)&a; + unsigned char *src = (unsigned char *)&d; - dst[0] = src[3]; - dst[1] = src[2]; - dst[2] = src[1]; - dst[3] = src[0]; - return a; + dst[0] = src[3]; + dst[1] = src[2]; + dst[2] = src[1]; + dst[3] = src[0]; + return a; } // unswap using char pointers -SIMD_FORCE_INLINE float btUnswapEndianFloat(unsigned int a) +SIMD_FORCE_INLINE float btUnswapEndianFloat(unsigned int a) { - float d = 0.0f; - unsigned char *src = (unsigned char *)&a; - unsigned char *dst = (unsigned char *)&d; + float d = 0.0f; + unsigned char *src = (unsigned char *)&a; + unsigned char *dst = (unsigned char *)&d; - dst[0] = src[3]; - dst[1] = src[2]; - dst[2] = src[1]; - dst[3] = src[0]; - - return d; -} - - -// swap using char pointers -SIMD_FORCE_INLINE void btSwapEndianDouble(double d, unsigned char* dst) -{ - unsigned char *src = (unsigned char *)&d; - - dst[0] = src[7]; - dst[1] = src[6]; - dst[2] = src[5]; - dst[3] = src[4]; - dst[4] = src[3]; - dst[5] = src[2]; - dst[6] = src[1]; - dst[7] = src[0]; - -} - -// unswap using char pointers -SIMD_FORCE_INLINE double btUnswapEndianDouble(const unsigned char *src) -{ - double d = 0.0; - unsigned char *dst = (unsigned char *)&d; - - dst[0] = src[7]; - dst[1] = src[6]; - dst[2] = src[5]; - dst[3] = src[4]; - dst[4] = src[3]; - dst[5] = src[2]; - dst[6] = src[1]; - dst[7] = src[0]; + dst[0] = src[3]; + dst[1] = src[2]; + dst[2] = src[1]; + dst[3] = src[0]; return d; } -template -SIMD_FORCE_INLINE void btSetZero(T* a, int n) +// swap using char pointers +SIMD_FORCE_INLINE void btSwapEndianDouble(double d, unsigned char *dst) { - T* acurr = a; - size_t ncurr = n; - while (ncurr > 0) - { - *(acurr++) = 0; - --ncurr; - } + unsigned char *src = (unsigned char *)&d; + + dst[0] = src[7]; + dst[1] = src[6]; + dst[2] = src[5]; + dst[3] = src[4]; + dst[4] = src[3]; + dst[5] = src[2]; + dst[6] = src[1]; + dst[7] = src[0]; } +// unswap using char pointers +SIMD_FORCE_INLINE double btUnswapEndianDouble(const unsigned char *src) +{ + double d = 0.0; + unsigned char *dst = (unsigned char *)&d; + + dst[0] = src[7]; + dst[1] = src[6]; + dst[2] = src[5]; + dst[3] = src[4]; + dst[4] = src[3]; + dst[5] = src[2]; + dst[6] = src[1]; + dst[7] = src[0]; + + return d; +} + +template +SIMD_FORCE_INLINE void btSetZero(T *a, int n) +{ + T *acurr = a; + size_t ncurr = n; + while (ncurr > 0) + { + *(acurr++) = 0; + --ncurr; + } +} SIMD_FORCE_INLINE btScalar btLargeDot(const btScalar *a, const btScalar *b, int n) -{ - btScalar p0,q0,m0,p1,q1,m1,sum; - sum = 0; - n -= 2; - while (n >= 0) { - p0 = a[0]; q0 = b[0]; - m0 = p0 * q0; - p1 = a[1]; q1 = b[1]; - m1 = p1 * q1; - sum += m0; - sum += m1; - a += 2; - b += 2; - n -= 2; - } - n += 2; - while (n > 0) { - sum += (*a) * (*b); - a++; - b++; - n--; - } - return sum; +{ + btScalar p0, q0, m0, p1, q1, m1, sum; + sum = 0; + n -= 2; + while (n >= 0) + { + p0 = a[0]; + q0 = b[0]; + m0 = p0 * q0; + p1 = a[1]; + q1 = b[1]; + m1 = p1 * q1; + sum += m0; + sum += m1; + a += 2; + b += 2; + n -= 2; + } + n += 2; + while (n > 0) + { + sum += (*a) * (*b); + a++; + b++; + n--; + } + return sum; } - // returns normalized value in range [-SIMD_PI, SIMD_PI] -SIMD_FORCE_INLINE btScalar btNormalizeAngle(btScalar angleInRadians) +SIMD_FORCE_INLINE btScalar btNormalizeAngle(btScalar angleInRadians) { angleInRadians = btFmod(angleInRadians, SIMD_2_PI); - if(angleInRadians < -SIMD_PI) + if (angleInRadians < -SIMD_PI) { return angleInRadians + SIMD_2_PI; } - else if(angleInRadians > SIMD_PI) + else if (angleInRadians > SIMD_PI) { return angleInRadians - SIMD_2_PI; } @@ -739,45 +751,38 @@ SIMD_FORCE_INLINE btScalar btNormalizeAngle(btScalar angleInRadians) } } - - ///rudimentary class to provide type info struct btTypedObject { btTypedObject(int objectType) - :m_objectType(objectType) + : m_objectType(objectType) { } - int m_objectType; + int m_objectType; inline int getObjectType() const { return m_objectType; } }; - - ///align a pointer to the provided alignment, upwards -template T* btAlignPointer(T* unalignedPtr, size_t alignment) +template +T *btAlignPointer(T *unalignedPtr, size_t alignment) { - struct btConvertPointerSizeT { - union - { - T* ptr; - size_t integer; + union { + T *ptr; + size_t integer; }; }; - btConvertPointerSizeT converter; - - + btConvertPointerSizeT converter; + const size_t bit_mask = ~(alignment - 1); - converter.ptr = unalignedPtr; - converter.integer += alignment-1; + converter.ptr = unalignedPtr; + converter.integer += alignment - 1; converter.integer &= bit_mask; return converter.ptr; } - -#endif //BT_SCALAR_H +#endif //BT_SCALAR_H diff --git a/test/BulletDynamics/CMakeLists.txt b/test/BulletDynamics/CMakeLists.txt new file mode 100644 index 000000000..ceeb2b4f4 --- /dev/null +++ b/test/BulletDynamics/CMakeLists.txt @@ -0,0 +1,24 @@ +SUBDIRS(pendulum ) + +INCLUDE_DIRECTORIES( + "${PROJECT_SOURCE_DIR}/src" + "${PROJECT_SOURCE_DIR}/test/gtest-1.7.0/include") + +ADD_DEFINITIONS(-DUSE_GTEST) +ADD_DEFINITIONS(-D_VARIADIC_MAX=10) + +LINK_LIBRARIES(BulletDynamics BulletCollision LinearMath gtest) + +IF (NOT WIN32) + LINK_LIBRARIES(pthread) +ENDIF() + +ADD_EXECUTABLE(Test_btKinematicCharacterController test_btKinematicCharacterController.cpp) + +ADD_TEST(Test_btKinematicCharacterController_PASS Test_btKinematicCharacterController) + +IF (INTERNAL_ADD_POSTFIX_EXECUTABLE_NAMES) + SET_TARGET_PROPERTIES(Test_btKinematicCharacterController PROPERTIES DEBUG_POSTFIX "_Debug") + SET_TARGET_PROPERTIES(Test_btKinematicCharacterController PROPERTIES MINSIZEREL_POSTFIX "_MinsizeRel") + SET_TARGET_PROPERTIES(Test_btKinematicCharacterController PROPERTIES RELWITHDEBINFO_POSTFIX "_RelWithDebugInfo") +ENDIF(INTERNAL_ADD_POSTFIX_EXECUTABLE_NAMES) diff --git a/test/BulletDynamics/test_btKinematicCharacterController.cpp b/test/BulletDynamics/test_btKinematicCharacterController.cpp new file mode 100644 index 000000000..4b0a567c0 --- /dev/null +++ b/test/BulletDynamics/test_btKinematicCharacterController.cpp @@ -0,0 +1,27 @@ + + +#include +#include +#include +#include + + +GTEST_TEST(BulletDynamics, KinematicCharacterController) { + + btPairCachingGhostObject* ghostObject = new btPairCachingGhostObject(); + btBoxShape* convexShape = new btBoxShape(btVector3(1, 1, 1)); + + //For now only a simple test that it initializes correctly. + btKinematicCharacterController* tested = new btKinematicCharacterController(ghostObject, convexShape, 1); + EXPECT_TRUE(tested); + + EXPECT_FLOAT_EQ(-9.8 * 3.0, tested->getGravity().x()); + EXPECT_FLOAT_EQ(0, tested->getGravity().y()); + EXPECT_FLOAT_EQ(0, tested->getGravity().z()); +} + + +int main(int argc, char **argv) { + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} \ No newline at end of file diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 713e4937e..ff1da9bff 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -3,5 +3,5 @@ IF(BUILD_BULLET3) SUBDIRS( InverseDynamics SharedMemory ) ENDIF(BUILD_BULLET3) -SUBDIRS( gtest-1.7.0 collision RobotLogging BulletDynamics/pendulum ) +SUBDIRS( gtest-1.7.0 collision RobotLogging BulletDynamics ) diff --git a/test/RobotClientAPI/SlopeFrictionMain.cpp b/test/RobotClientAPI/SlopeFrictionMain.cpp new file mode 100644 index 000000000..f0d802667 --- /dev/null +++ b/test/RobotClientAPI/SlopeFrictionMain.cpp @@ -0,0 +1,111 @@ +//todo: turn this into a gtest, comparing maximal and reduced coordinates contact behavior etc + +#include "b3RobotSimulatorClientAPI.h" +#include "../Utils/b3Clock.h" + +#include +#include +#include +#define ASSERT_EQ(a,b) assert((a)==(b)); +#include "MinitaurSetup.h" +#define NUM_SIM 1 +int main(int argc, char* argv[]) +{ + b3RobotSimulatorClientAPI* sims[2]; + b3Scalar fixedTimeStep = 1./240.; + for (int i=0;iconnect(eCONNECT_GUI);//eCONNECT_GUI);//DIRECT); + //Can also use eCONNECT_DIRECT,eCONNECT_SHARED_MEMORY,eCONNECT_UDP,eCONNECT_TCP, for example: + //sim->connect(eCONNECT_UDP, "localhost", 1234); + sim->configureDebugVisualizer( COV_ENABLE_GUI, 0); + // sim->configureDebugVisualizer( COV_ENABLE_SHADOWS, 0);//COV_ENABLE_WIREFRAME + //sim->setTimeOut(3); + //syncBodies is only needed when connecting to an existing physics server that has already some bodies + sim->syncBodies(); + + + sim->setTimeStep(fixedTimeStep); + + b3Quaternion q = sim->getQuaternionFromEuler(b3MakeVector3(0.1,0.2,0.3)); + b3Vector3 rpy; + rpy = sim->getEulerFromQuaternion(q); + + sim->setGravity(b3MakeVector3(0,0,-9.8)); + + //int blockId = sim->loadURDF("cube.urdf"); + //b3BodyInfo bodyInfo; + //sim->getBodyInfo(blockId,&bodyInfo); + + b3Quaternion orn; + orn.setEulerZYX(0,B3_PI*0.23,0); + + b3RobotSimulatorLoadUrdfFileArgs args; + args.m_startPosition.setValue(0,0,0); + args.m_startOrientation = orn; + args.m_useMultiBody = i==0? false : true;//true : false;//false : true; + + sim->loadURDF("plane.urdf",args); + + args.m_startPosition.setValue(0,0,0.66); + args.m_startOrientation.setEulerZYX(0,B3_PI*0.23,0); + sim->loadURDF("cube_soft.urdf",args); + + + double distance = 1.5; + double yaw = 50; + sim->resetDebugVisualizerCamera(distance,yaw,20,b3MakeVector3(0,0,0.1)); + sim->setRealTimeSimulation(false); + + } + int enableSim = 1; + while (sims[0]->canSubmitCommand()) + { + #if 0 + b3KeyboardEventsData keyEvents; + sim->getKeyboardEvents(&keyEvents); + if (keyEvents.m_numKeyboardEvents) + { + + //printf("num key events = %d]\n", keyEvents.m_numKeyboardEvents); + //m_keyState is a flag combination of eButtonIsDown,eButtonTriggered, eButtonReleased + for (int i=0;isetGravity(b3MakeVector3(0,0,-10)); + //printf("."); + if (enableSim) + { + sims[i]->stepSimulation(); + } + } + b3Clock::usleep(1000.*1000.*fixedTimeStep); + } + + printf("sim->disconnect\n"); + + for (int i=0;idisconnect(); + printf("delete sim\n"); + delete sims[i]; + + } + + + printf("exit\n"); + +} + +