Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Merge pull request #507 from erwincoumans/master

Browse Source 
Physics Shared Memory Client/Server: 	Allow to request the state of a rigid body
This commit is contained in:
erwincoumans
2015-10-27 16:54:57 -07:00
parent 5ded2d3ee4 2d5d89d999
commit 187b216750
8 changed files with 232 additions and 41 deletions
Download Patch File Download Diff File Expand all files Collapse all files

62
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -191,7 +191,7 @@ int b3JointControlSetDesiredForceTorque(b3SharedMemoryCommandHandle commandHandl
}
b3SharedMemoryCommandHandle b3RequestActualStateCommandInit(b3PhysicsClientHandle physClient)
b3SharedMemoryCommandHandle b3RequestActualStateCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
@@ -199,7 +199,7 @@ b3SharedMemoryCommandHandle b3RequestActualStateCommandInit(b3PhysicsClientHandl
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type =CMD_REQUEST_ACTUAL_STATE;
command->m_requestActualStateInformationCommandArgument.m_bodyUniqueId = 0;
command->m_requestActualStateInformationCommandArgument.m_bodyUniqueId = bodyUniqueId;
return (b3SharedMemoryCommandHandle) command;
}
@@ -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)
@@ -442,6 +467,11 @@ int b3GetStatusBodyIndex(b3SharedMemoryStatusHandle statusHandle)
bodyId = status->m_dataStreamArguments.m_bodyUniqueId;
break;
}
case CMD_RIGID_BODY_CREATION_COMPLETED:
{
bodyId = status->m_rigidBodyCreateArgs.m_bodyUniqueId;
break;
}
default:
{
b3Assert(0);

5
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -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.
@@ -97,7 +100,7 @@ b3SharedMemoryCommandHandle b3CreateSensorCommandInit(b3PhysicsClientHandle phys
int b3CreateSensorEnable6DofJointForceTorqueSensor(b3SharedMemoryCommandHandle commandHandle, int jointIndex, int enable);
int b3CreateSensorEnableIMUForLink(b3SharedMemoryCommandHandle commandHandle, int linkIndex, int enable);
b3SharedMemoryCommandHandle b3RequestActualStateCommandInit(b3PhysicsClientHandle physClient);
b3SharedMemoryCommandHandle b3RequestActualStateCommandInit(b3PhysicsClientHandle physClient,int bodyUniqueId);
void b3GetJointState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int jointIndex, struct b3JointSensorState *state);
int b3PickBody(struct SharedMemoryCommand *command,

27
examples/SharedMemory/PhysicsClientExample.cpp
View File

@@ -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,10 +214,26 @@ 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);
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
if (m_selectedBody>=0)
{
b3SharedMemoryCommandHandle commandHandle = b3RequestActualStateCommandInit(m_physicsClientHandle,m_selectedBody);
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
}
break;
};
@@ -360,6 +376,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 +532,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);
}
}
}

5
examples/SharedMemory/PhysicsClientSharedMemory.cpp
View File

