From 4bc31394a0790187a58425b5e5edd20f1fac63bb Mon Sep 17 00:00:00 2001 From: yunfeibai Date: Tue, 16 Aug 2016 16:57:48 -0700 Subject: [PATCH] Allow choosing loaded as btRigidBody with RobotSimAPI. --- data/cube_small.sdf | 35 +++++++++++++++++++ .../RoboticsLearning/GripperGraspExample.cpp | 6 ++-- examples/RoboticsLearning/b3RobotSimAPI.cpp | 6 +++- examples/RoboticsLearning/b3RobotSimAPI.h | 1 + examples/SharedMemory/PhysicsClientC_API.cpp | 12 +++++-- .../PhysicsServerCommandProcessor.cpp | 2 +- 6 files changed, 56 insertions(+), 6 deletions(-) create mode 100644 data/cube_small.sdf diff --git a/data/cube_small.sdf b/data/cube_small.sdf new file mode 100644 index 000000000..828b74df1 --- /dev/null +++ b/data/cube_small.sdf @@ -0,0 +1,35 @@ + + + + 0 0 0.107 0 0 0 + + + 0.1 + + 1.0 + 0 + 0 + 1.0 + 0 + 1.0 + + + + + + 0.05 0.05 0.05 + + + + + + + 0.05 0.05 0.05 + cube.obj + + + + + + + diff --git a/examples/RoboticsLearning/GripperGraspExample.cpp b/examples/RoboticsLearning/GripperGraspExample.cpp index ebdc39acb..514ac970d 100644 --- a/examples/RoboticsLearning/GripperGraspExample.cpp +++ b/examples/RoboticsLearning/GripperGraspExample.cpp @@ -82,7 +82,6 @@ public: m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } - { b3RobotSimLoadFileArgs args(""); args.m_fileName = "gripper/wsg50_with_r2d2_gripper.sdf"; @@ -132,9 +131,11 @@ public: { b3RobotSimLoadFileArgs args(""); b3RobotSimLoadFileResults results; - args.m_fileName = "cube_small.urdf"; + args.m_fileName = "cube_small.sdf"; args.m_startPosition.setValue(0,0,.107); args.m_startOrientation.setEulerZYX(0,0,0); + args.m_useMultiBody = false; + args.m_fileType = 2; m_robotSim.loadFile(args,results); } if (1) @@ -144,6 +145,7 @@ public: args.m_startPosition.setValue(0,0,0); args.m_startOrientation.setEulerZYX(0,0,0); args.m_forceOverrideFixedBase = true; + args.m_useMultiBody = true; b3RobotSimLoadFileResults results; m_robotSim.loadFile(args,results); diff --git a/examples/RoboticsLearning/b3RobotSimAPI.cpp b/examples/RoboticsLearning/b3RobotSimAPI.cpp index adbef7a20..634ba3cd2 100644 --- a/examples/RoboticsLearning/b3RobotSimAPI.cpp +++ b/examples/RoboticsLearning/b3RobotSimAPI.cpp @@ -211,7 +211,10 @@ public: return m_cs; } - virtual void createRigidBodyGraphicsObject(btRigidBody* body,const btVector3& color){} + virtual void createRigidBodyGraphicsObject(btRigidBody* body,const btVector3& color) + { + createCollisionObjectGraphicsObject((btCollisionObject*)body, color); + } btCollisionObject* m_obj; btVector3 m_color2; @@ -685,6 +688,7 @@ bool b3RobotSimAPI::loadFile(const struct b3RobotSimLoadFileArgs& args, b3RobotS { b3LoadUrdfCommandSetUseFixedBase(command,true); } + b3LoadUrdfCommandSetUseMultiBody(command, args.m_useMultiBody); statusHandle = submitClientCommandAndWaitStatusMultiThreaded(m_data->m_physicsClient, command); statusType = b3GetStatusType(statusHandle); diff --git a/examples/RoboticsLearning/b3RobotSimAPI.h b/examples/RoboticsLearning/b3RobotSimAPI.h index 3666700d2..9a4a14831 100644 --- a/examples/RoboticsLearning/b3RobotSimAPI.h +++ b/examples/RoboticsLearning/b3RobotSimAPI.h @@ -23,6 +23,7 @@ struct b3RobotSimLoadFileArgs b3Vector3 m_startPosition; b3Quaternion m_startOrientation; bool m_forceOverrideFixedBase; + bool m_useMultiBody; int m_fileType; diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 8530c0c5a..e60a4f5d2 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -55,6 +55,16 @@ b3SharedMemoryCommandHandle b3LoadUrdfCommandInit(b3PhysicsClientHandle physClie return (b3SharedMemoryCommandHandle) command; } +int b3LoadUrdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_LOAD_URDF); + command->m_updateFlags |=URDF_ARGS_USE_MULTIBODY; + command->m_urdfArguments.m_useMultiBody = useMultiBody; + + return 0; +} int b3LoadUrdfCommandSetUseFixedBase(b3SharedMemoryCommandHandle commandHandle, int useFixedBase) { @@ -67,8 +77,6 @@ int b3LoadUrdfCommandSetUseFixedBase(b3SharedMemoryCommandHandle commandHandle, return 0; } - - int b3LoadUrdfCommandSetStartPosition(b3SharedMemoryCommandHandle commandHandle, double startPosX,double startPosY,double startPosZ) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index d3ada5bca..f1436516b 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -877,6 +877,7 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto } btMultiBody* mb = creation.getBulletMultiBody(); + if (useMultiBody) { @@ -944,7 +945,6 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto } else { - btAssert(0); return true; }