Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Merge pull request #428 from erwincoumans/master

Browse Source 
add very simple (rudimentary) time series graphing example
This commit is contained in:
erwincoumans
2015-07-15 16:07:38 -07:00
parent 8f87ccee88 b563c7c8ce
commit b8e07e6a55
23 changed files with 1649 additions and 113 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
build3/premake4.lua
View File

@@ -66,6 +66,13 @@
description = "Enable Lua scipting support in Example Browser"
}
newoption {
trigger = "targetdir",
value = "path such as ../bin",
description = "Set the output location for the generated project files"
}
newoption
{
trigger = "without-gtest",
@@ -129,7 +136,9 @@
-- comment-out for now, URDF reader needs exceptions
-- flags { "NoRTTI", "NoExceptions"}
-- defines { "_HAS_EXCEPTIONS=0" }
targetdir "../bin"
--printf ( _OPTIONS["targetdir"] )
targetdir( _OPTIONS["targetdir"] or "../bin" )
location("./" .. act .. postfix)

15
build3/vs2010.bat
View File

@@ -1,7 +1,18 @@
rem premake4 --with-pe vs2010
rem premake4 --bullet2demos vs2010
premake4 vs2010
premake4 --targetdir="../bin" vs2010
rem premake4 --targetdir="../server2bin" vs2010
rem cd vs2010
rem rename 0_Bullet3Solution.sln 0_server.sln
rem cd ..
rem rename vs2010 vs2010_server
rem
rem premake4 --targetdir="../client2bin" vs2010
rem cd vs2010
rem rename 0_Bullet3Solution.sln 0_client.sln
rem cd ..
rem rename vs2010 vs2010_client
mkdir vs2010\cache
pause

49
data/hinge.urdf Normal file
View File

@@ -0,0 +1,49 @@
<?xml version="0.0" ?>
<robot name="urdf_robot">
<link name="baseLink">
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<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"/>
<geometry>
<box size="0.1 .1 .1"/>
</geometry>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.1 .1 .1"/>
</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"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.1 .1 .1"/>
</geometry>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.1 .1 .1"/>
</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"/>
</joint>
</robot>

BIN
data/r2d2_multibody.bullet
View File

Binary file not shown.

2
examples/CommonInterfaces/Common2dCanvasInterface.h
View File

@@ -7,6 +7,8 @@ struct Common2dCanvasInterface
virtual int createCanvas(const char* canvasName, int width, int height)=0;
virtual void destroyCanvas(int canvasId)=0;
virtual void setPixel(int canvasId, int x, int y, unsigned char red, unsigned char green,unsigned char blue, unsigned char alpha)=0;
virtual void getPixel(int canvasId, int x, int y, unsigned char& red, unsigned char& green,unsigned char& blue, unsigned char& alpha)=0;
virtual void refreshImageData(int canvasId)=0;
};

2
examples/CommonInterfaces/CommonGraphicsAppInterface.h
View File

