diff --git a/examples/SharedMemory/PhysicsClient.cpp b/examples/SharedMemory/PhysicsClient.cpp index 90c64a986..774f6097a 100644 --- a/examples/SharedMemory/PhysicsClient.cpp +++ b/examples/SharedMemory/PhysicsClient.cpp @@ -64,6 +64,7 @@ void MyCallback(int buttonId, bool buttonState, void* userPtr) case CMD_REQUEST_ACTUAL_STATE: case CMD_STEP_FORWARD_SIMULATION: case CMD_SHUTDOWN: + case CMD_SEND_DESIRED_STATE: case CMD_SEND_BULLET_DATA_STREAM: { cl->submitCommand(buttonId); @@ -133,7 +134,7 @@ void PhysicsClient::initPhysics() { 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_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); @@ -235,6 +236,10 @@ void PhysicsClient::processServerCommands() } break; } + case CMD_DESIRED_STATE_RECEIVED_COMPLETED: + { + break; + } case CMD_STEP_FORWARD_SIMULATION_COMPLETED: { break; @@ -335,6 +340,7 @@ void PhysicsClient::createClientCommand() switch (command) { + case CMD_LOAD_URDF: { if (!m_serverLoadUrdfOK) @@ -434,6 +440,51 @@ void PhysicsClient::createClientCommand() } break; } + case CMD_SEND_DESIRED_STATE: + { + if (m_serverLoadUrdfOK) + { + b3Printf("Sending desired state (pos, vel, torque)\n"); + m_testBlock1->m_clientCommands[0].m_type =CMD_SEND_DESIRED_STATE; + //todo: expose a drop box in the GUI for this + int controlMode = CONTROL_MODE_VELOCITY;//CONTROL_MODE_TORQUE; + + switch (controlMode) + { + case CONTROL_MODE_VELOCITY: + { + m_testBlock1->m_clientCommands[0].m_sendDesiredStateCommandArgument.m_controlMode = CONTROL_MODE_VELOCITY; + for (int i=0;im_desiredStateQdot[i] = 1; + m_testBlock1->m_desiredStateForceTorque[i] = 100; + } + break; + } + case CONTROL_MODE_TORQUE: + { + m_testBlock1->m_clientCommands[0].m_sendDesiredStateCommandArgument.m_controlMode = CONTROL_MODE_TORQUE; + for (int i=0;im_desiredStateForceTorque[i] = 100; + } + break; + } + default: + { + b3Printf("Unknown control mode in client CMD_SEND_DESIRED_STATE"); + btAssert(0); + } + } + + m_testBlock1->m_numClientCommands++; + + } else + { + b3Warning("Cannot send CMD_SEND_DESIRED_STATE, no URDF loaded\n"); + } + break; + } case CMD_STEP_FORWARD_SIMULATION: { if (m_serverLoadUrdfOK) diff --git a/examples/SharedMemory/PhysicsServer.cpp b/examples/SharedMemory/PhysicsServer.cpp index 3258deae1..2a7344c32 100644 --- a/examples/SharedMemory/PhysicsServer.cpp +++ b/examples/SharedMemory/PhysicsServer.cpp @@ -10,6 +10,7 @@ #include "../Importers/ImportURDFDemo/MyMultiBodyCreator.h" #include "../Importers/ImportURDFDemo/URDF2Bullet.h" #include "../Extras/Serialize/BulletWorldImporter/btBulletWorldImporter.h" +#include "BulletDynamics/Featherstone/btMultiBodyJointMotor.h" #include "SharedMemoryCommon.h" //const char* blaatnaam = "basename"; @@ -31,9 +32,12 @@ class PhysicsServer : public SharedMemoryCommon btAlignedObjectArray m_jointFeedbacks; btAlignedObjectArray m_worldImporters; btAlignedObjectArray m_urdfLinkNameMapper; + btHashMap m_multiBodyJointMotorMap; btAlignedObjectArray m_strings; bool m_wantsShutdown; + void createJointMotors(btMultiBody* body); + bool supportsJointMotor(btMultiBody* body, int linkIndex); public: PhysicsServer(GUIHelperInterface* helper); @@ -110,7 +114,7 @@ void PhysicsServer::initPhysics() //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] = -9.8; + grav[upAxis] = 0;//-9.8; this->m_dynamicsWorld->setGravity(grav); m_testBlock1 = (SharedMemoryExampleData*) m_sharedMemory->allocateSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE); @@ -143,6 +147,36 @@ bool PhysicsServer::wantsTermination() return m_wantsShutdown; } + +bool PhysicsServer::supportsJointMotor(btMultiBody* mb, int mbLinkIndex) +{ + bool canHaveMotor = (mb->getLink(mbLinkIndex).m_jointType==btMultibodyLink::eRevolute + ||mb->getLink(mbLinkIndex).m_jointType==btMultibodyLink::ePrismatic); + return canHaveMotor; + +} + +//for testing, create joint motors for revolute and prismatic joints +void PhysicsServer::createJointMotors(btMultiBody* mb) +{ + int numLinks = mb->getNumLinks(); + for (int i=0;isetMaxAppliedImpulse(0); + m_multiBodyJointMotorMap.insert(mbLinkIndex,motor); + m_dynamicsWorld->addMultiBodyConstraint(motor); + } + + } +} bool PhysicsServer::loadUrdf(const char* fileName, const btVector3& pos, const btQuaternion& orn, bool useMultiBody, bool useFixedBase) { @@ -167,6 +201,9 @@ bool PhysicsServer::loadUrdf(const char* fileName, const btVector3& pos, const b { if (mb) { + createJointMotors(mb); + + //serialize the btMultiBody and send the data to the client. This is one way to get the link/joint names across the (shared memory) wire UrdfLinkNameMapUtil* util = new UrdfLinkNameMapUtil; m_urdfLinkNameMapper.push_back(util); util->m_mb = mb; @@ -208,7 +245,7 @@ bool PhysicsServer::loadUrdf(const char* fileName, const btVector3& pos, const b return true; } else { - b3Warning("No multibody loaded from URDF"); + b3Warning("No multibody loaded from URDF. Could add btRigidBody+btTypedConstraint solution later."); return false; } } else @@ -307,6 +344,98 @@ void PhysicsServer::stepSimulation(float deltaTime) break; } + case CMD_SEND_DESIRED_STATE: + { + //for (int i=0;im_desiredStateForceTorque[i] = 100; + //} + + b3Printf("Processed CMD_SEND_DESIRED_STATE"); + if (m_dynamicsWorld->getNumMultibodies()>0) + { + btMultiBody* mb = m_dynamicsWorld->getMultiBody(0); + btAssert(mb); + + switch (clientCmd.m_sendDesiredStateCommandArgument.m_controlMode) + { + case CONTROL_MODE_TORQUE: + { + b3Printf("Using CONTROL_MODE_TORQUE"); + mb->clearForcesAndTorques(); + + int torqueIndex = 0; + btVector3 f(m_testBlock1->m_desiredStateForceTorque[0], + m_testBlock1->m_desiredStateForceTorque[1], + m_testBlock1->m_desiredStateForceTorque[2]); + btVector3 t(m_testBlock1->m_desiredStateForceTorque[3], + m_testBlock1->m_desiredStateForceTorque[4], + m_testBlock1->m_desiredStateForceTorque[5]); + torqueIndex+=6; + mb->addBaseForce(f); + mb->addBaseTorque(t); + for (int link=0;linkgetNumLinks();link++) + { + + for (int dof=0;dofgetLink(link).m_dofCount;dof++) + { + double torque = m_testBlock1->m_desiredStateForceTorque[torqueIndex]; + mb->addJointTorqueMultiDof(link,dof,torque); + torqueIndex++; + } + } + break; + } + case CONTROL_MODE_VELOCITY: + { + int numMotors = 0; + //find the joint motors and apply the desired velocity and maximum force/torque + if (m_dynamicsWorld->getNumMultibodies()>0) + { + btMultiBody* mb = m_dynamicsWorld->getMultiBody(0); + int dofIndex = 6;//skip the 3 linear + 3 angular degree of freedom entries of the base + for (int link=0;linkgetNumLinks();link++) + { + if (supportsJointMotor(mb,link)) + { + + btMultiBodyJointMotor** motorPtr = m_multiBodyJointMotorMap[link]; + if (motorPtr) + { + btMultiBodyJointMotor* motor = *motorPtr; + btScalar desiredVelocity = m_testBlock1->m_desiredStateQdot[dofIndex]; + motor->setVelocityTarget(desiredVelocity); + + btScalar physicsDeltaTime=1./60.;//todo: set the physicsDeltaTime explicitly in the API, separate from the 'stepSimulation' + + btScalar maxImp = m_testBlock1->m_desiredStateForceTorque[dofIndex]*physicsDeltaTime; + motor->setMaxAppliedImpulse(maxImp); + numMotors++; + + } + } + dofIndex += mb->getLink(link).m_dofCount; + } + } + b3Printf("Using CONTROL_MODE_TORQUE with %d motors", numMotors); + break; + } + default: + { + b3Printf("m_controlMode not implemented yet"); + break; + } + + } + } + + + + SharedMemoryCommand& serverCmd = m_testBlock1->m_serverCommands[0]; + serverCmd.m_type = CMD_DESIRED_STATE_RECEIVED_COMPLETED; + m_testBlock1->m_numServerCommands++; + break; + } case CMD_REQUEST_ACTUAL_STATE: { b3Printf("Sending the actual state (Q,U)"); diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 8e71cc3a3..5a886e1e8 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -63,9 +63,17 @@ struct SetJointFeedbackArgs bool m_isEnabled; }; +//todo: discuss and decide about control mode and combinations +enum { +// POSITION_CONTROL=0, + CONTROL_MODE_VELOCITY, + CONTROL_MODE_TORQUE, +}; + struct SendDesiredStateArgs { int m_bodyUniqueId; + int m_controlMode; }; struct RequestActualStateArgs @@ -109,7 +117,7 @@ struct SharedMemoryCommand UrdfArgs m_urdfArguments; BulletDataStreamArgs m_dataStreamArguments; StepSimulationArgs m_stepSimulationArguments; - SendDesiredStateArgs m_sendDesiredQandUStateCommandArgument; + SendDesiredStateArgs m_sendDesiredStateCommandArgument; RequestActualStateArgs m_requestActualStateInformationCommandArgument; SendActualStateArgs m_sendActualStateArgs; }; diff --git a/examples/SharedMemory/SharedMemoryInterface.h b/examples/SharedMemory/SharedMemoryInterface.h index e8e34b3d6..35d59b55c 100644 --- a/examples/SharedMemory/SharedMemoryInterface.h +++ b/examples/SharedMemory/SharedMemoryInterface.h @@ -26,13 +26,22 @@ struct SharedMemoryExampleData //desired state is only written by the client, read-only access by server is expected double m_desiredStateQ[MAX_DEGREE_OF_FREEDOM]; double m_desiredStateQdot[MAX_DEGREE_OF_FREEDOM]; + + //m_desiredStateForceTorque is either the actual applied force/torque (in CONTROL_MODE_TORQUE) or + //or m_desiredStateForceTorque is the maximum applied force/torque for the motor/constraint to reach the desired velocity in CONTROL_MODE_VELOCITY mode + double m_desiredStateForceTorque[MAX_DEGREE_OF_FREEDOM]; //actual state is only written by the server, read-only access by client is expected double m_actualStateQ[MAX_DEGREE_OF_FREEDOM]; double m_actualStateQdot[MAX_DEGREE_OF_FREEDOM]; double m_actualStateSensors[MAX_NUM_SENSORS];//these are force sensors and IMU information + //m_bulletStreamDataClientToServer is a way for the client to create collision shapes, rigid bodies and constraints + //the Bullet data structures are more general purpose than the capabilities of a URDF file. char m_bulletStreamDataClientToServer[SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE]; + + //m_bulletStreamDataServerToClient is used to send (debug) data from server to client, for + //example to provide all details of a multibody including joint/link names, after loading a URDF file. char m_bulletStreamDataServerToClient[SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE]; };