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

more work-in-progress on joint-torque sensor sample and server/client shared memory API

Browse Source
This commit is contained in:
erwin coumans
2015-06-22 15:30:57 -07:00
parent 4688540a98
commit b14afba350
6 changed files with 260 additions and 143 deletions
Download Patch File Download Diff File Expand all files Collapse all files

45
examples/MultiBody/TestJointTorqueSetup.cpp
View File

@@ -6,7 +6,7 @@
#include "../CommonInterfaces/CommonMultiBodyBase.h"
btScalar radius(0.2);
struct TestJointTorqueSetup : public CommonMultiBodyBase
{
@@ -78,17 +78,21 @@ void TestJointTorqueSetup::initPhysics()
//create a static ground object
if (1)
{
btVector3 groundHalfExtents(20,20,20);
btVector3 groundHalfExtents(1,1,0.2);
groundHalfExtents[upAxis]=1.f;
btBoxShape* box = new btBoxShape(groundHalfExtents);
box->initializePolyhedralFeatures();
m_guiHelper->createCollisionShapeGraphicsObject(box);
btTransform start; start.setIdentity();
btVector3 groundOrigin(0,0,0);
groundOrigin[upAxis]=-1.5;
btVector3 groundOrigin(-0.4f, 3.f, 0.f);
btVector3 basePosition = btVector3(-0.4f, 3.f, 0.f);
groundOrigin[upAxis] -=.5;
groundOrigin[2]-=0.6;
start.setOrigin(groundOrigin);
btRigidBody* body = createRigidBody(0,start,box);
body->setFriction(0);
btVector4 color = colors[curColor];
curColor++;
curColor&=3;
@@ -99,7 +103,7 @@ void TestJointTorqueSetup::initPhysics()
bool floating = false;
bool damping = true;
bool gyro = false;
int numLinks = 1;
int numLinks = 2;
bool spherical = false; //set it ot false -to use 1DoF hinges instead of 3DoF sphericals
bool canSleep = false;
bool selfCollide = false;
@@ -161,11 +165,25 @@ void TestJointTorqueSetup::initPhysics()
{
//pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), hingeJointAxis, parentComToCurrentPivot, currentPivotToCurrentCom, false);
if (i==0)
{
pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, i - 1,
btQuaternion(0.f, 0.f, 0.f, 1.f),
hingeJointAxis,
parentComToCurrentPivot,
currentPivotToCurrentCom, false);
} else
{
btVector3 parentComToCurrentCom(0, -radius * 2.f, 0); //par body's COM to cur body's COM offset
btVector3 currentPivotToCurrentCom(0, -radius, 0); //cur body's COM to cur body's PIV offset
btVector3 parentComToCurrentPivot = parentComToCurrentCom - currentPivotToCurrentCom; //par body's COM to cur body's PIV offset
pMultiBody->setupFixed(i, linkMass, linkInertiaDiag, i - 1,
btQuaternion(0.f, 0.f, 0.f, 1.f),
parentComToCurrentPivot,
currentPivotToCurrentCom, false);
}
//pMultiBody->setupFixed(i,linkMass,linkInertiaDiag,i-1,btQuaternion(0,0,0,1),parentComToCurrentPivot,currentPivotToCurrentCom,false);
@@ -255,7 +273,9 @@ void TestJointTorqueSetup::initPhysics()
//when syncing the btMultiBody link transforms to the btMultiBodyLinkCollider
tr.setOrigin(local_origin[0]);
tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3]));
btQuaternion orn(btVector3(0,0,1),0.25*3.1415926538);
tr.setRotation(orn);
col->setWorldTransform(tr);
bool isDynamic = (baseMass > 0 && floating);
@@ -290,8 +310,17 @@ void TestJointTorqueSetup::initPhysics()
// float pos[4]={posr.x(),posr.y(),posr.z(),1};
float quat[4]={-world_to_local[i+1].x(),-world_to_local[i+1].y(),-world_to_local[i+1].z(),world_to_local[i+1].w()};
btCollisionShape* shape =0;
if (i==0)
{
shape = new btBoxShape(btVector3(linkHalfExtents[0],linkHalfExtents[1],linkHalfExtents[2]));//btSphereShape(linkHalfExtents[0]);
} else
{
shape = new btSphereShape(radius);
}
btCollisionShape* shape = new btBoxShape(btVector3(linkHalfExtents[0],linkHalfExtents[1],linkHalfExtents[2]));//btSphereShape(linkHalfExtents[0]);
m_guiHelper->createCollisionShapeGraphicsObject(shape);
btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, i);
@@ -326,7 +355,7 @@ void TestJointTorqueSetup::initPhysics()
void TestJointTorqueSetup::stepSimulation(float deltaTime)
{
//m_multiBody->addLinkForce(0,btVector3(100,100,100));
if (0)//m_once)
if (1)//m_once)
{
m_once=false;
m_multiBody->addJointTorque(0, 10.0);

290
examples/SharedMemory/PhysicsClient.cpp
View File

@@ -10,6 +10,7 @@
class PhysicsClient : public SharedMemoryCommon
{
protected:
SharedMemoryInterface* m_sharedMemory;
SharedMemoryExampleData* m_testBlock1;
int m_counter;
@@ -19,7 +20,8 @@ class PhysicsClient : public SharedMemoryCommon
bool m_serverLoadUrdfOK;
bool m_waitingForServer;
void processServerCommands();
void createClientCommand();
public:
@@ -99,7 +101,7 @@ m_wantsTermination(false),
m_serverLoadUrdfOK(false),
m_waitingForServer(false)
{
b3Printf("Started PhysicsClient");
b3Printf("Started PhysicsClient\n");
#ifdef _WIN32
m_sharedMemory = new Win32SharedMemoryClient();
#else
@@ -109,7 +111,7 @@ m_waitingForServer(false)
PhysicsClient::~PhysicsClient()
{
b3Printf("~PhysicsClient");
b3Printf("~PhysicsClient\n");
m_sharedMemory->releaseSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE);
delete m_sharedMemory;
}
@@ -117,45 +119,61 @@ PhysicsClient::~PhysicsClient()
void PhysicsClient::initPhysics()
{
if (m_guiHelper && m_guiHelper->getParameterInterface())
{
bool isTrigger = false;
ButtonParams button("Load URDF",CMD_LOAD_URDF, isTrigger);
button.m_callback = MyCallback;
button.m_userPointer = this;
m_guiHelper->getParameterInterface()->registerButtonParameter(button);
}
{
bool isTrigger = false;
ButtonParams button("Load URDF",CMD_LOAD_URDF, isTrigger);
button.m_callback = MyCallback;
button.m_userPointer = this;
m_guiHelper->getParameterInterface()->registerButtonParameter(button);
}
{
bool isTrigger = false;
ButtonParams button("Step Sim",CMD_STEP_FORWARD_SIMULATION, isTrigger);
button.m_callback = MyCallback;
button.m_userPointer = this;
m_guiHelper->getParameterInterface()->registerButtonParameter(button);
}
{
bool isTrigger = false;
ButtonParams button("Step Sim",CMD_STEP_FORWARD_SIMULATION, isTrigger);
button.m_callback = MyCallback;
button.m_userPointer = this;
m_guiHelper->getParameterInterface()->registerButtonParameter(button);
}
{
bool isTrigger = false;
ButtonParams button("Get State",CMD_REQUEST_ACTUAL_STATE, isTrigger);
button.m_callback = MyCallback;
button.m_userPointer = this;
m_guiHelper->getParameterInterface()->registerButtonParameter(button);
}
{
bool isTrigger = false;
ButtonParams button("Send Desired State",CMD_SEND_DESIRED_STATE, isTrigger);
button.m_callback = MyCallback;
button.m_userPointer = this;
m_guiHelper->getParameterInterface()->registerButtonParameter(button);
}
{
bool isTrigger = false;
ButtonParams button("Shut Down",CMD_SHUTDOWN, isTrigger);
button.m_callback = MyCallback;
button.m_userPointer = this;
m_guiHelper->getParameterInterface()->registerButtonParameter(button);
}
} else
{
bool isTrigger = false;
ButtonParams button("Get State",CMD_REQUEST_ACTUAL_STATE, isTrigger);
button.m_callback = MyCallback;
button.m_userPointer = this;
m_guiHelper->getParameterInterface()->registerButtonParameter(button);
}
/*
{
bool isTrigger = false;
ButtonParams button("Send Desired State",CMD_SEND_DESIRED_STATE, isTrigger);
button.m_callback = MyCallback;
button.m_userPointer = this;
m_guiHelper->getParameterInterface()->registerButtonParameter(button);
}
*/
{
bool isTrigger = false;
ButtonParams button("Shut Down",CMD_SHUTDOWN, isTrigger);
button.m_callback = MyCallback;
button.m_userPointer = this;
m_guiHelper->getParameterInterface()->registerButtonParameter(button);
}
m_userCommandRequests.push_back(CMD_LOAD_URDF);
m_userCommandRequests.push_back(CMD_REQUEST_ACTUAL_STATE);
//m_userCommandRequests.push_back(CMD_SEND_DESIRED_STATE);
m_userCommandRequests.push_back(CMD_REQUEST_ACTUAL_STATE);
//m_userCommandRequests.push_back(CMD_SET_JOINT_FEEDBACK);
//m_userCommandRequests.push_back(CMD_CREATE_BOX_COLLISION_SHAPE);
//m_userCommandRequests.push_back(CMD_CREATE_RIGID_BODY);
m_userCommandRequests.push_back(CMD_STEP_FORWARD_SIMULATION);
m_userCommandRequests.push_back(CMD_REQUEST_ACTUAL_STATE);
m_userCommandRequests.push_back(CMD_SHUTDOWN);
}
m_testBlock1 = (SharedMemoryExampleData*)m_sharedMemory->allocateSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE);
@@ -164,106 +182,119 @@ void PhysicsClient::initPhysics()
// btAssert(m_testBlock1->m_magicId == SHARED_MEMORY_MAGIC_NUMBER);
if (m_testBlock1->m_magicId !=SHARED_MEMORY_MAGIC_NUMBER)
{
b3Error("Error: please start server before client");
b3Error("Error: please start server before client\n");
m_sharedMemory->releaseSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE);
m_testBlock1 = 0;
} else
{
b3Printf("Shared Memory status is OK");
b3Printf("Shared Memory status is OK\n");
}
}
} else
{
m_wantsTermination = true;
}
}
void PhysicsClient::stepSimulation(float deltaTime)
void PhysicsClient::processServerCommands()
{
btAssert(m_testBlock1);
if (m_testBlock1->m_numServerCommands> m_testBlock1->m_numProcessedServerCommands)
{
btAssert(m_testBlock1->m_numServerCommands==m_testBlock1->m_numProcessedServerCommands+1);
if (m_testBlock1)
{
//check progress and submit further commands
//we ignore overflow right now
const SharedMemoryCommand& serverCmd =m_testBlock1->m_serverCommands[0];
if (m_testBlock1->m_numServerCommands> m_testBlock1->m_numProcessedServerCommands)
{
btAssert(m_testBlock1->m_numServerCommands==m_testBlock1->m_numProcessedServerCommands+1);
//consume the command
switch (serverCmd.m_type)
{
const SharedMemoryCommand& serverCmd =m_testBlock1->m_serverCommands[0];
//consume the command
switch (serverCmd.m_type)
{
case CMD_URDF_LOADING_COMPLETED:
case CMD_URDF_LOADING_COMPLETED:
{
m_serverLoadUrdfOK = true;
b3Printf("Server loading the URDF OK\n");
break;
}
case CMD_STEP_FORWARD_SIMULATION_COMPLETED:
{
break;
}
case CMD_URDF_LOADING_FAILED:
{
b3Printf("Server failed loading the URDF...\n");
m_serverLoadUrdfOK = false;
break;
}
case CMD_ACTUAL_STATE_UPDATE_COMPLETED:
{
m_serverLoadUrdfOK = true;
b3Printf("Server loading the URDF OK");
b3Printf("Received actual state\n");
int numQ = m_testBlock1->m_serverCommands[0].m_sendActualStateArgs.m_numDegreeOfFreedomQ;
int numU = m_testBlock1->m_serverCommands[0].m_sendActualStateArgs.m_numDegreeOfFreedomU;
b3Printf("size Q = %d, size U = %d\n", numQ,numU);
char msg[1024];
sprintf(msg,"Q=[");
for (int i=0;i<numQ;i++)
{
if (i<numQ-1)
{
sprintf(msg,"%s%f,",msg,m_testBlock1->m_actualStateQ[i]);
} else
{
sprintf(msg,"%s%f",msg,m_testBlock1->m_actualStateQ[i]);
}
}
sprintf(msg,"%s]",msg);
b3Printf(msg);
b3Printf("\n");
break;
}
case CMD_STEP_FORWARD_SIMULATION_COMPLETED:
{
break;
}
case CMD_URDF_LOADING_FAILED:
{
b3Printf("Server failed loading the URDF...");
m_serverLoadUrdfOK = false;
break;
}
case CMD_ACTUAL_STATE_UPDATE_COMPLETED:
{
b3Printf("Received actual state");
int numQ = m_testBlock1->m_serverCommands[0].m_sendActualStateArgs.m_numDegreeOfFreedomQ;
int numU = m_testBlock1->m_serverCommands[0].m_sendActualStateArgs.m_numDegreeOfFreedomU;
b3Printf("size Q = %d, size U = %d\n", numQ,numU);
char msg[1024];
sprintf(msg,"Q=[");
for (int i=0;i<numQ;i++)
{
if (i<numQ-1)
{
sprintf(msg,"%s%f,",msg,m_testBlock1->m_actualStateQ[i]);
} else
{
sprintf(msg,"%s%f",msg,m_testBlock1->m_actualStateQ[i]);
}
}
sprintf(msg,"%s]",msg);
b3Printf(msg);
break;
}
default:
{
b3Error("Unknown server command");
btAssert(0);
}
};
m_testBlock1->m_numProcessedServerCommands++;
if (m_testBlock1->m_numServerCommands == m_testBlock1->m_numProcessedServerCommands)
default:
{
m_waitingForServer = false;
} else
{
m_waitingForServer = true;
b3Error("Unknown server command\n");
btAssert(0);
}
}
};
if (!m_waitingForServer)
m_testBlock1->m_numProcessedServerCommands++;
//we don't have more than 1 command outstanding (in total, either server or client)
btAssert(m_testBlock1->m_numProcessedServerCommands == m_testBlock1->m_numServerCommands);
if (m_testBlock1->m_numServerCommands == m_testBlock1->m_numProcessedServerCommands)
{
m_waitingForServer = false;
} else
{
m_waitingForServer = true;
}
}
}
void PhysicsClient::createClientCommand()
{
if (!m_waitingForServer)
{
//process outstanding requests
if (m_userCommandRequests.size())
{
b3Printf("Outstanding user command requests: %d\n", m_userCommandRequests.size());
int command = m_userCommandRequests[0];
m_userCommandRequests.remove(command);
//don't use 'remove' because it will re-order the commands
//m_userCommandRequests.remove(command);
//pop_front
for (int i=1;i<m_userCommandRequests.size();i++)
{
m_userCommandRequests[i-1] = m_userCommandRequests[i];
}
m_userCommandRequests.pop_back();
m_waitingForServer = true;
switch (command)
{
case CMD_LOAD_URDF:
@@ -276,11 +307,10 @@ void PhysicsClient::stepSimulation(float deltaTime)
m_testBlock1->m_clientCommands[0].m_urdfArguments.m_useMultiBody = true;
m_testBlock1->m_numClientCommands++;
b3Printf("Client created CMD_LOAD_URDF");
m_waitingForServer = true;
b3Printf("Client created CMD_LOAD_URDF\n");
} else
{
b3Warning("Server already loaded URDF, no client command submitted");
b3Warning("Server already loaded URDF, no client command submitted\n");
}
break;
}
@@ -288,13 +318,13 @@ void PhysicsClient::stepSimulation(float deltaTime)
{
if (m_serverLoadUrdfOK)
{
b3Printf("Requesting actual state");
b3Printf("Requesting actual state\n");
m_testBlock1->m_clientCommands[0].m_type =CMD_REQUEST_ACTUAL_STATE;
m_testBlock1->m_numClientCommands++;
} else
{
b3Warning("No URDF loaded");
b3Warning("No URDF loaded\n");
}
break;
}
@@ -307,10 +337,9 @@ void PhysicsClient::stepSimulation(float deltaTime)
m_testBlock1->m_clientCommands[0].m_stepSimulationArguments.m_deltaTimeInSeconds = 1./60.;
m_testBlock1->m_numClientCommands++;
b3Printf("client created CMD_STEP_FORWARD_SIMULATION %d\n", m_counter++);
m_waitingForServer = true;
} else
{
b3Warning("No URDF loaded yet, no client CMD_STEP_FORWARD_SIMULATION submitted");
b3Warning("No URDF loaded yet, no client CMD_STEP_FORWARD_SIMULATION submitted\n");
}
break;
}
@@ -319,20 +348,33 @@ void PhysicsClient::stepSimulation(float deltaTime)
m_wantsTermination = true;
m_testBlock1->m_clientCommands[0].m_type =CMD_SHUTDOWN;
m_testBlock1->m_numClientCommands++;
m_waitingForServer = true;
m_serverLoadUrdfOK = false;
b3Printf("client created CMD_SHUTDOWN\n");
break;
}
default:
{
b3Error("unknown command requested");
b3Error("unknown command requested\n");
}
}
}
}
}
}
void PhysicsClient::stepSimulation(float deltaTime)
{
btAssert(m_testBlock1);
if (m_testBlock1)
{
processServerCommands();
if (!m_waitingForServer)
{
createClientCommand();
}
}
}

