diff --git a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp index a1e234434..d912b1f0a 100644 --- a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp +++ b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp @@ -457,7 +457,11 @@ void ConvertURDF2Bullet(const URDFImporterInterface& u2b, MultiBodyCreationInter mb->finalizeMultiDof(); mb->setBaseWorldTransform(rootTransformInWorldSpace); - + btAlignedObjectArray scratch_q; + btAlignedObjectArray scratch_m; + mb->forwardKinematics(scratch_q,scratch_m); + mb->updateCollisionObjectWorldTransforms(scratch_q,scratch_m); + world1->addMultiBody(mb); } } diff --git a/examples/SharedMemory/PhysicsClient.cpp b/examples/SharedMemory/PhysicsClient.cpp index 5bbf80376..db3040c7d 100644 --- a/examples/SharedMemory/PhysicsClient.cpp +++ b/examples/SharedMemory/PhysicsClient.cpp @@ -8,15 +8,26 @@ #include "../../Extras/Serialize/BulletFileLoader/autogenerated/bullet.h" +//copied from btMultiBodyLink.h +enum JointType +{ + eRevoluteType = 0, + ePrismaticType = 1, +}; + struct PhysicsClientSharedMemoryInternalData { SharedMemoryInterface* m_sharedMemory; SharedMemoryExampleData* m_testBlock1; + btAlignedObjectArray m_robotMultiBodyData; + btAlignedObjectArray m_poweredJointInfo; + int m_counter; bool m_serverLoadUrdfOK; bool m_isConnected; bool m_waitingForServer; + bool m_hasLastServerStatus; PhysicsClientSharedMemoryInternalData() :m_sharedMemory(0), @@ -24,7 +35,8 @@ struct PhysicsClientSharedMemoryInternalData m_counter(0), m_serverLoadUrdfOK(false), m_isConnected(false), - m_waitingForServer(false) + m_waitingForServer(false), + m_hasLastServerStatus(false) { } @@ -37,6 +49,18 @@ struct PhysicsClientSharedMemoryInternalData }; + +int PhysicsClientSharedMemory::getNumPoweredJoints() const +{ + return m_data->m_poweredJointInfo.size(); +} + +void PhysicsClientSharedMemory::getPoweredJointInfo(int index, PoweredJointInfo& info) const +{ + info = m_data->m_poweredJointInfo[index]; +} + + PhysicsClientSharedMemory::PhysicsClientSharedMemory() { @@ -99,16 +123,19 @@ bool PhysicsClientSharedMemory::connect(bool allowSharedMemoryInitialization) -void PhysicsClientSharedMemory::processServerStatus() +bool PhysicsClientSharedMemory::processServerStatus(ServerStatus& serverStatus) { btAssert(m_data->m_testBlock1); - + bool hasStatus = false; + if (m_data->m_testBlock1->m_numServerCommands> m_data->m_testBlock1->m_numProcessedServerCommands) { btAssert(m_data->m_testBlock1->m_numServerCommands==m_data->m_testBlock1->m_numProcessedServerCommands+1); const SharedMemoryCommand& serverCmd =m_data->m_testBlock1->m_serverCommands[0]; - + hasStatus = true; + serverStatus = serverCmd; + //consume the command switch (serverCmd.m_type) { @@ -123,10 +150,14 @@ void PhysicsClientSharedMemory::processServerStatus() bParse::btBulletFile* bf = new bParse::btBulletFile(this->m_data->m_testBlock1->m_bulletStreamDataServerToClient,serverCmd.m_dataStreamArguments.m_streamChunkLength); bf->setFileDNAisMemoryDNA(); bf->parse(false); + m_data->m_robotMultiBodyData.push_back(bf); + for (int i=0;im_multiBodies.size();i++) { int flag = bf->getFlags(); - + int qOffset = 7; + int uOffset=6; + if ((flag&bParse::FD_DOUBLE_PRECISION)!=0) { Bullet::btMultiBodyDoubleData* mb = (Bullet::btMultiBodyDoubleData*)bf->m_multiBodies[i]; @@ -136,15 +167,31 @@ void PhysicsClientSharedMemory::processServerStatus() } for (int link=0;linkm_numLinks;link++) { - if (mb->m_links[link].m_linkName) + if ((mb->m_links[link].m_jointType == eRevoluteType)|| + (mb->m_links[link].m_jointType == ePrismaticType)) { - b3Printf("mb->m_links[%d].m_linkName = %s\n",link,mb->m_links[link].m_linkName); - } - if (mb->m_links[link].m_jointName) - { - b3Printf("mb->m_links[%d].m_jointName = %s\n",link,mb->m_links[link].m_jointName); + PoweredJointInfo info; + info.m_qIndex = qOffset; + info.m_uIndex = uOffset; + + if (mb->m_links[link].m_linkName) + { + b3Printf("mb->m_links[%d].m_linkName = %s\n",link,mb->m_links[link].m_linkName); + info.m_linkName = mb->m_links[link].m_linkName; + } + if (mb->m_links[link].m_jointName) + { + b3Printf("mb->m_links[%d].m_jointName = %s\n",link,mb->m_links[link].m_jointName); + 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); } + qOffset+= mb->m_links[link].m_posVarCount; + uOffset+= mb->m_links[link].m_dofCount; + } + } else { Bullet::btMultiBodyFloatData* mb = (Bullet::btMultiBodyFloatData*) bf->m_multiBodies[i]; @@ -154,19 +201,39 @@ void PhysicsClientSharedMemory::processServerStatus() } for (int link=0;linkm_numLinks;link++) { - if (mb->m_links[link].m_linkName) + if ((mb->m_links[link].m_jointType == eRevoluteType)|| + (mb->m_links[link].m_jointType == ePrismaticType)) { - b3Printf("mb->m_links[%d].m_linkName = %s\n",link,mb->m_links[link].m_linkName); - } - b3Printf("link [%d] type = %d",link, mb->m_links[link].m_jointType); - if (mb->m_links[link].m_jointName) - { - b3Printf("mb->m_links[%d].m_jointName = %s\n",link,mb->m_links[link].m_jointName); + PoweredJointInfo info; + info.m_qIndex = qOffset; + info.m_uIndex = uOffset; + + if (mb->m_links[link].m_linkName) + { + b3Printf("mb->m_links[%d].m_linkName = %s\n",link,mb->m_links[link].m_linkName); + info.m_linkName = mb->m_links[link].m_linkName; + } + if (mb->m_links[link].m_jointName) + { + b3Printf("mb->m_links[%d].m_jointName = %s\n",link,mb->m_links[link].m_jointName); + 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); } + qOffset+= mb->m_links[link].m_posVarCount; + uOffset+= mb->m_links[link].m_dofCount; } + } } - printf("ok!\n"); + if (bf->ok()) + { + b3Printf("Received robot description ok!\n"); + } else + { + b3Warning("Robot description not received"); + } } break; } @@ -212,21 +279,42 @@ void PhysicsClientSharedMemory::processServerStatus() b3Printf("size Q = %d, size U = %d\n", numQ,numU); char msg[1024]; - sprintf(msg,"Q=["); - - for (int i=0;im_waitingForServer = true; } } + return hasStatus; } bool PhysicsClientSharedMemory::canSubmitCommand() const @@ -378,7 +467,7 @@ bool PhysicsClientSharedMemory::submitClientCommand(const SharedMemoryCommand& c { m_data->m_testBlock1->m_clientCommands[0].m_type =CMD_STEP_FORWARD_SIMULATION; - m_data->m_testBlock1->m_clientCommands[0].m_stepSimulationArguments.m_deltaTimeInSeconds = 1./60.; + // m_data->m_testBlock1->m_clientCommands[0].m_stepSimulationArguments.m_deltaTimeInSeconds = 1./60.; m_data->m_testBlock1->m_numClientCommands++; b3Printf("client created CMD_STEP_FORWARD_SIMULATION %d\n", m_data->m_counter++); } else diff --git a/examples/SharedMemory/PhysicsClient.h b/examples/SharedMemory/PhysicsClient.h index dd15ad97e..83e022364 100644 --- a/examples/SharedMemory/PhysicsClient.h +++ b/examples/SharedMemory/PhysicsClient.h @@ -3,6 +3,17 @@ #include "SharedMemoryCommands.h" +#include + +struct PoweredJointInfo +{ + std::string m_linkName; + std::string m_jointName; + int m_jointType; + int m_qIndex; + int m_uIndex; +}; + class PhysicsClientSharedMemory //: public CommonPhysicsClientInterface { @@ -14,22 +25,22 @@ public: PhysicsClientSharedMemory(); virtual ~PhysicsClientSharedMemory(); - //todo: implement 'allocateSharedMemory' from client side in 'connect' call + //return true if connection succesfull, can also check 'isConnected' virtual bool connect(bool allowSharedMemoryInitialization = true); virtual bool isConnected() const; - virtual void processServerStatus(); - - virtual bool getLastServerStatus(ServerStatus& status) - { - return false; - } + // return true if there is a status, and fill in 'serverStatus' + virtual bool processServerStatus(ServerStatus& serverStatus); virtual bool canSubmitCommand() const; virtual bool submitClientCommand(const SharedMemoryCommand& command); + virtual int getNumPoweredJoints() const; + + virtual void getPoweredJointInfo(int index, PoweredJointInfo& info) const; + }; #endif //BT_PHYSICS_CLIENT_API_H diff --git a/examples/SharedMemory/PhysicsClientExample.cpp b/examples/SharedMemory/PhysicsClientExample.cpp index caf899980..da9327c83 100644 --- a/examples/SharedMemory/PhysicsClientExample.cpp +++ b/examples/SharedMemory/PhysicsClientExample.cpp @@ -211,7 +211,18 @@ void PhysicsClientExample::stepSimulation(float deltaTime) if (m_physicsClient.isConnected()) { - m_physicsClient.processServerStatus(); + ServerStatus status; + bool hasStatus = m_physicsClient.processServerStatus(status); + if (hasStatus && status.m_type == CMD_URDF_LOADING_COMPLETED) + { + for (int i=0;i m_jointFeedbacks; btAlignedObjectArray m_worldImporters; @@ -49,6 +49,7 @@ struct PhysicsServerInternalData :m_sharedMemory(0), m_testBlock1(0), m_isConnected(false), + m_physicsDeltaTime(1./60.), m_dynamicsWorld(0), m_guiHelper(0) { @@ -426,9 +427,7 @@ void PhysicsServerSharedMemory::processClientCommands() btScalar desiredVelocity = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[dofIndex]; motor->setVelocityTarget(desiredVelocity); - btScalar physicsDeltaTime=1./60.;//todo: set the physicsDeltaTime explicitly in the API, separate from the 'stepSimulation' - - btScalar maxImp = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex]*physicsDeltaTime; + btScalar maxImp = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex]*m_data->m_physicsDeltaTime; motor->setMaxAppliedImpulse(maxImp); numMotors++; @@ -552,8 +551,7 @@ void PhysicsServerSharedMemory::processClientCommands() { b3Printf("Step simulation request"); - double timeStep = clientCmd.m_stepSimulationArguments.m_deltaTimeInSeconds; - m_data->m_dynamicsWorld->stepSimulation(timeStep); + m_data->m_dynamicsWorld->stepSimulation(m_data->m_physicsDeltaTime); SharedMemoryCommand& serverCmd =m_data->m_testBlock1->m_serverCommands[0]; diff --git a/examples/SharedMemory/RobotControlExample.cpp b/examples/SharedMemory/RobotControlExample.cpp index 777181c66..b38f0a0ef 100644 --- a/examples/SharedMemory/RobotControlExample.cpp +++ b/examples/SharedMemory/RobotControlExample.cpp @@ -14,7 +14,13 @@ //const char* blaatnaam = "basename"; - +struct MyMotorInfo +{ + btScalar m_velTarget; + btScalar m_maxForce; + int m_uIndex; +}; +#define MAX_NUM_MOTORS 128 class RobotControlExample : public SharedMemoryCommon { @@ -27,10 +33,15 @@ class RobotControlExample : public SharedMemoryCommon btAlignedObjectArray m_userCommandRequests; + void createButton(const char* name, int id, bool isTrigger ); public: - + //@todo, add accessor methods + MyMotorInfo m_motorTargetVelocities[MAX_NUM_MOTORS]; + int m_numMotors; + + RobotControlExample(GUIHelperInterface* helper); virtual ~RobotControlExample(); @@ -73,7 +84,7 @@ void MyCallback2(int buttonId, bool buttonState, void* userPtr) case CMD_LOAD_URDF: { command.m_type =CMD_LOAD_URDF; - sprintf(command.m_urdfArguments.m_urdfFileName,"hinge.urdf");//r2d2.urdf"); + sprintf(command.m_urdfArguments.m_urdfFileName,"r2d2.urdf"); command.m_urdfArguments.m_initialPosition[0] = 0.0; command.m_urdfArguments.m_initialPosition[1] = 0.0; command.m_urdfArguments.m_initialPosition[2] = 0.0; @@ -107,6 +118,7 @@ void MyCallback2(int buttonId, bool buttonState, void* userPtr) case CMD_SEND_DESIRED_STATE: { + command.m_type =CMD_SEND_DESIRED_STATE; int controlMode = CONTROL_MODE_VELOCITY;//CONTROL_MODE_TORQUE; @@ -118,8 +130,15 @@ void MyCallback2(int buttonId, bool buttonState, void* userPtr) { for (int i=0;im_numMotors;i++) + { + btScalar targetVel = cl->m_motorTargetVelocities[i].m_velTarget; + int uIndex = cl->m_motorTargetVelocities[i].m_uIndex; + command.m_sendDesiredStateCommandArgument.m_desiredStateQdot[uIndex] = targetVel; + } break; } @@ -165,7 +184,8 @@ void RobotControlExample::createButton(const char* name, int buttonId, bool isTr RobotControlExample::RobotControlExample(GUIHelperInterface* helper) :SharedMemoryCommon(helper), m_wantsShutdown(false), -m_sequenceNumberGenerator(0) +m_sequenceNumberGenerator(0), +m_numMotors(0) { bool useServer = true; @@ -243,8 +263,36 @@ void RobotControlExample::stepSimulation(float deltaTime) if (m_physicsClient.isConnected()) { - m_physicsClient.processServerStatus(); - + + ServerStatus status; + bool hasStatus = m_physicsClient.processServerStatus(status); + if (hasStatus && status.m_type == CMD_URDF_LOADING_COMPLETED) + { + for (int i=0;im_velTarget = 0.f; + motorInfo->m_uIndex = info.m_uIndex; + + SliderParams slider(motorName,&motorInfo->m_velTarget); + slider.m_minVal=-4; + slider.m_maxVal=4; + m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); + m_numMotors++; + } + + } + } + + if (m_physicsClient.canSubmitCommand()) { if (m_userCommandRequests.size()) diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index c579f29b1..7299d692b 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -72,6 +72,7 @@ struct UrdfArgs struct BulletDataStreamArgs { int m_streamChunkLength; + int m_bodyUniqueId; }; struct SetJointFeedbackArgs @@ -108,17 +109,7 @@ 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 @@ -136,10 +127,6 @@ struct SendActualStateArgs -struct StepSimulationArgs -{ - double m_deltaTimeInSeconds; -}; struct SharedMemoryCommand { @@ -152,8 +139,7 @@ struct SharedMemoryCommand { UrdfArgs m_urdfArguments; BulletDataStreamArgs m_dataStreamArguments; - StepSimulationArgs m_stepSimulationArguments; - SendDesiredStateArgs m_sendDesiredStateCommandArgument; + SendDesiredStateArgs m_sendDesiredStateCommandArgument; RequestActualStateArgs m_requestActualStateInformationCommandArgument; SendActualStateArgs m_sendActualStateArgs; }; diff --git a/src/BulletDynamics/Featherstone/btMultiBody.cpp b/src/BulletDynamics/Featherstone/btMultiBody.cpp index e81a5846b..1b6e0b9b5 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBody.cpp @@ -2424,6 +2424,58 @@ void btMultiBody::forwardKinematics(btAlignedObjectArray& world_to } +void btMultiBody::updateCollisionObjectWorldTransforms(btAlignedObjectArray& world_to_local,btAlignedObjectArray& local_origin) +{ + world_to_local.resize(getNumLinks()+1); + local_origin.resize(getNumLinks()+1); + + world_to_local[0] = getWorldToBaseRot(); + local_origin[0] = getBasePos(); + + if (getBaseCollider()) + { + btVector3 posr = local_origin[0]; + // float pos[4]={posr.x(),posr.y(),posr.z(),1}; + btScalar quat[4]={-world_to_local[0].x(),-world_to_local[0].y(),-world_to_local[0].z(),world_to_local[0].w()}; + btTransform tr; + tr.setIdentity(); + tr.setOrigin(posr); + tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3])); + + getBaseCollider()->setWorldTransform(tr); + + } + + for (int k=0;km_link; + btAssert(link == m); + + int index = link+1; + + btVector3 posr = local_origin[index]; + // float pos[4]={posr.x(),posr.y(),posr.z(),1}; + btScalar quat[4]={-world_to_local[index].x(),-world_to_local[index].y(),-world_to_local[index].z(),world_to_local[index].w()}; + btTransform tr; + tr.setIdentity(); + tr.setOrigin(posr); + tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3])); + + col->setWorldTransform(tr); + } + } +} int btMultiBody::calculateSerializeBufferSize() const { @@ -2458,6 +2510,8 @@ const char* btMultiBody::serialize(void* dataBuffer, class btSerializer* seriali memPtr->m_jointType = getLink(i).m_jointType; memPtr->m_dofCount = getLink(i).m_dofCount; + memPtr->m_posVarCount = getLink(i).m_posVarCount; + getLink(i).m_inertiaLocal.serialize(memPtr->m_linkInertia); memPtr->m_linkMass = getLink(i).m_mass; memPtr->m_parentIndex = getLink(i).m_parent; diff --git a/src/BulletDynamics/Featherstone/btMultiBody.h b/src/BulletDynamics/Featherstone/btMultiBody.h index 9938149eb..b81f6335f 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.h +++ b/src/BulletDynamics/Featherstone/btMultiBody.h @@ -599,6 +599,8 @@ void addJointTorque(int i, btScalar Q); return m_internalNeedsJointFeedback; } void forwardKinematics(btAlignedObjectArray& scratch_q,btAlignedObjectArray& scratch_m); + + void updateCollisionObjectWorldTransforms(btAlignedObjectArray& scratch_q,btAlignedObjectArray& scratch_m); virtual int calculateSerializeBufferSize() const; diff --git a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp index d6a9d2468..fa4052d46 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp @@ -759,9 +759,7 @@ void btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep) int nLinks = bod->getNumLinks(); ///base + num m_links - world_to_local.resize(nLinks+1); - local_origin.resize(nLinks+1); - + if(bod->isMultiDof()) { if(!bod->isPosUpdated()) @@ -776,54 +774,14 @@ void btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep) } } else + { bod->stepPositions(timeStep); - - world_to_local[0] = bod->getWorldToBaseRot(); - local_origin[0] = bod->getBasePos(); - - if (bod->getBaseCollider()) - { - btVector3 posr = local_origin[0]; - // float pos[4]={posr.x(),posr.y(),posr.z(),1}; - btScalar quat[4]={-world_to_local[0].x(),-world_to_local[0].y(),-world_to_local[0].z(),world_to_local[0].w()}; - btTransform tr; - tr.setIdentity(); - tr.setOrigin(posr); - tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3])); - - bod->getBaseCollider()->setWorldTransform(tr); - - } - - for (int k=0;kgetNumLinks();k++) - { - const int parent = bod->getParent(k); - world_to_local[k+1] = bod->getParentToLocalRot(k) * world_to_local[parent+1]; - local_origin[k+1] = local_origin[parent+1] + (quatRotate(world_to_local[k+1].inverse() , bod->getRVector(k))); } + world_to_local.resize(nLinks+1); + local_origin.resize(nLinks+1); - - for (int m=0;mgetNumLinks();m++) - { - btMultiBodyLinkCollider* col = bod->getLink(m).m_collider; - if (col) - { - int link = col->m_link; - btAssert(link == m); - - int index = link+1; - - btVector3 posr = local_origin[index]; - // float pos[4]={posr.x(),posr.y(),posr.z(),1}; - btScalar quat[4]={-world_to_local[index].x(),-world_to_local[index].y(),-world_to_local[index].z(),world_to_local[index].w()}; - btTransform tr; - tr.setIdentity(); - tr.setOrigin(posr); - tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3])); - - col->setWorldTransform(tr); - } - } + bod->updateCollisionObjectWorldTransforms(world_to_local,local_origin); + } else { bod->clearVelocities();