AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2024-12-12 21:00:11 +00:00
Code Issues Releases Wiki Activity

initial implementation to send debug lines from physics server to client,

Browse Source 
need to add streaming because memory is too small to store all lines
initial test of PD control in physics server, need to switch to PD control for motor constraint, instead of using external forces.
This commit is contained in:
erwincoumans 2015-08-19 22:51:16 -07:00
parent 89765ceccf
commit 081a40d254
14 changed files with 750 additions and 168 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

52
data/hinge.urdf
View File

@ -2,48 +2,72 @@
<robot name="urdf_robot">
<link name="baseLink">
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<origin rpy="0 0 0" xyz="0 0 0.35"/>
<mass value="0.0"/>
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<origin rpy="0 0 0" xyz="0 0 0.35"/>
<geometry>
<box size="0.1 .1 .1"/>
<box size="0.08 0.16 0.7"/>
</geometry>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<origin rpy="0 0 0" xyz="0 0 0.35"/>
<geometry>
<box size="0.1 .1 .1"/>
<box size="0.08 0.16 0.7"/>
</geometry>
</collision>
</link>
<link name="childA">
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="10.0"/>
<inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
<origin rpy="0 0 0" xyz="0 0 -0.36"/>
<mass value="1.0"/>
<inertia ixx="0.048966669" ixy="0" ixz="0" iyy="0.046466667" iyz="0" izz="0.0041666669"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<origin rpy="0 0 0" xyz="0 0 -0.36"/>
<geometry>
<box size="0.1 .1 .1"/>
<box size="0.1 0.2 0.72"/>
</geometry>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<origin rpy="0 0 0" xyz="0 0 -0.36"/>
<geometry>
<box size="0.1 .1 .1"/>
<box size="0.1 0.2 0.72 "/>
</geometry>
</collision>
</link>
<joint name="joint_baseLink_childA" type="continuous">
<parent link="baseLink"/>
<child link="childA"/>
<origin xyz="0 0 1.0"/>
<axis xyz="0 0 1"/>
<origin xyz="0 0 0"/>
<axis xyz="1 0 0"/>
</joint>
<link name="childB">
<inertial>
<origin rpy="0 0 0" xyz="0 0 -0.36"/>
<mass value="1.0"/>
<inertia ixx="0.048966669" ixy="0" ixz="0" iyy="0.046466667" iyz="0" izz="0.0041666669"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 -0.36"/>
<geometry>
<sphere radius="0.2"/>
</geometry>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 -0.36"/>
<geometry>
<sphere radius="0.2"/>
</geometry>
</collision>
</link>
<joint name="joint_childA_childB" type="fixed">
<parent link="childA"/>
<child link="childB"/>
<origin xyz="0 0 -0.2"/>
</joint>
</robot>

6
examples/ExampleBrowser/ExampleEntries.cpp
View File

@ -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),

6
examples/ExampleBrowser/GwenGUISupport/GwenParameterInterface.cpp
View File

@ -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 );

6
examples/ExampleBrowser/GwenGUISupport/gwenUserInterface.cpp
View File

@ -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);

2
examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp
View File

@ -356,7 +356,7 @@ void ImportUrdfSetup::initPhysics()
bool createGround=true;
bool createGround=false;
if (createGround)
{
btVector3 groundHalfExtents(20,20,20);

25
examples/MultiBody/InvertedPendulumPDControl.cpp
View File

@ -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)

202
examples/SharedMemory/PhysicsClient.cpp
View File

@ -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<bParse::btBulletFile*> m_robotMultiBodyData;
btAlignedObjectArray<b3JointInfo> m_jointInfo;
btAlignedObjectArray<btVector3> m_debugLinesFrom;
btAlignedObjectArray<btVector3> m_debugLinesTo;
btAlignedObjectArray<btVector3> 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;link<mb->m_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;link<mb->m_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;i<numLines;i++)
{
m_data->m_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);
if (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;i<len;i++)
{
m_data->m_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();
}

13
examples/SharedMemory/PhysicsClient.h
View File

@ -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

35
examples/SharedMemory/PhysicsClientExample.cpp
View File

@ -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;i<numLines;i++)
{
this->m_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_physicsClient.getNumJoints();i++)

259
examples/SharedMemory/PhysicsServer.cpp
View File

@ -32,6 +32,47 @@ struct UrdfLinkNameMapUtil
}
};
struct SharedMemoryDebugDrawer : public btIDebugDraw
{
int m_debugMode;
btAlignedObjectArray<SharedMemLines> 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 (memRequirements<SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE)
{
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));
for (int i=0;i<numLines;i++)
{
linesFrom[i] = m_data->m_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;link<mb->getNumLinks();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();
}
}

12
examples/SharedMemory/PhysicsServer.h
View File

@ -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();
};

262
examples/SharedMemory/RobotControlExample.cpp
View File