@@ -187,7 +187,7 @@ struct CommonGraphicsApp
float cameraDistance = camera->getCameraDistance();
if (deltay<0 || cameraDistance>1)
{
cameraDistance -= deltay*0.1f;
cameraDistance -= deltay*0.01f;
if (cameraDistance<1)
cameraDistance=1;
camera->setCameraDistance(cameraDistance);

2
examples/ExampleBrowser/CMakeLists.txt
View File

@@ -19,6 +19,7 @@ SET(App_ExampleBrowser_SRCS
../SharedMemory/PhysicsClient.cpp
../SharedMemory/PhysicsServerExample.cpp
../SharedMemory/PhysicsClientExample.cpp
../SharedMemory/RobotControlExample.cpp
../SharedMemory/PosixSharedMemory.cpp
../SharedMemory/Win32SharedMemory.cpp
../BasicDemo/BasicExample.cpp
@@ -45,6 +46,7 @@ SET(App_ExampleBrowser_SRCS
../RenderingExamples/RaytracerSetup.h
../RenderingExamples/RenderInstancingDemo.cpp
../RenderingExamples/RenderInstancingDemo.h
../RenderingExamples/TimeSeriesExample.cpp
../Benchmarks/BenchmarkDemo.cpp
../Benchmarks/BenchmarkDemo.h
../Benchmarks/landscapeData.h

7
examples/ExampleBrowser/ExampleEntries.cpp
View File

@@ -32,8 +32,10 @@
#include "../DynamicControlDemo/MotorDemo.h"
#include "../RollingFrictionDemo/RollingFrictionDemo.h"
#include "../SharedMemory/PhysicsServerExample.h"
#include "../SharedMemory/RobotControlExample.h"
#include "../SharedMemory/PhysicsClientExample.h"
#include "../Constraints/TestHingeTorque.h"
#include "../RenderingExamples/TimeSeriesExample.h"
#ifdef ENABLE_LUA
@@ -185,6 +187,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,"Physics Server", "Create a physics server that communicates with a physics client over shared memory",
PhysicsServerCreateFunc),
ExampleEntry(1, "Physics Client", "Create a physics client that can communicate with a physics server over shared memory", PhysicsClientCreateFunc),
@@ -204,6 +210,7 @@ static ExampleEntry gDefaultExamples[]=
ExampleEntry(0,"Rendering"),
ExampleEntry(1,"Instanced Rendering", "Simple example of fast instanced rendering, only active when using OpenGL3+.",RenderInstancingCreateFunc),
ExampleEntry(1,"CoordinateSystemDemo","Show the axis and positive rotation direction around the axis.", CoordinateSystemCreateFunc),
ExampleEntry(1,"Time Series", "Render some value(s) in a 2D graph window, shifting to the left", TimeSeriesCreateFunc)
};

7
examples/ExampleBrowser/GwenGUISupport/GraphingTexture.h
View File

@@ -24,6 +24,13 @@ struct GraphingTexture
m_imageData[x*4+y*4*m_width+3] = alpha;
}
void getPixel(int x, int y, unsigned char& red, unsigned char& green, unsigned char& blue, unsigned char& alpha)
{
red = m_imageData[x*4+y*4*m_width+0];
green = m_imageData[x*4+y*4*m_width+1];
blue = m_imageData[x*4+y*4*m_width+2];
alpha = m_imageData[x*4+y*4*m_width+3];
}
void uploadImageData();
int getTextureId()

8
examples/ExampleBrowser/OpenGLExampleBrowser.cpp
View File

