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

add additional example using server and client for future simple robot control of simulated robot

Browse Source 
(load urdf, get state, set desired state, step simulation)
Create SharedMemoryCommand and put that into the queue
move arrays for Q, Qdot etc into the command
This commit is contained in:
erwin coumans
2015-07-15 15:23:40 -07:00
parent 80f8f940b3
commit b563c7c8ce
8 changed files with 246 additions and 117 deletions
Download Patch File Download Diff File Expand all files Collapse all files

102
examples/SharedMemory/RobotControlExample.cpp
View File

@@ -9,6 +9,8 @@
#include "PhysicsServer.h"
#include "PhysicsClient.h"
#include "SharedMemoryCommon.h"
#include "../Utils/b3Clock.h"
//const char* blaatnaam = "basename";
@@ -18,10 +20,12 @@ class RobotControlExample : public SharedMemoryCommon
{
PhysicsServerSharedMemory m_physicsServer;
PhysicsClientSharedMemory m_physicsClient;
b3Clock m_realtimeClock;
int m_sequenceNumberGenerator;
bool m_wantsShutdown;
btAlignedObjectArray<int> m_userCommandRequests;
btAlignedObjectArray<SharedMemoryCommand> m_userCommandRequests;
void createButton(const char* name, int id, bool isTrigger );
@@ -35,10 +39,14 @@ public:
virtual void stepSimulation(float deltaTime);
void enqueueCommand(int command)
void enqueueCommand(const SharedMemoryCommand& orgCommand)
{
m_userCommandRequests.push_back(command);
b3Printf("User put command request %d on queue (queue length = %d)\n",command, m_userCommandRequests.size());
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()
@@ -57,17 +65,85 @@ public:
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:
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;
}
@@ -88,7 +164,8 @@ void RobotControlExample::createButton(const char* name, int buttonId, bool isTr
RobotControlExample::RobotControlExample(GUIHelperInterface* helper)
:SharedMemoryCommon(helper),
m_wantsShutdown(false)
m_wantsShutdown(false),
m_sequenceNumberGenerator(0)
{
bool useServer = true;
@@ -131,6 +208,7 @@ void RobotControlExample::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);
@@ -141,7 +219,7 @@ void RobotControlExample::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())
@@ -172,7 +250,7 @@ void RobotControlExample::stepSimulation(float deltaTime)
if (m_userCommandRequests.size())
{
b3Printf("Outstanding user command requests: %d\n", m_userCommandRequests.size());
int command = m_userCommandRequests[0];
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++)
@@ -181,9 +259,7 @@ void RobotControlExample::stepSimulation(float deltaTime)
}
m_userCommandRequests.pop_back();
SharedMemoryCommand tmp;
tmp.m_type = command;
m_physicsClient.submitClientCommand(tmp);
m_physicsClient.submitClientCommand(cmd);
}
}
}