Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests 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
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"> <robot name="urdf_robot">
<link name="baseLink"> <link name="baseLink">
<inertial> <inertial>
<origin rpy="0 0 0" xyz="0 0 0"/> <origin rpy="0 0 0" xyz="0 0 0.35"/>
<mass value="0.0"/> <mass value="0.0"/>
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/> <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
</inertial> </inertial>
<visual> <visual>
<origin rpy="0 0 0" xyz="0 0 0"/> <origin rpy="0 0 0" xyz="0 0 0.35"/>
<geometry> <geometry>
<box size="0.1 .1 .1"/> <box size="0.08 0.16 0.7"/>
</geometry> </geometry>
</visual> </visual>
<collision> <collision>
<origin rpy="0 0 0" xyz="0 0 0"/> <origin rpy="0 0 0" xyz="0 0 0.35"/>
<geometry> <geometry>
<box size="0.1 .1 .1"/> <box size="0.08 0.16 0.7"/>
</geometry> </geometry>
</collision> </collision>
</link> </link>
<link name="childA"> <link name="childA">
<inertial> <inertial>
<origin rpy="0 0 0" xyz="0 0 0"/> <origin rpy="0 0 0" xyz="0 0 -0.36"/>
<mass value="10.0"/> <mass value="1.0"/>
<inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/> <inertia ixx="0.048966669" ixy="0" ixz="0" iyy="0.046466667" iyz="0" izz="0.0041666669"/>
</inertial> </inertial>
<visual> <visual>
<origin rpy="0 0 0" xyz="0 0 0"/> <origin rpy="0 0 0" xyz="0 0 -0.36"/>
<geometry> <geometry>
<box size="0.1 .1 .1"/> <box size="0.1 0.2 0.72"/>
</geometry> </geometry>
</visual> </visual>
<collision> <collision>
<origin rpy="0 0 0" xyz="0 0 0"/> <origin rpy="0 0 0" xyz="0 0 -0.36"/>
<geometry> <geometry>
<box size="0.1 .1 .1"/> <box size="0.1 0.2 0.72 "/>
</geometry> </geometry>
</collision> </collision>
</link> </link>
<joint name="joint_baseLink_childA" type="continuous"> <joint name="joint_baseLink_childA" type="continuous">
<parent link="baseLink"/> <parent link="baseLink"/>
<child link="childA"/> <child link="childA"/>
<origin xyz="0 0 1.0"/> <origin xyz="0 0 0"/>
<axis xyz="0 0 1"/> <axis xyz="1 0 0"/>
</joint> </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> </robot>

6
examples/ExampleBrowser/ExampleEntries.cpp
View File