@@ -578,6 +578,14 @@ struct QuickCanvas : public Common2dCanvasInterface
btAssert(m_curNumGraphWindows==1);
m_gt[canvasId]->setPixel(x,y,red,green,blue,alpha);
}
virtual void getPixel(int canvasId, int x, int y, unsigned char& red, unsigned char& green,unsigned char& blue, unsigned char& alpha)
{
btAssert(canvasId==0);//hardcoded
btAssert(m_curNumGraphWindows==1);
m_gt[canvasId]->getPixel(x,y,red,green,blue,alpha);
}
virtual void refreshImageData(int canvasId)
{
m_gt[canvasId]->uploadImageData();

1
examples/ExampleBrowser/premake4.lua
View File

@@ -52,6 +52,7 @@
"**.h",
"../SharedMemory/PhysicsServerExample.cpp",
"../SharedMemory/PhysicsClientExample.cpp",
"../SharedMemory/RobotControlExample.cpp",
"../SharedMemory/PhysicsServer.cpp",
"../SharedMemory/PhysicsClient.cpp",
"../SharedMemory/PosixSharedMemory.cpp",

4
examples/OpenGLWindow/SimpleCamera.cpp
View File

@@ -12,8 +12,8 @@ struct SimpleCameraInternalData
m_cameraUp(b3MakeVector3(0,1,0)),
m_cameraUpAxis(1),
m_cameraForward(b3MakeVector3(1,0,0)),
m_frustumZNear(1),
m_frustumZFar(10000),
m_frustumZNear(0.01),
m_frustumZFar(1000),
m_yaw(20),
m_pitch(0),
m_aspect(1)

1100
examples/RenderingExamples/TimeSeriesExample.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

8
examples/RenderingExamples/TimeSeriesExample.h Normal file
View File

@@ -0,0 +1,8 @@
#ifndef TIME_SERIES_EXAMPLE_H
#define TIME_SERIES_EXAMPLE_H
class CommonExampleInterface* TimeSeriesCreateFunc(struct CommonExampleOptions& options);
#endif//TIME_SERIES_EXAMPLE_H

57
examples/SharedMemory/PhysicsClient.cpp
View File

@@ -205,9 +205,10 @@ void PhysicsClientSharedMemory::processServerStatus()
case CMD_ACTUAL_STATE_UPDATE_COMPLETED:
{
b3Printf("Received actual state\n");
int numQ = m_data->m_testBlock1->m_serverCommands[0].m_sendActualStateArgs.m_numDegreeOfFreedomQ;
int numU = m_data->m_testBlock1->m_serverCommands[0].m_sendActualStateArgs.m_numDegreeOfFreedomU;
SharedMemoryCommand& 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);
char msg[1024];
@@ -217,10 +218,10 @@ void PhysicsClientSharedMemory::processServerStatus()
{
if (i<numQ-1)
{
sprintf(msg,"%s%f,",msg,m_data->m_testBlock1->m_actualStateQ[i]);
sprintf(msg,"%s%f,",msg,command.m_sendActualStateArgs.m_actualStateQ[i]);
} else
{
sprintf(msg,"%s%f",msg,m_data->m_testBlock1->m_actualStateQ[i]);
sprintf(msg,"%s%f",msg,command.m_sendActualStateArgs.m_actualStateQ[i]);
}
}
sprintf(msg,"%s]",msg);
@@ -271,18 +272,8 @@ bool PhysicsClientSharedMemory::submitClientCommand(const SharedMemoryCommand& c
{
if (!m_data->m_serverLoadUrdfOK)
{
m_data->m_testBlock1->m_clientCommands[0].m_type =CMD_LOAD_URDF;
sprintf(m_data->m_testBlock1->m_clientCommands[0].m_urdfArguments.m_urdfFileName,"r2d2.urdf");
m_data->m_testBlock1->m_clientCommands[0].m_urdfArguments.m_initialPosition[0] = 0.0;
m_data->m_testBlock1->m_clientCommands[0].m_urdfArguments.m_initialPosition[1] = 0.0;
m_data->m_testBlock1->m_clientCommands[0].m_urdfArguments.m_initialPosition[2] = 0.0;
m_data->m_testBlock1->m_clientCommands[0].m_urdfArguments.m_initialOrientation[0] = 0.0;
m_data->m_testBlock1->m_clientCommands[0].m_urdfArguments.m_initialOrientation[1] = 0.0;
m_data->m_testBlock1->m_clientCommands[0].m_urdfArguments.m_initialOrientation[2] = 0.0;
m_data->m_testBlock1->m_clientCommands[0].m_urdfArguments.m_initialOrientation[3] = 1.0;
m_data->m_testBlock1->m_clientCommands[0].m_urdfArguments.m_useFixedBase = false;
m_data->m_testBlock1->m_clientCommands[0].m_urdfArguments.m_useMultiBody = true;
m_data->m_testBlock1->m_clientCommands[0] = command;
m_data->m_testBlock1->m_numClientCommands++;
b3Printf("Client created CMD_LOAD_URDF\n");
} else
@@ -371,37 +362,7 @@ bool PhysicsClientSharedMemory::submitClientCommand(const SharedMemoryCommand& c
if (m_data->m_serverLoadUrdfOK)
{
b3Printf("Sending desired state (pos, vel, torque)\n");
m_data->m_testBlock1->m_clientCommands[0].m_type =CMD_SEND_DESIRED_STATE;
//todo: expose a drop box in the GUI for this
int controlMode = CONTROL_MODE_VELOCITY;//CONTROL_MODE_TORQUE;
switch (controlMode)
{
case CONTROL_MODE_VELOCITY:
{
m_data->m_testBlock1->m_clientCommands[0].m_sendDesiredStateCommandArgument.m_controlMode = CONTROL_MODE_VELOCITY;
for (int i=0;i<MAX_DEGREE_OF_FREEDOM;i++)
{
m_data->m_testBlock1->m_desiredStateQdot[i] = 1;
m_data->m_testBlock1->m_desiredStateForceTorque[i] = 100;
}
break;
}
case CONTROL_MODE_TORQUE:
{
m_data->m_testBlock1->m_clientCommands[0].m_sendDesiredStateCommandArgument.m_controlMode = CONTROL_MODE_TORQUE;
for (int i=0;i<MAX_DEGREE_OF_FREEDOM;i++)
{
m_data->m_testBlock1->m_desiredStateForceTorque[i] = 100;
}
break;
}
default:
{
b3Printf("Unknown control mode in client CMD_SEND_DESIRED_STATE");
btAssert(0);
}
}
m_data->m_testBlock1->m_clientCommands[0] = command;
m_data->m_testBlock1->m_numClientCommands++;

98
examples/SharedMemory/PhysicsClientExample.cpp
View File

@@ -16,7 +16,7 @@ protected:
bool m_wantsTermination;
btAlignedObjectArray<int> m_userCommandRequests;
btAlignedObjectArray<SharedMemoryCommand> m_userCommandRequests;
void createButton(const char* name, int id, bool isTrigger );
@@ -42,7 +42,7 @@ public:
{
return m_wantsTermination;
}
void enqueueCommand(int command);
void enqueueCommand(const SharedMemoryCommand& orgCommand);
};
@@ -50,17 +50,85 @@ public:
void MyCallback(int buttonId, bool buttonState, void* userPtr)
{
PhysicsClientExample* cl = (PhysicsClientExample*) userPtr;
SharedMemoryCommand command;
switch (buttonId)
{
case CMD_LOAD_URDF:
{
command.m_type =CMD_LOAD_URDF;
sprintf(command.m_urdfArguments.m_urdfFileName,"hinge.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;
command.m_urdfArguments.m_initialOrientation[0] = 0.0;
command.m_urdfArguments.m_initialOrientation[1] = 0.0;
command.m_urdfArguments.m_initialOrientation[2] = 0.0;
command.m_urdfArguments.m_initialOrientation[3] = 1.0;
command.m_urdfArguments.m_useFixedBase = false;
command.m_urdfArguments.m_useMultiBody = true;
cl->enqueueCommand(command);
break;
}
case CMD_CREATE_BOX_COLLISION_SHAPE:
{
command.m_type =CMD_CREATE_BOX_COLLISION_SHAPE;
cl->enqueueCommand(command);
break;
}
case CMD_REQUEST_ACTUAL_STATE:
{
command.m_type =CMD_REQUEST_ACTUAL_STATE;
cl->enqueueCommand(command);
break;
};
case CMD_STEP_FORWARD_SIMULATION:
case CMD_SHUTDOWN:
{
command.m_type =CMD_STEP_FORWARD_SIMULATION;
cl->enqueueCommand(command);
break;
}
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] = 1;
command.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[i] = 100;
}
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);
break;
}
case CMD_SEND_BULLET_DATA_STREAM:
{
cl->enqueueCommand(buttonId);
command.m_type = buttonId;
cl->enqueueCommand(command);
break;
}
@@ -72,12 +140,13 @@ void MyCallback(int buttonId, bool buttonState, void* userPtr)
};
}
void PhysicsClientExample::enqueueCommand(const SharedMemoryCommand& orgCommand)
{
m_userCommandRequests.push_back(orgCommand);
SharedMemoryCommand& cmd = m_userCommandRequests[m_userCommandRequests.size()-1];
void PhysicsClientExample::enqueueCommand(int command)
{
m_userCommandRequests.push_back(command);
b3Printf("User put command request %d on queue (queue length = %d)\n",command, m_userCommandRequests.size());
}
b3Printf("User put command request %d on queue (queue length = %d)\n",cmd.m_type, m_userCommandRequests.size());
}
PhysicsClientExample::PhysicsClientExample(GUIHelperInterface* helper)
@@ -114,6 +183,7 @@ void PhysicsClientExample::initPhysics()
} else
{
/*
m_userCommandRequests.push_back(CMD_LOAD_URDF);
m_userCommandRequests.push_back(CMD_REQUEST_ACTUAL_STATE);
m_userCommandRequests.push_back(CMD_SEND_DESIRED_STATE);
@@ -124,7 +194,8 @@ void PhysicsClientExample::initPhysics()
m_userCommandRequests.push_back(CMD_STEP_FORWARD_SIMULATION);
m_userCommandRequests.push_back(CMD_REQUEST_ACTUAL_STATE);
m_userCommandRequests.push_back(CMD_SHUTDOWN);
*/
}
if (!m_physicsClient.connect())
@@ -147,7 +218,7 @@ void PhysicsClientExample::stepSimulation(float deltaTime)
if (m_userCommandRequests.size())
{
b3Printf("Outstanding user command requests: %d\n", m_userCommandRequests.size());
int command = m_userCommandRequests[0];
SharedMemoryCommand command = 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++)
@@ -156,9 +227,8 @@ void PhysicsClientExample::stepSimulation(float deltaTime)
}
m_userCommandRequests.pop_back();
SharedMemoryCommand tmp;
tmp.m_type = command;
m_physicsClient.submitClientCommand(tmp);
m_physicsClient.submitClientCommand(command);
}
}
}

