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->SetHeight(300);
tab->SetWidth(140); tab->SetWidth(140);
tab->SetHeight(250); tab->SetHeight(1250);
//tab->Dock(Gwen::Pos::Left); //tab->Dock(Gwen::Pos::Left);
tab->Dock( Gwen::Pos::Fill ); tab->Dock( Gwen::Pos::Fill );
//tab->SetMargin( Gwen::Margin( 2, 2, 2, 2 ) ); //tab->SetMargin( Gwen::Margin( 2, 2, 2, 2 ) );

46
examples/SharedMemory/PhysicsClient.cpp
View File

@@ -22,7 +22,7 @@ struct PhysicsClientSharedMemoryInternalData
SharedMemoryBlock* m_testBlock1; SharedMemoryBlock* m_testBlock1;
btAlignedObjectArray<bParse::btBulletFile*> m_robotMultiBodyData; btAlignedObjectArray<bParse::btBulletFile*> m_robotMultiBodyData;
btAlignedObjectArray<PoweredJointInfo> m_poweredJointInfo; btAlignedObjectArray<b3JointInfo> m_jointInfo;
int m_counter; int m_counter;
bool m_serverLoadUrdfOK; 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]; const SharedMemoryStatus& serverCmd =m_data->m_testBlock1->m_serverCommands[0];
hasStatus = true; hasStatus = true;
serverStatus = serverCmd; serverStatus = serverCmd;
EnumSharedMemoryServerStatus s = (EnumSharedMemoryServerStatus)serverCmd.m_type;
//consume the command //consume the command
switch (serverCmd.m_type) switch (serverCmd.m_type)
{ {
@@ -183,10 +183,9 @@ bool PhysicsClientSharedMemory::processServerStatus(SharedMemoryStatus& serverSt
} }
for (int link=0;link<mb->m_numLinks;link++) 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_qIndex = qOffset;
info.m_uIndex = uOffset; info.m_uIndex = uOffset;
@@ -201,7 +200,12 @@ bool PhysicsClientSharedMemory::processServerStatus(SharedMemoryStatus& serverSt
info.m_jointName = mb->m_links[link].m_jointName; info.m_jointName = mb->m_links[link].m_jointName;
info.m_jointType = mb->m_links[link].m_jointType; 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; qOffset+= mb->m_links[link].m_posVarCount;
uOffset+= mb->m_links[link].m_dofCount; 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++) 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_qIndex = qOffset;
info.m_uIndex = uOffset; info.m_uIndex = uOffset;
@@ -235,7 +238,12 @@ bool PhysicsClientSharedMemory::processServerStatus(SharedMemoryStatus& serverSt
info.m_jointName = mb->m_links[link].m_jointName; info.m_jointName = mb->m_links[link].m_jointName;
info.m_jointType = mb->m_links[link].m_jointType; 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; qOffset+= mb->m_links[link].m_posVarCount;
uOffset+= mb->m_links[link].m_dofCount; uOffset+= mb->m_links[link].m_dofCount;
@@ -338,7 +346,7 @@ bool PhysicsClientSharedMemory::processServerStatus(SharedMemoryStatus& serverSt
} }
default: default:
{ {
b3Error("Unknown server command\n"); b3Error("Unknown server status\n");
btAssert(0); btAssert(0);
} }
}; };
@@ -355,7 +363,11 @@ bool PhysicsClientSharedMemory::processServerStatus(SharedMemoryStatus& serverSt
{ {
m_data->m_waitingForServer = true; 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; 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 ///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 //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) if (!m_data->m_waitingForServer)
{ {
m_data->m_testBlock1->m_clientCommands[0] = command; 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 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) b3PhysicsClientHandle b3ConnectSharedMemory( int allowSharedMemoryInitialization)
{ {
PhysicsClientSharedMemory* cl = new PhysicsClientSharedMemory(); PhysicsClientSharedMemory* cl = new PhysicsClientSharedMemory();
@@ -111,41 +222,19 @@ int b3SubmitClientCommand(b3PhysicsClientHandle physClient, struct SharedMemoryC
return (int)cl->submitClientCommand(*command); return (int)cl->submitClientCommand(*command);
} }
int b3GetNumJoints(b3PhysicsClientHandle physClient) int b3GetNumJoints(b3PhysicsClientHandle physClient)
{ {
PhysicsClientSharedMemory* cl = (PhysicsClientSharedMemory* ) 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; 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 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 b3InitPhysicsParamCommand(struct SharedMemoryCommand* command);
int b3PhysicsParamSetGravity(struct SharedMemoryCommand* command, double gravx,double gravy, double gravz); 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 b3LoadUrdfCommandSetUseMultiBody(struct SharedMemoryCommand* command, int useMultiBody);
int b3LoadUrdfCommandSetUseFixedBase(struct SharedMemoryCommand* command, int useFixedBase); 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 #ifdef __cplusplus
} }

8
examples/SharedMemory/PhysicsClientExample.cpp
View File

@@ -215,11 +215,11 @@ void PhysicsClientExample::stepSimulation(float deltaTime)
bool hasStatus = m_physicsClient.processServerStatus(status); bool hasStatus = m_physicsClient.processServerStatus(status);
if (hasStatus && status.m_type == CMD_URDF_LOADING_COMPLETED) 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; b3JointInfo info;
m_physicsClient.getPoweredJointInfo(i,info); m_physicsClient.getJointInfo(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); b3Printf("Joint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex);
} }
} }

133
examples/SharedMemory/PhysicsServer.cpp
View File

@@ -9,7 +9,7 @@
#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h" #include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
#include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h" #include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h"
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
#include "BulletDynamics/Featherstone/btMultiBodyJointFeedback.h"
#include "btBulletDynamicsCommon.h" #include "btBulletDynamicsCommon.h"
#include "../Extras/Serialize/BulletWorldImporter/btBulletWorldImporter.h" #include "../Extras/Serialize/BulletWorldImporter/btBulletWorldImporter.h"
@@ -35,7 +35,7 @@ struct PhysicsServerInternalData
SharedMemoryBlock* m_testBlock1; SharedMemoryBlock* m_testBlock1;
bool m_isConnected; bool m_isConnected;
btScalar m_physicsDeltaTime; btScalar m_physicsDeltaTime;
//btAlignedObjectArray<btJointFeedback*> m_jointFeedbacks; btAlignedObjectArray<btMultiBodyJointFeedback*> m_multiBodyJointFeedbacks;
btAlignedObjectArray<btBulletWorldImporter*> m_worldImporters; btAlignedObjectArray<btBulletWorldImporter*> m_worldImporters;
btAlignedObjectArray<UrdfLinkNameMapUtil*> m_urdfLinkNameMapper; btAlignedObjectArray<UrdfLinkNameMapUtil*> m_urdfLinkNameMapper;
@@ -98,11 +98,12 @@ bool PhysicsServerSharedMemory::connectSharedMemory(bool allowSharedMemoryInitia
m_data->m_guiHelper = guiHelper; m_data->m_guiHelper = guiHelper;
bool allowCreation = true; bool allowCreation = true;
bool allowConnectToExistingSharedMemory = false;
m_data->m_testBlock1 = (SharedMemoryBlock*)m_data->m_sharedMemory->allocateSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE,allowCreation); 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)
{ {
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) if (allowSharedMemoryInitialization)
{ {
@@ -287,16 +288,7 @@ bool PhysicsServerSharedMemory::loadUrdf(const char* fileName, const btVector3&
} else } else
{ {
btAssert(0); 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; 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; break;
} }
case CMD_SEND_DESIRED_STATE: case CMD_SEND_DESIRED_STATE:
@@ -448,6 +503,8 @@ void PhysicsServerSharedMemory::processClientCommands()
} }
case CONTROL_MODE_VELOCITY: case CONTROL_MODE_VELOCITY:
{ {
b3Printf("Using CONTROL_MODE_VELOCITY");
int numMotors = 0; int numMotors = 0;
//find the joint motors and apply the desired velocity and maximum force/torque //find the joint motors and apply the desired velocity and maximum force/torque
if (m_data->m_dynamicsWorld->getNumMultibodies()>0) if (m_data->m_dynamicsWorld->getNumMultibodies()>0)
@@ -475,7 +532,6 @@ void PhysicsServerSharedMemory::processClientCommands()
dofIndex += mb->getLink(link).m_dofCount; dofIndex += mb->getLink(link).m_dofCount;
} }
} }
b3Printf("Using CONTROL_MODE_TORQUE with %d motors", numMotors);
break; break;
} }
default: default:
@@ -544,7 +600,26 @@ void PhysicsServerSharedMemory::processClientCommands()
{ {
serverCmd.m_sendActualStateArgs.m_actualStateQdot[totalDegreeOfFreedomU++] = mb->getJointVelMultiDof(l)[d]; 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; serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomQ = totalDegreeOfFreedomQ;
@@ -617,11 +692,33 @@ void PhysicsServerSharedMemory::processClientCommands()
} }
case CMD_CREATE_BOX_COLLISION_SHAPE: 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; btTransform startTrans;
startTrans.setIdentity(); 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); btBulletWorldImporter* worldImporter = new btBulletWorldImporter(m_data->m_dynamicsWorld);
m_data->m_worldImporters.push_back(worldImporter); m_data->m_worldImporters.push_back(worldImporter);