@@ -204,8 +204,10 @@ static ExampleEntry gDefaultExamples[]=
ExampleEntry(0,"Experiments"), ExampleEntry(0,"Experiments"),
ExampleEntry(1,"Robot Control", "Perform some robot control tasks, using physics server and client that communicate over shared memory", ExampleEntry(1,"Robot Control (Velocity)", "Perform some robot control tasks, using physics server and client that communicate over shared memory",
RobotControlExampleCreateFunc), 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", ExampleEntry(1,"Physics Server", "Create a physics server that communicates with a physics client over shared memory",
PhysicsServerCreateFunc), 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); m_paramInternalData->m_buttonEventHandlers.push_back(handler);
button->SetPos( 5, m_gwenInternalData->m_curYposition ); button->SetPos( 5, m_gwenInternalData->m_curYposition );
button->SetWidth(120); button->SetWidth(220);
m_gwenInternalData->m_curYposition+=22; m_gwenInternalData->m_curYposition+=22;
@@ -152,7 +152,7 @@ void GwenParameterInterface::registerSliderFloatParameter(SliderParams& params)
//m_data->m_myControls.push_back(label); //m_data->m_myControls.push_back(label);
label->SetText( params.m_name); label->SetText( params.m_name);
label->SetPos( 10, 10 + 25 ); label->SetPos( 10, 10 + 25 );
label->SetWidth(110); label->SetWidth(210);
label->SetPos(10,m_gwenInternalData->m_curYposition); label->SetPos(10,m_gwenInternalData->m_curYposition);
m_gwenInternalData->m_curYposition+=22; m_gwenInternalData->m_curYposition+=22;
@@ -160,7 +160,7 @@ void GwenParameterInterface::registerSliderFloatParameter(SliderParams& params)
m_paramInternalData->m_sliders.push_back(pSlider); m_paramInternalData->m_sliders.push_back(pSlider);
//m_data->m_myControls.push_back(pSlider); //m_data->m_myControls.push_back(pSlider);
pSlider->SetPos( 10, m_gwenInternalData->m_curYposition ); pSlider->SetPos( 10, m_gwenInternalData->m_curYposition );
pSlider->SetSize( 100, 20 ); pSlider->SetSize( 200, 20 );
pSlider->SetRange( params.m_minVal, params.m_maxVal); pSlider->SetRange( params.m_minVal, params.m_maxVal);
pSlider->SetNotchCount(128);//float(params.m_maxVal-params.m_minVal)/100.f); pSlider->SetNotchCount(128);//float(params.m_maxVal-params.m_minVal)/100.f);
pSlider->SetClampToNotches( params.m_clampToNotches ); 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); Gwen::Controls::ScrollControl* windowRight= new Gwen::Controls::ScrollControl(m_data->pCanvas);
windowRight->Dock(Gwen::Pos::Right); windowRight->Dock(Gwen::Pos::Right);
windowRight->SetWidth(150); windowRight->SetWidth(250);
windowRight->SetHeight(250); windowRight->SetHeight(250);
windowRight->SetScroll(false,true); 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); Gwen::Controls::TabControl* tab = new Gwen::Controls::TabControl(windowRight);
//tab->SetHeight(300); //tab->SetHeight(300);
tab->SetWidth(140); tab->SetWidth(240);
tab->SetHeight(1250); tab->SetHeight(1250);
//tab->Dock(Gwen::Pos::Left); //tab->Dock(Gwen::Pos::Left);
tab->Dock( Gwen::Pos::Fill ); tab->Dock( Gwen::Pos::Fill );
@@ -438,7 +438,7 @@ void GwenUserInterface::registerToggleButton(int buttonId, const char* name)
///some heuristic to find the button location ///some heuristic to find the button location
int ypos = m_data->m_curYposition; int ypos = m_data->m_curYposition;
but->SetPos(10, ypos ); but->SetPos(10, ypos );
but->SetWidth( 100 ); but->SetWidth( 200 );
//but->SetBounds( 200, 30, 300, 200 ); //but->SetBounds( 200, 30, 300, 200 );
MyButtonHander* handler = new MyButtonHander(m_data, buttonId); 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) if (createGround)
{ {
btVector3 groundHalfExtents(20,20,20); 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)); m_dynamicsWorld->setGravity(btVector3(0,-10,0));
{ {
bool floating = false; bool fixedBase = true;
bool damping = false; bool damping = false;
bool gyro = false; bool gyro = false;
int numLinks = 2; int numLinks = 2;
bool spherical = false; //set it ot false -to use 1DoF hinges instead of 3DoF sphericals bool spherical = false; //set it ot false -to use 1DoF hinges instead of 3DoF sphericals
bool canSleep = false; bool canSleep = false;
bool selfCollide = 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 baseHalfExtents(0.04, 0.35, 0.08);
btVector3 basePosition = btVector3(-0.4f, 3.f, 0.f); btVector3 basePosition = btVector3(-0.4f, 3.f, 0.f);
//mbC->forceMultiDof(); //if !spherical, you can comment this line to check the 1DoF algorithm //mbC->forceMultiDof(); //if !spherical, you can comment this line to check the 1DoF algorithm
//init the base //init the base
btVector3 baseInertiaDiag(0.f, 0.f, 0.f); btVector3 baseInertiaDiag(0.f, 0.f, 0.f);
float baseMass = 1.f; float baseMass = 0.f;
if(baseMass) if(baseMass)
{ {
@@ -137,7 +137,7 @@ void InvertedPendulumPDControl::initPhysics()
} }
bool isMultiDof = true; 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; m_multiBody = pMultiBody;
btQuaternion baseOriQuat(0.f, 0.f, 0.f, 1.f); btQuaternion baseOriQuat(0.f, 0.f, 0.f, 1.f);
@@ -295,7 +295,7 @@ void InvertedPendulumPDControl::initPhysics()
tr.setRotation(orn); tr.setRotation(orn);
col->setWorldTransform(tr); col->setWorldTransform(tr);
bool isDynamic = (baseMass > 0 && floating); bool isDynamic = (baseMass > 0 && !fixedBase);
short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(btBroadphaseProxy::StaticFilter); short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(btBroadphaseProxy::StaticFilter);
short collisionFilterMask = isDynamic? short(btBroadphaseProxy::AllFilter) : short(btBroadphaseProxy::AllFilter ^ 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; int dof1 = 0;
btScalar qActual = m_multiBody->getJointPosMultiDof(joint)[dof1]; btScalar qActual = m_multiBody->getJointPosMultiDof(joint)[dof1];
btScalar qdActual = m_multiBody->getJointVelMultiDof(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 positionError = (qDesiredArray[joint]-qActual);
double desiredVelocity = 0;
btScalar force = kp * positionError - kd*qdActual; btScalar velocityError = (desiredVelocity-qdActual);
btScalar force = kp * positionError + kd*velocityError;
btClamp(force,-maxForce,maxForce); btClamp(force,-maxForce,maxForce);
// b3Printf("force = %f positionError = %f, qDesired = %f\n", force,positionError, target);
m_multiBody->addJointTorque(joint, force); 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); this->m_guiHelper->getAppInterface()->dumpNextFrameToPng(fileName);
} }
} }
m_dynamicsWorld->stepSimulation(1./240,0); m_dynamicsWorld->stepSimulation(1./60.,0);//240,0);
static int count = 0; static int count = 0;
if ((count& 0x0f)==0) if ((count& 0x0f)==0)

202
examples/SharedMemory/PhysicsClient.cpp
View File

