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

further work on shared memory API

Browse Source 
fix dependency of BulletDynamics to Bullet3Common (b3Printf)
This commit is contained in:
=
2015-08-02 14:00:43 -07:00
parent 19c5be5646
commit eb6663ed4b
15 changed files with 492 additions and 112 deletions
Download Patch File Download Diff File Expand all files Collapse all files

BIN
data/slope.bullet
View File

Binary file not shown.

2
examples/ExampleBrowser/GwenGUISupport/gwenUserInterface.cpp
View File

@@ -295,7 +295,7 @@ void GwenUserInterface::init(int width, int height,Gwen::Renderer::Base* rendere
//tab->SetHeight(300);
tab->SetWidth(140);
tab->SetHeight(250);
tab->SetHeight(1250);
//tab->Dock(Gwen::Pos::Left);
tab->Dock( Gwen::Pos::Fill );
//tab->SetMargin( Gwen::Margin( 2, 2, 2, 2 ) );

44
examples/SharedMemory/PhysicsClient.cpp
View File

@@ -22,7 +22,7 @@ struct PhysicsClientSharedMemoryInternalData
SharedMemoryBlock* m_testBlock1;
btAlignedObjectArray<bParse::btBulletFile*> m_robotMultiBodyData;
btAlignedObjectArray<PoweredJointInfo> m_poweredJointInfo;
btAlignedObjectArray<b3JointInfo> m_jointInfo;
int m_counter;
bool m_serverLoadUrdfOK;
@@ -51,14 +51,14 @@ struct PhysicsClientSharedMemoryInternalData
};
int PhysicsClientSharedMemory::getNumPoweredJoints() const
int PhysicsClientSharedMemory::getNumJoints() const
{
return m_data->m_poweredJointInfo.size();
return m_data->m_jointInfo.size();
}
void PhysicsClientSharedMemory::getPoweredJointInfo(int index, PoweredJointInfo& info) const
void PhysicsClientSharedMemory::getJointInfo(int index, b3JointInfo& info) const
{
info = m_data->m_poweredJointInfo[index];
info = m_data->m_jointInfo[index];
}
@@ -147,7 +147,7 @@ bool PhysicsClientSharedMemory::processServerStatus(SharedMemoryStatus& serverSt
const SharedMemoryStatus& serverCmd =m_data->m_testBlock1->m_serverCommands[0];
hasStatus = true;
serverStatus = serverCmd;
EnumSharedMemoryServerStatus s = (EnumSharedMemoryServerStatus)serverCmd.m_type;
//consume the command
switch (serverCmd.m_type)
{
@@ -183,10 +183,9 @@ bool PhysicsClientSharedMemory::processServerStatus(SharedMemoryStatus& serverSt
}
for (int link=0;link<mb->m_numLinks;link++)
{
if ((mb->m_links[link].m_jointType == eRevoluteType)||
(mb->m_links[link].m_jointType == ePrismaticType))
{
PoweredJointInfo info;
b3JointInfo info;
info.m_flags = 0;
info.m_qIndex = qOffset;
info.m_uIndex = uOffset;
@@ -201,7 +200,12 @@ bool PhysicsClientSharedMemory::processServerStatus(SharedMemoryStatus& serverSt
info.m_jointName = mb->m_links[link].m_jointName;
info.m_jointType = mb->m_links[link].m_jointType;
}
m_data->m_poweredJointInfo.push_back(info);
if ((mb->m_links[link].m_jointType == eRevoluteType)||
(mb->m_links[link].m_jointType == ePrismaticType))
{
info.m_flags |= JOINT_HAS_MOTORIZED_POWER;
}
m_data->m_jointInfo.push_back(info);
}
qOffset+= mb->m_links[link].m_posVarCount;
uOffset+= mb->m_links[link].m_dofCount;
@@ -217,10 +221,9 @@ bool PhysicsClientSharedMemory::processServerStatus(SharedMemoryStatus& serverSt
}
for (int link=0;link<mb->m_numLinks;link++)
{
if ((mb->m_links[link].m_jointType == eRevoluteType)||
(mb->m_links[link].m_jointType == ePrismaticType))
{
PoweredJointInfo info;
b3JointInfo info;
info.m_flags = 0;
info.m_qIndex = qOffset;
info.m_uIndex = uOffset;
@@ -235,7 +238,12 @@ bool PhysicsClientSharedMemory::processServerStatus(SharedMemoryStatus& serverSt
info.m_jointName = mb->m_links[link].m_jointName;
info.m_jointType = mb->m_links[link].m_jointType;
}
m_data->m_poweredJointInfo.push_back(info);
if ((mb->m_links[link].m_jointType == eRevoluteType)||
(mb->m_links[link].m_jointType == ePrismaticType))
{
info.m_flags |= JOINT_HAS_MOTORIZED_POWER;
}
m_data->m_jointInfo.push_back(info);
}
qOffset+= mb->m_links[link].m_posVarCount;
uOffset+= mb->m_links[link].m_dofCount;
@@ -338,7 +346,7 @@ bool PhysicsClientSharedMemory::processServerStatus(SharedMemoryStatus& serverSt
}
default:
{
b3Error("Unknown server command\n");
b3Error("Unknown server status\n");
btAssert(0);
}
};
@@ -355,6 +363,10 @@ bool PhysicsClientSharedMemory::processServerStatus(SharedMemoryStatus& serverSt
{
m_data->m_waitingForServer = true;
}
} else
{
b3Printf("m_numServerStatus = %d, processed = %d\n", m_data->m_testBlock1->m_numServerCommands,
m_data->m_testBlock1->m_numProcessedServerCommands);
}
return hasStatus;
}
@@ -368,6 +380,8 @@ bool PhysicsClientSharedMemory::submitClientCommand(const SharedMemoryCommand& c
{
///at the moment we allow a maximum of 1 outstanding command, so we check for this
//once the server processed the command and returns a status, we clear the flag "m_data->m_waitingForServer" and allow submitting the next command
btAssert(!m_data->m_waitingForServer);
if (!m_data->m_waitingForServer)
{
m_data->m_testBlock1->m_clientCommands[0] = command;

4
examples/SharedMemory/PhysicsClient.h
View File

@@ -26,9 +26,9 @@ public:
virtual bool submitClientCommand(const SharedMemoryCommand& command);
virtual int getNumPoweredJoints() const;
virtual int getNumJoints() const;
virtual void getPoweredJointInfo(int index, PoweredJointInfo& info) const;
virtual void getJointInfo(int index, b3JointInfo& info) const;
};

145
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -80,6 +80,117 @@ int b3InitStepSimulationCommand(struct SharedMemoryCommand* command)
}
int b3JointControlCommandInit(struct SharedMemoryCommand* command, int controlMode)
{
b3Assert(command);
command->m_type = CMD_SEND_DESIRED_STATE;
command->m_sendDesiredStateCommandArgument.m_controlMode = controlMode;
command->m_updateFlags = 0;
return 0;
}
int b3JointControlSetDesiredVelocity(struct SharedMemoryCommand* command, int dofIndex, double value)
{
b3Assert(command);
command->m_sendDesiredStateCommandArgument.m_desiredStateQdot[dofIndex] = 1;
return 0;
}
int b3JointControlSetMaximumForce(struct SharedMemoryCommand* command, int dofIndex, double value)
{
b3Assert(command);
command->m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex] = value;
return 0;
}
int b3JointControlSetDesiredForceTorque(struct SharedMemoryCommand* command, int dofIndex, double value)
{
b3Assert(command);
command->m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex] = value;
return 0;
}
int b3RequestActualStateCommandInit(struct SharedMemoryCommand* command)
{
b3Assert(command);
command->m_type =CMD_REQUEST_ACTUAL_STATE;
return 0;
}
int b3CreateBoxShapeCommandInit(struct SharedMemoryCommand* command)
{
b3Assert(command);
command->m_type = CMD_CREATE_BOX_COLLISION_SHAPE;
command->m_updateFlags =0;
return 0;
}
int b3CreateBoxCommandSetStartPosition(struct SharedMemoryCommand* command, double startPosX,double startPosY,double startPosZ)
{
b3Assert(command);
b3Assert(command->m_type == CMD_CREATE_BOX_COLLISION_SHAPE);
command->m_updateFlags |=BOX_SHAPE_HAS_INITIAL_POSITION;
command->m_createBoxShapeArguments.m_initialPosition[0] = startPosX;
command->m_createBoxShapeArguments.m_initialPosition[1] = startPosY;
command->m_createBoxShapeArguments.m_initialPosition[2] = startPosZ;
return 0;
}
int b3CreateBoxCommandSetStartOrientation(struct SharedMemoryCommand* command, double startOrnX,double startOrnY,double startOrnZ, double startOrnW)
{
b3Assert(command);
b3Assert(command->m_type == CMD_CREATE_BOX_COLLISION_SHAPE);
command->m_updateFlags |=BOX_SHAPE_HAS_INITIAL_ORIENTATION;
command->m_createBoxShapeArguments.m_initialOrientation[0] = startOrnX;
command->m_createBoxShapeArguments.m_initialOrientation[1] = startOrnY;
command->m_createBoxShapeArguments.m_initialOrientation[2] = startOrnZ;
command->m_createBoxShapeArguments.m_initialOrientation[3] = startOrnW;
return 0;
}
int b3CreateBoxCommandSetHalfExtents(struct SharedMemoryCommand* command, double halfExtentsX,double halfExtentsY,double halfExtentsZ)
{
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 b3CreateSensorCommandInit(struct SharedMemoryCommand* command)
{
b3Assert(command);
command->m_type = CMD_CREATE_SENSOR;
command->m_updateFlags = 0;
command->m_createSensorArguments.m_numJointSensorChanges = 0;
return 0;
}
int b3CreateSensorEnable6DofJointForceTorqueSensor(struct SharedMemoryCommand* command, int jointIndex, int enable)
{
b3Assert(command);
b3Assert(command->m_type == CMD_CREATE_SENSOR);
int curIndex = command->m_createSensorArguments.m_numJointSensorChanges;
command->m_createSensorArguments.m_jointIndex[curIndex] = jointIndex;
command->m_createSensorArguments.m_enableJointForceSensor[curIndex] = enable;
command->m_createSensorArguments.m_numJointSensorChanges++;
return 0;
}
b3PhysicsClientHandle b3ConnectSharedMemory( int allowSharedMemoryInitialization)
{
PhysicsClientSharedMemory* cl = new PhysicsClientSharedMemory();
@@ -111,41 +222,19 @@ int b3SubmitClientCommand(b3PhysicsClientHandle physClient, struct SharedMemoryC
return (int)cl->submitClientCommand(*command);
}
int b3GetNumJoints(b3PhysicsClientHandle physClient)
{
PhysicsClientSharedMemory* cl = (PhysicsClientSharedMemory* ) physClient;
return cl->getNumPoweredJoints();
return cl->getNumJoints();
}
void b3GetPoweredJointInfo(b3PhysicsClientHandle physClient, int linkIndex, struct PoweredJointInfo* info)
void b3GetJointInfo(b3PhysicsClientHandle physClient, int linkIndex, struct b3JointInfo* info)
{
PhysicsClientSharedMemory* cl = (PhysicsClientSharedMemory* ) physClient;
cl->getPoweredJointInfo(linkIndex,*info);
cl->getJointInfo(linkIndex,*info);
}
#if 0
#include "SharedMemoryBlock.h"
#define B3_DECLARE_HANDLE(name) typedef struct name##__ { int unused; } *name
B3_DECLARE_HANDLE(b3PhysicsClientHandle);
b3PhysicsClientHandle b3ConnectSharedMemory(int memKey, int allowSharedMemoryInitialization);
void b3DisconnectSharedMemory(b3PhysicsClientHandle physClient);
int b3ProcessServerStatus(b3PhysicsClientHandle physClient, struct SharedMemoryStatus* status);
int b3CanSubmitCommand(b3PhysicsClientHandle physClient);
int b3SubmitClientCommand(b3PhysicsClientHandle physClient, struct SharedMemoryCommand* command);
int b3GetNumPoweredJoints(b3PhysicsClientHandle physClient);
void b3GetPoweredJointInfo(int linkIndex, struct PoweredJointInfo* info);
int b3InitPhysicsParamCommand(struct SharedMemoryCommand* command);
int b3PhysicsParamSetGravity(struct SharedMemoryCommand* command, double gravx,double gravy, double gravz);
#endif

31
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -22,9 +22,9 @@ int b3CanSubmitCommand(b3PhysicsClientHandle physClient);
int b3SubmitClientCommand(b3PhysicsClientHandle physClient, struct SharedMemoryCommand* command);
int b3GetNumPoweredJoints(b3PhysicsClientHandle physClient);
int b3GetNumJoints(b3PhysicsClientHandle physClient);
void b3GetPoweredJointInfo(b3PhysicsClientHandle physClient, int linkIndex, struct PoweredJointInfo* info);
void b3GetJointInfo(b3PhysicsClientHandle physClient, int linkIndex, struct b3JointInfo* info);
int b3InitPhysicsParamCommand(struct SharedMemoryCommand* command);
int b3PhysicsParamSetGravity(struct SharedMemoryCommand* command, double gravx,double gravy, double gravz);
@@ -39,6 +39,33 @@ int b3LoadUrdfCommandSetStartOrientation(struct SharedMemoryCommand* command, do
int b3LoadUrdfCommandSetUseMultiBody(struct SharedMemoryCommand* command, int useMultiBody);
int b3LoadUrdfCommandSetUseFixedBase(struct SharedMemoryCommand* command, int useFixedBase);
///Set joint control variables such as desired position/angle, desired velocity,
///applied joint forces, dependent on the control mode (CONTROL_MODE_VELOCITY or CONTROL_MODE_TORQUE)
int b3JointControlCommandInit(struct SharedMemoryCommand* command, int controlMode);
//Only use when controlMode is CONTROL_MODE_VELOCITY
int b3JointControlSetDesiredVelocity(struct SharedMemoryCommand* command, int dofIndex, double value);
int b3JointControlSetMaximumForce(struct SharedMemoryCommand* command, int dofIndex, double value);
///Only use if when controlMode is CONTROL_MODE_TORQUE,
int b3JointControlSetDesiredForceTorque(struct SharedMemoryCommand* command, int dofIndex, double value);
///the creation of collision shapes and rigid bodies etc is likely going to change,
///but good to have a b3CreateBoxShapeCommandInit for now
//create a box of size (1,1,1) at world origin (0,0,0) at orientation quat (0,0,0,1)
//after that, you can optionally adjust the initial position, orientation and size
int b3CreateBoxShapeCommandInit(struct SharedMemoryCommand* command);
int b3CreateBoxCommandSetStartPosition(struct SharedMemoryCommand* command, double startPosX,double startPosY,double startPosZ);
int b3CreateBoxCommandSetStartOrientation(struct SharedMemoryCommand* command, double startOrnX,double startOrnY,double startOrnZ, double startOrnW);
int b3CreateBoxCommandSetHalfExtents(struct SharedMemoryCommand* command, double halfExtentsX,double halfExtentsY,double halfExtentsZ);
int b3CreateSensorCommandInit(struct SharedMemoryCommand* command);
int b3CreateSensorEnable6DofJointForceTorqueSensor(struct SharedMemoryCommand* command, int dofIndex, int enable);
int b3RequestActualStateCommandInit(struct SharedMemoryCommand* command);
#ifdef __cplusplus
}

8
examples/SharedMemory/PhysicsClientExample.cpp
View File

@@ -215,11 +215,11 @@ void PhysicsClientExample::stepSimulation(float deltaTime)
bool hasStatus = m_physicsClient.processServerStatus(status);
if (hasStatus && status.m_type == CMD_URDF_LOADING_COMPLETED)
{
for (int i=0;i<m_physicsClient.getNumPoweredJoints();i++)
for (int i=0;i<m_physicsClient.getNumJoints();i++)
{
PoweredJointInfo info;
m_physicsClient.getPoweredJointInfo(i,info);
b3Printf("1-DOF PoweredJoint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex);
b3JointInfo info;
m_physicsClient.getJointInfo(i,info);
b3Printf("Joint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex);
}
}

127
examples/SharedMemory/PhysicsServer.cpp
View File

@@ -9,7 +9,7 @@
#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
#include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h"
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
#include "BulletDynamics/Featherstone/btMultiBodyJointFeedback.h"
#include "btBulletDynamicsCommon.h"
#include "../Extras/Serialize/BulletWorldImporter/btBulletWorldImporter.h"
@@ -35,7 +35,7 @@ struct PhysicsServerInternalData
SharedMemoryBlock* m_testBlock1;
bool m_isConnected;
btScalar m_physicsDeltaTime;
//btAlignedObjectArray<btJointFeedback*> m_jointFeedbacks;
btAlignedObjectArray<btMultiBodyJointFeedback*> m_multiBodyJointFeedbacks;
btAlignedObjectArray<btBulletWorldImporter*> m_worldImporters;
btAlignedObjectArray<UrdfLinkNameMapUtil*> m_urdfLinkNameMapper;
@@ -98,11 +98,12 @@ bool PhysicsServerSharedMemory::connectSharedMemory(bool allowSharedMemoryInitia
m_data->m_guiHelper = guiHelper;
bool allowCreation = true;
bool allowConnectToExistingSharedMemory = false;
m_data->m_testBlock1 = (SharedMemoryBlock*)m_data->m_sharedMemory->allocateSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE,allowCreation);
if (m_data->m_testBlock1)
{
if (m_data->m_testBlock1->m_magicId !=SHARED_MEMORY_MAGIC_NUMBER)
if (!allowConnectToExistingSharedMemory || (m_data->m_testBlock1->m_magicId !=SHARED_MEMORY_MAGIC_NUMBER))
{
if (allowSharedMemoryInitialization)
{
@@ -287,16 +288,7 @@ bool PhysicsServerSharedMemory::loadUrdf(const char* fileName, const btVector3&
} else
{
btAssert(0);
/*
for (int i=0;i<m_data->m_dynamicsWorld->getNumConstraints();i++)
{
btTypedConstraint* c = m_data->m_dynamicsWorld->getConstraint(i);
btJointFeedback* fb = new btJointFeedback();
m_data->m_jointFeedbacks.push_back(fb);
c->setJointFeedback(fb);
}
*/
return true;
}
@@ -407,6 +399,69 @@ void PhysicsServerSharedMemory::processClientCommands()
break;
}
case CMD_CREATE_SENSOR:
{
b3Printf("Processed CMD_CREATE_SENSOR");
if (m_data->m_dynamicsWorld->getNumMultibodies()>0)
{
btMultiBody* mb = m_data->m_dynamicsWorld->getMultiBody(0);
btAssert(mb);
for (int i=0;i<clientCmd.m_createSensorArguments.m_numJointSensorChanges;i++)
{
int jointIndex = clientCmd.m_createSensorArguments.m_jointIndex[i];
if (clientCmd.m_createSensorArguments.m_enableJointForceSensor[i])
{
if (mb->getLink(jointIndex).m_jointFeedback)
{
b3Warning("CMD_CREATE_SENSOR: sensor for joint [%d] already enabled", jointIndex);
} else
{
btMultiBodyJointFeedback* fb = new btMultiBodyJointFeedback();
fb->m_reactionForces.setZero();
mb->getLink(jointIndex).m_jointFeedback = fb;
m_data->m_multiBodyJointFeedbacks.push_back(fb);
};
} else
{
if (mb->getLink(jointIndex).m_jointFeedback)
{
m_data->m_multiBodyJointFeedbacks.remove(mb->getLink(jointIndex).m_jointFeedback);
delete mb->getLink(jointIndex).m_jointFeedback;
mb->getLink(jointIndex).m_jointFeedback=0;
} else
{
b3Warning("CMD_CREATE_SENSOR: cannot perform sensor removal request, no sensor on joint [%d]", jointIndex);
};
}
}
} else
{
b3Warning("No btMultiBody in the world. btRigidBody/btTypedConstraint sensor not hooked up yet");
}
#if 0
//todo(erwincoumans) here is some sample code to hook up a force/torque sensor for btTypedConstraint/btRigidBody
/*
for (int i=0;i<m_data->m_dynamicsWorld->getNumConstraints();i++)
{
btTypedConstraint* c = m_data->m_dynamicsWorld->getConstraint(i);
btJointFeedback* fb = new btJointFeedback();
m_data->m_jointFeedbacks.push_back(fb);
c->setJointFeedback(fb);
}
*/
#endif
SharedMemoryStatus& serverCmd =m_data->createServerStatus(CMD_CLIENT_COMMAND_COMPLETED,clientCmd.m_sequenceNumber,timeStamp);
m_data->submitServerStatus(serverCmd);
break;
}
case CMD_SEND_DESIRED_STATE:
@@ -448,6 +503,8 @@ void PhysicsServerSharedMemory::processClientCommands()
}
case CONTROL_MODE_VELOCITY:
{
b3Printf("Using CONTROL_MODE_VELOCITY");
int numMotors = 0;
//find the joint motors and apply the desired velocity and maximum force/torque
if (m_data->m_dynamicsWorld->getNumMultibodies()>0)
@@ -475,7 +532,6 @@ void PhysicsServerSharedMemory::processClientCommands()
dofIndex += mb->getLink(link).m_dofCount;
}
}
b3Printf("Using CONTROL_MODE_TORQUE with %d motors", numMotors);
break;
}
default:
@@ -545,6 +601,25 @@ void PhysicsServerSharedMemory::processClientCommands()
serverCmd.m_sendActualStateArgs.m_actualStateQdot[totalDegreeOfFreedomU++] = mb->getJointVelMultiDof(l)[d];
}
if (0 == mb->getLink(l).m_jointFeedback)
{
for (int d=0;d<6;d++)
{
serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+d]=0;
}
} else
{
btVector3 sensedForce = mb->getLink(l).m_jointFeedback->m_reactionForces.getLinear();
btVector3 sensedTorque = mb->getLink(l).m_jointFeedback->m_reactionForces.getLinear();
serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+0] = sensedForce[0];
serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+1] = sensedForce[1];
serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+2] = sensedForce[2];
serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+3] = sensedTorque[0];
serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+4] = sensedTorque[1];
serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+5] = sensedTorque[2];
}
}
serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomQ = totalDegreeOfFreedomQ;
@@ -617,10 +692,32 @@ void PhysicsServerSharedMemory::processClientCommands()
}
case CMD_CREATE_BOX_COLLISION_SHAPE:
{
btVector3 halfExtents(30,30,1);
btVector3 halfExtents(1,1,1);
if (clientCmd.m_updateFlags | BOX_SHAPE_HAS_HALF_EXTENTS)
{
halfExtents = btVector3(
clientCmd.m_createBoxShapeArguments.m_halfExtentsX,
clientCmd.m_createBoxShapeArguments.m_halfExtentsY,
clientCmd.m_createBoxShapeArguments.m_halfExtentsZ);
}
btTransform startTrans;
startTrans.setIdentity();
startTrans.setOrigin(btVector3(0,0,-4));
if (clientCmd.m_updateFlags | BOX_SHAPE_HAS_INITIAL_POSITION)
{
startTrans.setOrigin(btVector3(
clientCmd.m_createBoxShapeArguments.m_initialPosition[0],
clientCmd.m_createBoxShapeArguments.m_initialPosition[1],
clientCmd.m_createBoxShapeArguments.m_initialPosition[2]));
}
if (clientCmd.m_updateFlags | BOX_SHAPE_HAS_INITIAL_ORIENTATION)
{
startTrans.setRotation(btQuaternion(
clientCmd.m_createBoxShapeArguments.m_initialOrientation[0],
clientCmd.m_createBoxShapeArguments.m_initialOrientation[1],
clientCmd.m_createBoxShapeArguments.m_initialOrientation[2],
clientCmd.m_createBoxShapeArguments.m_initialOrientation[3]));
}
btBulletWorldImporter* worldImporter = new btBulletWorldImporter(m_data->m_dynamicsWorld);
m_data->m_worldImporters.push_back(worldImporter);

16
examples/SharedMemory/RobotControlExample.cpp
View File

@@ -167,7 +167,12 @@ void MyCallback2(int buttonId, bool buttonState, void* userPtr)
for (int i=0;i<cl->m_numMotors;i++)
{
btScalar targetVel = cl->m_motorTargetVelocities[i].m_velTarget;
int uIndex = cl->m_motorTargetVelocities[i].m_uIndex;
if (targetVel>1)
{
printf("testme");
}
command.m_sendDesiredStateCommandArgument.m_desiredStateQdot[uIndex] = targetVel;
}
@@ -300,12 +305,14 @@ void RobotControlExample::stepSimulation(float deltaTime)
bool hasStatus = m_physicsClient.processServerStatus(status);
if (hasStatus && status.m_type == CMD_URDF_LOADING_COMPLETED)
{
for (int i=0;i<m_physicsClient.getNumPoweredJoints();i++)
for (int i=0;i<m_physicsClient.getNumJoints();i++)
{
PoweredJointInfo info;
m_physicsClient.getPoweredJointInfo(i,info);
b3Printf("1-DOF PoweredJoint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex);
b3JointInfo info;
m_physicsClient.getJointInfo(i,info);
b3Printf("Joint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex);
if (info.m_flags & JOINT_HAS_MOTORIZED_POWER)
{
if (m_numMotors<MAX_NUM_MOTORS)
{
char motorName[1024];
@@ -319,6 +326,7 @@ void RobotControlExample::stepSimulation(float deltaTime)
slider.m_maxVal=4;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
m_numMotors++;
}
}
}

51
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -30,10 +30,11 @@ enum EnumSharedMemoryClientCommand
// CMD_DELETE_BOX_COLLISION_SHAPE,
// CMD_CREATE_RIGID_BODY,
// CMD_DELETE_RIGID_BODY,
// CMD_SET_JOINT_FEEDBACK,///enable or disable joint feedback for force/torque sensors
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,
CMD_SEND_PHYSICS_SIMULATION_PARAMETERS,
CMD_SEND_DESIRED_STATE,
CMD_SEND_DESIRED_STATE,//todo: reconsider naming, for example SET_JOINT_CONTROL_VARIABLE?
CMD_REQUEST_ACTUAL_STATE,
CMD_STEP_FORWARD_SIMULATION,
CMD_SHUTDOWN,
@@ -42,7 +43,7 @@ enum EnumSharedMemoryClientCommand
enum EnumSharedMemoryServerStatus
{
CMD_SHARED_MEMORY_NOT_INITIALIZED,
CMD_SHARED_MEMORY_NOT_INITIALIZED=0,
CMD_WAITING_FOR_CLIENT_COMMAND,
//CMD_CLIENT_COMMAND_COMPLETED is a generic 'completed' status that doesn't need special handling on the client
@@ -65,8 +66,8 @@ enum EnumSharedMemoryServerStatus
};
#define SHARED_MEMORY_SERVER_TEST_C
#define MAX_DEGREE_OF_FREEDOM 1024
#define MAX_NUM_SENSORS 1024
#define MAX_DEGREE_OF_FREEDOM 256
#define MAX_NUM_SENSORS 256
#define MAX_URDF_FILENAME_LENGTH 1024
enum EnumUrdfArgsUpdateFlags
@@ -105,7 +106,7 @@ struct SetJointFeedbackArgs
//todo: discuss and decide about control mode and combinations
enum {
// POSITION_CONTROL=0,
CONTROL_MODE_VELOCITY,
CONTROL_MODE_VELOCITY=0,
CONTROL_MODE_TORQUE,
};
@@ -173,13 +174,38 @@ struct SendActualStateArgs
//actual state is only written by the server, read-only access by client is expected
double m_actualStateQ[MAX_DEGREE_OF_FREEDOM];
double m_actualStateQdot[MAX_DEGREE_OF_FREEDOM];
double m_actualStateSensors[MAX_NUM_SENSORS];//these are force sensors and IMU information
//measured 6DOF force/torque sensors: force[x,y,z] and torque[x,y,z]
double m_jointReactionForces[6*MAX_DEGREE_OF_FREEDOM];
};
struct CreateSensorArgs
{
int m_bodyUniqueId;
int m_numJointSensorChanges;
int m_jointIndex[MAX_DEGREE_OF_FREEDOM];
int m_enableJointForceSensor[MAX_DEGREE_OF_FREEDOM];
};
typedef struct SharedMemoryCommand SharedMemoryCommand_t;
enum EnumBoxShapeFlags
{
BOX_SHAPE_HAS_INITIAL_POSITION=1,
BOX_SHAPE_HAS_INITIAL_ORIENTATION=2,
BOX_SHAPE_HAS_HALF_EXTENTS=4
};
///This command will be replaced to allow arbitrary collision shape types
struct CreateBoxShapeArgs
{
double m_halfExtentsX;
double m_halfExtentsY;
double m_halfExtentsZ;
double m_initialPosition[3];
double m_initialOrientation[4];
};
struct SharedMemoryCommand
{
@@ -199,6 +225,8 @@ struct SharedMemoryCommand
struct BulletDataStreamArgs m_dataStreamArguments;
struct SendDesiredStateArgs m_sendDesiredStateCommandArgument;
struct RequestActualStateArgs m_requestActualStateInformationCommandArgument;
struct CreateSensorArgs m_createSensorArguments;
struct CreateBoxShapeArgs m_createBoxShapeArguments;
};
};
@@ -219,15 +247,22 @@ struct SharedMemoryStatus
typedef struct SharedMemoryStatus SharedMemoryStatus_t;
struct PoweredJointInfo
enum JointInfoFlags
{
JOINT_HAS_MOTORIZED_POWER=1,
};
struct b3JointInfo
{
char* m_linkName;
char* m_jointName;
int m_jointType;
int m_qIndex;
int m_uIndex;
///
int m_flags;
};
#endif //SHARED_MEMORY_COMMANDS_H

10
src/Bullet3Common/b3Logging.h
View File

@@ -2,6 +2,10 @@
#ifndef B3_LOGGING_H
#define B3_LOGGING_H
#ifdef __cplusplus
extern "C" {
#endif
///We add the do/while so that the statement "if (condition) b3Printf("test"); else {...}" would fail
///You can also customize the message by uncommenting out a different line below
#define b3Printf(...) b3OutputPrintfVarArgsInternal(__VA_ARGS__)
@@ -18,6 +22,7 @@
void b3EnterProfileZone(const char* name);
void b3LeaveProfileZone();
#ifdef __cplusplus
class b3ProfileZone
{
@@ -34,6 +39,7 @@ public:
};
#define B3_PROFILE( name ) b3ProfileZone __profile( name )
#endif
#else //B3_NO_PROFILE
@@ -64,6 +70,8 @@ void b3OutputPrintfVarArgsInternal(const char *str, ...);
void b3OutputWarningMessageVarArgsInternal(const char *str, ...);
void b3OutputErrorMessageVarArgsInternal(const char *str, ...);
#ifdef __cplusplus
}
#endif
#endif//B3_LOGGING_H

6
src/BulletCollision/CollisionShapes/btConvexShape.cpp
View File

@@ -48,7 +48,7 @@ btConvexShape::~btConvexShape()
}
void btConvexShape::project(const btTransform& trans, const btVector3& dir, btScalar& min, btScalar& max) const
void btConvexShape::project(const btTransform& trans, const btVector3& dir, btScalar& min, btScalar& max, btVector3& witnesPtMin,btVector3& witnesPtMax) const
{
btVector3 localAxis = dir*trans.getBasis();
btVector3 vtx1 = trans(localGetSupportingVertex(localAxis));
@@ -56,12 +56,16 @@ void btConvexShape::project(const btTransform& trans, const btVector3& dir, btSc
min = vtx1.dot(dir);
max = vtx2.dot(dir);
witnesPtMax = vtx2;
witnesPtMin = vtx1;
if(min>max)
{
btScalar tmp = min;
min = max;
max = tmp;
witnesPtMax = vtx1;
witnesPtMin = vtx2;
}
}

3
src/BulletCollision/CollisionShapes/btConvexShape.h
View File

@@ -52,7 +52,8 @@ public:
btScalar getMarginNonVirtual () const;
void getAabbNonVirtual (const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const;
virtual void project(const btTransform& trans, const btVector3& dir, btScalar& min, btScalar& max) const;
virtual void project(const btTransform& trans, const btVector3& dir, btScalar& minProj, btScalar& maxProj, btVector3& witnesPtMin,btVector3& witnesPtMax) const;
//notice that the vectors should be unit length

3
src/BulletDynamics/Featherstone/btMultiBody.cpp
View File

@@ -847,7 +847,7 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt,
zeroAccSpatFrc[i+1].setVector(-(rot_from_world[i+1] * linkAppliedTorque), -(rot_from_world[i+1] * linkAppliedForce ));
if (0)
#if 0
{
b3Printf("stepVelocitiesMultiDof zeroAccSpatFrc[%d] linear:%f,%f,%f, angular:%f,%f,%f",
@@ -860,6 +860,7 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt,
zeroAccSpatFrc[i+1].m_bottomVec[1],
zeroAccSpatFrc[i+1].m_bottomVec[2]);
}
#endif
//
//adding damping terms (only)
btScalar linDampMult = 1., angDampMult = 1.;

100
test/SharedMemory/test.c
View File

@@ -1,6 +1,8 @@
#include "SharedMemoryBlock.h"
#include "PhysicsClientC_API.h"
#include "SharedMemoryCommands.h"
#include "Bullet3Common/b3Logging.h"
#include <string.h>
struct test
{
@@ -8,16 +10,20 @@ struct test
};
#include <stdio.h>
#define MAX_TIMEOUT 1024
#define MAX_TIMEOUT 1024*1024*1024
int main(int argc, char* argv[])
{
int ret ,allowSharedMemoryInitialization=0;
int timeout = MAX_TIMEOUT;
int sensorJointIndexLeft=-1;
int sensorJointIndexRight=-1;
const char* urdfFileName = "r2d2.urdf";
double gravx=0, gravy=0, gravz=0;
double gravx=0, gravy=0, gravz=-9.8;
double timeStep = 1./60.;
double startPosX, startPosY,startPosZ;
b3Printf("timeout = %d\n",timeout);
SharedMemoryCommand_t command;
SharedMemoryStatus_t status;
@@ -34,6 +40,7 @@ int main(int argc, char* argv[])
ret = b3SubmitClientCommand(sm, &command);
timeout = MAX_TIMEOUT;
while ((timeout-- > 0) && b3ProcessServerStatus(sm, &status)==0) {}
b3Printf("timeout = %d\n",timeout);
ret = b3LoadUrdfCommandInit(&command, urdfFileName);
@@ -46,14 +53,103 @@ int main(int argc, char* argv[])
ret = b3SubmitClientCommand(sm, &command);
timeout = MAX_TIMEOUT;
while ((timeout-- > 0) && b3ProcessServerStatus(sm, &status)==0) {}
b3Printf("timeout = %d\n",timeout);
int numJoints = b3GetNumJoints(sm);
for (int i=0;i<numJoints;i++)
{
struct b3JointInfo jointInfo;
b3GetJointInfo(sm,i,&jointInfo);
printf("jointInfo[%d].m_jointName=%s\n",i,jointInfo.m_jointName);
//pick the joint index based on joint name
if (strstr(jointInfo.m_jointName,"base_to_left_leg"))
{
sensorJointIndexLeft = i;
}
if (strstr(jointInfo.m_jointName,"base_to_right_leg"))
{
sensorJointIndexRight = i;
}
}
if ((sensorJointIndexLeft>=0) || (sensorJointIndexRight>=0))
{
ret = b3CreateSensorCommandInit(&command);
if (sensorJointIndexLeft>=0)
{
ret = b3CreateSensorEnable6DofJointForceTorqueSensor(&command, sensorJointIndexLeft, 1);
}
if(sensorJointIndexRight>=0)
{
ret = b3CreateSensorEnable6DofJointForceTorqueSensor(&command, sensorJointIndexRight, 1);
}
ret = b3SubmitClientCommand(sm, &command);
timeout = MAX_TIMEOUT;
while ((timeout-- > 0) && b3ProcessServerStatus(sm, &status)==0) {}
}
ret = b3CreateBoxShapeCommandInit(&command);
ret = b3CreateBoxCommandSetStartPosition(&command, 0,0,-1);
ret = b3CreateBoxCommandSetStartOrientation(&command,0,0,0,1);
ret = b3CreateBoxCommandSetHalfExtents(&command, 10,10,1);
ret = b3SubmitClientCommand(sm, &command);
timeout = MAX_TIMEOUT;
while ((timeout-- > 0) && b3ProcessServerStatus(sm, &status)==0) {}
b3RequestActualStateCommandInit(&command);
ret = b3SubmitClientCommand(sm, &command);
timeout = MAX_TIMEOUT;
while ((timeout-- > 0) && b3ProcessServerStatus(sm, &status)==0) {}
int posVarCount =status.m_sendActualStateArgs.m_numDegreeOfFreedomQ;
int dofCount =status.m_sendActualStateArgs.m_numDegreeOfFreedomU;
b3Printf("posVarCount = %d\n",posVarCount);
printf("dofCount = %d\n",dofCount);
b3JointControlCommandInit(&command, CONTROL_MODE_VELOCITY);
for (int dofIndex=0;dofIndex<dofCount;dofIndex++)
{
b3JointControlSetDesiredVelocity(&command,dofIndex,1);
b3JointControlSetMaximumForce(&command,dofIndex,100);
}
ret = b3SubmitClientCommand(sm, &command);
timeout = MAX_TIMEOUT;
while ((timeout-- > 0) && b3ProcessServerStatus(sm, &status)==0) {}
///perform some simulation steps for testing
for (int i=0;i<100;i++)
{
ret = b3InitStepSimulationCommand(&command);
ret = b3SubmitClientCommand(sm, &command);
timeout = MAX_TIMEOUT;
while ((timeout-- > 0) && b3ProcessServerStatus(sm, &status)==0) {}
}
b3RequestActualStateCommandInit(&command);
ret = b3SubmitClientCommand(sm, &command);
timeout = MAX_TIMEOUT;
while ((timeout-- > 0) && b3ProcessServerStatus(sm, &status)==0) {}
if (sensorJointIndexLeft>=0)
{
b3Printf("Sensor for joint [%d] = %f,%f,%f\n", sensorJointIndexLeft,
status.m_sendActualStateArgs.m_jointReactionForces[6*sensorJointIndexLeft+0],
status.m_sendActualStateArgs.m_jointReactionForces[6*sensorJointIndexLeft+1],
status.m_sendActualStateArgs.m_jointReactionForces[6*sensorJointIndexLeft+2]);
}
if (sensorJointIndexRight>=0)
{
b3Printf("Sensor for joint [%d] = %f,%f,%f\n", sensorJointIndexRight,
status.m_sendActualStateArgs.m_jointReactionForces[6*sensorJointIndexRight+0],
status.m_sendActualStateArgs.m_jointReactionForces[6*sensorJointIndexRight+1],
status.m_sendActualStateArgs.m_jointReactionForces[6*sensorJointIndexRight+2]);
}
}
b3DisconnectSharedMemory(sm);
}