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

prepare for joint/torque feedback. First for btRigidBody, then btMultiBody.

Browse Source
This commit is contained in:
erwin coumans
2015-06-09 18:13:05 -07:00
parent 2c3db1c631
commit 2d79dda032
3 changed files with 52 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
examples/SharedMemory/PhysicsClient.cpp
View File

@@ -225,6 +225,9 @@ void PhysicsClient::stepSimulation(float deltaTime)
{
m_testBlock1->m_clientCommands[0].m_type =CMD_LOAD_URDF;
sprintf(m_testBlock1->m_clientCommands[0].m_urdfArguments.m_urdfFileName,"r2d2.urdf");
m_testBlock1->m_clientCommands[0].m_urdfArguments.m_useFixedBase = false;
m_testBlock1->m_clientCommands[0].m_urdfArguments.m_useMultiBody = true;
m_testBlock1->m_numClientCommands++;
b3Printf("Client created CMD_LOAD_URDF");
m_waitingForServer = true;

49
examples/SharedMemory/PhysicsServer.cpp
View File

@@ -15,6 +15,8 @@ class PhysicsServer : public SharedMemoryCommon
{
SharedMemoryInterface* m_sharedMemory;
SharedMemoryExampleData* m_testBlock1;
btAlignedObjectArray<btJointFeedback*> m_jointFeedbacks;
bool m_wantsShutdown;
public:
@@ -88,8 +90,10 @@ void PhysicsServer::initPhysics()
m_guiHelper->setUpAxis(upAxis);
createEmptyDynamicsWorld();
//todo: create a special debug drawer that will cache the lines, so we can send the debug info over the wire
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
btVector3 grav(0,0,0);
grav[upAxis] = -10;
grav[upAxis] = -9.8;
this->m_dynamicsWorld->setGravity(grav);
m_testBlock1 = (SharedMemoryExampleData*) m_sharedMemory->allocateSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE);
@@ -142,8 +146,29 @@ bool PhysicsServer::loadUrdf(const char* fileName, const btVector3& pos, const b
ConvertURDF2Bullet(u2b,creation, tr,m_dynamicsWorld,useMultiBody,u2b.getPathPrefix());
btMultiBody* mb = creation.getBulletMultiBody();
if (useMultiBody)
{
if (mb)
{
return true;
} else
{
b3Warning("No multibody loaded from URDF");
return false;
}
} else
{
for (int i=0;i<m_dynamicsWorld->getNumConstraints();i++)
{
btTypedConstraint* c = m_dynamicsWorld->getConstraint(i);
btJointFeedback* fb = new btJointFeedback();
m_jointFeedbacks.push_back(fb);
c->setJointFeedback(fb);
}
return true;
}
}
return false;
@@ -176,8 +201,9 @@ void PhysicsServer::stepSimulation(float deltaTime)
//load the actual URDF and send a report: completed or failed
bool completedOk = loadUrdf(clientCmd.m_urdfArguments.m_urdfFileName,
btVector3(0,0,0), btQuaternion(0,0,0,1),true,true );
btVector3(0,0,0), btQuaternion(0,0,0,1),clientCmd.m_urdfArguments.m_useMultiBody,clientCmd.m_urdfArguments.m_useFixedBase);
SharedMemoryCommand& serverCmd =m_testBlock1->m_serverCommands[0];
if (completedOk)
@@ -203,6 +229,23 @@ void PhysicsServer::stepSimulation(float deltaTime)
serverCmd.m_type =CMD_STEP_FORWARD_SIMULATION_COMPLETED;
m_testBlock1->m_numServerCommands++;
//now we send back the actual q, q' and force/torque and IMU sensor values
for (int i=0;i<m_jointFeedbacks.size();i++)
{
printf("Applied force A:(%f,%f,%f), torque A:(%f,%f,%f)\nForce B:(%f,%f,%f), torque B:(%f,%f,%f)\n",
m_jointFeedbacks[i]->m_appliedForceBodyA.x(),
m_jointFeedbacks[i]->m_appliedForceBodyA.y(),
m_jointFeedbacks[i]->m_appliedForceBodyA.z(),
m_jointFeedbacks[i]->m_appliedTorqueBodyA.x(),
m_jointFeedbacks[i]->m_appliedTorqueBodyA.y(),
m_jointFeedbacks[i]->m_appliedTorqueBodyA.z(),
m_jointFeedbacks[i]->m_appliedForceBodyB.x(),
m_jointFeedbacks[i]->m_appliedForceBodyB.y(),
m_jointFeedbacks[i]->m_appliedForceBodyB.z(),
m_jointFeedbacks[i]->m_appliedTorqueBodyB.x(),
m_jointFeedbacks[i]->m_appliedTorqueBodyB.y(),
m_jointFeedbacks[i]->m_appliedTorqueBodyB.z());
}
break;
}
case CMD_SHUTDOWN:

2
examples/SharedMemory/SharedMemoryInterface.h
View File

@@ -31,6 +31,8 @@ enum SharedMemoryClientCommand{
struct UrdfCommandArgument
{
char m_urdfFileName[MAX_URDF_FILENAME_LENGTH];
bool m_useMultiBody;
bool m_useFixedBase;
};
struct StepSimulationCommandArgument