@@ -2,6 +2,8 @@
#include "PosixSharedMemory.h" #include "PosixSharedMemory.h"
#include "Win32SharedMemory.h" #include "Win32SharedMemory.h"
#include "LinearMath/btAlignedObjectArray.h" #include "LinearMath/btAlignedObjectArray.h"
#include "LinearMath/btVector3.h"
#include "Bullet3Common/b3Logging.h" #include "Bullet3Common/b3Logging.h"
#include "../Utils/b3ResourcePath.h" #include "../Utils/b3ResourcePath.h"
#include "../../Extras/Serialize/BulletFileLoader/btBulletFile.h" #include "../../Extras/Serialize/BulletFileLoader/btBulletFile.h"
@@ -23,15 +25,19 @@ struct PhysicsClientSharedMemoryInternalData
btAlignedObjectArray<bParse::btBulletFile*> m_robotMultiBodyData; btAlignedObjectArray<bParse::btBulletFile*> m_robotMultiBodyData;
btAlignedObjectArray<b3JointInfo> m_jointInfo; btAlignedObjectArray<b3JointInfo> m_jointInfo;
btAlignedObjectArray<btVector3> m_debugLinesFrom;
btAlignedObjectArray<btVector3> m_debugLinesTo;
btAlignedObjectArray<btVector3> m_debugLinesColor;
int m_sharedMemoryKey;
int m_counter; int m_counter;
bool m_serverLoadUrdfOK; bool m_serverLoadUrdfOK;
bool m_isConnected; bool m_isConnected;
bool m_waitingForServer; bool m_waitingForServer;
bool m_hasLastServerStatus; bool m_hasLastServerStatus;
int m_sharedMemoryKey;
bool m_verboseOutput;
PhysicsClientSharedMemoryInternalData() PhysicsClientSharedMemoryInternalData()
:m_sharedMemory(0), :m_sharedMemory(0),
m_testBlock1(0), m_testBlock1(0),
@@ -40,7 +46,8 @@ struct PhysicsClientSharedMemoryInternalData
m_isConnected(false), m_isConnected(false),
m_waitingForServer(false), m_waitingForServer(false),
m_hasLastServerStatus(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; return false;
} else } 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; m_data->m_isConnected = true;
} }
} else } else
@@ -166,13 +176,19 @@ bool PhysicsClientSharedMemory::processServerStatus(SharedMemoryStatus& serverSt
{ {
case CMD_CLIENT_COMMAND_COMPLETED: case CMD_CLIENT_COMMAND_COMPLETED:
{ {
b3Printf("Server completed command"); if (m_data->m_verboseOutput)
{
b3Printf("Server completed command");
}
break; break;
} }
case CMD_URDF_LOADING_COMPLETED: case CMD_URDF_LOADING_COMPLETED:
{ {
m_data->m_serverLoadUrdfOK = true; 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) 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]; Bullet::btMultiBodyDoubleData* mb = (Bullet::btMultiBodyDoubleData*)bf->m_multiBodies[i];
if (mb->m_baseName) 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++) for (int link=0;link<mb->m_numLinks;link++)
{ {
{ {
@@ -201,15 +221,22 @@ bool PhysicsClientSharedMemory::processServerStatus(SharedMemoryStatus& serverSt
info.m_flags = 0; info.m_flags = 0;
info.m_qIndex = qOffset; info.m_qIndex = qOffset;
info.m_uIndex = uOffset; info.m_uIndex = uOffset;
info.m_linkIndex = link;
if (mb->m_links[link].m_linkName) 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; info.m_linkName = mb->m_links[link].m_linkName;
} }
if (mb->m_links[link].m_jointName) 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_jointName = mb->m_links[link].m_jointName;
info.m_jointType = mb->m_links[link].m_jointType; 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]; Bullet::btMultiBodyFloatData* mb = (Bullet::btMultiBodyFloatData*) bf->m_multiBodies[i];
if (mb->m_baseName) 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++) 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) 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; info.m_linkName = mb->m_links[link].m_linkName;
} }
if (mb->m_links[link].m_jointName) 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_jointName = mb->m_links[link].m_jointName;
info.m_jointType = mb->m_links[link].m_jointType; info.m_jointType = mb->m_links[link].m_jointType;
} }
@@ -266,7 +302,10 @@ bool PhysicsClientSharedMemory::processServerStatus(SharedMemoryStatus& serverSt
} }
if (bf->ok()) if (bf->ok())
{ {
b3Printf("Received robot description ok!\n"); if (m_data->m_verboseOutput)
{
b3Printf("Received robot description ok!\n");
}
} else } else
{ {
b3Warning("Robot description not received"); b3Warning("Robot description not received");
@@ -276,24 +315,36 @@ bool PhysicsClientSharedMemory::processServerStatus(SharedMemoryStatus& serverSt
} }
case CMD_DESIRED_STATE_RECEIVED_COMPLETED: case CMD_DESIRED_STATE_RECEIVED_COMPLETED:
{ {
b3Printf("Server received desired state"); if (m_data->m_verboseOutput)
{
b3Printf("Server received desired state");
}
break; break;
} }
case CMD_STEP_FORWARD_SIMULATION_COMPLETED: case CMD_STEP_FORWARD_SIMULATION_COMPLETED:
{ {
b3Printf("Server completed step simulation"); if (m_data->m_verboseOutput)
{
b3Printf("Server completed step simulation");
}
break; break;
} }
case CMD_URDF_LOADING_FAILED: 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; m_data->m_serverLoadUrdfOK = false;
break; break;
} }
case CMD_BULLET_DATA_STREAM_RECEIVED_COMPLETED: 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: 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; break;
} }
@@ -310,12 +364,18 @@ bool PhysicsClientSharedMemory::processServerStatus(SharedMemoryStatus& serverSt
case CMD_ACTUAL_STATE_UPDATE_COMPLETED: 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]; SharedMemoryStatus& command = m_data->m_testBlock1->m_serverCommands[0];
int numQ = command.m_sendActualStateArgs.m_numDegreeOfFreedomQ; int numQ = command.m_sendActualStateArgs.m_numDegreeOfFreedomQ;
int numU = command.m_sendActualStateArgs.m_numDegreeOfFreedomU; 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]; char msg[1024];
{ {
@@ -333,7 +393,10 @@ bool PhysicsClientSharedMemory::processServerStatus(SharedMemoryStatus& serverSt
} }
sprintf(msg,"%s]",msg); sprintf(msg,"%s]",msg);
} }
b3Printf(msg); if (m_data->m_verboseOutput)
{
b3Printf(msg);
}
{ {
sprintf(msg,"U=["); sprintf(msg,"U=[");
@@ -351,12 +414,52 @@ bool PhysicsClientSharedMemory::processServerStatus(SharedMemoryStatus& serverSt
sprintf(msg,"%s]",msg); sprintf(msg,"%s]",msg);
} }
b3Printf(msg); if (m_data->m_verboseOutput)
{
b3Printf(msg);
}
b3Printf("\n"); if (m_data->m_verboseOutput)
{
b3Printf("\n");
}
break; 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: default:
{ {
b3Error("Unknown server status\n"); b3Error("Unknown server status\n");
@@ -378,8 +481,11 @@ bool PhysicsClientSharedMemory::processServerStatus(SharedMemoryStatus& serverSt
} }
} else } 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); m_data->m_testBlock1->m_numProcessedServerCommands);
}
} }
return hasStatus; return hasStatus;
} }
@@ -407,8 +513,8 @@ bool PhysicsClientSharedMemory::submitClientCommand(const SharedMemoryCommand& c
m_data->m_robotMultiBodyData.clear(); m_data->m_robotMultiBodyData.clear();
m_data->m_jointInfo.clear(); m_data->m_jointInfo.clear();
} }
m_data->m_testBlock1->m_clientCommands[0] = command; m_data->m_testBlock1->m_clientCommands[0] = command;
m_data->m_testBlock1->m_numClientCommands++; m_data->m_testBlock1->m_numClientCommands++;
m_data->m_waitingForServer = true; m_data->m_waitingForServer = true;
@@ -417,3 +523,47 @@ bool PhysicsClientSharedMemory::submitClientCommand(const SharedMemoryCommand& c
return false; 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 #define BT_PHYSICS_CLIENT_API_H
#include "SharedMemoryCommands.h" #include "SharedMemoryCommands.h"
#include "LinearMath/btVector3.h"
class PhysicsClientSharedMemory
class PhysicsClientSharedMemory //: public CommonPhysicsClientInterface
{ {
struct PhysicsClientSharedMemoryInternalData* m_data; struct PhysicsClientSharedMemoryInternalData* m_data;
protected: protected:
@@ -33,6 +33,15 @@ public:
virtual void getJointInfo(int index, b3JointInfo& info) const; virtual void getJointInfo(int index, b3JointInfo& info) const;
virtual void setSharedMemoryKey(int key); 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 #endif //BT_PHYSICS_CLIENT_API_H

35
examples/SharedMemory/PhysicsClientExample.cpp
View File

@@ -47,11 +47,12 @@ public:
virtual void resetCamera() virtual void resetCamera()
{ {
float dist = 1; float dist = 5;
float pitch = 50; float pitch = 50;
float yaw = 35; 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]); m_guiHelper->resetCamera(dist,pitch,yaw,targetPos[0],targetPos[1],targetPos[2]);
} }
virtual bool wantsTermination() virtual bool wantsTermination()
@@ -67,7 +68,18 @@ public:
void enqueueCommand(const SharedMemoryCommand& orgCommand); void enqueueCommand(const SharedMemoryCommand& orgCommand);
virtual void exitPhysics(){}; 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 void physicsDebugDraw(int debugFlags){}
virtual bool mouseMoveCallback(float x,float y){return false;}; virtual bool mouseMoveCallback(float x,float y){return false;};
virtual bool mouseButtonCallback(int button, int state, 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: case CMD_LOAD_URDF:
{ {
command.m_type =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_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[1] = 0.0;
command.m_urdfArguments.m_initialPosition[2] = 0.0; command.m_urdfArguments.m_initialPosition[2] = 0.0;
command.m_urdfArguments.m_initialOrientation[0] = 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; command.m_type =CMD_STEP_FORWARD_SIMULATION;
cl->enqueueCommand(command); cl->enqueueCommand(command);
command.m_type =CMD_REQUEST_DEBUG_LINES;
cl->enqueueCommand(command);
break; break;
} }
@@ -230,6 +247,9 @@ void PhysicsClientExample::initPhysics()
{ {
if (m_guiHelper && m_guiHelper->getParameterInterface()) if (m_guiHelper && m_guiHelper->getParameterInterface())
{ {
int upAxis = 2;
m_guiHelper->setUpAxis(upAxis);
createButtons(); createButtons();
} else } 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); 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) if (hasStatus && status.m_type == CMD_URDF_LOADING_COMPLETED)
{ {
for (int i=0;i<m_physicsClient.getNumJoints();i++) 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 struct PhysicsServerInternalData
{ {
SharedMemoryInterface* m_sharedMemory; SharedMemoryInterface* m_sharedMemory;
@@ -51,18 +92,23 @@ struct PhysicsServerInternalData
btMultiBodyConstraintSolver* m_solver; btMultiBodyConstraintSolver* m_solver;
btDefaultCollisionConfiguration* m_collisionConfiguration; btDefaultCollisionConfiguration* m_collisionConfiguration;
btMultiBodyDynamicsWorld* m_dynamicsWorld; btMultiBodyDynamicsWorld* m_dynamicsWorld;
SharedMemoryDebugDrawer* m_debugDrawer;
struct GUIHelperInterface* m_guiHelper; struct GUIHelperInterface* m_guiHelper;
int m_sharedMemoryKey; int m_sharedMemoryKey;
bool m_verboseOutput;
PhysicsServerInternalData() PhysicsServerInternalData()
:m_sharedMemory(0), :m_sharedMemory(0),
m_testBlock1(0), m_testBlock1(0),
m_isConnected(false), m_isConnected(false),
m_physicsDeltaTime(1./60.), m_physicsDeltaTime(1./240.),//240.),
m_dynamicsWorld(0), m_dynamicsWorld(0),
m_debugDrawer(0),
m_guiHelper(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_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_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)); m_data->m_dynamicsWorld->setGravity(btVector3(0, 0, 0));
} }
@@ -189,6 +238,9 @@ void PhysicsServerSharedMemory::deleteDynamicsWorld()
delete m_data->m_dynamicsWorld; delete m_data->m_dynamicsWorld;
m_data->m_dynamicsWorld=0; m_data->m_dynamicsWorld=0;
delete m_data->m_debugDrawer;
m_data->m_debugDrawer=0;
delete m_data->m_solver; delete m_data->m_solver;
m_data->m_solver=0; m_data->m_solver=0;
@@ -221,12 +273,18 @@ bool PhysicsServerSharedMemory::connectSharedMemory( struct GUIHelperInterface*
if (m_data->m_testBlock1) if (m_data->m_testBlock1)
{ {
int magicId =m_data->m_testBlock1->m_magicId; 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) if (m_data->m_testBlock1->m_magicId !=SHARED_MEMORY_MAGIC_NUMBER)
{ {
InitSharedMemoryBlock(m_data->m_testBlock1); 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; m_data->m_isConnected = true;
} else } else
{ {
@@ -246,21 +304,33 @@ bool PhysicsServerSharedMemory::connectSharedMemory( struct GUIHelperInterface*
void PhysicsServerSharedMemory::disconnectSharedMemory(bool deInitializeSharedMemory) void PhysicsServerSharedMemory::disconnectSharedMemory(bool deInitializeSharedMemory)
{ {
b3Printf("releaseSharedMemory1\n"); if (m_data->m_verboseOutput)
{
b3Printf("releaseSharedMemory1\n");
}
if (m_data->m_testBlock1) if (m_data->m_testBlock1)
{ {
b3Printf("m_testBlock1\n"); if (m_data->m_verboseOutput)
{
b3Printf("m_testBlock1\n");
}
if (deInitializeSharedMemory) if (deInitializeSharedMemory)
{ {
m_data->m_testBlock1->m_magicId = 0; 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); btAssert(m_data->m_sharedMemory);
m_data->m_sharedMemory->releaseSharedMemory(m_data->m_sharedMemoryKey, SHARED_MEMORY_SIZE); m_data->m_sharedMemory->releaseSharedMemory(m_data->m_sharedMemoryKey, SHARED_MEMORY_SIZE);
} }
if (m_data->m_sharedMemory) if (m_data->m_sharedMemory)
{ {
b3Printf("m_sharedMemory\n"); if (m_data->m_verboseOutput)
{
b3Printf("m_sharedMemory\n");
}
delete m_data->m_sharedMemory; delete m_data->m_sharedMemory;
m_data->m_sharedMemory = 0; m_data->m_sharedMemory = 0;
m_data->m_testBlock1 = 0; m_data->m_testBlock1 = 0;
@@ -269,19 +339,31 @@ void PhysicsServerSharedMemory::disconnectSharedMemory(bool deInitializeSharedMe
void PhysicsServerSharedMemory::releaseSharedMemory() void PhysicsServerSharedMemory::releaseSharedMemory()
{ {
b3Printf("releaseSharedMemory1\n"); if (m_data->m_verboseOutput)
{
b3Printf("releaseSharedMemory1\n");
}
if (m_data->m_testBlock1) 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; 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); btAssert(m_data->m_sharedMemory);
m_data->m_sharedMemory->releaseSharedMemory( m_data->m_sharedMemoryKey m_data->m_sharedMemory->releaseSharedMemory( m_data->m_sharedMemoryKey
, SHARED_MEMORY_SIZE); , SHARED_MEMORY_SIZE);
} }
if (m_data->m_sharedMemory) if (m_data->m_sharedMemory)
{ {
b3Printf("m_sharedMemory\n"); if (m_data->m_verboseOutput)
{
b3Printf("m_sharedMemory\n");
}
delete m_data->m_sharedMemory; delete m_data->m_sharedMemory;
m_data->m_sharedMemory = 0; m_data->m_sharedMemory = 0;
m_data->m_testBlock1 = 0; m_data->m_testBlock1 = 0;
@@ -336,7 +418,10 @@ bool PhysicsServerSharedMemory::loadUrdf(const char* fileName, const btVector3&
bool loadOk = u2b.loadURDF(fileName, useFixedBase); bool loadOk = u2b.loadURDF(fileName, useFixedBase);
if (loadOk) if (loadOk)
{ {
b3Printf("loaded %s OK!", fileName); if (m_data->m_verboseOutput)
{
b3Printf("loaded %s OK!", fileName);
}
btTransform tr; btTransform tr;
tr.setIdentity(); tr.setIdentity();
@@ -438,7 +523,10 @@ void PhysicsServerSharedMemory::processClientCommands()
{ {
case CMD_SEND_BULLET_DATA_STREAM: 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); btBulletWorldImporter* worldImporter = new btBulletWorldImporter(m_data->m_dynamicsWorld);
m_data->m_worldImporters.push_back(worldImporter); m_data->m_worldImporters.push_back(worldImporter);
@@ -457,6 +545,41 @@ void PhysicsServerSharedMemory::processClientCommands()
break; 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: case CMD_LOAD_URDF:
{ {
//at the moment, we only load 1 urdf / robot //at the moment, we only load 1 urdf / robot
@@ -467,7 +590,10 @@ void PhysicsServerSharedMemory::processClientCommands()
break; break;
} }
const UrdfArgs& urdfArgs = clientCmd.m_urdfArguments; 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((clientCmd.m_updateFlags&URDF_ARGS_FILE_NAME) !=0);
btAssert(urdfArgs.m_urdfFileName); btAssert(urdfArgs.m_urdfFileName);
btVector3 initialPos(0,0,0); btVector3 initialPos(0,0,0);
@@ -517,7 +643,10 @@ void PhysicsServerSharedMemory::processClientCommands()
} }
case CMD_CREATE_SENSOR: 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) if (m_data->m_dynamicsWorld->getNumMultibodies()>0)
{ {
@@ -580,7 +709,10 @@ void PhysicsServerSharedMemory::processClientCommands()
} }
case CMD_SEND_DESIRED_STATE: case CMD_SEND_DESIRED_STATE:
{ {
if (m_data->m_verboseOutput)
{
b3Printf("Processed CMD_SEND_DESIRED_STATE"); b3Printf("Processed CMD_SEND_DESIRED_STATE");
}
if (m_data->m_dynamicsWorld->getNumMultibodies()>0) if (m_data->m_dynamicsWorld->getNumMultibodies()>0)
{ {
btMultiBody* mb = m_data->m_dynamicsWorld->getMultiBody(0); btMultiBody* mb = m_data->m_dynamicsWorld->getMultiBody(0);
@@ -590,7 +722,10 @@ void PhysicsServerSharedMemory::processClientCommands()
{ {
case CONTROL_MODE_TORQUE: case CONTROL_MODE_TORQUE:
{ {
b3Printf("Using CONTROL_MODE_TORQUE"); if (m_data->m_verboseOutput)
{
b3Printf("Using CONTROL_MODE_TORQUE");
}
mb->clearForcesAndTorques(); mb->clearForcesAndTorques();
int torqueIndex = 0; int torqueIndex = 0;
@@ -617,7 +752,10 @@ void PhysicsServerSharedMemory::processClientCommands()
} }
case CONTROL_MODE_VELOCITY: case CONTROL_MODE_VELOCITY:
{ {
b3Printf("Using CONTROL_MODE_VELOCITY"); if (m_data->m_verboseOutput)
{
b3Printf("Using CONTROL_MODE_VELOCITY");
}
int numMotors = 0; int numMotors = 0;
//find the joint motors and apply the desired velocity and maximum force/torque //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; 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; break;
} }
default: default:
{ {
b3Printf("m_controlMode not implemented yet"); b3Warning("m_controlMode not implemented yet");
break; break;
} }
@@ -663,7 +858,10 @@ void PhysicsServerSharedMemory::processClientCommands()
} }
case CMD_REQUEST_ACTUAL_STATE: 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) if (m_data->m_dynamicsWorld->getNumMultibodies()>0)
{ {
btMultiBody* mb = m_data->m_dynamicsWorld->getMultiBody(0); btMultiBody* mb = m_data->m_dynamicsWorld->getMultiBody(0);
@@ -754,8 +952,11 @@ void PhysicsServerSharedMemory::processClientCommands()
case CMD_STEP_FORWARD_SIMULATION: case CMD_STEP_FORWARD_SIMULATION:
{ {
b3Printf("Step simulation request"); if (m_data->m_verboseOutput)
m_data->m_dynamicsWorld->stepSimulation(m_data->m_physicsDeltaTime); {
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); SharedMemoryStatus& serverCmd =m_data->createServerStatus(CMD_STEP_FORWARD_SIMULATION_COMPLETED,clientCmd.m_sequenceNumber,timeStamp);
m_data->submitServerStatus(serverCmd); m_data->submitServerStatus(serverCmd);
@@ -771,7 +972,10 @@ void PhysicsServerSharedMemory::processClientCommands()
clientCmd.m_physSimParamArgs.m_gravityAcceleration[1], clientCmd.m_physSimParamArgs.m_gravityAcceleration[1],
clientCmd.m_physSimParamArgs.m_gravityAcceleration[2]); clientCmd.m_physSimParamArgs.m_gravityAcceleration[2]);
this->m_data->m_dynamicsWorld->setGravity(grav); 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: 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 ///@todo: implement this
m_data->m_dynamicsWorld->setGravity(btVector3(0,0,0)); 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) if (clientCmd.m_updateFlags & BOX_SHAPE_HAS_INITIAL_ORIENTATION)
{ {
b3Printf("test\n");
startTrans.setRotation(btQuaternion( startTrans.setRotation(btQuaternion(
clientCmd.m_createBoxShapeArguments.m_initialOrientation[0], clientCmd.m_createBoxShapeArguments.m_initialOrientation[0],
clientCmd.m_createBoxShapeArguments.m_initialOrientation[1], clientCmd.m_createBoxShapeArguments.m_initialOrientation[1],
@@ -884,10 +1090,9 @@ void PhysicsServerSharedMemory::physicsDebugDraw(int debugDrawFlags)
{ {
if (m_data->m_dynamicsWorld->getDebugDrawer()) if (m_data->m_dynamicsWorld->getDebugDrawer())
{ {
m_data->m_debugDrawer->m_lines.clear();
m_data->m_dynamicsWorld->getDebugDrawer()->setDebugMode(debugDrawFlags); m_data->m_dynamicsWorld->getDebugDrawer()->setDebugMode(debugDrawFlags);
} }
m_data->m_dynamicsWorld->debugDrawWorld(); m_data->m_dynamicsWorld->debugDrawWorld();
} }
} }

12
examples/SharedMemory/PhysicsServer.h
View File

@@ -1,6 +1,16 @@
#ifndef PHYSICS_SERVER_SHARED_MEMORY_H #ifndef PHYSICS_SERVER_SHARED_MEMORY_H
#define 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 class PhysicsServerSharedMemory
{ {
struct PhysicsServerInternalData* m_data; struct PhysicsServerInternalData* m_data;
@@ -37,7 +47,7 @@ public:
//to a physics client, over shared memory //to a physics client, over shared memory
void physicsDebugDraw(int debugDrawFlags); void physicsDebugDraw(int debugDrawFlags);
void renderScene(); void renderScene();
}; };

262
examples/SharedMemory/RobotControlExample.cpp
View File

@@ -11,6 +11,7 @@
#include "SharedMemoryCommon.h" #include "SharedMemoryCommon.h"
#include "../Utils/b3Clock.h" #include "../Utils/b3Clock.h"
#include "PhysicsClientC_API.h" #include "PhysicsClientC_API.h"
#include "../Utils/b3ResourcePath.h"
//const char* blaatnaam = "basename"; //const char* blaatnaam = "basename";
@@ -18,8 +19,11 @@
struct MyMotorInfo struct MyMotorInfo
{ {
btScalar m_velTarget; btScalar m_velTarget;
btScalar m_posTarget;
btScalar m_maxForce; btScalar m_maxForce;
int m_uIndex; int m_uIndex;
int m_posIndex;
}; };
#define MAX_NUM_MOTORS 128 #define MAX_NUM_MOTORS 128
@@ -39,18 +43,21 @@ class RobotControlExample : public SharedMemoryCommon
public: public:
//@todo, add accessor methods //@todo, add accessor methods
MyMotorInfo m_motorTargetVelocities[MAX_NUM_MOTORS]; MyMotorInfo m_motorTargetState[MAX_NUM_MOTORS];
int m_numMotors;
int m_numMotors;
RobotControlExample(GUIHelperInterface* helper); int m_option;
bool m_verboseOutput;
RobotControlExample(GUIHelperInterface* helper, int option);
virtual ~RobotControlExample(); virtual ~RobotControlExample();
virtual void initPhysics(); virtual void initPhysics();
virtual void stepSimulation(float deltaTime); virtual void stepSimulation(float deltaTime);
void prepareControlCommand(SharedMemoryCommand& cmd);
void enqueueCommand(const SharedMemoryCommand& orgCommand) void enqueueCommand(const SharedMemoryCommand& orgCommand)
{ {
m_userCommandRequests.push_back(orgCommand); m_userCommandRequests.push_back(orgCommand);
@@ -58,7 +65,10 @@ public:
cmd.m_sequenceNumber = m_sequenceNumberGenerator++; cmd.m_sequenceNumber = m_sequenceNumberGenerator++;
cmd.m_timeStamp = m_realtimeClock.getTimeMicroseconds(); 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() virtual void resetCamera()
@@ -81,6 +91,7 @@ public:
virtual void physicsDebugDraw(int debugFlags) virtual void physicsDebugDraw(int debugFlags)
{ {
m_physicsServer.physicsDebugDraw(debugFlags); m_physicsServer.physicsDebugDraw(debugFlags);
} }
virtual bool mouseMoveCallback(float x,float y){return false;}; virtual bool mouseMoveCallback(float x,float y){return false;};
virtual bool mouseButtonCallback(int button, int state, 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_type =CMD_LOAD_URDF;
command.m_updateFlags = URDF_ARGS_FILE_NAME|URDF_ARGS_INITIAL_POSITION|URDF_ARGS_INITIAL_ORIENTATION; 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[0] = 0.0;
command.m_urdfArguments.m_initialPosition[1] = 0.0; command.m_urdfArguments.m_initialPosition[1] = 0.0;
command.m_urdfArguments.m_initialPosition[2] = 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 //#ifdef USE_C_API
b3InitPhysicsParamCommand(&command); b3InitPhysicsParamCommand(&command);
b3PhysicsParamSetGravity(&command, 0,0,-10); b3PhysicsParamSetGravity(&command, 1,1,-10);
// #else // #else
@@ -182,54 +193,16 @@ void MyCallback2(int buttonId, bool buttonState, void* userPtr)
case CMD_SEND_DESIRED_STATE: case CMD_SEND_DESIRED_STATE:
{ {
command.m_type =CMD_SEND_DESIRED_STATE; command.m_type =CMD_SEND_DESIRED_STATE;
int controlMode = CONTROL_MODE_VELOCITY;//CONTROL_MODE_TORQUE; cl->prepareControlCommand(command);
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);
}
}
cl->enqueueCommand(command); cl->enqueueCommand(command);
break; break;
} }
case CMD_SEND_BULLET_DATA_STREAM: case CMD_SEND_BULLET_DATA_STREAM:
{ {
command.m_type = buttonId; command.m_type = buttonId;
sprintf(command.m_dataStreamArguments.m_bulletFileName,"slope.bullet");
command.m_dataStreamArguments.m_streamChunkLength = 0;
cl->enqueueCommand(command); cl->enqueueCommand(command);
break; 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 ) void RobotControlExample::createButton(const char* name, int buttonId, bool isTrigger )
{ {
ButtonParams button(name,buttonId, 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); m_guiHelper->getParameterInterface()->registerButtonParameter(button);
} }
RobotControlExample::RobotControlExample(GUIHelperInterface* helper) RobotControlExample::RobotControlExample(GUIHelperInterface* helper, int option)
:SharedMemoryCommon(helper), :SharedMemoryCommon(helper),
m_wantsShutdown(false), m_wantsShutdown(false),
m_sequenceNumberGenerator(0), m_sequenceNumberGenerator(0),
m_numMotors(0) m_numMotors(0),
m_option(option),
m_verboseOutput(false)
{ {
bool useServer = true; bool useServer = true;
@@ -328,7 +363,8 @@ bool RobotControlExample::wantsTermination()
void RobotControlExample::stepSimulation(float deltaTime) void RobotControlExample::stepSimulation(float deltaTime)
{ {
m_physicsServer.processClientCommands();
m_physicsServer.processClientCommands();
if (m_physicsClient.isConnected()) if (m_physicsClient.isConnected())
{ {
@@ -341,23 +377,57 @@ void RobotControlExample::stepSimulation(float deltaTime)
{ {
b3JointInfo info; b3JointInfo info;
m_physicsClient.getJointInfo(i,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 (info.m_flags & JOINT_HAS_MOTORIZED_POWER)
{ {
if (m_numMotors<MAX_NUM_MOTORS) if (m_numMotors<MAX_NUM_MOTORS)
{ {
char motorName[1024];
sprintf(motorName,"%s q'", info.m_jointName); switch (m_option)
MyMotorInfo* motorInfo = &m_motorTargetVelocities[m_numMotors]; {
motorInfo->m_velTarget = 0.f; case ROBOT_VELOCITY_CONTROL:
motorInfo->m_uIndex = info.m_uIndex; {
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); SliderParams slider(motorName,&motorInfo->m_velTarget);
slider.m_minVal=-4; slider.m_minVal=-4;
slider.m_maxVal=4; slider.m_maxVal=4;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
m_numMotors++; 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()) if (m_userCommandRequests.size())
{ {
b3Printf("Outstanding user command requests: %d\n", m_userCommandRequests.size()); if (m_verboseOutput)
SharedMemoryCommand& cmd = m_userCommandRequests[0]; {
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 //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++) for (int i=1;i<m_userCommandRequests.size();i++)
@@ -379,7 +452,62 @@ void RobotControlExample::stepSimulation(float deltaTime)
} }
m_userCommandRequests.pop_back(); 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); 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) 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) if (gSharedMemoryKey>=0)
{ {
example->setSharedMemoryKey(gSharedMemoryKey); example->setSharedMemoryKey(gSharedMemoryKey);

6
examples/SharedMemory/RobotControlExample.h
View File

@@ -1,6 +1,12 @@
#ifndef ROBOT_CONTROL_EXAMPLE_H #ifndef ROBOT_CONTROL_EXAMPLE_H
#define 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); class CommonExampleInterface* RobotControlExampleCreateFunc(struct CommonExampleOptions& options);
#endif //ROBOT_CONTROL_EXAMPLE_H #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_PHYSICS_SIMULATION_PARAMETERS,
CMD_SEND_DESIRED_STATE,//todo: reconsider naming, for example SET_JOINT_CONTROL_VARIABLE? CMD_SEND_DESIRED_STATE,//todo: reconsider naming, for example SET_JOINT_CONTROL_VARIABLE?
CMD_REQUEST_ACTUAL_STATE, CMD_REQUEST_ACTUAL_STATE,
CMD_REQUEST_DEBUG_LINES,
CMD_STEP_FORWARD_SIMULATION, CMD_STEP_FORWARD_SIMULATION,
CMD_RESET_SIMULATION, CMD_RESET_SIMULATION,
CMD_MAX_CLIENT_COMMANDS CMD_MAX_CLIENT_COMMANDS
@@ -60,6 +61,8 @@ enum EnumSharedMemoryServerStatus
CMD_SET_JOINT_FEEDBACK_COMPLETED, CMD_SET_JOINT_FEEDBACK_COMPLETED,
CMD_ACTUAL_STATE_UPDATE_COMPLETED, CMD_ACTUAL_STATE_UPDATE_COMPLETED,
CMD_ACTUAL_STATE_UPDATE_FAILED, CMD_ACTUAL_STATE_UPDATE_FAILED,
CMD_DEBUG_LINES_COMPLETED,
CMD_DEBUG_LINES_OVERFLOW_FAILED,
CMD_DESIRED_STATE_RECEIVED_COMPLETED, CMD_DESIRED_STATE_RECEIVED_COMPLETED,
CMD_STEP_FORWARD_SIMULATION_COMPLETED, CMD_STEP_FORWARD_SIMULATION_COMPLETED,
CMD_MAX_SERVER_COMMANDS CMD_MAX_SERVER_COMMANDS
@@ -69,6 +72,7 @@ enum EnumSharedMemoryServerStatus
#define MAX_DEGREE_OF_FREEDOM 256 #define MAX_DEGREE_OF_FREEDOM 256
#define MAX_NUM_SENSORS 256 #define MAX_NUM_SENSORS 256
#define MAX_URDF_FILENAME_LENGTH 1024 #define MAX_URDF_FILENAME_LENGTH 1024
#define MAX_FILENAME_LENGTH MAX_URDF_FILENAME_LENGTH
enum EnumUrdfArgsUpdateFlags enum EnumUrdfArgsUpdateFlags
{ {
@@ -92,6 +96,7 @@ struct UrdfArgs
struct BulletDataStreamArgs struct BulletDataStreamArgs
{ {
char m_bulletFileName[MAX_FILENAME_LENGTH];
int m_streamChunkLength; int m_streamChunkLength;
int m_bodyUniqueId; int m_bodyUniqueId;
}; };
@@ -108,6 +113,7 @@ enum {
// POSITION_CONTROL=0, // POSITION_CONTROL=0,
CONTROL_MODE_VELOCITY=0, CONTROL_MODE_VELOCITY=0,
CONTROL_MODE_TORQUE, CONTROL_MODE_TORQUE,
CONTROL_MODE_POSITION_VELOCITY_PD,
}; };
///InitPoseArgs is mainly to initialize (teleport) the robot in a particular position ///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. ///Controlling a robot involves sending the desired state to its joint motor controllers.
///The control mode determines the state variables used for motor control. ///The control mode determines the state variables used for motor control.
struct SendDesiredStateArgs struct SendDesiredStateArgs
@@ -128,12 +145,22 @@ struct SendDesiredStateArgs
int m_bodyUniqueId; int m_bodyUniqueId;
int m_controlMode; 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 //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]; 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]; double m_desiredStateQdot[MAX_DEGREE_OF_FREEDOM];
//m_desiredStateForceTorque is either the actual applied force/torque (in CONTROL_MODE_TORQUE) or //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]; double m_desiredStateForceTorque[MAX_DEGREE_OF_FREEDOM];
}; };
@@ -227,6 +254,7 @@ struct SharedMemoryCommand
struct RequestActualStateArgs m_requestActualStateInformationCommandArgument; struct RequestActualStateArgs m_requestActualStateInformationCommandArgument;
struct CreateSensorArgs m_createSensorArguments; struct CreateSensorArgs m_createSensorArguments;
struct CreateBoxShapeArgs m_createBoxShapeArguments; struct CreateBoxShapeArgs m_createBoxShapeArguments;
struct RequestDebugLinesArgs m_requestDebugLinesArguments;
}; };
}; };
@@ -242,6 +270,7 @@ struct SharedMemoryStatus
{ {
struct BulletDataStreamArgs m_dataStreamArguments; struct BulletDataStreamArgs m_dataStreamArguments;
struct SendActualStateArgs m_sendActualStateArgs; struct SendActualStateArgs m_sendActualStateArgs;
struct SendDebugLinesArgs m_sendDebugLinesArgs;
}; };
}; };
@@ -258,6 +287,7 @@ struct b3JointInfo
int m_jointType; int m_jointType;
int m_qIndex; int m_qIndex;
int m_uIndex; int m_uIndex;
int m_linkIndex;
/// ///
int m_flags; int m_flags;
}; };