48
examples/SharedMemory/PhysicsServer.cpp
View File

@@ -385,12 +385,12 @@ void PhysicsServerSharedMemory::processClientCommands()
mb->clearForcesAndTorques();
int torqueIndex = 0;
btVector3 f(m_data->m_testBlock1->m_desiredStateForceTorque[0],
m_data->m_testBlock1->m_desiredStateForceTorque[1],
m_data->m_testBlock1->m_desiredStateForceTorque[2]);
btVector3 t(m_data->m_testBlock1->m_desiredStateForceTorque[3],
m_data->m_testBlock1->m_desiredStateForceTorque[4],
m_data->m_testBlock1->m_desiredStateForceTorque[5]);
btVector3 f(clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[0],
clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[1],
clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[2]);
btVector3 t(clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[3],
clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[4],
clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[5]);
torqueIndex+=6;
mb->addBaseForce(f);
mb->addBaseTorque(t);
@@ -399,7 +399,7 @@ void PhysicsServerSharedMemory::processClientCommands()
for (int dof=0;dof<mb->getLink(link).m_dofCount;dof++)
{
double torque = m_data->m_testBlock1->m_desiredStateForceTorque[torqueIndex];
double torque = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[torqueIndex];
mb->addJointTorqueMultiDof(link,dof,torque);
torqueIndex++;
}
@@ -423,12 +423,12 @@ void PhysicsServerSharedMemory::processClientCommands()
if (motorPtr)
{
btMultiBodyJointMotor* motor = *motorPtr;
btScalar desiredVelocity = m_data->m_testBlock1->m_desiredStateQdot[dofIndex];
btScalar desiredVelocity = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[dofIndex];
motor->setVelocityTarget(desiredVelocity);
btScalar physicsDeltaTime=1./60.;//todo: set the physicsDeltaTime explicitly in the API, separate from the 'stepSimulation'
btScalar maxImp = m_data->m_testBlock1->m_desiredStateForceTorque[dofIndex]*physicsDeltaTime;
btScalar maxImp = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex]*physicsDeltaTime;
motor->setMaxAppliedImpulse(maxImp);
numMotors++;
@@ -478,37 +478,37 @@ void PhysicsServerSharedMemory::processClientCommands()
tr.setRotation(mb->getWorldToBaseRot().inverse());
//base position in world space, carthesian
m_data->m_testBlock1->m_actualStateQ[0] = tr.getOrigin()[0];
m_data->m_testBlock1->m_actualStateQ[1] = tr.getOrigin()[1];
m_data->m_testBlock1->m_actualStateQ[2] = tr.getOrigin()[2];
serverCmd.m_sendActualStateArgs.m_actualStateQ[0] = tr.getOrigin()[0];
serverCmd.m_sendActualStateArgs.m_actualStateQ[1] = tr.getOrigin()[1];
serverCmd.m_sendActualStateArgs.m_actualStateQ[2] = tr.getOrigin()[2];
//base orientation, quaternion x,y,z,w, in world space, carthesian
m_data->m_testBlock1->m_actualStateQ[3] = tr.getRotation()[0];
m_data->m_testBlock1->m_actualStateQ[4] = tr.getRotation()[1];
m_data->m_testBlock1->m_actualStateQ[5] = tr.getRotation()[2];
m_data->m_testBlock1->m_actualStateQ[6] = tr.getRotation()[3];
serverCmd.m_sendActualStateArgs.m_actualStateQ[3] = tr.getRotation()[0];
serverCmd.m_sendActualStateArgs.m_actualStateQ[4] = tr.getRotation()[1];
serverCmd.m_sendActualStateArgs.m_actualStateQ[5] = tr.getRotation()[2];
serverCmd.m_sendActualStateArgs.m_actualStateQ[6] = tr.getRotation()[3];
totalDegreeOfFreedomQ +=7;//pos + quaternion
//base linear velocity (in world space, carthesian)
m_data->m_testBlock1->m_actualStateQdot[0] = mb->getBaseVel()[0];
m_data->m_testBlock1->m_actualStateQdot[1] = mb->getBaseVel()[1];
m_data->m_testBlock1->m_actualStateQdot[2] = mb->getBaseVel()[2];
serverCmd.m_sendActualStateArgs.m_actualStateQdot[0] = mb->getBaseVel()[0];
serverCmd.m_sendActualStateArgs.m_actualStateQdot[1] = mb->getBaseVel()[1];
serverCmd.m_sendActualStateArgs.m_actualStateQdot[2] = mb->getBaseVel()[2];
//base angular velocity (in world space, carthesian)
m_data->m_testBlock1->m_actualStateQdot[3] = mb->getBaseOmega()[0];
m_data->m_testBlock1->m_actualStateQdot[4] = mb->getBaseOmega()[1];
m_data->m_testBlock1->m_actualStateQdot[5] = mb->getBaseOmega()[2];
serverCmd.m_sendActualStateArgs.m_actualStateQdot[3] = mb->getBaseOmega()[0];
serverCmd.m_sendActualStateArgs.m_actualStateQdot[4] = mb->getBaseOmega()[1];
serverCmd.m_sendActualStateArgs.m_actualStateQdot[5] = mb->getBaseOmega()[2];
totalDegreeOfFreedomU += 6;//3 linear and 3 angular DOF
}
for (int l=0;l<mb->getNumLinks();l++)
{
for (int d=0;d<mb->getLink(l).m_posVarCount;d++)
{
m_data->m_testBlock1->m_actualStateQ[totalDegreeOfFreedomQ++] = mb->getJointPosMultiDof(l)[d];
serverCmd.m_sendActualStateArgs.m_actualStateQ[totalDegreeOfFreedomQ++] = mb->getJointPosMultiDof(l)[d];
}
for (int d=0;d<mb->getLink(l).m_dofCount;d++)
{
m_data->m_testBlock1->m_actualStateQdot[totalDegreeOfFreedomU++] = mb->getJointVelMultiDof(l)[d];
serverCmd.m_sendActualStateArgs.m_actualStateQdot[totalDegreeOfFreedomU++] = mb->getJointVelMultiDof(l)[d];
}
}