46
examples/SharedMemory/RobotControlExample.cpp
View File

@@ -167,8 +167,13 @@ void MyCallback2(int buttonId, bool buttonState, void* userPtr)
for (int i=0;i<cl->m_numMotors;i++) for (int i=0;i<cl->m_numMotors;i++)
{ {
btScalar targetVel = cl->m_motorTargetVelocities[i].m_velTarget; btScalar targetVel = cl->m_motorTargetVelocities[i].m_velTarget;
int uIndex = cl->m_motorTargetVelocities[i].m_uIndex; int uIndex = cl->m_motorTargetVelocities[i].m_uIndex;
command.m_sendDesiredStateCommandArgument.m_desiredStateQdot[uIndex] = targetVel; if (targetVel>1)
{
printf("testme");
}
command.m_sendDesiredStateCommandArgument.m_desiredStateQdot[uIndex] = targetVel;
} }
break; break;
@@ -300,26 +305,29 @@ void RobotControlExample::stepSimulation(float deltaTime)
bool hasStatus = m_physicsClient.processServerStatus(status); bool hasStatus = m_physicsClient.processServerStatus(status);
if (hasStatus && status.m_type == CMD_URDF_LOADING_COMPLETED) 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; b3JointInfo info;
m_physicsClient.getPoweredJointInfo(i,info); m_physicsClient.getJointInfo(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); b3Printf("Joint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex);
if (m_numMotors<MAX_NUM_MOTORS) if (info.m_flags & JOINT_HAS_MOTORIZED_POWER)
{ {
char motorName[1024]; if (m_numMotors<MAX_NUM_MOTORS)
sprintf(motorName,"%s q'", info.m_jointName); {
MyMotorInfo* motorInfo = &m_motorTargetVelocities[m_numMotors]; char motorName[1024];
motorInfo->m_velTarget = 0.f; sprintf(motorName,"%s q'", info.m_jointName);
motorInfo->m_uIndex = info.m_uIndex; MyMotorInfo* motorInfo = &m_motorTargetVelocities[m_numMotors];
motorInfo->m_velTarget = 0.f;
SliderParams slider(motorName,&motorInfo->m_velTarget); motorInfo->m_uIndex = info.m_uIndex;
slider.m_minVal=-4;
slider.m_maxVal=4; SliderParams slider(motorName,&motorInfo->m_velTarget);
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); slider.m_minVal=-4;
m_numMotors++; 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_DELETE_BOX_COLLISION_SHAPE,
// CMD_CREATE_RIGID_BODY, // CMD_CREATE_RIGID_BODY,
// CMD_DELETE_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_INIT_POSE,
CMD_SEND_PHYSICS_SIMULATION_PARAMETERS, 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_REQUEST_ACTUAL_STATE,
CMD_STEP_FORWARD_SIMULATION, CMD_STEP_FORWARD_SIMULATION,
CMD_SHUTDOWN, CMD_SHUTDOWN,
@@ -42,7 +43,7 @@ enum EnumSharedMemoryClientCommand
enum EnumSharedMemoryServerStatus enum EnumSharedMemoryServerStatus
{ {
CMD_SHARED_MEMORY_NOT_INITIALIZED, CMD_SHARED_MEMORY_NOT_INITIALIZED=0,
CMD_WAITING_FOR_CLIENT_COMMAND, CMD_WAITING_FOR_CLIENT_COMMAND,
//CMD_CLIENT_COMMAND_COMPLETED is a generic 'completed' status that doesn't need special handling on the client //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 SHARED_MEMORY_SERVER_TEST_C
#define MAX_DEGREE_OF_FREEDOM 1024 #define MAX_DEGREE_OF_FREEDOM 256
#define MAX_NUM_SENSORS 1024 #define MAX_NUM_SENSORS 256
#define MAX_URDF_FILENAME_LENGTH 1024 #define MAX_URDF_FILENAME_LENGTH 1024
enum EnumUrdfArgsUpdateFlags enum EnumUrdfArgsUpdateFlags
@@ -105,7 +106,7 @@ struct SetJointFeedbackArgs
//todo: discuss and decide about control mode and combinations //todo: discuss and decide about control mode and combinations
enum { enum {
// POSITION_CONTROL=0, // POSITION_CONTROL=0,
CONTROL_MODE_VELOCITY, CONTROL_MODE_VELOCITY=0,
CONTROL_MODE_TORQUE, CONTROL_MODE_TORQUE,
}; };
@@ -173,13 +174,38 @@ struct SendActualStateArgs
//actual state is only written by the server, read-only access by client is expected //actual state is only written by the server, read-only access by client is expected
double m_actualStateQ[MAX_DEGREE_OF_FREEDOM]; double m_actualStateQ[MAX_DEGREE_OF_FREEDOM];
double m_actualStateQdot[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; 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 struct SharedMemoryCommand
{ {
@@ -199,6 +225,8 @@ struct SharedMemoryCommand
struct BulletDataStreamArgs m_dataStreamArguments; struct BulletDataStreamArgs m_dataStreamArguments;
struct SendDesiredStateArgs m_sendDesiredStateCommandArgument; struct SendDesiredStateArgs m_sendDesiredStateCommandArgument;
struct RequestActualStateArgs m_requestActualStateInformationCommandArgument; struct RequestActualStateArgs m_requestActualStateInformationCommandArgument;
struct CreateSensorArgs m_createSensorArguments;
struct CreateBoxShapeArgs m_createBoxShapeArguments;
}; };
}; };
@@ -219,15 +247,22 @@ struct SharedMemoryStatus
typedef struct SharedMemoryStatus SharedMemoryStatus_t; typedef struct SharedMemoryStatus SharedMemoryStatus_t;
struct PoweredJointInfo enum JointInfoFlags
{
JOINT_HAS_MOTORIZED_POWER=1,
};
struct b3JointInfo
{ {
char* m_linkName; char* m_linkName;
char* m_jointName; char* m_jointName;
int m_jointType; int m_jointType;
int m_qIndex; int m_qIndex;
int m_uIndex; int m_uIndex;
///
int m_flags;
}; };
#endif //SHARED_MEMORY_COMMANDS_H #endif //SHARED_MEMORY_COMMANDS_H

10
src/Bullet3Common/b3Logging.h
View File

@@ -2,6 +2,10 @@
#ifndef B3_LOGGING_H #ifndef B3_LOGGING_H
#define 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 ///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 ///You can also customize the message by uncommenting out a different line below
#define b3Printf(...) b3OutputPrintfVarArgsInternal(__VA_ARGS__) #define b3Printf(...) b3OutputPrintfVarArgsInternal(__VA_ARGS__)
@@ -18,6 +22,7 @@
void b3EnterProfileZone(const char* name); void b3EnterProfileZone(const char* name);
void b3LeaveProfileZone(); void b3LeaveProfileZone();
#ifdef __cplusplus
class b3ProfileZone class b3ProfileZone
{ {
@@ -34,6 +39,7 @@ public:
}; };
#define B3_PROFILE( name ) b3ProfileZone __profile( name ) #define B3_PROFILE( name ) b3ProfileZone __profile( name )
#endif
#else //B3_NO_PROFILE #else //B3_NO_PROFILE
@@ -64,6 +70,8 @@ void b3OutputPrintfVarArgsInternal(const char *str, ...);
void b3OutputWarningMessageVarArgsInternal(const char *str, ...); void b3OutputWarningMessageVarArgsInternal(const char *str, ...);
void b3OutputErrorMessageVarArgsInternal(const char *str, ...); void b3OutputErrorMessageVarArgsInternal(const char *str, ...);
#ifdef __cplusplus
}
#endif
#endif//B3_LOGGING_H #endif//B3_LOGGING_H

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

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

@@ -52,7 +52,8 @@ public:
btScalar getMarginNonVirtual () const; btScalar getMarginNonVirtual () const;
void getAabbNonVirtual (const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) 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 //notice that the vectors should be unit length

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

@@ -846,8 +846,8 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt,
btVector3 linkAppliedTorque =isConstraintPass ? m_links[i].m_appliedConstraintTorque : m_links[i].m_appliedTorque; btVector3 linkAppliedTorque =isConstraintPass ? m_links[i].m_appliedConstraintTorque : m_links[i].m_appliedTorque;
zeroAccSpatFrc[i+1].setVector(-(rot_from_world[i+1] * linkAppliedTorque), -(rot_from_world[i+1] * linkAppliedForce )); 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", 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[1],
zeroAccSpatFrc[i+1].m_bottomVec[2]); zeroAccSpatFrc[i+1].m_bottomVec[2]);
} }
#endif
// //
//adding damping terms (only) //adding damping terms (only)
btScalar linDampMult = 1., angDampMult = 1.; btScalar linDampMult = 1., angDampMult = 1.;