@ -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;i<MAX_DEGREE_OF_FREEDOM;i++)
{
command.m_sendDesiredStateCommandArgument.m_desiredStateQdot[i] = 0;
command.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[i] = 1000;
}
for (int i=0;i<cl->m_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;i<MAX_DEGREE_OF_FREEDOM;i++)
{
command.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[i] = 100;
}
break;
}
default:
{
b3Printf("Unknown control mode in client CMD_SEND_DESIRED_STATE");
btAssert(0);
}
}
command.m_type =CMD_SEND_DESIRED_STATE;
cl->prepareControlCommand(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;i<MAX_DEGREE_OF_FREEDOM;i++)
{
command.m_sendDesiredStateCommandArgument.m_desiredStateQdot[i] = 0;
command.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[i] = 0;
}
switch (m_option)
{
case ROBOT_VELOCITY_CONTROL:
{
command.m_sendDesiredStateCommandArgument.m_controlMode = CONTROL_MODE_VELOCITY;
for (int i=0;i<MAX_DEGREE_OF_FREEDOM;i++)
{
command.m_sendDesiredStateCommandArgument.m_desiredStateQdot[i] = 0;
command.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[i] = 1000;
}
for (int i=0;i<m_numMotors;i++)
{
btScalar targetVel = m_motorTargetState[i].m_velTarget;
int uIndex = m_motorTargetState[i].m_uIndex;
command.m_sendDesiredStateCommandArgument.m_desiredStateQdot[uIndex] = targetVel;
}
break;
}
case ROBOT_PD_CONTROL:
{
command.m_sendDesiredStateCommandArgument.m_controlMode = CONTROL_MODE_POSITION_VELOCITY_PD;
for (int i=0;i<m_numMotors;i++)
{
int uIndex = m_motorTargetState[i].m_uIndex;
command.m_sendDesiredStateCommandArgument.m_Kp[uIndex] = 10;
command.m_sendDesiredStateCommandArgument.m_Kd[uIndex] = 2;
command.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[uIndex] = 1000;//max force
btScalar targetVel = m_motorTargetState[i].m_velTarget;
command.m_sendDesiredStateCommandArgument.m_desiredStateQdot[uIndex] = targetVel;
int posIndex = m_motorTargetState[i].m_posIndex;
btScalar targetPos = m_motorTargetState[i].m_posTarget;
command.m_sendDesiredStateCommandArgument.m_desiredStateQ[posIndex] = targetPos;
}
break;
}
default:
{
b3Warning("Unknown control mode in RobotControlExample::prepareControlCommand");
}
};
}
void RobotControlExample::createButton(const char* name, int buttonId, bool isTrigger )
{
ButtonParams button(name,buttonId, isTrigger);
@ -249,11 +282,13 @@ void RobotControlExample::createButton(const char* name, int buttonId, bool isTr
m_guiHelper->getParameterInterface()->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_numMotors<MAX_NUM_MOTORS)
{
char motorName[1024];
sprintf(motorName,"%s q'", info.m_jointName);
MyMotorInfo* motorInfo = &m_motorTargetVelocities[m_numMotors];
motorInfo->m_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<m_userCommandRequests.size();i++)
@ -379,7 +452,62 @@ void RobotControlExample::stepSimulation(float deltaTime)
}
m_userCommandRequests.pop_back();
if (cmd.m_type == CMD_SEND_BULLET_DATA_STREAM)
{
char relativeFileName[1024];
bool fileFound = b3ResourcePath::findResourcePath(cmd.m_dataStreamArguments.m_bulletFileName,relativeFileName,1024);
if (fileFound)
{
FILE *fp = fopen(relativeFileName, "rb");
if (fp)
{
fseek(fp, 0L, SEEK_END);
int mFileLen = ftell(fp);
fseek(fp, 0L, SEEK_SET);
if (mFileLen<SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE)
{
char* data = (char*)malloc(mFileLen);
fread(data, mFileLen, 1, fp);
fclose(fp);
cmd.m_dataStreamArguments.m_streamChunkLength = mFileLen;
m_physicsClient.uploadBulletFileToSharedMemory(data,mFileLen);
if (m_verboseOutput)
{
b3Printf("Loaded bullet data chunks into shared memory\n");
}
free(data);
} else
{
b3Warning("Bullet file size (%d) exceeds of streaming memory chunk size (%d)\n", mFileLen,SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
}
} else
{
b3Warning("Cannot open file %s\n", relativeFileName);
}
} else
{
b3Warning("Cannot find file %s\n", cmd.m_dataStreamArguments.m_bulletFileName);
}
}
m_physicsClient.submitClientCommand(cmd);
} else
{
if (m_numMotors)
{
SharedMemoryCommand command;
command.m_type =CMD_SEND_DESIRED_STATE;
prepareControlCommand(command);
enqueueCommand(command);
command.m_type =CMD_STEP_FORWARD_SIMULATION;
enqueueCommand(command);
}
}
}
}
@ -389,7 +517,7 @@ extern int gSharedMemoryKey;
class CommonExampleInterface* RobotControlExampleCreateFunc(struct CommonExampleOptions& options)
{
RobotControlExample* example = new RobotControlExample(options.m_guiHelper);
RobotControlExample* example = new RobotControlExample(options.m_guiHelper, options.m_option);
if (gSharedMemoryKey>=0)
{
example->setSharedMemoryKey(gSharedMemoryKey);

6
examples/SharedMemory/RobotControlExample.h
View File

@ -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

32
examples/SharedMemory/SharedMemoryCommands.h
View File

@ -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;
};