4
examples/SharedMemory/PhysicsServerExample.cpp
View File

@@ -39,8 +39,6 @@ public:
virtual void stepSimulation(float deltaTime);
bool loadUrdf(const char* fileName, const btVector3& pos, const btQuaternion& orn,
bool useMultiBody, bool useFixedBase);
virtual void resetCamera()
{
@@ -59,7 +57,7 @@ PhysicsServerExample::PhysicsServerExample(GUIHelperInterface* helper)
m_wantsShutdown(false)
{
b3Printf("Started PhysicsServer\n");
bool useServer = true;
}

274
examples/SharedMemory/RobotControlExample.cpp Normal file
View File

@@ -0,0 +1,274 @@
#include "RobotControlExample.h"
#include "../CommonInterfaces/CommonParameterInterface.h"
#include "PhysicsServer.h"
#include "PhysicsClient.h"
#include "SharedMemoryCommon.h"
#include "../Utils/b3Clock.h"
//const char* blaatnaam = "basename";
class RobotControlExample : public SharedMemoryCommon
{
PhysicsServerSharedMemory m_physicsServer;
PhysicsClientSharedMemory m_physicsClient;
b3Clock m_realtimeClock;
int m_sequenceNumberGenerator;
bool m_wantsShutdown;
btAlignedObjectArray<SharedMemoryCommand> m_userCommandRequests;
void createButton(const char* name, int id, bool isTrigger );
public:
RobotControlExample(GUIHelperInterface* helper);
virtual ~RobotControlExample();
virtual void initPhysics();
virtual void stepSimulation(float deltaTime);
void enqueueCommand(const SharedMemoryCommand& orgCommand)
{
m_userCommandRequests.push_back(orgCommand);
SharedMemoryCommand& cmd = m_userCommandRequests[m_userCommandRequests.size()-1];
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());
}
virtual void resetCamera()
{
float dist = 5;
float pitch = 50;
float yaw = 35;
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();
};
void MyCallback2(int buttonId, bool buttonState, void* userPtr)
{
RobotControlExample* cl = (RobotControlExample*) userPtr;
SharedMemoryCommand command;
switch (buttonId)
{
case CMD_LOAD_URDF:
{
command.m_type =CMD_LOAD_URDF;
sprintf(command.m_urdfArguments.m_urdfFileName,"hinge.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;
command.m_urdfArguments.m_initialOrientation[0] = 0.0;
command.m_urdfArguments.m_initialOrientation[1] = 0.0;
command.m_urdfArguments.m_initialOrientation[2] = 0.0;
command.m_urdfArguments.m_initialOrientation[3] = 1.0;
command.m_urdfArguments.m_useFixedBase = false;
command.m_urdfArguments.m_useMultiBody = true;
cl->enqueueCommand(command);
break;
}
case CMD_CREATE_BOX_COLLISION_SHAPE:
{
command.m_type =CMD_CREATE_BOX_COLLISION_SHAPE;
cl->enqueueCommand(command);
break;
}
case CMD_REQUEST_ACTUAL_STATE:
{
command.m_type =CMD_REQUEST_ACTUAL_STATE;
cl->enqueueCommand(command);
break;
};
case CMD_STEP_FORWARD_SIMULATION:
{
command.m_type =CMD_STEP_FORWARD_SIMULATION;
cl->enqueueCommand(command);
break;
}
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] = 1;
command.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[i] = 100;
}
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);
break;
}
case CMD_SEND_BULLET_DATA_STREAM:
{
command.m_type = buttonId;
cl->enqueueCommand(command);
break;
}
default:
{
b3Error("Unknown buttonId");
btAssert(0);
}
};
}
void RobotControlExample::createButton(const char* name, int buttonId, bool isTrigger )
{
ButtonParams button(name,buttonId, isTrigger);
button.m_callback = MyCallback2;
button.m_userPointer = this;
m_guiHelper->getParameterInterface()->registerButtonParameter(button);
}
RobotControlExample::RobotControlExample(GUIHelperInterface* helper)
:SharedMemoryCommon(helper),
m_wantsShutdown(false),
m_sequenceNumberGenerator(0)
{
bool useServer = true;
}
RobotControlExample::~RobotControlExample()
{
bool deInitializeSharedMemory = true;
m_physicsServer.disconnectSharedMemory(deInitializeSharedMemory);
}
void RobotControlExample::initPhysics()
{
///for this testing we use Z-axis up
int upAxis = 2;
m_guiHelper->setUpAxis(upAxis);
createEmptyDynamicsWorld();
//todo: create a special debug drawer that will cache the lines, so we can send the debug info over the wire
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
btVector3 grav(0,0,0);
grav[upAxis] = 0;//-9.8;
this->m_dynamicsWorld->setGravity(grav);
bool allowSharedMemoryInitialization = true;
m_physicsServer.connectSharedMemory(allowSharedMemoryInitialization, m_dynamicsWorld,m_guiHelper);
if (m_guiHelper && m_guiHelper->getParameterInterface())
{
bool isTrigger = false;
createButton("Load URDF",CMD_LOAD_URDF, isTrigger);
createButton("Step Sim",CMD_STEP_FORWARD_SIMULATION, isTrigger);
createButton("Send Bullet Stream",CMD_SEND_BULLET_DATA_STREAM, isTrigger);
createButton("Get State",CMD_REQUEST_ACTUAL_STATE, isTrigger);
createButton("Send Desired State",CMD_SEND_DESIRED_STATE, isTrigger);
createButton("Create Box Collider",CMD_CREATE_BOX_COLLISION_SHAPE,isTrigger);
} else
{
/*
m_userCommandRequests.push_back(CMD_LOAD_URDF);
m_userCommandRequests.push_back(CMD_REQUEST_ACTUAL_STATE);
m_userCommandRequests.push_back(CMD_SEND_DESIRED_STATE);
m_userCommandRequests.push_back(CMD_REQUEST_ACTUAL_STATE);
//m_userCommandRequests.push_back(CMD_SET_JOINT_FEEDBACK);
m_userCommandRequests.push_back(CMD_CREATE_BOX_COLLISION_SHAPE);
//m_userCommandRequests.push_back(CMD_CREATE_RIGID_BODY);
m_userCommandRequests.push_back(CMD_STEP_FORWARD_SIMULATION);
m_userCommandRequests.push_back(CMD_REQUEST_ACTUAL_STATE);
m_userCommandRequests.push_back(CMD_SHUTDOWN);
*/
}
if (!m_physicsClient.connect())
{
b3Warning("Cannot eonnect to physics client");
}
}
bool RobotControlExample::wantsTermination()
{
return m_wantsShutdown;
}
void RobotControlExample::stepSimulation(float deltaTime)
{
m_physicsServer.processClientCommands();
if (m_physicsClient.isConnected())
{
m_physicsClient.processServerStatus();
if (m_physicsClient.canSubmitCommand())
{
if (m_userCommandRequests.size())
{
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++)
{
m_userCommandRequests[i-1] = m_userCommandRequests[i];
}
m_userCommandRequests.pop_back();
m_physicsClient.submitClientCommand(cmd);
}
}
}
}
class CommonExampleInterface* RobotControlExampleCreateFunc(struct CommonExampleOptions& options)
{
return new RobotControlExample(options.m_guiHelper);
}

