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

migrating from b3RobotSimAPI to b3RobotSimulatorClientAPI (step by step)

Browse Source 
allow to run the client/server code in the example browser without this b3RobotSimAPI hack.
This commit is contained in:
Erwin Coumans
2017-05-13 11:15:20 -07:00
parent f80838e989
commit 0944790577
7 changed files with 246 additions and 46 deletions
Download Patch File Download Diff File Expand all files Collapse all files

107
examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp
View File

@@ -2,29 +2,32 @@
//#include "SharedMemoryCommands.h"
#include "SharedMemory/PhysicsClientC_API.h"
#include "../SharedMemory/PhysicsClientC_API.h"
#ifdef BT_ENABLE_ENET
#include "SharedMemory/PhysicsClientUDP_C_API.h"
#include "../SharedMemory/PhysicsClientUDP_C_API.h"
#endif //PHYSICS_UDP
#ifdef BT_ENABLE_CLSOCKET
#include "SharedMemory/PhysicsClientTCP_C_API.h"
#include "../SharedMemory/PhysicsClientTCP_C_API.h"
#endif //PHYSICS_TCP
#include "SharedMemory/PhysicsDirectC_API.h"
#include "../SharedMemory/PhysicsDirectC_API.h"
#include "SharedMemory/SharedMemoryInProcessPhysicsC_API.h"
#include "../SharedMemory/SharedMemoryInProcessPhysicsC_API.h"
#include "SharedMemory/SharedMemoryPublic.h"
#include "../SharedMemory/SharedMemoryPublic.h"
#include "Bullet3Common/b3Logging.h"
struct b3RobotSimulatorClientAPI_InternalData
{
b3PhysicsClientHandle m_physicsClientHandle;
struct GUIHelperInterface* m_guiHelper;
b3RobotSimulatorClientAPI_InternalData()
: m_physicsClientHandle(0)
: m_physicsClientHandle(0),
m_guiHelper(0)
{
}
};
@@ -39,6 +42,26 @@ b3RobotSimulatorClientAPI::~b3RobotSimulatorClientAPI()
delete m_data;
}
void b3RobotSimulatorClientAPI::setGuiHelper(struct GUIHelperInterface* guiHelper)
{
m_data->m_guiHelper = guiHelper;
}
void b3RobotSimulatorClientAPI::renderScene()
{
if (!isConnected())
{
b3Warning("Not connected");
return;
}
if (m_data->m_guiHelper)
{
b3InProcessRenderSceneInternal(m_data->m_physicsClientHandle);
}
}
bool b3RobotSimulatorClientAPI::connect(int mode, const std::string& hostName, int portOrKey)
{
if (m_data->m_physicsClientHandle)
@@ -55,6 +78,12 @@ bool b3RobotSimulatorClientAPI::connect(int mode, const std::string& hostName, i
switch (mode)
{
case eCONNECT_EXISTING_EXAMPLE_BROWSER:
{
sm = b3CreateInProcessPhysicsServerFromExistingExampleBrowserAndConnect(m_data->m_guiHelper);
break;
}
case eCONNECT_GUI:
{
int argc = 0;
@@ -644,8 +673,72 @@ bool b3RobotSimulatorClientAPI::getJointState(int bodyUniqueId, int jointIndex,
return false;
}
bool b3RobotSimulatorClientAPI::getJointStates(int bodyUniqueId, b3JointStates2& state)
{
if (!isConnected())
{
b3Warning("Not connected");
return false;
}
b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(m_data->m_physicsClientHandle,bodyUniqueId);
b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
if (statusHandle)
{
// double rootInertialFrame[7];
const double* rootLocalInertialFrame;
const double* actualStateQ;
const double* actualStateQdot;
const double* jointReactionForces;
int stat = b3GetStatusActualState(statusHandle,
&state.m_bodyUniqueId,
&state.m_numDegreeOfFreedomQ,
&state.m_numDegreeOfFreedomU,
&rootLocalInertialFrame,
&actualStateQ,
&actualStateQdot,
&jointReactionForces);
if (stat)
{
state.m_actualStateQ.resize(state.m_numDegreeOfFreedomQ);
state.m_actualStateQdot.resize(state.m_numDegreeOfFreedomU);
for (int i=0;i<state.m_numDegreeOfFreedomQ;i++)
{
state.m_actualStateQ[i] = actualStateQ[i];
}
for (int i=0;i<state.m_numDegreeOfFreedomU;i++)
{
state.m_actualStateQdot[i] = actualStateQdot[i];
}
int numJoints = getNumJoints(bodyUniqueId);
state.m_jointReactionForces.resize(6*numJoints);
for (int i=0;i<numJoints*6;i++)
{
state.m_jointReactionForces[i] = jointReactionForces[i];
}
return true;
}
//rootInertialFrame,
// &state.m_actualStateQ[0],
// &state.m_actualStateQdot[0],
// &state.m_jointReactionForces[0]);
}
return false;
}
bool b3RobotSimulatorClientAPI::resetJointState(int bodyUniqueId, int jointIndex, double targetValue)
{
if (!isConnected())
{
b3Warning("Not connected");
return false;
}
b3SharedMemoryCommandHandle commandHandle;
b3SharedMemoryStatusHandle statusHandle;
int numJoints;

18
examples/RobotSimulator/b3RobotSimulatorClientAPI.h
View File

@@ -124,6 +124,17 @@ struct b3RobotSimulatorInverseKinematicsResults
b3AlignedObjectArray<double> m_calculatedJointPositions;
};
struct b3JointStates2
{
int m_bodyUniqueId;
int m_numDegreeOfFreedomQ;
int m_numDegreeOfFreedomU;
b3Transform m_rootLocalInertialFrame;
b3AlignedObjectArray<double> m_actualStateQ;
b3AlignedObjectArray<double> m_actualStateQdot;
b3AlignedObjectArray<double> m_jointReactionForces;
};
///The b3RobotSimulatorClientAPI is pretty much the C++ version of pybullet
///as documented in the pybullet Quickstart Guide
///https://docs.google.com/document/d/10sXEhzFRSnvFcl3XxNGhnD4N2SedqwdAvK3dsihxVUA
@@ -137,6 +148,11 @@ public:
b3RobotSimulatorClientAPI();
virtual ~b3RobotSimulatorClientAPI();
//setGuiHelper is only used when embedded in existing example browser
void setGuiHelper(struct GUIHelperInterface* guiHelper);
//renderScene is only used when embedded in existing example browser
virtual void renderScene();
bool connect(int mode, const std::string& hostName = "localhost", int portOrKey = -1);
void disconnect();
@@ -177,6 +193,8 @@ public:
bool getJointState(int bodyUniqueId, int jointIndex, struct b3JointSensorState* state);
bool getJointStates(int bodyUniqueId, b3JointStates2& state);
bool resetJointState(int bodyUniqueId, int jointIndex, double targetValue);
void setJointMotorControl(int bodyUniqueId, int jointIndex, const struct b3RobotSimulatorJointMotorArgs& args);

55
examples/RoboticsLearning/KukaGraspExample.cpp
View File

@@ -11,8 +11,8 @@
#include "../SharedMemory/PhysicsServerSharedMemory.h"
#include "../SharedMemory/PhysicsClientC_API.h"
#include <string>
#include "../RobotSimulator/b3RobotSimulatorClientAPI.h"
#include "b3RobotSimAPI.h"
#include "../Utils/b3Clock.h"
///quick demo showing the right-handed coordinate system and positive rotations around each axis
@@ -20,7 +20,8 @@ class KukaGraspExample : public CommonExampleInterface
{
CommonGraphicsApp* m_app;
GUIHelperInterface* m_guiHelper;
b3RobotSimAPI m_robotSim;
b3RobotSimulatorClientAPI m_robotSim;
int m_kukaIndex;
IKTrajectoryHelper m_ikHelper;
@@ -78,19 +79,18 @@ public:
int mode = eCONNECT_EXISTING_EXAMPLE_BROWSER;
m_robotSim.setGuiHelper(m_guiHelper);
bool connected = m_robotSim.connect(mode);
bool connected = m_robotSim.connect(m_guiHelper);
// 0;//m_robotSim.connect(m_guiHelper);
b3Printf("robotSim connected = %d",connected);
{
b3RobotSimLoadFileArgs args("");
args.m_fileName = "kuka_iiwa/model.urdf";
args.m_startPosition.setValue(0,0,0);
b3RobotSimLoadFileResults results;
if (m_robotSim.loadFile(args, results) && results.m_uniqueObjectIds.size()==1)
m_kukaIndex = m_robotSim.loadURDF("kuka_iiwa/model.urdf");
if (m_kukaIndex >=0)
{
m_kukaIndex = results.m_uniqueObjectIds[0];
int numJoints = m_robotSim.getNumJoints(m_kukaIndex);
b3Printf("numJoints = %d",numJoints);
@@ -113,23 +113,8 @@ public:
*/
}
if (0)
{
b3RobotSimLoadFileArgs args("");
args.m_fileName = "kiva_shelf/model.sdf";
args.m_forceOverrideFixedBase = true;
args.m_fileType = B3_SDF_FILE;
args.m_startOrientation = b3Quaternion(0,0,0,1);
b3RobotSimLoadFileResults results;
m_robotSim.loadFile(args,results);
}
{
b3RobotSimLoadFileArgs args("");
args.m_fileName = "plane.urdf";
args.m_startPosition.setValue(0,0,0);
args.m_forceOverrideFixedBase = true;
b3RobotSimLoadFileResults results;
m_robotSim.loadFile(args,results);
m_robotSim.loadURDF("plane.urdf");
m_robotSim.setGravity(b3MakeVector3(0,0,0));
}
@@ -157,9 +142,7 @@ public:
{
double q_current[7]={0,0,0,0,0,0,0};
double world_position[3]={0,0,0};
double world_orientation[4]={0,0,0,0};
b3JointStates jointStates;
b3JointStates2 jointStates;
if (m_robotSim.getJointStates(m_kukaIndex,jointStates))
{
@@ -171,9 +154,12 @@ public:
}
}
// compute body position and orientation
m_robotSim.getLinkState(0, 6, world_position, world_orientation);
m_worldPos.setValue(world_position[0], world_position[1], world_position[2]);
m_worldOri.setValue(world_orientation[0], world_orientation[1], world_orientation[2], world_orientation[3]);
b3LinkState linkState;
m_robotSim.getLinkState(0, 6, &linkState);
m_worldPos.setValue(linkState.m_worldLinkFramePosition[0], linkState.m_worldLinkFramePosition[1], linkState.m_worldLinkFramePosition[2]);
m_worldOri.setValue(linkState.m_worldLinkFrameOrientation[0], linkState.m_worldLinkFrameOrientation[1], linkState.m_worldLinkFrameOrientation[2], linkState.m_worldLinkFrameOrientation[3]);
b3Vector3DoubleData targetPosDataOut;
m_targetPos.serializeDouble(targetPosDataOut);
@@ -185,8 +171,8 @@ public:
m_worldOri.serializeDouble(worldOriDataOut);
b3RobotSimInverseKinematicArgs ikargs;
b3RobotSimInverseKinematicsResults ikresults;
b3RobotSimulatorInverseKinematicArgs ikargs;
b3RobotSimulatorInverseKinematicsResults ikresults;
ikargs.m_bodyUniqueId = m_kukaIndex;
@@ -247,7 +233,7 @@ public:
//copy the IK result to the desired state of the motor/actuator
for (int i=0;i<numJoints;i++)
{
b3JointMotorArgs t(CONTROL_MODE_POSITION_VELOCITY_PD);
b3RobotSimulatorJointMotorArgs t(CONTROL_MODE_POSITION_VELOCITY_PD);
t.m_targetPosition = ikresults.m_calculatedJointPositions[i];
t.m_maxTorqueValue = 100.0;
t.m_kp= 1.0;
@@ -257,6 +243,7 @@ public:
}
}
}

2
examples/SharedMemory/PhysicsClientSharedMemory.h
View File

@@ -14,7 +14,7 @@ protected:
void processBodyJointInfo(int bodyUniqueId, const struct SharedMemoryStatus& serverCmd);
void resetData();
void removeCachedBody(int bodyUniqueId);
virtual void renderSceneInternal() {};
public:
PhysicsClientSharedMemory();
virtual ~PhysicsClientSharedMemory();

100
examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp
View File

@@ -4,6 +4,9 @@
#include "PhysicsClientSharedMemory.h"
#include"../ExampleBrowser/InProcessExampleBrowser.h"
#include "PhysicsServerExample.h"
#include "../CommonInterfaces/CommonExampleInterface.h"
#include "InProcessMemory.h"
#include "Bullet3Common/b3Logging.h"
class InProcessPhysicsClientSharedMemoryMainThread : public PhysicsClientSharedMemory
@@ -124,3 +127,100 @@ b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnect(int argc, char* a
return (b3PhysicsClientHandle ) cl;
}
class InProcessPhysicsClientExistingExampleBrowser : public PhysicsClientSharedMemory
{
CommonExampleInterface* m_physicsServerExample;
SharedMemoryInterface* m_sharedMem;
b3Clock m_clock;
unsigned long long int m_prevTime;
public:
InProcessPhysicsClientExistingExampleBrowser (struct GUIHelperInterface* guiHelper)
{
m_sharedMem = new InProcessMemory;
CommonExampleOptions options(guiHelper);
options.m_sharedMem = m_sharedMem;
m_physicsServerExample = PhysicsServerCreateFunc(options);
m_physicsServerExample ->initPhysics();
m_physicsServerExample ->resetCamera();
setSharedMemoryInterface(m_sharedMem);
m_clock.reset();
m_prevTime = m_clock.getTimeMicroseconds();
}
virtual ~InProcessPhysicsClientExistingExampleBrowser()
{
m_physicsServerExample->exitPhysics();
//s_instancingRenderer->removeAllInstances();
delete m_physicsServerExample;
delete m_sharedMem;
}
// return non-null if there is a status, nullptr otherwise
virtual const struct SharedMemoryStatus* processServerStatus()
{
//m_physicsServerExample->updateGraphics();
unsigned long long int curTime = m_clock.getTimeMicroseconds();
unsigned long long int dtMicro = curTime - m_prevTime;
m_prevTime = curTime;
double dt = double(dtMicro)/1000000.;
m_physicsServerExample->stepSimulation(dt);
#if 0
{
//if (btIsExampleBrowserMainThreadTerminated(m_data))
//{
// PhysicsClientSharedMemory::disconnectSharedMemory();
//}
}
//{
//unsigned long int ms = m_clock.getTimeMilliseconds();
//if (ms>20)
//{
// B3_PROFILE("m_clock.reset()");
//
// m_clock.reset();
//btUpdateInProcessExampleBrowserMainThread(m_data);
//}
}
#endif
{
b3Clock::usleep(0);
}
const SharedMemoryStatus* stat = 0;
{
stat = PhysicsClientSharedMemory::processServerStatus();
}
return stat;
}
virtual void renderScene()
{
m_physicsServerExample->renderScene();
}
};
void b3InProcessRenderSceneInternal(b3PhysicsClientHandle clientHandle)
{
InProcessPhysicsClientExistingExampleBrowser* cl = (InProcessPhysicsClientExistingExampleBrowser*) clientHandle;
cl->renderScene();
}
b3PhysicsClientHandle b3CreateInProcessPhysicsServerFromExistingExampleBrowserAndConnect(struct GUIHelperInterface* guiHelper)
{
InProcessPhysicsClientExistingExampleBrowser* cl = new InProcessPhysicsClientExistingExampleBrowser(guiHelper);
//InProcessPhysicsClientFromGuiHelper* cl = new InProcessPhysicsClientFromGuiHelper(guiHelper);
cl->connect();
return (b3PhysicsClientHandle ) cl;
}

3
examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.h
View File

@@ -14,8 +14,9 @@ b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnect(int argc, char* a
b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnectMainThread(int argc, char* argv[]);
b3PhysicsClientHandle b3CreateInProcessPhysicsServerFromExistingExampleBrowserAndConnect(struct GUIHelperInterface* guiHelper);
void b3InProcessRenderSceneInternal(b3PhysicsClientHandle clientHandle);
#ifdef __cplusplus
}

1
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -503,6 +503,7 @@ enum eCONNECT_METHOD {
eCONNECT_SHARED_MEMORY = 3,
eCONNECT_UDP = 4,
eCONNECT_TCP = 5,
eCONNECT_EXISTING_EXAMPLE_BROWSER=6,
};
enum eURDF_Flags