diff --git a/data/hinge.urdf b/data/hinge.urdf index 121eac702..c3ca40b84 100644 --- a/data/hinge.urdf +++ b/data/hinge.urdf @@ -2,48 +2,72 @@ - + - + - + - + - + - - - + + + - + - + - + - + - - + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp index a0ae8396e..edef28e17 100644 --- a/examples/ExampleBrowser/ExampleEntries.cpp +++ b/examples/ExampleBrowser/ExampleEntries.cpp @@ -204,8 +204,10 @@ static ExampleEntry gDefaultExamples[]= ExampleEntry(0,"Experiments"), - ExampleEntry(1,"Robot Control", "Perform some robot control tasks, using physics server and client that communicate over shared memory", - RobotControlExampleCreateFunc), + ExampleEntry(1,"Robot Control (Velocity)", "Perform some robot control tasks, using physics server and client that communicate over shared memory", + RobotControlExampleCreateFunc,ROBOT_VELOCITY_CONTROL), + ExampleEntry(1,"Robot Control (PD)", "Perform some robot control tasks, using physics server and client that communicate over shared memory", + RobotControlExampleCreateFunc,ROBOT_PD_CONTROL), ExampleEntry(1,"Physics Server", "Create a physics server that communicates with a physics client over shared memory", PhysicsServerCreateFunc), diff --git a/examples/ExampleBrowser/GwenGUISupport/GwenParameterInterface.cpp b/examples/ExampleBrowser/GwenGUISupport/GwenParameterInterface.cpp index 50ec24648..02c4cda11 100644 --- a/examples/ExampleBrowser/GwenGUISupport/GwenParameterInterface.cpp +++ b/examples/ExampleBrowser/GwenGUISupport/GwenParameterInterface.cpp @@ -139,7 +139,7 @@ void GwenParameterInterface::registerButtonParameter(ButtonParams& params) m_paramInternalData->m_buttonEventHandlers.push_back(handler); button->SetPos( 5, m_gwenInternalData->m_curYposition ); - button->SetWidth(120); + button->SetWidth(220); m_gwenInternalData->m_curYposition+=22; @@ -152,7 +152,7 @@ void GwenParameterInterface::registerSliderFloatParameter(SliderParams& params) //m_data->m_myControls.push_back(label); label->SetText( params.m_name); label->SetPos( 10, 10 + 25 ); - label->SetWidth(110); + label->SetWidth(210); label->SetPos(10,m_gwenInternalData->m_curYposition); m_gwenInternalData->m_curYposition+=22; @@ -160,7 +160,7 @@ void GwenParameterInterface::registerSliderFloatParameter(SliderParams& params) m_paramInternalData->m_sliders.push_back(pSlider); //m_data->m_myControls.push_back(pSlider); pSlider->SetPos( 10, m_gwenInternalData->m_curYposition ); - pSlider->SetSize( 100, 20 ); + pSlider->SetSize( 200, 20 ); pSlider->SetRange( params.m_minVal, params.m_maxVal); pSlider->SetNotchCount(128);//float(params.m_maxVal-params.m_minVal)/100.f); pSlider->SetClampToNotches( params.m_clampToNotches ); diff --git a/examples/ExampleBrowser/GwenGUISupport/gwenUserInterface.cpp b/examples/ExampleBrowser/GwenGUISupport/gwenUserInterface.cpp index bc73c6f08..678bae4cc 100644 --- a/examples/ExampleBrowser/GwenGUISupport/gwenUserInterface.cpp +++ b/examples/ExampleBrowser/GwenGUISupport/gwenUserInterface.cpp @@ -286,7 +286,7 @@ void GwenUserInterface::init(int width, int height,Gwen::Renderer::Base* rendere */ Gwen::Controls::ScrollControl* windowRight= new Gwen::Controls::ScrollControl(m_data->pCanvas); windowRight->Dock(Gwen::Pos::Right); - windowRight->SetWidth(150); + windowRight->SetWidth(250); windowRight->SetHeight(250); windowRight->SetScroll(false,true); @@ -296,7 +296,7 @@ void GwenUserInterface::init(int width, int height,Gwen::Renderer::Base* rendere Gwen::Controls::TabControl* tab = new Gwen::Controls::TabControl(windowRight); //tab->SetHeight(300); - tab->SetWidth(140); + tab->SetWidth(240); tab->SetHeight(1250); //tab->Dock(Gwen::Pos::Left); tab->Dock( Gwen::Pos::Fill ); @@ -438,7 +438,7 @@ void GwenUserInterface::registerToggleButton(int buttonId, const char* name) ///some heuristic to find the button location int ypos = m_data->m_curYposition; but->SetPos(10, ypos ); - but->SetWidth( 100 ); + but->SetWidth( 200 ); //but->SetBounds( 200, 30, 300, 200 ); MyButtonHander* handler = new MyButtonHander(m_data, buttonId); diff --git a/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp b/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp index 4ca11ec55..c75a6e2da 100644 --- a/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp +++ b/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp @@ -356,7 +356,7 @@ void ImportUrdfSetup::initPhysics() - bool createGround=true; + bool createGround=false; if (createGround) { btVector3 groundHalfExtents(20,20,20); diff --git a/examples/MultiBody/InvertedPendulumPDControl.cpp b/examples/MultiBody/InvertedPendulumPDControl.cpp index bd8115cad..59c8b5770 100644 --- a/examples/MultiBody/InvertedPendulumPDControl.cpp +++ b/examples/MultiBody/InvertedPendulumPDControl.cpp @@ -112,21 +112,21 @@ void InvertedPendulumPDControl::initPhysics() m_dynamicsWorld->setGravity(btVector3(0,-10,0)); { - bool floating = false; + bool fixedBase = true; bool damping = false; bool gyro = false; int numLinks = 2; bool spherical = false; //set it ot false -to use 1DoF hinges instead of 3DoF sphericals bool canSleep = false; bool selfCollide = false; - btVector3 linkHalfExtents(0.05, 0.37, 0.1); + btVector3 linkHalfExtents(0.05, 0.37, 0.1); btVector3 baseHalfExtents(0.04, 0.35, 0.08); btVector3 basePosition = btVector3(-0.4f, 3.f, 0.f); //mbC->forceMultiDof(); //if !spherical, you can comment this line to check the 1DoF algorithm //init the base btVector3 baseInertiaDiag(0.f, 0.f, 0.f); - float baseMass = 1.f; + float baseMass = 0.f; if(baseMass) { @@ -137,7 +137,7 @@ void InvertedPendulumPDControl::initPhysics() } bool isMultiDof = true; - btMultiBody *pMultiBody = new btMultiBody(numLinks, baseMass, baseInertiaDiag, !floating, canSleep, isMultiDof); + btMultiBody *pMultiBody = new btMultiBody(numLinks, 0, baseInertiaDiag, fixedBase, canSleep, isMultiDof); m_multiBody = pMultiBody; btQuaternion baseOriQuat(0.f, 0.f, 0.f, 1.f); @@ -295,7 +295,7 @@ void InvertedPendulumPDControl::initPhysics() tr.setRotation(orn); col->setWorldTransform(tr); - bool isDynamic = (baseMass > 0 && floating); + bool isDynamic = (baseMass > 0 && !fixedBase); short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(btBroadphaseProxy::StaticFilter); short collisionFilterMask = isDynamic? short(btBroadphaseProxy::AllFilter) : short(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter); @@ -391,19 +391,12 @@ void InvertedPendulumPDControl::stepSimulation(float deltaTime) int dof1 = 0; btScalar qActual = m_multiBody->getJointPosMultiDof(joint)[dof1]; btScalar qdActual = m_multiBody->getJointVelMultiDof(joint)[dof1]; - // b3Printf("Joint Pos[%d]=%f, Vel = %f\n", joint, qActual, qdActual); - btScalar positionError = (qDesiredArray[joint]-qActual); - - btScalar force = kp * positionError - kd*qdActual; + double desiredVelocity = 0; + btScalar velocityError = (desiredVelocity-qdActual); + btScalar force = kp * positionError + kd*velocityError; btClamp(force,-maxForce,maxForce); - - // b3Printf("force = %f positionError = %f, qDesired = %f\n", force,positionError, target); m_multiBody->addJointTorque(joint, force); - - //btScalar torque = m_multiBody->getJointTorque(0); - // b3Printf("t = %f,%f,%f\n",torque,torque,torque);//[0],torque[1],torque[2]); - } @@ -425,7 +418,7 @@ void InvertedPendulumPDControl::stepSimulation(float deltaTime) this->m_guiHelper->getAppInterface()->dumpNextFrameToPng(fileName); } } - m_dynamicsWorld->stepSimulation(1./240,0); + m_dynamicsWorld->stepSimulation(1./60.,0);//240,0); static int count = 0; if ((count& 0x0f)==0) diff --git a/examples/SharedMemory/PhysicsClient.cpp b/examples/SharedMemory/PhysicsClient.cpp index 547c0a436..9b493d0f4 100644 --- a/examples/SharedMemory/PhysicsClient.cpp +++ b/examples/SharedMemory/PhysicsClient.cpp @@ -2,6 +2,8 @@ #include "PosixSharedMemory.h" #include "Win32SharedMemory.h" #include "LinearMath/btAlignedObjectArray.h" +#include "LinearMath/btVector3.h" + #include "Bullet3Common/b3Logging.h" #include "../Utils/b3ResourcePath.h" #include "../../Extras/Serialize/BulletFileLoader/btBulletFile.h" @@ -23,15 +25,19 @@ struct PhysicsClientSharedMemoryInternalData btAlignedObjectArray m_robotMultiBodyData; btAlignedObjectArray m_jointInfo; + btAlignedObjectArray m_debugLinesFrom; + btAlignedObjectArray m_debugLinesTo; + btAlignedObjectArray m_debugLinesColor; - int m_sharedMemoryKey; int m_counter; bool m_serverLoadUrdfOK; bool m_isConnected; bool m_waitingForServer; bool m_hasLastServerStatus; - + int m_sharedMemoryKey; + bool m_verboseOutput; + PhysicsClientSharedMemoryInternalData() :m_sharedMemory(0), m_testBlock1(0), @@ -40,7 +46,8 @@ struct PhysicsClientSharedMemoryInternalData m_isConnected(false), m_waitingForServer(false), m_hasLastServerStatus(false), - m_sharedMemoryKey(SHARED_MEMORY_KEY) + m_sharedMemoryKey(SHARED_MEMORY_KEY), + m_verboseOutput(false) { } @@ -123,7 +130,10 @@ bool PhysicsClientSharedMemory::connect() return false; } else { - b3Printf("Connected to existing shared memory, status OK.\n"); + if (m_data->m_verboseOutput) + { + b3Printf("Connected to existing shared memory, status OK.\n"); + } m_data->m_isConnected = true; } } else @@ -166,13 +176,19 @@ bool PhysicsClientSharedMemory::processServerStatus(SharedMemoryStatus& serverSt { case CMD_CLIENT_COMMAND_COMPLETED: { - b3Printf("Server completed command"); + if (m_data->m_verboseOutput) + { + b3Printf("Server completed command"); + } break; } case CMD_URDF_LOADING_COMPLETED: { m_data->m_serverLoadUrdfOK = true; - b3Printf("Server loading the URDF OK\n"); + if (m_data->m_verboseOutput) + { + b3Printf("Server loading the URDF OK\n"); + } if (serverCmd.m_dataStreamArguments.m_streamChunkLength>0) { @@ -192,8 +208,12 @@ bool PhysicsClientSharedMemory::processServerStatus(SharedMemoryStatus& serverSt Bullet::btMultiBodyDoubleData* mb = (Bullet::btMultiBodyDoubleData*)bf->m_multiBodies[i]; if (mb->m_baseName) { - b3Printf("mb->m_baseName = %s\n",mb->m_baseName); + if (m_data->m_verboseOutput) + { + b3Printf("mb->m_baseName = %s\n",mb->m_baseName); + } } + for (int link=0;linkm_numLinks;link++) { { @@ -201,15 +221,22 @@ bool PhysicsClientSharedMemory::processServerStatus(SharedMemoryStatus& serverSt info.m_flags = 0; info.m_qIndex = qOffset; info.m_uIndex = uOffset; - + info.m_linkIndex = link; + if (mb->m_links[link].m_linkName) { - b3Printf("mb->m_links[%d].m_linkName = %s\n",link,mb->m_links[link].m_linkName); + if (m_data->m_verboseOutput) + { + 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); + if (m_data->m_verboseOutput) + { + 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; } @@ -230,7 +257,10 @@ bool PhysicsClientSharedMemory::processServerStatus(SharedMemoryStatus& serverSt Bullet::btMultiBodyFloatData* mb = (Bullet::btMultiBodyFloatData*) bf->m_multiBodies[i]; if (mb->m_baseName) { - b3Printf("mb->m_baseName = %s\n",mb->m_baseName); + if (m_data->m_verboseOutput) + { + b3Printf("mb->m_baseName = %s\n",mb->m_baseName); + } } for (int link=0;linkm_numLinks;link++) { @@ -242,12 +272,18 @@ bool PhysicsClientSharedMemory::processServerStatus(SharedMemoryStatus& serverSt if (mb->m_links[link].m_linkName) { - b3Printf("mb->m_links[%d].m_linkName = %s\n",link,mb->m_links[link].m_linkName); + if (m_data->m_verboseOutput) + { + 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); + if (m_data->m_verboseOutput) + { + 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; } @@ -266,7 +302,10 @@ bool PhysicsClientSharedMemory::processServerStatus(SharedMemoryStatus& serverSt } if (bf->ok()) { - b3Printf("Received robot description ok!\n"); + if (m_data->m_verboseOutput) + { + b3Printf("Received robot description ok!\n"); + } } else { b3Warning("Robot description not received"); @@ -276,24 +315,36 @@ bool PhysicsClientSharedMemory::processServerStatus(SharedMemoryStatus& serverSt } case CMD_DESIRED_STATE_RECEIVED_COMPLETED: { - b3Printf("Server received desired state"); + if (m_data->m_verboseOutput) + { + b3Printf("Server received desired state"); + } break; } case CMD_STEP_FORWARD_SIMULATION_COMPLETED: { - b3Printf("Server completed step simulation"); + if (m_data->m_verboseOutput) + { + b3Printf("Server completed step simulation"); + } break; } case CMD_URDF_LOADING_FAILED: { - b3Printf("Server failed loading the URDF...\n"); + if (m_data->m_verboseOutput) + { + b3Printf("Server failed loading the URDF...\n"); + } m_data->m_serverLoadUrdfOK = false; break; } case CMD_BULLET_DATA_STREAM_RECEIVED_COMPLETED: { - b3Printf("Server received bullet data stream OK\n"); + if (m_data->m_verboseOutput) + { + b3Printf("Server received bullet data stream OK\n"); + } @@ -302,7 +353,10 @@ bool PhysicsClientSharedMemory::processServerStatus(SharedMemoryStatus& serverSt } case CMD_BULLET_DATA_STREAM_RECEIVED_FAILED: { - b3Printf("Server failed receiving bullet data stream\n"); + if (m_data->m_verboseOutput) + { + b3Printf("Server failed receiving bullet data stream\n"); + } break; } @@ -310,12 +364,18 @@ bool PhysicsClientSharedMemory::processServerStatus(SharedMemoryStatus& serverSt case CMD_ACTUAL_STATE_UPDATE_COMPLETED: { - b3Printf("Received actual state\n"); + if (m_data->m_verboseOutput) + { + b3Printf("Received actual state\n"); + } SharedMemoryStatus& command = m_data->m_testBlock1->m_serverCommands[0]; int numQ = command.m_sendActualStateArgs.m_numDegreeOfFreedomQ; int numU = command.m_sendActualStateArgs.m_numDegreeOfFreedomU; - b3Printf("size Q = %d, size U = %d\n", numQ,numU); + if (m_data->m_verboseOutput) + { + b3Printf("size Q = %d, size U = %d\n", numQ,numU); + } char msg[1024]; { @@ -333,7 +393,10 @@ bool PhysicsClientSharedMemory::processServerStatus(SharedMemoryStatus& serverSt } sprintf(msg,"%s]",msg); } - b3Printf(msg); + if (m_data->m_verboseOutput) + { + b3Printf(msg); + } { sprintf(msg,"U=["); @@ -351,12 +414,52 @@ bool PhysicsClientSharedMemory::processServerStatus(SharedMemoryStatus& serverSt sprintf(msg,"%s]",msg); } - b3Printf(msg); + if (m_data->m_verboseOutput) + { + b3Printf(msg); + } - b3Printf("\n"); + if (m_data->m_verboseOutput) + { + b3Printf("\n"); + } break; } + case CMD_DEBUG_LINES_COMPLETED: + { + if (m_data->m_verboseOutput) + { + b3Printf("Success receiving %d debug lines",serverCmd.m_sendDebugLinesArgs.m_numDebugLines); + } + + int numLines = serverCmd.m_sendDebugLinesArgs.m_numDebugLines; + btVector3* linesFrom = (btVector3*)&m_data->m_testBlock1->m_bulletStreamDataServerToClient[0]; + btVector3* linesTo = (btVector3*)(&m_data->m_testBlock1->m_bulletStreamDataServerToClient[0]+numLines*sizeof(btVector3)); + btVector3* linesColor = (btVector3*)(&m_data->m_testBlock1->m_bulletStreamDataServerToClient[0]+2*numLines*sizeof(btVector3)); + + m_data->m_debugLinesFrom.resize(numLines); + m_data->m_debugLinesTo.resize(numLines); + m_data->m_debugLinesColor.resize(numLines); + + for (int i=0;im_debugLinesFrom[i] = linesFrom[i]; + m_data->m_debugLinesTo[i] = linesTo[i]; + m_data->m_debugLinesColor[i] = linesColor[i]; + } + break; + } + case CMD_DEBUG_LINES_OVERFLOW_FAILED: + { + b3Warning("Error receiving debug lines"); + m_data->m_debugLinesFrom.resize(0); + m_data->m_debugLinesTo.resize(0); + m_data->m_debugLinesColor.resize(0); + + break; + } + default: { b3Error("Unknown server status\n"); @@ -378,8 +481,11 @@ bool PhysicsClientSharedMemory::processServerStatus(SharedMemoryStatus& serverSt } } else { - b3Printf("m_numServerStatus = %d, processed = %d\n", m_data->m_testBlock1->m_numServerCommands, + if (m_data->m_verboseOutput) + { + b3Printf("m_numServerStatus = %d, processed = %d\n", m_data->m_testBlock1->m_numServerCommands, m_data->m_testBlock1->m_numProcessedServerCommands); + } } return hasStatus; } @@ -407,8 +513,8 @@ bool PhysicsClientSharedMemory::submitClientCommand(const SharedMemoryCommand& c m_data->m_robotMultiBodyData.clear(); m_data->m_jointInfo.clear(); - } + m_data->m_testBlock1->m_clientCommands[0] = command; m_data->m_testBlock1->m_numClientCommands++; m_data->m_waitingForServer = true; @@ -417,3 +523,47 @@ bool PhysicsClientSharedMemory::submitClientCommand(const SharedMemoryCommand& c return false; } +void PhysicsClientSharedMemory::uploadBulletFileToSharedMemory(const char* data, int len) +{ + btAssert(len=SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE) + { + b3Warning("uploadBulletFileToSharedMemory %d exceeds max size %d\n",len,SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); + } else + { + for (int i=0;im_testBlock1->m_bulletStreamDataClientToServer[i] = data[i]; + } + } +} + + +const btVector3* PhysicsClientSharedMemory::getDebugLinesFrom() const +{ + if (m_data->m_debugLinesFrom.size()) + { + return &m_data->m_debugLinesFrom[0]; + } + return 0; +} +const btVector3* PhysicsClientSharedMemory::getDebugLinesTo() const +{ + if (m_data->m_debugLinesTo.size()) + { + return &m_data->m_debugLinesTo[0]; + } + return 0; +} +const btVector3* PhysicsClientSharedMemory::getDebugLinesColor() const +{ + if (m_data->m_debugLinesColor.size()) + { + return &m_data->m_debugLinesColor[0]; + } + return 0; +} +int PhysicsClientSharedMemory::getNumDebugLines() const +{ + return m_data->m_debugLinesFrom.size(); +} \ No newline at end of file diff --git a/examples/SharedMemory/PhysicsClient.h b/examples/SharedMemory/PhysicsClient.h index 0f26ac375..094a81b38 100644 --- a/examples/SharedMemory/PhysicsClient.h +++ b/examples/SharedMemory/PhysicsClient.h @@ -2,9 +2,9 @@ #define BT_PHYSICS_CLIENT_API_H #include "SharedMemoryCommands.h" +#include "LinearMath/btVector3.h" - -class PhysicsClientSharedMemory //: public CommonPhysicsClientInterface +class PhysicsClientSharedMemory { struct PhysicsClientSharedMemoryInternalData* m_data; protected: @@ -33,6 +33,15 @@ public: virtual void getJointInfo(int index, b3JointInfo& info) const; virtual void setSharedMemoryKey(int key); + + virtual void uploadBulletFileToSharedMemory(const char* data, int len); + + virtual int getNumDebugLines() const; + + virtual const btVector3* getDebugLinesFrom() const; + virtual const btVector3* getDebugLinesTo() const; + virtual const btVector3* getDebugLinesColor() const; + }; #endif //BT_PHYSICS_CLIENT_API_H diff --git a/examples/SharedMemory/PhysicsClientExample.cpp b/examples/SharedMemory/PhysicsClientExample.cpp index 3bf852810..190fb6f50 100644 --- a/examples/SharedMemory/PhysicsClientExample.cpp +++ b/examples/SharedMemory/PhysicsClientExample.cpp @@ -47,11 +47,12 @@ public: virtual void resetCamera() { - float dist = 1; + float dist = 5; float pitch = 50; float yaw = 35; - float targetPos[3]={-3,2.8,-2.5}; + float targetPos[3]={0,0,0};//-3,2.8,-2.5}; m_guiHelper->resetCamera(dist,pitch,yaw,targetPos[0],targetPos[1],targetPos[2]); + } virtual bool wantsTermination() @@ -67,7 +68,18 @@ public: void enqueueCommand(const SharedMemoryCommand& orgCommand); virtual void exitPhysics(){}; - virtual void renderScene(){} + virtual void renderScene() + { + int numLines = m_physicsClient.getNumDebugLines(); + const btVector3* fromLines = m_physicsClient.getDebugLinesFrom(); + const btVector3* toLines = m_physicsClient.getDebugLinesTo(); + const btVector3* colorLines = m_physicsClient.getDebugLinesColor(); + btScalar lineWidth = 2; + for (int i=0;im_guiHelper->getRenderInterface()->drawLine(fromLines[i],toLines[i],colorLines[i],lineWidth); + } + } virtual void physicsDebugDraw(int debugFlags){} virtual bool mouseMoveCallback(float x,float y){return false;}; virtual bool mouseButtonCallback(int button, int state, float x, float y){return false;} @@ -95,8 +107,11 @@ void MyCallback(int buttonId, bool buttonState, void* userPtr) case CMD_LOAD_URDF: { command.m_type =CMD_LOAD_URDF; - sprintf(command.m_urdfArguments.m_urdfFileName,"kuka_lwr/kuka.urdf"); + sprintf(command.m_urdfArguments.m_urdfFileName,"r2d2.urdf");//kuka_lwr/kuka.urdf"); command.m_urdfArguments.m_initialPosition[0] = 0.0; + command.m_updateFlags = + URDF_ARGS_FILE_NAME| URDF_ARGS_INITIAL_POSITION|URDF_ARGS_INITIAL_ORIENTATION|URDF_ARGS_USE_MULTIBODY|URDF_ARGS_USE_FIXED_BASE; + command.m_urdfArguments.m_initialPosition[1] = 0.0; command.m_urdfArguments.m_initialPosition[2] = 0.0; command.m_urdfArguments.m_initialOrientation[0] = 0.0; @@ -129,6 +144,8 @@ void MyCallback(int buttonId, bool buttonState, void* userPtr) { command.m_type =CMD_STEP_FORWARD_SIMULATION; cl->enqueueCommand(command); + command.m_type =CMD_REQUEST_DEBUG_LINES; + cl->enqueueCommand(command); break; } @@ -230,6 +247,9 @@ void PhysicsClientExample::initPhysics() { if (m_guiHelper && m_guiHelper->getParameterInterface()) { + int upAxis = 2; + m_guiHelper->setUpAxis(upAxis); + createButtons(); } else @@ -273,7 +293,12 @@ void PhysicsClientExample::stepSimulation(float deltaTime) b3Printf("Joint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex); } - + if (hasStatus && status.m_type ==CMD_DEBUG_LINES_COMPLETED) + { + //store the debug lines for drawing + + + } if (hasStatus && status.m_type == CMD_URDF_LOADING_COMPLETED) { for (int i=0;i m_lines; + + SharedMemoryDebugDrawer () + :m_debugMode(0) + { + } + virtual void drawContactPoint(const btVector3& PointOnB,const btVector3& normalOnB,btScalar distance,int lifeTime,const btVector3& color) + { + } + + virtual void reportErrorWarning(const char* warningString) + { + } + + virtual void draw3dText(const btVector3& location,const char* textString) + { + } + + virtual void setDebugMode(int debugMode) + { + m_debugMode = debugMode; + } + + virtual int getDebugMode() const + { + return m_debugMode; + } + virtual void drawLine(const btVector3& from,const btVector3& to,const btVector3& color) + { + SharedMemLines line; + line.m_from = from; + line.m_to = to; + line.m_color = color; + m_lines.push_back(line); + } +}; struct PhysicsServerInternalData { SharedMemoryInterface* m_sharedMemory; @@ -51,18 +92,23 @@ struct PhysicsServerInternalData btMultiBodyConstraintSolver* m_solver; btDefaultCollisionConfiguration* m_collisionConfiguration; btMultiBodyDynamicsWorld* m_dynamicsWorld; + SharedMemoryDebugDrawer* m_debugDrawer; + struct GUIHelperInterface* m_guiHelper; int m_sharedMemoryKey; + bool m_verboseOutput; PhysicsServerInternalData() :m_sharedMemory(0), m_testBlock1(0), m_isConnected(false), - m_physicsDeltaTime(1./60.), + m_physicsDeltaTime(1./240.),//240.), m_dynamicsWorld(0), + m_debugDrawer(0), m_guiHelper(0), - m_sharedMemoryKey(SHARED_MEMORY_KEY) + m_sharedMemoryKey(SHARED_MEMORY_KEY), + m_verboseOutput(false) { } @@ -123,6 +169,9 @@ void PhysicsServerSharedMemory::createEmptyDynamicsWorld() m_data->m_solver = new btMultiBodyConstraintSolver; m_data->m_dynamicsWorld = new btMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, m_data->m_solver, m_data->m_collisionConfiguration); + + m_data->m_debugDrawer = new SharedMemoryDebugDrawer(); + m_data->m_dynamicsWorld->setDebugDrawer(m_data->m_debugDrawer); m_data->m_dynamicsWorld->setGravity(btVector3(0, 0, 0)); } @@ -189,6 +238,9 @@ void PhysicsServerSharedMemory::deleteDynamicsWorld() delete m_data->m_dynamicsWorld; m_data->m_dynamicsWorld=0; + delete m_data->m_debugDrawer; + m_data->m_debugDrawer=0; + delete m_data->m_solver; m_data->m_solver=0; @@ -221,12 +273,18 @@ bool PhysicsServerSharedMemory::connectSharedMemory( struct GUIHelperInterface* if (m_data->m_testBlock1) { int magicId =m_data->m_testBlock1->m_magicId; - b3Printf("magicId = %d\n", magicId); + if (m_data->m_verboseOutput) + { + b3Printf("magicId = %d\n", magicId); + } if (m_data->m_testBlock1->m_magicId !=SHARED_MEMORY_MAGIC_NUMBER) { InitSharedMemoryBlock(m_data->m_testBlock1); - b3Printf("Created and initialized shared memory block\n"); + if (m_data->m_verboseOutput) + { + b3Printf("Created and initialized shared memory block\n"); + } m_data->m_isConnected = true; } else { @@ -246,21 +304,33 @@ bool PhysicsServerSharedMemory::connectSharedMemory( struct GUIHelperInterface* void PhysicsServerSharedMemory::disconnectSharedMemory(bool deInitializeSharedMemory) { - b3Printf("releaseSharedMemory1\n"); + if (m_data->m_verboseOutput) + { + b3Printf("releaseSharedMemory1\n"); + } if (m_data->m_testBlock1) { - b3Printf("m_testBlock1\n"); + if (m_data->m_verboseOutput) + { + b3Printf("m_testBlock1\n"); + } if (deInitializeSharedMemory) { m_data->m_testBlock1->m_magicId = 0; - b3Printf("De-initialized shared memory, magic id = %d\n",m_data->m_testBlock1->m_magicId); + if (m_data->m_verboseOutput) + { + b3Printf("De-initialized shared memory, magic id = %d\n",m_data->m_testBlock1->m_magicId); + } } btAssert(m_data->m_sharedMemory); m_data->m_sharedMemory->releaseSharedMemory(m_data->m_sharedMemoryKey, SHARED_MEMORY_SIZE); } if (m_data->m_sharedMemory) { - b3Printf("m_sharedMemory\n"); + if (m_data->m_verboseOutput) + { + b3Printf("m_sharedMemory\n"); + } delete m_data->m_sharedMemory; m_data->m_sharedMemory = 0; m_data->m_testBlock1 = 0; @@ -269,19 +339,31 @@ void PhysicsServerSharedMemory::disconnectSharedMemory(bool deInitializeSharedMe void PhysicsServerSharedMemory::releaseSharedMemory() { - b3Printf("releaseSharedMemory1\n"); + if (m_data->m_verboseOutput) + { + b3Printf("releaseSharedMemory1\n"); + } if (m_data->m_testBlock1) { - b3Printf("m_testBlock1\n"); + if (m_data->m_verboseOutput) + { + b3Printf("m_testBlock1\n"); + } m_data->m_testBlock1->m_magicId = 0; - b3Printf("magic id = %d\n",m_data->m_testBlock1->m_magicId); + if (m_data->m_verboseOutput) + { + b3Printf("magic id = %d\n",m_data->m_testBlock1->m_magicId); + } btAssert(m_data->m_sharedMemory); m_data->m_sharedMemory->releaseSharedMemory( m_data->m_sharedMemoryKey , SHARED_MEMORY_SIZE); } if (m_data->m_sharedMemory) { - b3Printf("m_sharedMemory\n"); + if (m_data->m_verboseOutput) + { + b3Printf("m_sharedMemory\n"); + } delete m_data->m_sharedMemory; m_data->m_sharedMemory = 0; m_data->m_testBlock1 = 0; @@ -336,7 +418,10 @@ bool PhysicsServerSharedMemory::loadUrdf(const char* fileName, const btVector3& bool loadOk = u2b.loadURDF(fileName, useFixedBase); if (loadOk) { - b3Printf("loaded %s OK!", fileName); + if (m_data->m_verboseOutput) + { + b3Printf("loaded %s OK!", fileName); + } btTransform tr; tr.setIdentity(); @@ -438,7 +523,10 @@ void PhysicsServerSharedMemory::processClientCommands() { case CMD_SEND_BULLET_DATA_STREAM: { - b3Printf("Processed CMD_SEND_BULLET_DATA_STREAM length %d",clientCmd.m_dataStreamArguments.m_streamChunkLength); + if (m_data->m_verboseOutput) + { + b3Printf("Processed CMD_SEND_BULLET_DATA_STREAM length %d",clientCmd.m_dataStreamArguments.m_streamChunkLength); + } btBulletWorldImporter* worldImporter = new btBulletWorldImporter(m_data->m_dynamicsWorld); m_data->m_worldImporters.push_back(worldImporter); @@ -457,6 +545,41 @@ void PhysicsServerSharedMemory::processClientCommands() break; } + case CMD_REQUEST_DEBUG_LINES: + { + int curFlags =m_data->m_debugDrawer->getDebugMode(); + int debugMode = btIDebugDraw::DBG_DrawWireframe|btIDebugDraw::DBG_DrawAabb; + //|btIDebugDraw::DBG_DrawAabb| + // btIDebugDraw::DBG_DrawConstraints |btIDebugDraw::DBG_DrawConstraintLimits ; + m_data->m_debugDrawer->setDebugMode(debugMode); + m_data->m_dynamicsWorld->debugDrawWorld(); + m_data->m_debugDrawer->setDebugMode(curFlags); + + int numLines = m_data->m_debugDrawer->m_lines.size(); + int memRequirements = numLines*sizeof(btVector3)*3; + if (memRequirementsm_testBlock1->m_bulletStreamDataServerToClient[0]; + btVector3* linesTo = (btVector3*)(&m_data->m_testBlock1->m_bulletStreamDataServerToClient[0]+numLines*sizeof(btVector3)); + btVector3* linesColor = (btVector3*)(&m_data->m_testBlock1->m_bulletStreamDataServerToClient[0]+2*numLines*sizeof(btVector3)); + + for (int i=0;im_debugDrawer->m_lines[i].m_from; + linesTo[i] = m_data->m_debugDrawer->m_lines[i].m_to; + linesColor[i] = m_data->m_debugDrawer->m_lines[i].m_color; + } + SharedMemoryStatus& status = m_data->createServerStatus(CMD_DEBUG_LINES_COMPLETED,clientCmd.m_sequenceNumber,timeStamp); + status.m_sendDebugLinesArgs.m_numDebugLines = numLines; + m_data->submitServerStatus(status); + } else + { + SharedMemoryStatus& status = m_data->createServerStatus(CMD_DEBUG_LINES_OVERFLOW_FAILED,clientCmd.m_sequenceNumber,timeStamp); + m_data->submitServerStatus(status); + } + break; + } case CMD_LOAD_URDF: { //at the moment, we only load 1 urdf / robot @@ -467,7 +590,10 @@ void PhysicsServerSharedMemory::processClientCommands() break; } const UrdfArgs& urdfArgs = clientCmd.m_urdfArguments; - b3Printf("Processed CMD_LOAD_URDF:%s", urdfArgs.m_urdfFileName); + if (m_data->m_verboseOutput) + { + b3Printf("Processed CMD_LOAD_URDF:%s", urdfArgs.m_urdfFileName); + } btAssert((clientCmd.m_updateFlags&URDF_ARGS_FILE_NAME) !=0); btAssert(urdfArgs.m_urdfFileName); btVector3 initialPos(0,0,0); @@ -517,7 +643,10 @@ void PhysicsServerSharedMemory::processClientCommands() } case CMD_CREATE_SENSOR: { - b3Printf("Processed CMD_CREATE_SENSOR"); + if (m_data->m_verboseOutput) + { + b3Printf("Processed CMD_CREATE_SENSOR"); + } if (m_data->m_dynamicsWorld->getNumMultibodies()>0) { @@ -580,7 +709,10 @@ void PhysicsServerSharedMemory::processClientCommands() } case CMD_SEND_DESIRED_STATE: { + if (m_data->m_verboseOutput) + { b3Printf("Processed CMD_SEND_DESIRED_STATE"); + } if (m_data->m_dynamicsWorld->getNumMultibodies()>0) { btMultiBody* mb = m_data->m_dynamicsWorld->getMultiBody(0); @@ -590,7 +722,10 @@ void PhysicsServerSharedMemory::processClientCommands() { case CONTROL_MODE_TORQUE: { - b3Printf("Using CONTROL_MODE_TORQUE"); + if (m_data->m_verboseOutput) + { + b3Printf("Using CONTROL_MODE_TORQUE"); + } mb->clearForcesAndTorques(); int torqueIndex = 0; @@ -617,7 +752,10 @@ void PhysicsServerSharedMemory::processClientCommands() } case CONTROL_MODE_VELOCITY: { - b3Printf("Using CONTROL_MODE_VELOCITY"); + if (m_data->m_verboseOutput) + { + b3Printf("Using CONTROL_MODE_VELOCITY"); + } int numMotors = 0; //find the joint motors and apply the desired velocity and maximum force/torque @@ -646,11 +784,68 @@ void PhysicsServerSharedMemory::processClientCommands() dofIndex += mb->getLink(link).m_dofCount; } } + break; + } + case CONTROL_MODE_POSITION_VELOCITY_PD: + { + if (m_data->m_verboseOutput) + { + b3Printf("Using CONTROL_MODE_POSITION_VELOCITY_PD"); + } + //compute the force base on PD control + mb->clearForcesAndTorques(); + + int numMotors = 0; + //find the joint motors and apply the desired velocity and maximum force/torque + if (m_data->m_dynamicsWorld->getNumMultibodies()>0) + { + btMultiBody* mb = m_data->m_dynamicsWorld->getMultiBody(0); + int velIndex = 6;//skip the 3 linear + 3 angular degree of freedom velocity entries of the base + int posIndex = 7;//skip 3 positional and 4 orientation (quaternion) positional degrees of freedom of the base + for (int link=0;linkgetNumLinks();link++) + { + if (supportsJointMotor(mb,link)) + { + + btMultiBodyJointMotor** motorPtr = m_data->m_multiBodyJointMotorMap[link]; + if (motorPtr) + { + btMultiBodyJointMotor* motor = *motorPtr; + motor->setMaxAppliedImpulse(0); + } + + btScalar desiredVelocity = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[velIndex]; + btScalar desiredPosition = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[posIndex]; + + btScalar maxForce = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[velIndex]; + btScalar kp = clientCmd.m_sendDesiredStateCommandArgument.m_Kp[velIndex]; + btScalar kd = clientCmd.m_sendDesiredStateCommandArgument.m_Kd[velIndex]; + + int dof1 = 0; + btScalar qActual = mb->getJointPosMultiDof(link)[dof1]; + btScalar qdActual = mb->getJointVelMultiDof(link)[dof1]; + btScalar positionError = (desiredPosition-qActual); + btScalar velocityError = (desiredVelocity-qdActual); + btScalar force = kp * positionError + kd*velocityError; + btClamp(force,-maxForce,maxForce); + if (m_data->m_verboseOutput) + { + b3Printf("Apply force %f (kp=%f, kd=%f at link %d\n", force,kp,kd,link); + } + mb->addJointTorque(link, force);//we assume we have 1-DOF motors only at the moment + //mb->addJointTorqueMultiDof(link,&force); + numMotors++; + } + velIndex += mb->getLink(link).m_dofCount; + posIndex += mb->getLink(link).m_posVarCount; + } + } + break; } default: { - b3Printf("m_controlMode not implemented yet"); + b3Warning("m_controlMode not implemented yet"); break; } @@ -663,7 +858,10 @@ void PhysicsServerSharedMemory::processClientCommands() } case CMD_REQUEST_ACTUAL_STATE: { - b3Printf("Sending the actual state (Q,U)"); + if (m_data->m_verboseOutput) + { + b3Printf("Sending the actual state (Q,U)"); + } if (m_data->m_dynamicsWorld->getNumMultibodies()>0) { btMultiBody* mb = m_data->m_dynamicsWorld->getMultiBody(0); @@ -754,8 +952,11 @@ void PhysicsServerSharedMemory::processClientCommands() case CMD_STEP_FORWARD_SIMULATION: { - b3Printf("Step simulation request"); - m_data->m_dynamicsWorld->stepSimulation(m_data->m_physicsDeltaTime); + if (m_data->m_verboseOutput) + { + b3Printf("Step simulation request"); + } + m_data->m_dynamicsWorld->stepSimulation(m_data->m_physicsDeltaTime,0); SharedMemoryStatus& serverCmd =m_data->createServerStatus(CMD_STEP_FORWARD_SIMULATION_COMPLETED,clientCmd.m_sequenceNumber,timeStamp); m_data->submitServerStatus(serverCmd); @@ -771,7 +972,10 @@ void PhysicsServerSharedMemory::processClientCommands() clientCmd.m_physSimParamArgs.m_gravityAcceleration[1], clientCmd.m_physSimParamArgs.m_gravityAcceleration[2]); this->m_data->m_dynamicsWorld->setGravity(grav); - b3Printf("Updated Gravity: %f,%f,%f",grav[0],grav[1],grav[2]); + if (m_data->m_verboseOutput) + { + b3Printf("Updated Gravity: %f,%f,%f",grav[0],grav[1],grav[2]); + } } @@ -783,7 +987,10 @@ void PhysicsServerSharedMemory::processClientCommands() }; case CMD_INIT_POSE: { - b3Printf("Server Init Pose not implemented yet"); + if (m_data->m_verboseOutput) + { + b3Printf("Server Init Pose not implemented yet"); + } ///@todo: implement this m_data->m_dynamicsWorld->setGravity(btVector3(0,0,0)); @@ -831,7 +1038,6 @@ void PhysicsServerSharedMemory::processClientCommands() if (clientCmd.m_updateFlags & BOX_SHAPE_HAS_INITIAL_ORIENTATION) { - b3Printf("test\n"); startTrans.setRotation(btQuaternion( clientCmd.m_createBoxShapeArguments.m_initialOrientation[0], clientCmd.m_createBoxShapeArguments.m_initialOrientation[1], @@ -884,10 +1090,9 @@ void PhysicsServerSharedMemory::physicsDebugDraw(int debugDrawFlags) { if (m_data->m_dynamicsWorld->getDebugDrawer()) { + m_data->m_debugDrawer->m_lines.clear(); m_data->m_dynamicsWorld->getDebugDrawer()->setDebugMode(debugDrawFlags); } m_data->m_dynamicsWorld->debugDrawWorld(); } } - - diff --git a/examples/SharedMemory/PhysicsServer.h b/examples/SharedMemory/PhysicsServer.h index 53d566add..5981c6adb 100644 --- a/examples/SharedMemory/PhysicsServer.h +++ b/examples/SharedMemory/PhysicsServer.h @@ -1,6 +1,16 @@ #ifndef PHYSICS_SERVER_SHARED_MEMORY_H #define PHYSICS_SERVER_SHARED_MEMORY_H +#include "LinearMath/btVector3.h" + +struct SharedMemLines +{ + btVector3 m_from; + btVector3 m_to; + btVector3 m_color; +}; + + class PhysicsServerSharedMemory { struct PhysicsServerInternalData* m_data; @@ -37,7 +47,7 @@ public: //to a physics client, over shared memory void physicsDebugDraw(int debugDrawFlags); void renderScene(); - + }; diff --git a/examples/SharedMemory/RobotControlExample.cpp b/examples/SharedMemory/RobotControlExample.cpp index bdd20f296..deec441a8 100644 --- a/examples/SharedMemory/RobotControlExample.cpp +++ b/examples/SharedMemory/RobotControlExample.cpp @@ -11,6 +11,7 @@ #include "SharedMemoryCommon.h" #include "../Utils/b3Clock.h" #include "PhysicsClientC_API.h" +#include "../Utils/b3ResourcePath.h" //const char* blaatnaam = "basename"; @@ -18,8 +19,11 @@ struct MyMotorInfo { btScalar m_velTarget; + btScalar m_posTarget; btScalar m_maxForce; int m_uIndex; + int m_posIndex; + }; #define MAX_NUM_MOTORS 128 @@ -39,18 +43,21 @@ class RobotControlExample : public SharedMemoryCommon public: //@todo, add accessor methods - MyMotorInfo m_motorTargetVelocities[MAX_NUM_MOTORS]; - int m_numMotors; + MyMotorInfo m_motorTargetState[MAX_NUM_MOTORS]; - - RobotControlExample(GUIHelperInterface* helper); + int m_numMotors; + int m_option; + bool m_verboseOutput; + + RobotControlExample(GUIHelperInterface* helper, int option); virtual ~RobotControlExample(); virtual void initPhysics(); virtual void stepSimulation(float deltaTime); - + void prepareControlCommand(SharedMemoryCommand& cmd); + void enqueueCommand(const SharedMemoryCommand& orgCommand) { m_userCommandRequests.push_back(orgCommand); @@ -58,7 +65,10 @@ public: cmd.m_sequenceNumber = m_sequenceNumberGenerator++; cmd.m_timeStamp = m_realtimeClock.getTimeMicroseconds(); - b3Printf("User put command request %d on queue (queue length = %d)\n",cmd.m_type, m_userCommandRequests.size()); + if (m_verboseOutput) + { + b3Printf("User put command request %d on queue (queue length = %d)\n",cmd.m_type, m_userCommandRequests.size()); + } } virtual void resetCamera() @@ -81,6 +91,7 @@ public: virtual void physicsDebugDraw(int debugFlags) { m_physicsServer.physicsDebugDraw(debugFlags); + } virtual bool mouseMoveCallback(float x,float y){return false;}; virtual bool mouseButtonCallback(int button, int state, float x, float y){return false;} @@ -111,7 +122,7 @@ void MyCallback2(int buttonId, bool buttonState, void* userPtr) { command.m_type =CMD_LOAD_URDF; command.m_updateFlags = URDF_ARGS_FILE_NAME|URDF_ARGS_INITIAL_POSITION|URDF_ARGS_INITIAL_ORIENTATION; - sprintf(command.m_urdfArguments.m_urdfFileName,"r2d2.urdf"); + sprintf(command.m_urdfArguments.m_urdfFileName,"kuka_lwr/kuka.urdf");//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; @@ -129,7 +140,7 @@ void MyCallback2(int buttonId, bool buttonState, void* userPtr) { //#ifdef USE_C_API b3InitPhysicsParamCommand(&command); - b3PhysicsParamSetGravity(&command, 0,0,-10); + b3PhysicsParamSetGravity(&command, 1,1,-10); // #else @@ -182,54 +193,16 @@ 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; - - command.m_sendDesiredStateCommandArgument.m_controlMode = controlMode; - //todo: expose a drop box in the GUI for this - switch (controlMode) - { - case CONTROL_MODE_VELOCITY: - { - for (int i=0;im_numMotors;i++) - { - btScalar targetVel = cl->m_motorTargetVelocities[i].m_velTarget; - - int uIndex = cl->m_motorTargetVelocities[i].m_uIndex; - if (targetVel>1) - { - printf("testme"); - } - command.m_sendDesiredStateCommandArgument.m_desiredStateQdot[uIndex] = targetVel; - - } - break; - } - case CONTROL_MODE_TORQUE: - { - for (int i=0;iprepareControlCommand(command); cl->enqueueCommand(command); break; } case CMD_SEND_BULLET_DATA_STREAM: { command.m_type = buttonId; + sprintf(command.m_dataStreamArguments.m_bulletFileName,"slope.bullet"); + command.m_dataStreamArguments.m_streamChunkLength = 0; cl->enqueueCommand(command); break; } @@ -241,6 +214,66 @@ void MyCallback2(int buttonId, bool buttonState, void* userPtr) } }; } + + +void RobotControlExample::prepareControlCommand(SharedMemoryCommand& command) +{ + for (int i=0;igetParameterInterface()->registerButtonParameter(button); } -RobotControlExample::RobotControlExample(GUIHelperInterface* helper) +RobotControlExample::RobotControlExample(GUIHelperInterface* helper, int option) :SharedMemoryCommon(helper), m_wantsShutdown(false), m_sequenceNumberGenerator(0), -m_numMotors(0) +m_numMotors(0), +m_option(option), +m_verboseOutput(false) { bool useServer = true; @@ -328,7 +363,8 @@ bool RobotControlExample::wantsTermination() void RobotControlExample::stepSimulation(float deltaTime) { - m_physicsServer.processClientCommands(); + + m_physicsServer.processClientCommands(); if (m_physicsClient.isConnected()) { @@ -341,23 +377,57 @@ void RobotControlExample::stepSimulation(float deltaTime) { b3JointInfo info; m_physicsClient.getJointInfo(i,info); - b3Printf("Joint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex); + if (m_verboseOutput) + { + b3Printf("Joint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex); + } if (info.m_flags & JOINT_HAS_MOTORIZED_POWER) { if (m_numMotorsm_velTarget = 0.f; - motorInfo->m_uIndex = info.m_uIndex; + + switch (m_option) + { + case ROBOT_VELOCITY_CONTROL: + { + char motorName[1024]; + sprintf(motorName,"%s q'", info.m_jointName); + MyMotorInfo* motorInfo = &m_motorTargetState[m_numMotors]; + motorInfo->m_velTarget = 0.f; + motorInfo->m_posTarget = 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++; + SliderParams slider(motorName,&motorInfo->m_velTarget); + slider.m_minVal=-4; + slider.m_maxVal=4; + m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); + m_numMotors++; + break; + } + case ROBOT_PD_CONTROL: + { + char motorName[1024]; + sprintf(motorName,"%s q", info.m_jointName); + MyMotorInfo* motorInfo = &m_motorTargetState[m_numMotors]; + motorInfo->m_velTarget = 0.f; + motorInfo->m_posTarget = 0.f; + motorInfo->m_uIndex = info.m_uIndex; + motorInfo->m_posIndex = info.m_qIndex; + + + SliderParams slider(motorName,&motorInfo->m_posTarget); + slider.m_minVal=-SIMD_PI; + slider.m_maxVal=SIMD_PI; + m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); + m_numMotors++; + break; + } + default: + { + b3Warning("Unknown control mode in RobotControlExample::stepSimulation"); + } + }; } } @@ -369,8 +439,11 @@ void RobotControlExample::stepSimulation(float deltaTime) { if (m_userCommandRequests.size()) { - b3Printf("Outstanding user command requests: %d\n", m_userCommandRequests.size()); - SharedMemoryCommand& cmd = m_userCommandRequests[0]; + if (m_verboseOutput) + { + b3Printf("Outstanding user command requests: %d\n", m_userCommandRequests.size()); + } + SharedMemoryCommand cmd = m_userCommandRequests[0]; //a manual 'pop_front', we don't use 'remove' because it will re-order the commands for (int i=1;i=0) { example->setSharedMemoryKey(gSharedMemoryKey); diff --git a/examples/SharedMemory/RobotControlExample.h b/examples/SharedMemory/RobotControlExample.h index 3d661d2e1..68cdb9173 100644 --- a/examples/SharedMemory/RobotControlExample.h +++ b/examples/SharedMemory/RobotControlExample.h @@ -1,6 +1,12 @@ #ifndef ROBOT_CONTROL_EXAMPLE_H #define ROBOT_CONTROL_EXAMPLE_H +enum EnumRobotControls +{ + ROBOT_VELOCITY_CONTROL=0, + ROBOT_PD_CONTROL=2, +}; + class CommonExampleInterface* RobotControlExampleCreateFunc(struct CommonExampleOptions& options); #endif //ROBOT_CONTROL_EXAMPLE_H diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index acf149746..52af0ca8a 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -36,6 +36,7 @@ enum EnumSharedMemoryClientCommand CMD_SEND_PHYSICS_SIMULATION_PARAMETERS, CMD_SEND_DESIRED_STATE,//todo: reconsider naming, for example SET_JOINT_CONTROL_VARIABLE? CMD_REQUEST_ACTUAL_STATE, + CMD_REQUEST_DEBUG_LINES, CMD_STEP_FORWARD_SIMULATION, CMD_RESET_SIMULATION, CMD_MAX_CLIENT_COMMANDS @@ -60,6 +61,8 @@ enum EnumSharedMemoryServerStatus CMD_SET_JOINT_FEEDBACK_COMPLETED, CMD_ACTUAL_STATE_UPDATE_COMPLETED, CMD_ACTUAL_STATE_UPDATE_FAILED, + CMD_DEBUG_LINES_COMPLETED, + CMD_DEBUG_LINES_OVERFLOW_FAILED, CMD_DESIRED_STATE_RECEIVED_COMPLETED, CMD_STEP_FORWARD_SIMULATION_COMPLETED, CMD_MAX_SERVER_COMMANDS @@ -69,6 +72,7 @@ enum EnumSharedMemoryServerStatus #define MAX_DEGREE_OF_FREEDOM 256 #define MAX_NUM_SENSORS 256 #define MAX_URDF_FILENAME_LENGTH 1024 +#define MAX_FILENAME_LENGTH MAX_URDF_FILENAME_LENGTH enum EnumUrdfArgsUpdateFlags { @@ -92,6 +96,7 @@ struct UrdfArgs struct BulletDataStreamArgs { + char m_bulletFileName[MAX_FILENAME_LENGTH]; int m_streamChunkLength; int m_bodyUniqueId; }; @@ -108,6 +113,7 @@ enum { // POSITION_CONTROL=0, CONTROL_MODE_VELOCITY=0, CONTROL_MODE_TORQUE, + CONTROL_MODE_POSITION_VELOCITY_PD, }; ///InitPoseArgs is mainly to initialize (teleport) the robot in a particular position @@ -121,6 +127,17 @@ struct InitPoseArgs }; +struct RequestDebugLinesArgs +{ + int m_debugMode; +}; + +struct SendDebugLinesArgs +{ + int m_numDebugLines; +}; + + ///Controlling a robot involves sending the desired state to its joint motor controllers. ///The control mode determines the state variables used for motor control. struct SendDesiredStateArgs @@ -128,12 +145,22 @@ struct SendDesiredStateArgs int m_bodyUniqueId; int m_controlMode; + //PD parameters in case m_controlMode == CONTROL_MODE_POSITION_VELOCITY_PD + double m_Kp[MAX_DEGREE_OF_FREEDOM];//indexed by degree of freedom, 6 for base, and then the dofs for each link + double m_Kd[MAX_DEGREE_OF_FREEDOM];//indexed by degree of freedom, 6 for base, and then the dofs for each link + //desired state is only written by the client, read-only access by server is expected + + //m_desiredStateQ is indexed by position variables, + //starting with 3 base position variables, 4 base orientation variables (quaternion), then link position variables double m_desiredStateQ[MAX_DEGREE_OF_FREEDOM]; + + //m_desiredStateQdot is index by velocity degrees of freedom, 3 linear and 3 angular variables for the base and then link velocity variables 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 + //or the maximum applied force/torque for the PD/motor/constraint to reach the desired velocity in CONTROL_MODE_VELOCITY and CONTROL_MODE_POSITION_VELOCITY_PD mode + //indexed by degree of freedom, 6 dof base, and then dofs for each link double m_desiredStateForceTorque[MAX_DEGREE_OF_FREEDOM]; }; @@ -227,6 +254,7 @@ struct SharedMemoryCommand struct RequestActualStateArgs m_requestActualStateInformationCommandArgument; struct CreateSensorArgs m_createSensorArguments; struct CreateBoxShapeArgs m_createBoxShapeArguments; + struct RequestDebugLinesArgs m_requestDebugLinesArguments; }; }; @@ -242,6 +270,7 @@ struct SharedMemoryStatus { struct BulletDataStreamArgs m_dataStreamArguments; struct SendActualStateArgs m_sendActualStateArgs; + struct SendDebugLinesArgs m_sendDebugLinesArgs; }; }; @@ -258,6 +287,7 @@ struct b3JointInfo int m_jointType; int m_qIndex; int m_uIndex; + int m_linkIndex; /// int m_flags; };