8
examples/SharedMemory/RobotControlExample.h Normal file
View File

@@ -0,0 +1,8 @@
#ifndef ROBOT_CONTROL_EXAMPLE_H
#define ROBOT_CONTROL_EXAMPLE_H
class CommonExampleInterface* RobotControlExampleCreateFunc(struct CommonExampleOptions& options);
#endif //ROBOT_CONTROL_EXAMPLE_H

38
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -4,6 +4,24 @@
//this is a very experimental draft of commands. We will iterate on this API (commands, arguments etc)
#ifdef __GNUC__
#include <stdint.h>
typedef int32_t smInt32_t;
typedef int64_t smInt64_t;
typedef uint32_t smUint32_t;
typedef uint64_t smUint64_t;
#elif defined(_MSC_VER)
typedef __int32 smInt32_t;
typedef __int64 smInt64_t;
typedef unsigned __int32 smUint32_t;
typedef unsigned __int64 smUint64_t;
#else
typedef int smInt32_t;
typedef long long int smInt64_t;
typedef unsigned int smUint32_t;
typedef unsigned long long int smUint64_t;
#endif
enum SharedMemoryServerCommand
{
CMD_URDF_LOADING_COMPLETED,
@@ -74,6 +92,15 @@ struct SendDesiredStateArgs
{
int m_bodyUniqueId;
int m_controlMode;
//desired state is only written by the client, read-only access by server is expected
double m_desiredStateQ[MAX_DEGREE_OF_FREEDOM];
double m_desiredStateQdot[MAX_DEGREE_OF_FREEDOM];
//m_desiredStateForceTorque is either the actual applied force/torque (in CONTROL_MODE_TORQUE) or
//or m_desiredStateForceTorque is the maximum applied force/torque for the motor/constraint to reach the desired velocity in CONTROL_MODE_VELOCITY mode
double m_desiredStateForceTorque[MAX_DEGREE_OF_FREEDOM];
};
struct RequestActualStateArgs
@@ -99,6 +126,12 @@ struct SendActualStateArgs
int m_bodyUniqueId;
int m_numDegreeOfFreedomQ;
int m_numDegreeOfFreedomU;
//actual state is only written by the server, read-only access by client is expected
double m_actualStateQ[MAX_DEGREE_OF_FREEDOM];
double m_actualStateQdot[MAX_DEGREE_OF_FREEDOM];
double m_actualStateSensors[MAX_NUM_SENSORS];//these are force sensors and IMU information
};
@@ -111,7 +144,10 @@ struct StepSimulationArgs
struct SharedMemoryCommand
{
int m_type;
smUint64_t m_timeStamp;
int m_sequenceNumber;
union
{
UrdfArgs m_urdfArguments;

18
examples/SharedMemory/SharedMemoryInterface.h
View File

@@ -3,7 +3,7 @@
#define SHARED_MEMORY_KEY 12347
#define SHARED_MEMORY_MAGIC_NUMBER 64738
#define SHARED_MEMORY_MAX_COMMANDS 64
#define SHARED_MEMORY_MAX_COMMANDS 32
#define SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE (256*1024)
#include "SharedMemoryCommands.h"
@@ -20,22 +20,6 @@ struct SharedMemoryExampleData
int m_numServerCommands;
int m_numProcessedServerCommands;
//TODO: it is better to move this single desired/actual state (m_desiredStateQ, m_actualStateQ etc) into the command,
//so we can deal with multiple bodies/robots
//desired state is only written by the client, read-only access by server is expected
double m_desiredStateQ[MAX_DEGREE_OF_FREEDOM];
double m_desiredStateQdot[MAX_DEGREE_OF_FREEDOM];
//m_desiredStateForceTorque is either the actual applied force/torque (in CONTROL_MODE_TORQUE) or
//or m_desiredStateForceTorque is the maximum applied force/torque for the motor/constraint to reach the desired velocity in CONTROL_MODE_VELOCITY mode
double m_desiredStateForceTorque[MAX_DEGREE_OF_FREEDOM];
//actual state is only written by the server, read-only access by client is expected
double m_actualStateQ[MAX_DEGREE_OF_FREEDOM];
double m_actualStateQdot[MAX_DEGREE_OF_FREEDOM];
double m_actualStateSensors[MAX_NUM_SENSORS];//these are force sensors and IMU information
//m_bulletStreamDataClientToServer is a way for the client to create collision shapes, rigid bodies and constraints
//the Bullet data structures are more general purpose than the capabilities of a URDF file.
char m_bulletStreamDataClientToServer[SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE];

1
examples/SharedMemory/premake4.lua
View File

@@ -33,6 +33,7 @@ files {
"../Importers/ImportURDFDemo/URDF2Bullet.cpp",
"../Importers/ImportURDFDemo/URDF2Bullet.h",
"../Utils/b3ResourcePath.cpp",
"../Utils/b3Clock.cpp",
"../../Extras/Serialize/BulletWorldImporter/*",
"../../Extras/Serialize/BulletFileLoader/*",
"../Importers/ImportURDFDemo/URDFImporterInterface.h",