diff --git a/examples/SharedMemory/PhysicsClient.cpp b/examples/SharedMemory/PhysicsClient.cpp index 70ae962b2..37d01c739 100644 --- a/examples/SharedMemory/PhysicsClient.cpp +++ b/examples/SharedMemory/PhysicsClient.cpp @@ -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; diff --git a/examples/SharedMemory/PhysicsServer.cpp b/examples/SharedMemory/PhysicsServer.cpp index 3a72a907e..1f0165d4f 100644 --- a/examples/SharedMemory/PhysicsServer.cpp +++ b/examples/SharedMemory/PhysicsServer.cpp @@ -15,6 +15,8 @@ class PhysicsServer : public SharedMemoryCommon { SharedMemoryInterface* m_sharedMemory; SharedMemoryExampleData* m_testBlock1; + + btAlignedObjectArray 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;igetNumConstraints();i++) + { + btTypedConstraint* c = m_dynamicsWorld->getConstraint(i); + btJointFeedback* fb = new btJointFeedback(); + m_jointFeedbacks.push_back(fb); + c->setJointFeedback(fb); - return true; + } + 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) @@ -202,7 +228,24 @@ 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;im_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: diff --git a/examples/SharedMemory/SharedMemoryInterface.h b/examples/SharedMemory/SharedMemoryInterface.h index cd060788d..c69dad4ec 100644 --- a/examples/SharedMemory/SharedMemoryInterface.h +++ b/examples/SharedMemory/SharedMemoryInterface.h @@ -31,6 +31,8 @@ enum SharedMemoryClientCommand{ struct UrdfCommandArgument { char m_urdfFileName[MAX_URDF_FILENAME_LENGTH]; + bool m_useMultiBody; + bool m_useFixedBase; }; struct StepSimulationCommandArgument