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