From 9cc2b1ec12645e2e67a44e0c5ddabdc63f0ab87d Mon Sep 17 00:00:00 2001 From: erwin coumans Date: Tue, 27 Oct 2015 14:55:46 -0700 Subject: [PATCH] Physics SharedMemory: Add flags BOX_SHAPE_HAS_MASS and BOX_SHAPE_HAS_COLLISION_SHAPE_TYPE to CMD_CREATE_RIGID_BODY (which maps to CMD_CREATE_BOX_COLLISION_SHAPE for backward compatibility for now) Need to add way to receive world transform --- examples/SharedMemory/PhysicsClientC_API.cpp | 53 +++++++++---- examples/SharedMemory/PhysicsClientC_API.h | 3 + .../SharedMemory/PhysicsClientExample.cpp | 20 ++++- examples/SharedMemory/PhysicsServer.cpp | 76 ++++++++++++++++++- examples/SharedMemory/SharedMemoryCommands.h | 7 +- examples/SharedMemory/SharedMemoryPublic.h | 17 ++++- 6 files changed, 154 insertions(+), 22 deletions(-) diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 06d1f6778..639191b14 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -246,6 +246,45 @@ int b3CreateBoxCommandSetStartPosition(b3SharedMemoryCommandHandle commandHandle return 0; } + +int b3CreateBoxCommandSetHalfExtents(b3SharedMemoryCommandHandle commandHandle, double halfExtentsX,double halfExtentsY,double halfExtentsZ) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_CREATE_BOX_COLLISION_SHAPE); + command->m_updateFlags |=BOX_SHAPE_HAS_HALF_EXTENTS; + + command->m_createBoxShapeArguments.m_halfExtentsX = halfExtentsX; + command->m_createBoxShapeArguments.m_halfExtentsY = halfExtentsY; + command->m_createBoxShapeArguments.m_halfExtentsZ = halfExtentsZ; + + return 0; +} + + +int b3CreateBoxCommandSetMass(b3SharedMemoryCommandHandle commandHandle, double mass) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_CREATE_BOX_COLLISION_SHAPE); + command->m_updateFlags |=BOX_SHAPE_HAS_MASS; + command->m_createBoxShapeArguments.m_mass = mass; + return 0; +} + + +int b3CreateBoxCommandSetCollisionShapeType(b3SharedMemoryCommandHandle commandHandle, int collisionShapeType) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_CREATE_BOX_COLLISION_SHAPE); + command->m_updateFlags |=BOX_SHAPE_HAS_COLLISION_SHAPE_TYPE; + command->m_createBoxShapeArguments.m_collisionShapeType = collisionShapeType; + + return 0; +} + + int b3CreateBoxCommandSetStartOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX,double startOrnY,double startOrnZ, double startOrnW) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; @@ -328,20 +367,6 @@ int b3CreatePoseCommandSetJointPosition(b3PhysicsClientHandle physClient, b3Shar } -int b3CreateBoxCommandSetHalfExtents(b3SharedMemoryCommandHandle commandHandle, double halfExtentsX,double halfExtentsY,double halfExtentsZ) -{ - struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; - b3Assert(command); - b3Assert(command->m_type == CMD_CREATE_BOX_COLLISION_SHAPE); - command->m_updateFlags |=BOX_SHAPE_HAS_HALF_EXTENTS; - - command->m_createBoxShapeArguments.m_halfExtentsX = halfExtentsX; - command->m_createBoxShapeArguments.m_halfExtentsY = halfExtentsY; - command->m_createBoxShapeArguments.m_halfExtentsZ = halfExtentsZ; - - return 0; -} - b3SharedMemoryCommandHandle b3CreateSensorCommandInit(b3PhysicsClientHandle physClient) diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 23aa5f2d4..df456c938 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -84,6 +84,9 @@ b3SharedMemoryCommandHandle b3CreateBoxShapeCommandInit(b3PhysicsClientHandle ph int b3CreateBoxCommandSetStartPosition(b3SharedMemoryCommandHandle commandHandle, double startPosX,double startPosY,double startPosZ); int b3CreateBoxCommandSetStartOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX,double startOrnY,double startOrnZ, double startOrnW); int b3CreateBoxCommandSetHalfExtents(b3SharedMemoryCommandHandle commandHandle, double halfExtentsX,double halfExtentsY,double halfExtentsZ); +int b3CreateBoxCommandSetMass(b3SharedMemoryCommandHandle commandHandle, double mass); +int b3CreateBoxCommandSetCollisionShapeType(b3SharedMemoryCommandHandle commandHandle, int collisionShapeType); + ///Initialize (teleport) the pose of a body/robot. You can individually set the base position, base orientation and joint angles. ///This will set all velocities of base and joints to zero. diff --git a/examples/SharedMemory/PhysicsClientExample.cpp b/examples/SharedMemory/PhysicsClientExample.cpp index ab444279b..76e533914 100644 --- a/examples/SharedMemory/PhysicsClientExample.cpp +++ b/examples/SharedMemory/PhysicsClientExample.cpp @@ -194,7 +194,7 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId) case CMD_LOAD_URDF: { - b3SharedMemoryCommandHandle commandHandle = b3LoadUrdfCommandInit(m_physicsClientHandle, "r2d2.urdf");//kuka_lwr/kuka.urdf"); + b3SharedMemoryCommandHandle commandHandle = b3LoadUrdfCommandInit(m_physicsClientHandle, "kuka_lwr/kuka.urdf"); //setting the initial position, orientation and other arguments are optional double startPosX = 0; @@ -214,6 +214,19 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId) b3SubmitClientCommand(m_physicsClientHandle, commandHandle); break; } + case CMD_CREATE_RIGID_BODY: + { + b3SharedMemoryCommandHandle commandHandle = b3CreateBoxShapeCommandInit(m_physicsClientHandle); + b3CreateBoxCommandSetStartPosition(commandHandle,0,0,0); + b3CreateBoxCommandSetMass(commandHandle,1); + b3CreateBoxCommandSetCollisionShapeType(commandHandle,COLLISION_SHAPE_TYPE_CYLINDER_Y); + double radius = 0.2; + double halfHeight = 0.5; + b3CreateBoxCommandSetHalfExtents(commandHandle,radius,halfHeight,radius); + b3SubmitClientCommand(m_physicsClientHandle, commandHandle); + break; + } + case CMD_REQUEST_ACTUAL_STATE: { b3SharedMemoryCommandHandle commandHandle = b3RequestActualStateCommandInit(m_physicsClientHandle); @@ -360,6 +373,7 @@ void PhysicsClientExample::createButtons() createButton("Get State",CMD_REQUEST_ACTUAL_STATE, isTrigger); createButton("Send Desired State",CMD_SEND_DESIRED_STATE, isTrigger); createButton("Create Box Collider",CMD_CREATE_BOX_COLLISION_SHAPE,isTrigger); + createButton("Create Cylinder Body",CMD_CREATE_RIGID_BODY,isTrigger); createButton("Reset Simulation",CMD_RESET_SIMULATION,isTrigger); createButton("Initialize Pose",CMD_INIT_POSE, isTrigger); @@ -515,8 +529,8 @@ void PhysicsClientExample::stepSimulation(float deltaTime) { enqueueCommand(CMD_SEND_DESIRED_STATE); enqueueCommand(CMD_STEP_FORWARD_SIMULATION); - enqueueCommand(CMD_REQUEST_DEBUG_LINES); - enqueueCommand(CMD_REQUEST_ACTUAL_STATE); + //enqueueCommand(CMD_REQUEST_DEBUG_LINES); + //enqueueCommand(CMD_REQUEST_ACTUAL_STATE); } } } diff --git a/examples/SharedMemory/PhysicsServer.cpp b/examples/SharedMemory/PhysicsServer.cpp index a9bdc787e..2ecc95c67 100644 --- a/examples/SharedMemory/PhysicsServer.cpp +++ b/examples/SharedMemory/PhysicsServer.cpp @@ -1309,6 +1309,7 @@ void PhysicsServerSharedMemory::processClientCommands() break; } + case CMD_CREATE_RIGID_BODY: case CMD_CREATE_BOX_COLLISION_SHAPE: { btVector3 halfExtents(1,1,1); @@ -1339,11 +1340,82 @@ void PhysicsServerSharedMemory::processClientCommands() clientCmd.m_createBoxShapeArguments.m_initialOrientation[3])); } + btScalar mass = 0.f; + if (clientCmd.m_updateFlags & BOX_SHAPE_HAS_MASS) + { + mass = clientCmd.m_createBoxShapeArguments.m_mass; + } + + int shapeType = COLLISION_SHAPE_TYPE_BOX; + + if (clientCmd.m_updateFlags & BOX_SHAPE_HAS_COLLISION_SHAPE_TYPE) + { + shapeType = clientCmd.m_createBoxShapeArguments.m_collisionShapeType; + } + btBulletWorldImporter* worldImporter = new btBulletWorldImporter(m_data->m_dynamicsWorld); m_data->m_worldImporters.push_back(worldImporter); - btCollisionShape* shape = worldImporter->createBoxShape(halfExtents); - btScalar mass = 0.f; + btCollisionShape* shape = 0; + + switch (shapeType) + { + case COLLISION_SHAPE_TYPE_CYLINDER_X: + { + btScalar radius = halfExtents[1]; + btScalar height = halfExtents[0]; + shape = worldImporter->createCylinderShapeX(radius,height); + break; + } + case COLLISION_SHAPE_TYPE_CYLINDER_Y: + { + btScalar radius = halfExtents[0]; + btScalar height = halfExtents[1]; + shape = worldImporter->createCylinderShapeY(radius,height); + break; + } + case COLLISION_SHAPE_TYPE_CYLINDER_Z: + { + btScalar radius = halfExtents[1]; + btScalar height = halfExtents[2]; + shape = worldImporter->createCylinderShapeZ(radius,height); + break; + } + case COLLISION_SHAPE_TYPE_CAPSULE_X: + { + btScalar radius = halfExtents[1]; + btScalar height = halfExtents[0]; + shape = worldImporter->createCapsuleShapeX(radius,height); + break; + } + case COLLISION_SHAPE_TYPE_CAPSULE_Y: + { + btScalar radius = halfExtents[0]; + btScalar height = halfExtents[1]; + shape = worldImporter->createCapsuleShapeY(radius,height); + break; + } + case COLLISION_SHAPE_TYPE_CAPSULE_Z: + { + btScalar radius = halfExtents[1]; + btScalar height = halfExtents[2]; + shape = worldImporter->createCapsuleShapeZ(radius,height); + break; + } + case COLLISION_SHAPE_TYPE_SPHERE: + { + btScalar radius = halfExtents[0]; + shape = worldImporter->createSphereShape(radius); + break; + } + case COLLISION_SHAPE_TYPE_BOX: + default: + { + shape = worldImporter->createBoxShape(halfExtents); + } + } + + bool isDynamic = (mass>0); worldImporter->createRigidBody(isDynamic,mass,startTrans,shape,0); m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld); diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index b009dc760..49893f2be 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -202,7 +202,9 @@ enum EnumBoxShapeFlags { BOX_SHAPE_HAS_INITIAL_POSITION=1, BOX_SHAPE_HAS_INITIAL_ORIENTATION=2, - BOX_SHAPE_HAS_HALF_EXTENTS=4 + BOX_SHAPE_HAS_HALF_EXTENTS=4, + BOX_SHAPE_HAS_MASS=8, + BOX_SHAPE_HAS_COLLISION_SHAPE_TYPE=16, }; ///This command will be replaced to allow arbitrary collision shape types struct CreateBoxShapeArgs @@ -211,6 +213,9 @@ struct CreateBoxShapeArgs double m_halfExtentsY; double m_halfExtentsZ; + double m_mass; + int m_collisionShapeType;//see SharedMemoryPublic.h + double m_initialPosition[3]; double m_initialOrientation[4]; }; diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index fcdd9ff73..b9cec004e 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -9,8 +9,8 @@ enum EnumSharedMemoryClientCommand CMD_SEND_BULLET_DATA_STREAM, CMD_CREATE_BOX_COLLISION_SHAPE, // CMD_DELETE_BOX_COLLISION_SHAPE, -// CMD_CREATE_RIGID_BODY, -// CMD_DELETE_RIGID_BODY, + CMD_CREATE_RIGID_BODY, + CMD_DELETE_RIGID_BODY, CMD_CREATE_SENSOR,///enable or disable joint feedback for force/torque sensors // CMD_REQUEST_SENSOR_MEASUREMENTS,//see CMD_REQUEST_ACTUAL_STATE/CMD_ACTUAL_STATE_UPDATE_COMPLETED CMD_INIT_POSE, @@ -57,6 +57,19 @@ enum JointInfoFlags { JOINT_HAS_MOTORIZED_POWER=1, }; + +enum +{ + COLLISION_SHAPE_TYPE_BOX=1, + COLLISION_SHAPE_TYPE_CYLINDER_X, + COLLISION_SHAPE_TYPE_CYLINDER_Y, + COLLISION_SHAPE_TYPE_CYLINDER_Z, + COLLISION_SHAPE_TYPE_CAPSULE_X, + COLLISION_SHAPE_TYPE_CAPSULE_Y, + COLLISION_SHAPE_TYPE_CAPSULE_Z, + COLLISION_SHAPE_TYPE_SPHERE +}; + struct b3JointInfo { char* m_linkName;