112
test/SharedMemory/test.c
View File

@@ -1,6 +1,8 @@
#include "SharedMemoryBlock.h" #include "SharedMemoryBlock.h"
#include "PhysicsClientC_API.h" #include "PhysicsClientC_API.h"
#include "SharedMemoryCommands.h" #include "SharedMemoryCommands.h"
#include "Bullet3Common/b3Logging.h"
#include <string.h>
struct test struct test
{ {
@@ -8,17 +10,21 @@ struct test
}; };
#include <stdio.h> #include <stdio.h>
#define MAX_TIMEOUT 1024 #define MAX_TIMEOUT 1024*1024*1024
int main(int argc, char* argv[]) int main(int argc, char* argv[])
{ {
int ret ,allowSharedMemoryInitialization=0; int ret ,allowSharedMemoryInitialization=0;
int timeout = MAX_TIMEOUT; int timeout = MAX_TIMEOUT;
int sensorJointIndexLeft=-1;
int sensorJointIndexRight=-1;
const char* urdfFileName = "r2d2.urdf"; 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 timeStep = 1./60.;
double startPosX, startPosY,startPosZ; double startPosX, startPosY,startPosZ;
b3Printf("timeout = %d\n",timeout);
SharedMemoryCommand_t command; SharedMemoryCommand_t command;
SharedMemoryStatus_t status; SharedMemoryStatus_t status;
@@ -34,7 +40,8 @@ int main(int argc, char* argv[])
ret = b3SubmitClientCommand(sm, &command); ret = b3SubmitClientCommand(sm, &command);
timeout = MAX_TIMEOUT; timeout = MAX_TIMEOUT;
while ((timeout-- > 0) && b3ProcessServerStatus(sm, &status)==0) {} while ((timeout-- > 0) && b3ProcessServerStatus(sm, &status)==0) {}
b3Printf("timeout = %d\n",timeout);
ret = b3LoadUrdfCommandInit(&command, urdfFileName); ret = b3LoadUrdfCommandInit(&command, urdfFileName);
//setting the initial position, orientation and other arguments are optional //setting the initial position, orientation and other arguments are optional
@@ -46,14 +53,103 @@ int main(int argc, char* argv[])
ret = b3SubmitClientCommand(sm, &command); ret = b3SubmitClientCommand(sm, &command);
timeout = MAX_TIMEOUT; timeout = MAX_TIMEOUT;
while ((timeout-- > 0) && b3ProcessServerStatus(sm, &status)==0) {} 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;
ret = b3InitStepSimulationCommand(&command); 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); ret = b3SubmitClientCommand(sm, &command);
timeout = MAX_TIMEOUT; timeout = MAX_TIMEOUT;
while ((timeout-- > 0) && b3ProcessServerStatus(sm, &status)==0) {} 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); b3DisconnectSharedMemory(sm);
} }