6
examples/SharedMemory/PhysicsServer.cpp
View File

@@ -62,8 +62,10 @@ m_wantsShutdown(false)
void PhysicsServer::releaseSharedMemory()
{
b3Printf("releaseSharedMemory1\n");
if (m_testBlock1)
{
b3Printf("m_testBlock1\n");
m_testBlock1->m_magicId = 0;
b3Printf("magic id = %d\n",m_testBlock1->m_magicId);
btAssert(m_sharedMemory);
@@ -71,7 +73,7 @@ void PhysicsServer::releaseSharedMemory()
}
if (m_sharedMemory)
{
b3Printf("m_sharedMemory\n");
delete m_sharedMemory;
m_sharedMemory = 0;
m_testBlock1 = 0;
@@ -343,6 +345,8 @@ void PhysicsServer::stepSimulation(float deltaTime)
}
if (wantsShutdown)
{
b3Printf("releaseSharedMemory!\n");
m_wantsShutdown = true;
releaseSharedMemory();
}

39
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -4,19 +4,29 @@
//this is a very experimental draft of commands. We will iterate on this API (commands, arguments etc)
enum SharedMemoryServerCommand{
enum SharedMemoryServerCommand
{
CMD_URDF_LOADING_COMPLETED,
CMD_URDF_LOADING_FAILED,
CMD_BOX_COLLISION_SHAPE_CREATION_COMPLETED,
CMD_RIGID_BODY_CREATION_COMPLETED,
CMD_SET_JOINT_FEEDBACK_COMPLETED,
CMD_ACTUAL_STATE_UPDATE_COMPLETED,
// CMD_DESIRED_STATE_RECEIVED_COMPLETED,
CMD_DESIRED_STATE_RECEIVED_COMPLETED,
CMD_STEP_FORWARD_SIMULATION_COMPLETED,
CMD_MAX_SERVER_COMMANDS
};
enum SharedMemoryClientCommand{
enum SharedMemoryClientCommand
{
CMD_LOAD_URDF,
CMD_CREATE_BOX_COLLISION_SHAPE,
CMD_DELETE_BOX_COLLISION_SHAPE,
CMD_CREATE_RIGID_BODY,
CMD_DELETE_RIGID_BODY,
CMD_SET_JOINT_FEEDBACK,///enable or disable joint feedback
CMD_SEND_DESIRED_STATE,
CMD_REQUEST_ACTUAL_STATE,
// CMD_SEND_DESIRED_STATE,
CMD_STEP_FORWARD_SIMULATION, //includes CMD_REQUEST_STATE
CMD_SHUTDOWN,
CMD_MAX_CLIENT_COMMANDS
@@ -35,6 +45,13 @@ struct UrdfArgs
bool m_useFixedBase;
};
struct SetJointFeedbackArgs
{
int m_bodyUniqueId;
int m_linkId;
bool m_isEnabled;
};
struct SendDesiredStateArgs
{
int m_bodyUniqueId;
@@ -45,6 +62,19 @@ struct RequestActualStateArgs
int m_bodyUniqueId;
};
struct CreateBoxShapeArgs
{
double m_halfExtents[3];
};
struct CreateRigidBodyArgs
{
int m_shapeId;
double m_initialPosition[3];
double m_initialOrientation[3];
};
struct SendActualStateArgs
{
int m_bodyUniqueId;
@@ -53,6 +83,7 @@ struct SendActualStateArgs
};
struct StepSimulationArgs
{
double m_deltaTimeInSeconds;

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

@@ -1027,8 +1027,10 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt,
m_internalNeedsJointFeedback = true;
m_links[i].m_jointFeedback->m_spatialInertia = spatInertia[i+1];
m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec = rot_from_parent[0].transpose()*(spatInertia[i+1]*spatAcc[i+1]+zeroAccSpatFrc[i+1]).m_bottomVec;
m_links[i].m_jointFeedback->m_reactionForces.m_topVec = rot_from_parent[0].transpose()*(spatInertia[i+1]*spatAcc[i+1]+zeroAccSpatFrc[i+1]).m_topVec;
m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec = rot_from_parent[i+1].transpose()*(spatInertia[i+1]*spatAcc[i+1]+zeroAccSpatFrc[i+1]).m_bottomVec;
m_links[i].m_jointFeedback->m_reactionForces.m_topVec = rot_from_parent[i+1].transpose()*(spatInertia[i+1]*spatAcc[i+1]+zeroAccSpatFrc[i+1]).m_topVec;
//m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec = (spatInertia[i+1]*spatAcc[i+1]+zeroAccSpatFrc[i+1]).m_bottomVec;
//m_links[i].m_jointFeedback->m_reactionForces.m_topVec = (spatInertia[i+1]*spatAcc[i+1]+zeroAccSpatFrc[i+1]).m_topVec;
}
}

9
src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp
View File

@@ -858,6 +858,15 @@ void btMultiBodyDynamicsWorld::debugDrawWorld()
btVector3 to = tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
getDebugDrawer()->drawLine(from,to,color);
}
if (bod->getLink(m).m_jointType==btMultibodyLink::eFixed)
{
btVector3 vec = quatRotate(tr.getRotation(),bod->getLink(m).m_axes[0].m_bottomVec);
btVector4 color(0,0,0,1);//1,1,1);
btVector3 from = vec+tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
btVector3 to = tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
getDebugDrawer()->drawLine(from,to,color);
}
if (bod->getLink(m).m_jointType==btMultibodyLink::ePrismatic)
{
btVector3 vec = quatRotate(tr.getRotation(),bod->getLink(m).m_axes[0].m_bottomVec);