@@ -450,6 +450,11 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
break;
}
case CMD_RIGID_BODY_CREATION_COMPLETED:
{
break;
}
case CMD_DEBUG_LINES_OVERFLOW_FAILED: {
b3Warning("Error receiving debug lines");
m_data->m_debugLinesFrom.resize(0);

141
examples/SharedMemory/PhysicsServer.cpp
View File

@@ -79,12 +79,14 @@ struct SharedMemoryDebugDrawer : public btIDebugDraw
struct InteralBodyData
{
btMultiBody* m_multiBody;
btRigidBody* m_rigidBody;
int m_testData;
btTransform m_rootLocalInertialFrame;
InteralBodyData()
:m_multiBody(0),
m_rigidBody(0),
m_testData(0)
{
m_rootLocalInertialFrame.setIdentity();
@@ -1089,14 +1091,10 @@ void PhysicsServerSharedMemory::processClientCommands()
}
int bodyUniqueId = clientCmd.m_requestActualStateInformationCommandArgument.m_bodyUniqueId;
InteralBodyData* body = m_data->getHandle(bodyUniqueId);
btMultiBody* mb = 0;
if (body)
if (body && body->m_multiBody)
{
mb = body->m_multiBody;
}
if (mb)
{
btMultiBody* mb = body->m_multiBody;
SharedMemoryStatus& serverCmd = m_data->createServerStatus(CMD_ACTUAL_STATE_UPDATE_COMPLETED,clientCmd.m_sequenceNumber,timeStamp);
serverCmd.m_sendActualStateArgs.m_bodyUniqueId = bodyUniqueId;
@@ -1192,10 +1190,48 @@ void PhysicsServerSharedMemory::processClientCommands()
} else
{
if (body && body->m_rigidBody)
{
btRigidBody* rb = body->m_rigidBody;
SharedMemoryStatus& serverCmd = m_data->createServerStatus(CMD_ACTUAL_STATE_UPDATE_COMPLETED,clientCmd.m_sequenceNumber,timeStamp);
serverCmd.m_sendActualStateArgs.m_bodyUniqueId = bodyUniqueId;
int totalDegreeOfFreedomQ = 0;
int totalDegreeOfFreedomU = 0;
b3Warning("Request state but no multibody available");
SharedMemoryStatus& serverCmd = m_data->createServerStatus(CMD_ACTUAL_STATE_UPDATE_FAILED,clientCmd.m_sequenceNumber,timeStamp);
m_data->submitServerStatus(serverCmd);
btTransform tr = rb->getWorldTransform();
//base position in world space, carthesian
serverCmd.m_sendActualStateArgs.m_actualStateQ[0] = tr.getOrigin()[0];
serverCmd.m_sendActualStateArgs.m_actualStateQ[1] = tr.getOrigin()[1];
serverCmd.m_sendActualStateArgs.m_actualStateQ[2] = tr.getOrigin()[2];
//base orientation, quaternion x,y,z,w, in world space, carthesian
serverCmd.m_sendActualStateArgs.m_actualStateQ[3] = tr.getRotation()[0];
serverCmd.m_sendActualStateArgs.m_actualStateQ[4] = tr.getRotation()[1];
serverCmd.m_sendActualStateArgs.m_actualStateQ[5] = tr.getRotation()[2];
serverCmd.m_sendActualStateArgs.m_actualStateQ[6] = tr.getRotation()[3];
totalDegreeOfFreedomQ +=7;//pos + quaternion
//base linear velocity (in world space, carthesian)
serverCmd.m_sendActualStateArgs.m_actualStateQdot[0] = rb->getLinearVelocity()[0];
serverCmd.m_sendActualStateArgs.m_actualStateQdot[1] = rb->getLinearVelocity()[1];
serverCmd.m_sendActualStateArgs.m_actualStateQdot[2] = rb->getLinearVelocity()[2];
//base angular velocity (in world space, carthesian)
serverCmd.m_sendActualStateArgs.m_actualStateQdot[3] = rb->getAngularVelocity()[0];
serverCmd.m_sendActualStateArgs.m_actualStateQdot[4] = rb->getAngularVelocity()[1];
serverCmd.m_sendActualStateArgs.m_actualStateQdot[5] = rb->getAngularVelocity()[2];
totalDegreeOfFreedomU += 6;//3 linear and 3 angular DOF
serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomQ = totalDegreeOfFreedomQ;
serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomU = totalDegreeOfFreedomU;
m_data->submitServerStatus(serverCmd);
} else
{
b3Warning("Request state but no multibody or rigid body available");
SharedMemoryStatus& serverCmd = m_data->createServerStatus(CMD_ACTUAL_STATE_UPDATE_FAILED,clientCmd.m_sequenceNumber,timeStamp);
m_data->submitServerStatus(serverCmd);
}
}
break;
@@ -1309,6 +1345,7 @@ void PhysicsServerSharedMemory::processClientCommands()
break;
}
case CMD_CREATE_RIGID_BODY:
case CMD_CREATE_BOX_COLLISION_SHAPE:
{
btVector3 halfExtents(1,1,1);
@@ -1339,16 +1376,92 @@ 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);
btRigidBody* rb = worldImporter->createRigidBody(isDynamic,mass,startTrans,shape,0);
m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld);
SharedMemoryStatus& serverCmd =m_data->createServerStatus(CMD_CLIENT_COMMAND_COMPLETED,clientCmd.m_sequenceNumber,timeStamp);
SharedMemoryStatus& serverCmd =m_data->createServerStatus(CMD_RIGID_BODY_CREATION_COMPLETED,clientCmd.m_sequenceNumber,timeStamp);
int bodyUniqueId = m_data->allocHandle();
InternalBodyHandle* bodyHandle = m_data->getHandle(bodyUniqueId);
serverCmd.m_rigidBodyCreateArgs.m_bodyUniqueId = bodyUniqueId;
bodyHandle->m_rootLocalInertialFrame.setIdentity();
bodyHandle->m_rigidBody = rb;
m_data->submitServerStatus(serverCmd);
break;

12
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -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];
};
@@ -240,6 +245,10 @@ struct SharedMemoryCommand
};
};
struct RigidBodyCreateArgs
{
int m_bodyUniqueId;
};
struct SharedMemoryStatus
{
@@ -253,6 +262,7 @@ struct SharedMemoryStatus
struct BulletDataStreamArgs m_dataStreamArguments;
struct SendActualStateArgs m_sendActualStateArgs;
struct SendDebugLinesArgs m_sendDebugLinesArgs;
struct RigidBodyCreateArgs m_rigidBodyCreateArgs;
};
};

17
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -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;

4
test/SharedMemory/test.c
View File

@@ -121,7 +121,7 @@ int main(int argc, char* argv[])
{
int statusType;
b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(sm);
b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(sm,bodyIndex);
b3SharedMemoryStatusHandle statusHandle;
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
statusType = b3GetStatusType(statusHandle);
@@ -157,7 +157,7 @@ int main(int argc, char* argv[])
}
{
b3SharedMemoryStatusHandle state = b3SubmitClientCommandAndWaitStatus(sm, b3RequestActualStateCommandInit(sm));
b3SharedMemoryStatusHandle state = b3SubmitClientCommandAndWaitStatus(sm, b3RequestActualStateCommandInit(sm,bodyIndex));
if (sensorJointIndexLeft>=0)
{