AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2025-01-05 15:21:06 +00:00
Code Issues Releases Wiki Activity

Prepare/allow for non-Bullet2-based physics command processor in pybullet/Bullet-C-API

Browse Source 
!!! Make sure to add examples/SharedMemory/PhysicsServerExampleBullet2.cpp to your build system, if needed
Bump up pybullet to version 1.0.9
This commit is contained in:
erwincoumans 2017-05-30 19:54:55 -07:00
parent 978dd5844d
commit 83f910711a
26 changed files with 327 additions and 855 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
examples/CommonInterfaces/CommonExampleInterface.h
View File

@ -3,6 +3,13 @@
#ifndef COMMON_EXAMPLE_INTERFACE_H
#define COMMON_EXAMPLE_INTERFACE_H
struct CommandProcessorCreationInterface
{
virtual class CommandProcessorInterface* createCommandProcessor()=0;
virtual void deleteCommandProcessor(CommandProcessorInterface*)=0;
};
struct CommonExampleOptions
{
struct GUIHelperInterface* m_guiHelper;
@ -11,13 +18,14 @@ struct CommonExampleOptions
int m_option;
const char* m_fileName;
class SharedMemoryInterface* m_sharedMem;
CommandProcessorCreationInterface* m_commandProcessorCreation;
CommonExampleOptions(struct GUIHelperInterface* helper, int option=0)
:m_guiHelper(helper),
m_option(option),
m_fileName(0),
m_sharedMem(0)
m_sharedMem(0),
m_commandProcessorCreation(0)
{
}

1
examples/ExampleBrowser/CMakeLists.txt
View File

@ -175,6 +175,7 @@ SET(BulletExampleBrowser_SRCS
../SharedMemory/PhysicsClient.cpp
../SharedMemory/PhysicsClientC_API.cpp
../SharedMemory/PhysicsServerExample.cpp
../SharedMemory/PhysicsServerExampleBullet2.cpp
../SharedMemory/PhysicsClientExample.cpp
../SharedMemory/PosixSharedMemory.cpp
../SharedMemory/Win32SharedMemory.cpp

4
examples/ExampleBrowser/ExampleEntries.cpp
View File

@ -39,7 +39,7 @@
#include "../FractureDemo/FractureDemo.h"
#include "../DynamicControlDemo/MotorDemo.h"
#include "../RollingFrictionDemo/RollingFrictionDemo.h"
#include "../SharedMemory/PhysicsServerExample.h"
#include "../SharedMemory/PhysicsServerExampleBullet2.h"
#include "../SharedMemory/PhysicsClientExample.h"
#include "../Constraints/TestHingeTorque.h"
#include "../RenderingExamples/TimeSeriesExample.h"
@ -138,7 +138,7 @@ static ExampleEntry gDefaultExamples[]=
ExampleEntry(0,"Physics Client-Server"),
ExampleEntry(1,"Physics Server", "Create a physics server that communicates with a physics client over shared memory. You can connect to the server using pybullet, a PhysicsClient or a UDP/TCP Bridge.",
PhysicsServerCreateFunc),
PhysicsServerCreateFuncBullet2),
ExampleEntry(1, "Physics Client (Shared Mem)", "Create a physics client that can communicate with a physics server over shared memory.", PhysicsClientCreateFunc),
// ExampleEntry(1,"Physics Server (Logging)", "Create a physics server that communicates with a physics client over shared memory. It will log all commands to a file.",

10
examples/ExampleBrowser/InProcessExampleBrowser.cpp
View File

@ -28,6 +28,8 @@ void* ExampleBrowserMemoryFunc();
#include "EmptyExample.h"
#include "../SharedMemory/PhysicsServerExample.h"
#include "../SharedMemory/PhysicsServerExampleBullet2.h"
#include "../SharedMemory/PhysicsClientExample.h"
#ifndef _WIN32
@ -124,14 +126,14 @@ static ExampleEntryPhysicsServer gDefaultExamplesPhysicsServer[]=
ExampleEntryPhysicsServer(0,"Robotics Control"),
ExampleEntryPhysicsServer(1,"Physics Server", "Create a physics server that communicates with a physics client over shared memory",
PhysicsServerCreateFunc),
PhysicsServerCreateFuncBullet2),
ExampleEntryPhysicsServer(1,"Physics Server (RTC)", "Create a physics server that communicates with a physics client over shared memory. At each update, the Physics Server will continue calling 'stepSimulation' based on the real-time clock (RTC).",
PhysicsServerCreateFunc,PHYSICS_SERVER_USE_RTC_CLOCK),
PhysicsServerCreateFuncBullet2,PHYSICS_SERVER_USE_RTC_CLOCK),
ExampleEntryPhysicsServer(1,"Physics Server (Logging)", "Create a physics server that communicates with a physics client over shared memory. It will log all commands to a file.",
PhysicsServerCreateFunc,PHYSICS_SERVER_ENABLE_COMMAND_LOGGING),
PhysicsServerCreateFuncBullet2,PHYSICS_SERVER_ENABLE_COMMAND_LOGGING),
ExampleEntryPhysicsServer(1,"Physics Server (Replay Log)", "Create a physics server that replay a command log from disk.",
PhysicsServerCreateFunc,PHYSICS_SERVER_REPLAY_FROM_COMMAND_LOG),
PhysicsServerCreateFuncBullet2,PHYSICS_SERVER_REPLAY_FROM_COMMAND_LOG),
};

1
examples/ExampleBrowser/premake4.lua
View File

@ -81,6 +81,7 @@ project "App_BulletExampleBrowser"
"../SharedMemory/PhysicsClientC_API.cpp",
"../SharedMemory/PhysicsClientC_API.h",
"../SharedMemory/PhysicsServerExample.cpp",
"../SharedMemory/PhysicsServerExampleBullet2.cpp",
"../SharedMemory/PhysicsClientExample.cpp",
"../SharedMemory/PhysicsServer.cpp",
"../SharedMemory/PhysicsServerSharedMemory.cpp",

1
examples/RobotSimulator/CMakeLists.txt
View File

@ -32,6 +32,7 @@ SET(RobotSimulator_SRCS
../../examples/SharedMemory/PhysicsServer.cpp
../../examples/SharedMemory/PhysicsServer.h
../../examples/SharedMemory/PhysicsServerExample.cpp
../../examples/SharedMemory/PhysicsServerExampleBullet2.cpp
../../examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp
../../examples/SharedMemory/PhysicsServerSharedMemory.cpp
../../examples/SharedMemory/PhysicsServerSharedMemory.h

1
examples/RobotSimulator/premake4.lua
View File

@ -10,6 +10,7 @@ myfiles =
"../../examples/SharedMemory/PhysicsServer.cpp",
"../../examples/SharedMemory/PhysicsServer.h",
"../../examples/SharedMemory/PhysicsServerExample.cpp",
"../../examples/SharedMemory/PhysicsServerExampleBullet2.cpp",
"../../examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp",
"../../examples/SharedMemory/PhysicsServerSharedMemory.cpp",
"../../examples/SharedMemory/PhysicsServerSharedMemory.h",

1
examples/SharedMemory/CMakeLists.txt
View File

@ -7,6 +7,7 @@ SET(SharedMemory_SRCS
PhysicsClientSharedMemory.cpp
PhysicsClientExample.cpp
PhysicsServerExample.cpp
PhysicsServerExampleBullet2.cpp
PhysicsServerSharedMemory.cpp
PhysicsServerSharedMemory.h
PhysicsServer.cpp

20
examples/SharedMemory/PhysicsClientExample.cpp
View File

@ -5,7 +5,7 @@
#include "../CommonInterfaces/Common2dCanvasInterface.h"
#include "SharedMemoryCommon.h"
#include "../CommonInterfaces/CommonParameterInterface.h"
#include "PhysicsServerCommandProcessor.h"
#include "PhysicsClientC_API.h"
#include "PhysicsClient.h"
//#include "SharedMemoryCommands.h"
@ -37,6 +37,7 @@ class PhysicsClientExample : public SharedMemoryCommon
{
protected:
b3PhysicsClientHandle m_physicsClientHandle;
//this m_physicsServer is only used when option eCLIENTEXAMPLE_SERVER is enabled
PhysicsServerSharedMemory m_physicsServer;
@ -520,10 +521,26 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId)
}
struct Bullet2CommandProcessorCreation3 : public CommandProcessorCreationInterface
{
virtual class CommandProcessorInterface* createCommandProcessor()
{
PhysicsServerCommandProcessor* proc = new PhysicsServerCommandProcessor;
return proc;
}
virtual void deleteCommandProcessor(CommandProcessorInterface* proc)
{
delete proc;
}
};
static Bullet2CommandProcessorCreation3 sB2PC2;
PhysicsClientExample::PhysicsClientExample(GUIHelperInterface* helper, int options)
:SharedMemoryCommon(helper),
m_physicsClientHandle(0),
m_physicsServer(&sB2PC2,0,0),
m_wantsTermination(false),
m_sharedMemoryKey(SHARED_MEMORY_KEY),
m_selectedBody(-1),
@ -565,6 +582,7 @@ PhysicsClientExample::~PhysicsClientExample()
m_canvas->destroyCanvas(m_canvasSegMaskIndex);
}
b3Printf("~PhysicsClientExample\n");
}

7
examples/SharedMemory/PhysicsCommandProcessorInterface.h
View File

@ -31,6 +31,7 @@ public:
class btVector3;
class btQuaternion;
class CommandProcessorInterface : public PhysicsCommandProcessorInterface
{
@ -41,6 +42,7 @@ public:
virtual void syncPhysicsToGraphics()=0;
virtual void stepSimulationRealTime(double dtInSec,const struct b3VRControllerEvent* vrEvents, int numVREvents, const struct b3KeyboardEvent* keyEvents, int numKeyEvents)=0;
virtual void enableRealTimeSimulation(bool enableRealTimeSim)=0;
virtual bool isRealTimeSimulationEnabled() const=0;
virtual void enableCommandLogging(bool enable, const char* fileName)=0;
virtual void replayFromLogFile(const char* fileName)=0;
@ -50,6 +52,11 @@ public:
virtual bool movePickedBody(const btVector3& rayFromWorld, const btVector3& rayToWorld)=0;
virtual void removePickingConstraint()=0;
virtual const btVector3& getVRTeleportPosition() const=0;
virtual void setVRTeleportPosition(const btVector3& vrReleportPos)=0;
virtual const btQuaternion& getVRTeleportOrientation() const=0;
virtual void setVRTeleportOrientation(const btQuaternion& vrReleportOrn)=0;
};
#endif //PHYSICS_COMMAND_PROCESSOR_INTERFACE_H

26
examples/SharedMemory/PhysicsLoopBack.cpp
View File

@ -2,25 +2,44 @@
#include "PhysicsServerSharedMemory.h"
#include "PhysicsClientSharedMemory.h"
#include "../CommonInterfaces/CommonGUIHelperInterface.h"
#include "PhysicsServerCommandProcessor.h"
#include "../CommonInterfaces/CommonExampleInterface.h"
struct PhysicsLoopBackInternalData
{
CommandProcessorInterface* m_commandProcessor;
PhysicsClientSharedMemory* m_physicsClient;
PhysicsServerSharedMemory* m_physicsServer;
DummyGUIHelper m_noGfx;
PhysicsLoopBackInternalData()
:m_physicsClient(0),
:m_commandProcessor(0),
m_physicsClient(0),
m_physicsServer(0)
{
}
};
struct Bullet2CommandProcessorCreation2 : public CommandProcessorCreationInterface
{
virtual class CommandProcessorInterface* createCommandProcessor()
{
PhysicsServerCommandProcessor* proc = new PhysicsServerCommandProcessor;
return proc;
}
virtual void deleteCommandProcessor(CommandProcessorInterface* proc)
{
delete proc;
}
};
static Bullet2CommandProcessorCreation2 sB2Proc;
PhysicsLoopBack::PhysicsLoopBack()
{
m_data = new PhysicsLoopBackInternalData;
m_data->m_physicsServer = new PhysicsServerSharedMemory();
m_data->m_physicsServer = new PhysicsServerSharedMemory(&sB2Proc, 0,0);
m_data->m_physicsClient = new PhysicsClientSharedMemory();
}
@ -28,6 +47,7 @@ PhysicsLoopBack::~PhysicsLoopBack()
{
delete m_data->m_physicsClient;
delete m_data->m_physicsServer;
delete m_data->m_commandProcessor;
delete m_data;
}

623
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@ -49,9 +49,8 @@
//@todo(erwincoumans) those globals are hacks for a VR demo, move this to Python/pybullet!
btVector3 gLastPickPos(0, 0, 0);
bool gCloseToKuka=false;
bool gEnableRealTimeSimVR=false;
bool gCreateDefaultRobotAssets = false;
int gInternalSimFlags = 0;
bool gResetSimulation = 0;
int gVRTrackingObjectUniqueId = -1;
@ -59,9 +58,9 @@ int gVRTrackingObjectFlag = VR_CAMERA_TRACK_OBJECT_ORIENTATION;
btTransform gVRTrackingObjectTr = btTransform::getIdentity();
int gMaxNumCmdPer1ms = -1;//experiment: add some delay to avoid threads starving other threads
int gCreateObjectSimVR = -1;
int gEnableKukaControl = 0;
btVector3 gVRTeleportPos1(0,0,0);
btQuaternion gVRTeleportOrn(0, 0, 0,1);
@ -1137,25 +1136,13 @@ struct PhysicsServerCommandProcessorInternalData
bool m_allowRealTimeSimulation;
bool m_hasGround;
b3VRControllerEvents m_vrControllerEvents;
btAlignedObjectArray<b3KeyboardEvent> m_keyboardEvents;
btMultiBodyFixedConstraint* m_gripperRigidbodyFixed;
btMultiBody* m_gripperMultiBody;
btMultiBodyFixedConstraint* m_kukaGripperFixed;
btMultiBody* m_kukaGripperMultiBody;
btMultiBodyPoint2Point* m_kukaGripperRevolute1;
btMultiBodyPoint2Point* m_kukaGripperRevolute2;
int m_huskyId;
int m_KukaId;
int m_sphereId;
int m_gripperId;
CommandLogger* m_commandLogger;
CommandLogPlayback* m_logPlayback;
@ -1231,17 +1218,6 @@ struct PhysicsServerCommandProcessorInternalData
PhysicsServerCommandProcessorInternalData()
:
m_allowRealTimeSimulation(false),
m_hasGround(false),
m_gripperRigidbodyFixed(0),
m_gripperMultiBody(0),
m_kukaGripperFixed(0),
m_kukaGripperMultiBody(0),
m_kukaGripperRevolute1(0),
m_kukaGripperRevolute2(0),
m_huskyId(-1),
m_KukaId(-1),
m_sphereId(-1),
m_gripperId(-1),
m_commandLogger(0),
m_logPlayback(0),
m_physicsDeltaTime(1./240.),
@ -4087,7 +4063,6 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_INTERNAL_SIMULATION_FLAGS)
{
//these flags are for internal/temporary/easter-egg/experimental demo purposes, use at own risk
gCreateDefaultRobotAssets = (clientCmd.m_physSimParamArgs.m_internalSimFlags & SIM_PARAM_INTERNAL_CREATE_ROBOT_ASSETS);
gInternalSimFlags = clientCmd.m_physSimParamArgs.m_internalSimFlags;
}
@ -4111,11 +4086,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
{
gContactBreakingThreshold = clientCmd.m_physSimParamArgs.m_contactBreakingThreshold;
}
if (clientCmd.m_updateFlags&SIM_PARAM_MAX_CMD_PER_1MS)
{
gMaxNumCmdPer1ms = clientCmd.m_physSimParamArgs.m_maxNumCmdPer1ms;
}
if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_COLLISION_FILTER_MODE)
{
m_data->m_broadphaseCollisionFilterCallback->m_filterMode = clientCmd.m_physSimParamArgs.m_collisionFilterMode;
@ -6282,7 +6253,7 @@ btQuaternion gVRController2Orn(0,0,0,1);
btScalar gVRGripper2Analog = 0;
btScalar gVRGripperAnalog = 0;
bool gVRGripperClosed = false;
int gDroppedSimulationSteps = 0;
@ -6295,6 +6266,11 @@ void PhysicsServerCommandProcessor::enableRealTimeSimulation(bool enableRealTime
m_data->m_allowRealTimeSimulation = enableRealTimeSim;
}
bool PhysicsServerCommandProcessor::isRealTimeSimulationEnabled() const
{
return m_data->m_allowRealTimeSimulation;
}
void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec, const struct b3VRControllerEvent* vrControllerEvents, int numVRControllerEvents,const struct b3KeyboardEvent* keyEvents, int numKeyEvents)
{
m_data->m_vrControllerEvents.addNewVREvents(vrControllerEvents,numVRControllerEvents);
@ -6348,23 +6324,22 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec, const
if (gVRTrackingObjectUniqueId>=0)
{
gVRTrackingObjectTr.setOrigin(bodyHandle->m_multiBody->getBaseWorldTransform().getOrigin());
gVRTeleportPos1 = gVRTrackingObjectTr.getOrigin();
}
if (gVRTrackingObjectFlag&VR_CAMERA_TRACK_OBJECT_ORIENTATION)
{
gVRTrackingObjectTr.setBasis(bodyHandle->m_multiBody->getBaseWorldTransform().getBasis());
gVRTeleportOrn = gVRTrackingObjectTr.getRotation();
}
}
}
if ((m_data->m_allowRealTimeSimulation) && m_data->m_guiHelper)
{
///this hardcoded C++ scene creation is temporary for demo purposes. It will be done in Python later...
if (gCreateDefaultRobotAssets)
{
createDefaultRobotAssets();
}
int maxSteps = m_data->m_numSimulationSubSteps+3;
if (m_data->m_numSimulationSubSteps)
{
@ -6413,553 +6388,29 @@ void PhysicsServerCommandProcessor::resetSimulation()
m_data->m_bodyHandles.exitHandles();
m_data->m_bodyHandles.initHandles();
m_data->m_hasGround = false;
m_data->m_gripperRigidbodyFixed = 0;
}
//todo: move this to Python/scripting (it is almost ready to be removed!)
void PhysicsServerCommandProcessor::createDefaultRobotAssets()
{
static btAlignedObjectArray<char> gBufferServerToClient;
gBufferServerToClient.resize(SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
int bodyId = 0;
if (gCreateObjectSimVR >= 0)
{
gCreateObjectSimVR = -1;
btMatrix3x3 mat(gVRGripperOrn);
btScalar spawnDistance = 0.1;
btVector3 spawnDir = mat.getColumn(0);
btVector3 shiftPos = spawnDir*spawnDistance;
btVector3 spawnPos = gVRGripperPos + shiftPos;
loadUrdf("sphere_small.urdf", spawnPos, gVRGripperOrn, true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size(),0);
//loadUrdf("lego/lego.urdf", spawnPos, gVRGripperOrn, true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
m_data->m_sphereId = bodyId;
InteralBodyData* parentBody = m_data->m_bodyHandles.getHandle(bodyId);
if (parentBody->m_multiBody)
{
parentBody->m_multiBody->setBaseVel(spawnDir * 5);
}
}
if (!m_data->m_hasGround)
{
m_data->m_hasGround = true;
loadUrdf("plane.urdf", btVector3(0, 0, 0), btQuaternion(0, 0, 0, 1), true, true, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("samurai.urdf", btVector3(0, 0, 0), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
// loadUrdf("quadruped/quadruped.urdf", btVector3(2, 2, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
if (m_data->m_gripperRigidbodyFixed == 0)
{
int bodyId = 0;
if (loadUrdf("pr2_gripper.urdf", btVector3(-0.2, 0.15, 0.9), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()))
{
InteralBodyData* parentBody = m_data->m_bodyHandles.getHandle(bodyId);
if (parentBody->m_multiBody)
{
parentBody->m_multiBody->setHasSelfCollision(0);
btVector3 pivotInParent(0.2, 0, 0);
btMatrix3x3 frameInParent;
//frameInParent.setRotation(btQuaternion(0, 0, 0, 1));
frameInParent.setIdentity();
btVector3 pivotInChild(0, 0, 0);
btMatrix3x3 frameInChild;
frameInChild.setIdentity();
m_data->m_gripperRigidbodyFixed = new btMultiBodyFixedConstraint(parentBody->m_multiBody, -1, 0, pivotInParent, pivotInChild, frameInParent, frameInChild);
m_data->m_gripperMultiBody = parentBody->m_multiBody;
if (m_data->m_gripperMultiBody->getNumLinks() > 2)
{
m_data->m_gripperMultiBody->setJointPos(0, 0);
m_data->m_gripperMultiBody->setJointPos(2, 0);
}
m_data->m_gripperRigidbodyFixed->setMaxAppliedImpulse(500);
btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*)m_data->m_dynamicsWorld;
world->addMultiBodyConstraint(m_data->m_gripperRigidbodyFixed);
}
}
}
loadUrdf("kuka_iiwa/model_vr_limits.urdf", btVector3(1.4, -0.2, 0.6), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
m_data->m_KukaId = bodyId;
if (m_data->m_KukaId>=0)
{
InteralBodyData* kukaBody = m_data->m_bodyHandles.getHandle(m_data->m_KukaId);
if (kukaBody->m_multiBody && kukaBody->m_multiBody->getNumDofs() == 7)
{
btScalar q[7];
q[0] = 0;// -SIMD_HALF_PI;
q[1] = 0;
q[2] = 0;
q[3] = SIMD_HALF_PI;
q[4] = 0;
q[5] = -SIMD_HALF_PI*0.66;
q[6] = 0;
for (int i = 0; i < 7; i++)
{
kukaBody->m_multiBody->setJointPos(i, q[i]);
}
btAlignedObjectArray<btQuaternion> scratch_q;
btAlignedObjectArray<btVector3> scratch_m;
kukaBody->m_multiBody->forwardKinematics(scratch_q, scratch_m);
int nLinks = kukaBody->m_multiBody->getNumLinks();
scratch_q.resize(nLinks + 1);
scratch_m.resize(nLinks + 1);
kukaBody->m_multiBody->updateCollisionObjectWorldTransforms(scratch_q, scratch_m);
}
}
#if 1
loadUrdf("lego/lego.urdf", btVector3(1.0, -0.2, .7), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("lego/lego.urdf", btVector3(1.0, -0.2, .8), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("lego/lego.urdf", btVector3(1.0, -0.2, .9), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
#endif
// loadUrdf("r2d2.urdf", btVector3(-2, -4, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
#if 1
// Load one motor gripper for kuka
loadSdf("gripper/wsg50_one_motor_gripper_new_free_base.sdf", &gBufferServerToClient[0], gBufferServerToClient.size(), true,CUF_USE_SDF);
m_data->m_gripperId = bodyId + 1;
{
InteralBodyData* gripperBody = m_data->m_bodyHandles.getHandle(m_data->m_gripperId);
// Reset the default gripper motor maximum torque for damping to 0
for (int i = 0; i < gripperBody->m_multiBody->getNumLinks(); i++)
{
if (supportsJointMotor(gripperBody->m_multiBody, i))
{
btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)gripperBody->m_multiBody->getLink(i).m_userPtr;
if (motor)
{
motor->setMaxAppliedImpulse(0);
}
}
}
}
#endif
#if 1
for (int i = 0; i < 6; i++)
{
loadUrdf("jenga/jenga.urdf", btVector3(1.3-0.1*i,-0.7, .75), btQuaternion(btVector3(0,1,0),SIMD_HALF_PI), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
}
#endif
//loadUrdf("nao/nao.urdf", btVector3(2,5, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
// Add slider joint for fingers
btVector3 pivotInParent1(-0.055, 0, 0.02);
btVector3 pivotInChild1(0, 0, 0);
btMatrix3x3 frameInParent1(btQuaternion(0, 0, 0, 1.0));
btMatrix3x3 frameInChild1(btQuaternion(0, 0, 0, 1.0));
btVector3 jointAxis1(1.0, 0, 0);
btVector3 pivotInParent2(0.055, 0, 0.02);
btVector3 pivotInChild2(0, 0, 0);
btMatrix3x3 frameInParent2(btQuaternion(0, 0, 0, 1.0));
btMatrix3x3 frameInChild2(btQuaternion(0, 0, 1.0, 0));
btVector3 jointAxis2(1.0, 0, 0);
if (m_data->m_gripperId>=0)
{
InteralBodyData* gripperBody = m_data->m_bodyHandles.getHandle(m_data->m_gripperId);
m_data->m_kukaGripperRevolute1 = new btMultiBodyPoint2Point(gripperBody->m_multiBody, 2, gripperBody->m_multiBody, 4, pivotInParent1, pivotInChild1);
m_data->m_kukaGripperRevolute1->setMaxAppliedImpulse(5.0);
m_data->m_kukaGripperRevolute2 = new btMultiBodyPoint2Point(gripperBody->m_multiBody, 3, gripperBody->m_multiBody, 6, pivotInParent2, pivotInChild2);
m_data->m_kukaGripperRevolute2->setMaxAppliedImpulse(5.0);
m_data->m_dynamicsWorld->addMultiBodyConstraint(m_data->m_kukaGripperRevolute1);
m_data->m_dynamicsWorld->addMultiBodyConstraint(m_data->m_kukaGripperRevolute2);
}
if (m_data->m_KukaId>=0)
{
InteralBodyData* kukaBody = m_data->m_bodyHandles.getHandle(m_data->m_KukaId);
if (kukaBody->m_multiBody && kukaBody->m_multiBody->getNumDofs()==7)
{
if (m_data->m_gripperId>=0)
{
InteralBodyData* gripperBody = m_data->m_bodyHandles.getHandle(m_data->m_gripperId);
gripperBody->m_multiBody->setHasSelfCollision(0);
btVector3 pivotInParent(0, 0, 0.05);
btMatrix3x3 frameInParent;
frameInParent.setIdentity();
btVector3 pivotInChild(0, 0, 0);
btMatrix3x3 frameInChild;
frameInChild.setIdentity();
m_data->m_kukaGripperFixed = new btMultiBodyFixedConstraint(kukaBody->m_multiBody, 6, gripperBody->m_multiBody, 0, pivotInParent, pivotInChild, frameInParent, frameInChild);
m_data->m_kukaGripperMultiBody = gripperBody->m_multiBody;
m_data->m_kukaGripperFixed->setMaxAppliedImpulse(500);
m_data->m_dynamicsWorld->addMultiBodyConstraint(m_data->m_kukaGripperFixed);
}
}
}
#if 0
for (int i = 0; i < 10; i++)
{
loadUrdf("cube.urdf", btVector3(-4, -2, 0.5 + i), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
}
loadUrdf("sphere2.urdf", btVector3(-5, 0, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("sphere2.urdf", btVector3(-5, 0, 2), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("sphere2.urdf", btVector3(-5, 0, 3), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
#endif
btTransform objectLocalTr[] = {
btTransform(btQuaternion(0, 0, 0, 1), btVector3(0.0, 0.0, 0.0)),
btTransform(btQuaternion(btVector3(0,0,1),-SIMD_HALF_PI), btVector3(0.0, 0.15, 0.64)),
btTransform(btQuaternion(0, 0, 0, 1), btVector3(0.1, 0.15, 0.85)),
btTransform(btQuaternion(0, 0, 0, 1), btVector3(-0.4, 0.05, 0.85)),
btTransform(btQuaternion(0, 0, 0, 1), btVector3(-0.3, -0.05, 0.7)),
btTransform(btQuaternion(0, 0, 0, 1), btVector3(0.1, 0.05, 0.7)),
btTransform(btQuaternion(0, 0, 0, 1), btVector3(-0.2, 0.15, 0.7)),
btTransform(btQuaternion(0, 0, 0, 1), btVector3(-0.2, 0.15, 0.9)),
btTransform(btQuaternion(0, 0, 0, 1), btVector3(0.2, 0.05, 0.8))
};
btAlignedObjectArray<btTransform> objectWorldTr;
int numOb = sizeof(objectLocalTr) / sizeof(btTransform);
objectWorldTr.resize(numOb);
btTransform tr;
tr.setIdentity();
tr.setRotation(btQuaternion(btVector3(0, 0, 1), SIMD_HALF_PI));
tr.setOrigin(btVector3(1.0, -0.2, 0));
for (int i = 0; i < numOb; i++)
{
objectWorldTr[i] = tr*objectLocalTr[i];
}
// Table area
loadUrdf("table/table.urdf", objectWorldTr[0].getOrigin(), objectWorldTr[0].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
//loadUrdf("tray/tray_textured.urdf", objectWorldTr[1].getOrigin(), objectWorldTr[1].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
//loadUrdf("cup_small.urdf", objectWorldTr[2].getOrigin(), objectWorldTr[2].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
//loadUrdf("pitcher_small.urdf", objectWorldTr[3].getOrigin(), objectWorldTr[3].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("teddy_vhacd.urdf", objectWorldTr[4].getOrigin(), objectWorldTr[4].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("cube_small.urdf", objectWorldTr[5].getOrigin(), objectWorldTr[5].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("sphere_small.urdf", objectWorldTr[6].getOrigin(), objectWorldTr[6].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("duck_vhacd.urdf", objectWorldTr[7].getOrigin(), objectWorldTr[7].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
//loadUrdf("Apple/apple.urdf", objectWorldTr[8].getOrigin(), objectWorldTr[8].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
// Shelf area
loadSdf("kiva_shelf/model.sdf", &gBufferServerToClient[0], gBufferServerToClient.size(), true, CUF_USE_SDF);
loadUrdf("teddy_vhacd.urdf", btVector3(-0.1, 0.6, 0.85), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("sphere_small.urdf", btVector3(-0.1, 0.6, 1.25), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("cube_small.urdf", btVector3(0.3, 0.6, 0.85), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
// Chess area
loadUrdf("table_square/table_square.urdf", btVector3(-1.0, 0, 0.0), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
//loadUrdf("pawn.urdf", btVector3(-0.8, -0.1, 0.7), btQuaternion(btVector3(1, 0, 0), SIMD_HALF_PI), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
//loadUrdf("queen.urdf", btVector3(-0.9, -0.2, 0.7), btQuaternion(btVector3(1, 0, 0), SIMD_HALF_PI), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
//loadUrdf("king.urdf", btVector3(-1.0, 0, 0.7), btQuaternion(btVector3(1, 0, 0), SIMD_HALF_PI), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
//loadUrdf("bishop.urdf", btVector3(-1.1, 0.1, 0.7), btQuaternion(btVector3(1, 0, 0), SIMD_HALF_PI), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
//loadUrdf("rook.urdf", btVector3(-1.2, 0, 0.7), btQuaternion(btVector3(1, 0, 0), SIMD_HALF_PI), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
//loadUrdf("knight.urdf", btVector3(-1.2, 0.2, 0.7), btQuaternion(btVector3(1, 0, 0), SIMD_HALF_PI), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("husky/husky.urdf", btVector3(2, -5, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
m_data->m_huskyId = bodyId;
m_data->m_dynamicsWorld->setGravity(btVector3(0, 0, -10));
}
if (m_data->m_kukaGripperFixed && m_data->m_kukaGripperMultiBody)
{
InteralBodyData* childBody = m_data->m_bodyHandles.getHandle(m_data->m_gripperId);
// Add gripper controller
btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)childBody->m_multiBody->getLink(1).m_userPtr;
if (motor)
{
btScalar posTarget = (-0.048)*btMin(btScalar(0.75), gVRGripper2Analog) / 0.75;
motor->setPositionTarget(posTarget, .8);
motor->setVelocityTarget(0.0, .5);
motor->setMaxAppliedImpulse(1.0);
}
}
if (m_data->m_gripperRigidbodyFixed && m_data->m_gripperMultiBody)
{
m_data->m_gripperRigidbodyFixed->setFrameInB(btMatrix3x3(gVRGripperOrn));
m_data->m_gripperRigidbodyFixed->setPivotInB(gVRGripperPos);
btScalar avg = 0.f;
for (int i = 0; i < m_data->m_gripperMultiBody->getNumLinks(); i++)
{
if (supportsJointMotor(m_data->m_gripperMultiBody, i))
{
btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)m_data->m_gripperMultiBody->getLink(i ).m_userPtr;
if (motor)
{
motor->setErp(0.2);
btScalar posTarget = 0.1 + (1 - btMin(btScalar(0.75),gVRGripperAnalog)*btScalar(1.5))*SIMD_HALF_PI*0.29;
btScalar maxPosTarget = 0.55;
btScalar correction = 0.f;
if (avg)
{
correction = m_data->m_gripperMultiBody->getJointPos(i) - avg;
}
if (m_data->m_gripperMultiBody->getJointPos(i) < 0)
{
m_data->m_gripperMultiBody->setJointPos(i,0);
}
if (m_data->m_gripperMultiBody->getJointPos(i) > maxPosTarget)
{
m_data->m_gripperMultiBody->setJointPos(i, maxPosTarget);
}
if (avg)
{
motor->setPositionTarget(avg, 1);
}
else
{
motor->setPositionTarget(posTarget, 1);
}
motor->setVelocityTarget(0, 0.5);
btScalar maxImp = (1+0.1*i)*m_data->m_physicsDeltaTime;
motor->setMaxAppliedImpulse(maxImp);
avg = m_data->m_gripperMultiBody->getJointPos(i);
//motor->setRhsClamp(gRhsClamp);
}
}
}
}
// Inverse kinematics for KUKA
if (m_data->m_KukaId>=0)
{
InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(m_data->m_KukaId);
if (bodyHandle && bodyHandle->m_multiBody && bodyHandle->m_multiBody->getNumDofs()==7)
{
btMultiBody* mb = bodyHandle->m_multiBody;
btScalar sqLen = (mb->getBaseWorldTransform().getOrigin() - gVRController2Pos).length2();
btScalar distanceThreshold = 1.3;
gCloseToKuka=(sqLen<(distanceThreshold*distanceThreshold));
int numDofs = bodyHandle->m_multiBody->getNumDofs();
btAlignedObjectArray<double> q_new;
btAlignedObjectArray<double> q_current;
q_current.resize(numDofs);
for (int i = 0; i < numDofs; i++)
{
q_current[i] = bodyHandle->m_multiBody->getJointPos(i);
}
q_new.resize(numDofs);
//sensible rest-pose
q_new[0] = 0;// -SIMD_HALF_PI;
q_new[1] = 0;
q_new[2] = 0;
q_new[3] = SIMD_HALF_PI;
q_new[4] = 0;
q_new[5] = -SIMD_HALF_PI*0.66;
q_new[6] = 0;
if (gCloseToKuka && gEnableKukaControl)
{
double dampIk[6] = {1.0, 1.0, 1.0, 1.0, 1.0, 0.0};
IKTrajectoryHelper** ikHelperPtrPtr = m_data->m_inverseKinematicsHelpers.find(bodyHandle->m_multiBody);
IKTrajectoryHelper* ikHelperPtr = 0;
if (ikHelperPtrPtr)
{
ikHelperPtr = *ikHelperPtrPtr;
}
else
{
IKTrajectoryHelper* tmpHelper = new IKTrajectoryHelper;
m_data->m_inverseKinematicsHelpers.insert(bodyHandle->m_multiBody, tmpHelper);
ikHelperPtr = tmpHelper;
}
int endEffectorLinkIndex = 6;
if (ikHelperPtr && (endEffectorLinkIndex<bodyHandle->m_multiBody->getNumLinks()))
{
b3AlignedObjectArray<double> jacobian_linear;
jacobian_linear.resize(3*numDofs);
b3AlignedObjectArray<double> jacobian_angular;
jacobian_angular.resize(3*numDofs);
int jacSize = 0;
btInverseDynamics::MultiBodyTree* tree = m_data->findOrCreateTree(bodyHandle->m_multiBody);
if (tree)
{
jacSize = jacobian_linear.size();
// Set jacobian value
int baseDofs = bodyHandle->m_multiBody->hasFixedBase() ? 0 : 6;
btInverseDynamics::vecx nu(numDofs+baseDofs), qdot(numDofs + baseDofs), q(numDofs + baseDofs), joint_force(numDofs + baseDofs);
for (int i = 0; i < numDofs; i++)
{
q_current[i] = bodyHandle->m_multiBody->getJointPos(i);
q[i+baseDofs] = bodyHandle->m_multiBody->getJointPos(i);
qdot[i + baseDofs] = 0;
nu[i+baseDofs] = 0;
}
// Set the gravity to correspond to the world gravity
btInverseDynamics::vec3 id_grav(m_data->m_dynamicsWorld->getGravity());
if (-1 != tree->setGravityInWorldFrame(id_grav) &&
-1 != tree->calculateInverseDynamics(q, qdot, nu, &joint_force))
{
tree->calculateJacobians(q);
btInverseDynamics::mat3x jac_t(3,numDofs);
btInverseDynamics::mat3x jac_r(3,numDofs);
tree->getBodyJacobianTrans(endEffectorLinkIndex+1, &jac_t);
tree->getBodyJacobianRot(endEffectorLinkIndex+1, &jac_r);
for (int i = 0; i < 3; ++i)
{
for (int j = 0; j < numDofs; ++j)
{
jacobian_linear[i*numDofs+j] = jac_t(i,j);
jacobian_angular[i*numDofs+j] = jac_r(i,j);
}
}
}
}
int ikMethod= IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE;//IK2_VEL_DLS_WITH_ORIENTATION; //IK2_VEL_DLS;
btVector3DoubleData endEffectorWorldPosition;
btVector3DoubleData endEffectorWorldOrientation;
btVector3DoubleData targetWorldPosition;
btVector3DoubleData targetWorldOrientation;
btVector3 endEffectorPosWorld = bodyHandle->m_multiBody->getLink(endEffectorLinkIndex).m_cachedWorldTransform.getOrigin();
btQuaternion endEffectorOriWorld = bodyHandle->m_multiBody->getLink(endEffectorLinkIndex).m_cachedWorldTransform.getRotation();
btVector4 endEffectorOri(endEffectorOriWorld.x(),endEffectorOriWorld.y(),endEffectorOriWorld.z(),endEffectorOriWorld.w());
// Prescribed position and orientation
static btScalar time=0.f;
time+=0.01;
btVector3 targetPos(0.4-0.4*b3Cos( time), 0, 0.8+0.4*b3Cos( time));
targetPos +=mb->getBasePos();
btVector4 downOrn(0,1,0,0);
// Controller orientation
btVector4 controllerOrn(gVRController2Orn.x(), gVRController2Orn.y(), gVRController2Orn.z(), gVRController2Orn.w());
// Set position and orientation
endEffectorPosWorld.serializeDouble(endEffectorWorldPosition);
endEffectorOri.serializeDouble(endEffectorWorldOrientation);
downOrn.serializeDouble(targetWorldOrientation);
//targetPos.serializeDouble(targetWorldPosition);
gVRController2Pos.serializeDouble(targetWorldPosition);
//controllerOrn.serializeDouble(targetWorldOrientation);
if (ikMethod == IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE)
{
btAlignedObjectArray<double> lower_limit;
btAlignedObjectArray<double> upper_limit;
btAlignedObjectArray<double> joint_range;
btAlignedObjectArray<double> rest_pose;
lower_limit.resize(numDofs);
upper_limit.resize(numDofs);
joint_range.resize(numDofs);
rest_pose.resize(numDofs);
lower_limit[0] = -.967;
lower_limit[1] = -2.0;
lower_limit[2] = -2.96;
lower_limit[3] = 0.19;
lower_limit[4] = -2.96;
lower_limit[5] = -2.09;
lower_limit[6] = -3.05;
upper_limit[0] = .96;
upper_limit[1] = 2.0;
upper_limit[2] = 2.96;
upper_limit[3] = 2.29;
upper_limit[4] = 2.96;
upper_limit[5] = 2.09;
upper_limit[6] = 3.05;
joint_range[0] = 5.8;
joint_range[1] = 4;
joint_range[2] = 5.8;
joint_range[3] = 4;
joint_range[4] = 5.8;
joint_range[5] = 4;
joint_range[6] = 6;
rest_pose[0] = 0;
rest_pose[1] = 0;
rest_pose[2] = 0;
rest_pose[3] = SIMD_HALF_PI;
rest_pose[4] = 0;
rest_pose[5] = -SIMD_HALF_PI*0.66;
rest_pose[6] = 0;
ikHelperPtr->computeNullspaceVel(numDofs, &q_current[0], &lower_limit[0], &upper_limit[0], &joint_range[0], &rest_pose[0]);
}
ikHelperPtr->computeIK(targetWorldPosition.m_floats, targetWorldOrientation.m_floats,
endEffectorWorldPosition.m_floats, endEffectorWorldOrientation.m_floats,
&q_current[0],
numDofs, endEffectorLinkIndex,
&q_new[0], ikMethod, &jacobian_linear[0], &jacobian_angular[0], jacSize*2, dampIk);
}
}
//directly set the position of the links, only for debugging IK, don't use this method!
#if 0
if (0)
{
for (int i=0;i<mb->getNumLinks();i++)
{
btScalar desiredPosition = q_new[i];
mb->setJointPosMultiDof(i,&desiredPosition);
}
} else
#endif
{
int numMotors = 0;
//find the joint motors and apply the desired velocity and maximum force/torque
{
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* motor = (btMultiBodyJointMotor*)mb->getLink(link).m_userPtr;
if (motor)
{
btScalar desiredVelocity = 0.f;
btScalar desiredPosition = q_new[link];
motor->setRhsClamp(gRhsClamp);
//printf("link %d: %f", link, q_new[link]);
motor->setVelocityTarget(desiredVelocity,1.0);
motor->setPositionTarget(desiredPosition,0.6);
btScalar maxImp = 1.0;
motor->setMaxAppliedImpulse(maxImp);
numMotors++;
}
}
velIndex += mb->getLink(link).m_dofCount;
posIndex += mb->getLink(link).m_posVarCount;
}
}
}
}
}
}
void PhysicsServerCommandProcessor::setTimeOut(double /*timeOutInSeconds*/)
{
}
const btVector3& PhysicsServerCommandProcessor::getVRTeleportPosition() const
{
return gVRTeleportPos1;
}
void PhysicsServerCommandProcessor::setVRTeleportPosition(const btVector3& vrTeleportPos)
{
gVRTeleportPos1 = vrTeleportPos;
}
const btQuaternion& PhysicsServerCommandProcessor::getVRTeleportOrientation() const
{
return gVRTeleportOrn;
}
void PhysicsServerCommandProcessor::setVRTeleportOrientation(const btQuaternion& vrTeleportOrn)
{
gVRTeleportOrn = vrTeleportOrn;
}

13
examples/SharedMemory/PhysicsServerCommandProcessor.h
View File

@ -20,11 +20,6 @@ class PhysicsServerCommandProcessor : public CommandProcessorInterface
struct PhysicsServerCommandProcessorInternalData* m_data;
//todo: move this to physics client side / Python
void createDefaultRobotAssets();
void resetSimulation();
protected:
@ -101,9 +96,17 @@ public:
virtual void stepSimulationRealTime(double dtInSec, const struct b3VRControllerEvent* vrEvents, int numVREvents, const struct b3KeyboardEvent* keyEvents, int numKeyEvents);
virtual void enableRealTimeSimulation(bool enableRealTimeSim);
virtual bool isRealTimeSimulationEnabled() const;
void applyJointDamping(int bodyUniqueId);
virtual void setTimeOut(double timeOutInSeconds);
virtual const btVector3& getVRTeleportPosition() const;
virtual void setVRTeleportPosition(const btVector3& vrReleportPos);
virtual const btQuaternion& getVRTeleportOrientation() const;
virtual void setVRTeleportOrientation(const btQuaternion& vrReleportOrn);
};
#endif //PHYSICS_SERVER_COMMAND_PROCESSOR_H

340
examples/SharedMemory/PhysicsServerExample.cpp
View File

@ -4,9 +4,7 @@
#include "PhysicsServerExample.h"
#ifdef B3_USE_MIDI
#include "RtMidi.h"
#endif//B3_USE_MIDI
#include "PhysicsServerSharedMemory.h"
#include "Bullet3Common/b3CommandLineArgs.h"
@ -24,37 +22,26 @@
//@todo(erwincoumans) those globals are hacks for a VR demo, move this to Python/pybullet!
extern btVector3 gLastPickPos;
bool gEnablePicking=true;
bool gEnableTeleporting=true;
bool gEnableRendering= true;
bool gEnableSyncPhysicsRendering= true;
bool gEnableUpdateDebugDrawLines = true;
//extern btVector3 gLastPickPos;
btVector3 gVRTeleportPosLocal(0,0,0);
btQuaternion gVRTeleportOrnLocal(0,0,0,1);
extern btVector3 gVRTeleportPos1;
extern btQuaternion gVRTeleportOrn;
btScalar gVRTeleportRotZ = 0;
extern btVector3 gVRGripperPos;
extern btQuaternion gVRGripperOrn;
extern btVector3 gVRController2Pos;
extern btQuaternion gVRController2Orn;
extern btScalar gVRGripperAnalog;
extern btScalar gVRGripper2Analog;
extern bool gCloseToKuka;
extern bool gEnableRealTimeSimVR;
extern bool gCreateDefaultRobotAssets;
extern int gInternalSimFlags;
extern int gCreateObjectSimVR;
extern bool gResetSimulation;
extern int gEnableKukaControl;
int gGraspingController = -1;
extern btScalar simTimeScalingFactor;
bool gBatchUserDebugLines = true;
extern bool gVRGripperClosed;
const char* startFileNameVR = "0_VRDemoSettings.txt";
@ -82,70 +69,20 @@ static void loadCurrentSettingsVR(b3CommandLineArgs& args)
};
//remember the settings (you don't want to re-tune again and again...)
static void saveCurrentSettingsVR()
static void saveCurrentSettingsVR(const btVector3& VRTeleportPos1)
{
FILE* f = fopen(startFileNameVR, "w");
if (f)
{
fprintf(f, "--camPosX= %f\n", gVRTeleportPos1[0]);
fprintf(f, "--camPosY= %f\n", gVRTeleportPos1[1]);
fprintf(f, "--camPosZ= %f\n", gVRTeleportPos1[2]);
fprintf(f, "--camPosX= %f\n", VRTeleportPos1[0]);
fprintf(f, "--camPosY= %f\n", VRTeleportPos1[1]);
fprintf(f, "--camPosZ= %f\n", VRTeleportPos1[2]);
fprintf(f, "--camRotZ= %f\n", gVRTeleportRotZ);
fclose(f);
}
};
#if B3_USE_MIDI
static float getParamf(float rangeMin, float rangeMax, int midiVal)
{
float v = rangeMin + (rangeMax - rangeMin)* (float(midiVal / 127.));
return v;
}
void midiCallback(double deltatime, std::vector< unsigned char > *message, void *userData)
{
unsigned int nBytes = message->size();
for (unsigned int i = 0; i<nBytes; i++)
std::cout << "Byte " << i << " = " << (int)message->at(i) << ", ";
if (nBytes > 0)
std::cout << "stamp = " << deltatime << std::endl;
if (nBytes > 2)
{
if (message->at(0) == 176)
{
if (message->at(1) == 16)
{
gVRTeleportRotZ= getParamf(-3.1415, 3.1415, message->at(2));
gVRTeleportOrn = btQuaternion(btVector3(0, 0, 1), gVRTeleportRotZ);
saveCurrentSettingsVR();
// b3Printf("gVRTeleportOrnLocal rotZ = %f\n", gVRTeleportRotZ);
}
if (message->at(1) == 32)
{
gCreateDefaultRobotAssets = 1;
}
for (int i = 0; i < 3; i++)
{
if (message->at(1) == i)
{
gVRTeleportPos1[i] = getParamf(-2, 2, message->at(2));
saveCurrentSettingsVR();
// b3Printf("gVRTeleportPos[%d] = %f\n", i,gVRTeleportPosLocal[i]);
}
}
}
}
}
#endif //B3_USE_MIDI
bool gDebugRenderToggle = false;
void MotionThreadFunc(void* userPtr,void* lsMemory);
void* MotionlsMemoryFunc();
@ -291,7 +228,6 @@ struct MotionThreadLocalStorage
float clampedDeltaTime = 0.2;
extern int gMaxNumCmdPer1ms;
void MotionThreadFunc(void* userPtr,void* lsMemory)
@ -325,16 +261,7 @@ void MotionThreadFunc(void* userPtr,void* lsMemory)
{
if (gMaxNumCmdPer1ms>0)
{
if (numCmdSinceSleep1ms>gMaxNumCmdPer1ms)
{
BT_PROFILE("usleep(10)");
b3Clock::usleep(10);
numCmdSinceSleep1ms = 0;
sleepClock.reset();
}
}
if (sleepClock.getTimeMilliseconds()>1)
{
sleepClock.reset();
@ -1229,9 +1156,7 @@ class PhysicsServerExample : public SharedMemoryCommon
MotionArgs m_args[MAX_MOTION_NUM_THREADS];
MultiThreadedOpenGLGuiHelper* m_multiThreadedHelper;
bool m_wantsShutdown;
#ifdef B3_USE_MIDI
RtMidiIn* m_midi;
#endif
bool m_isConnected;
btClock m_clock;
bool m_replay;
@ -1243,7 +1168,7 @@ class PhysicsServerExample : public SharedMemoryCommon
public:
PhysicsServerExample(MultiThreadedOpenGLGuiHelper* helper, SharedMemoryInterface* sharedMem=0, int options=0);
PhysicsServerExample(MultiThreadedOpenGLGuiHelper* helper, CommandProcessorCreationInterface* commandProcessorCreator, SharedMemoryInterface* sharedMem=0, int options=0);
virtual ~PhysicsServerExample();
@ -1434,43 +1359,54 @@ public:
if (window->isModifierKeyPressed(B3G_SHIFT))
shift=0.01;
btVector3 VRTeleportPos =this->m_physicsServer.getVRTeleportPosition();
if (key=='w' && state)
{
gVRTeleportPos1[0]+=shift;
saveCurrentSettingsVR();
VRTeleportPos[0]+=shift;
m_physicsServer.setVRTeleportPosition(VRTeleportPos);
saveCurrentSettingsVR(VRTeleportPos);
}
if (key=='s' && state)
{
gVRTeleportPos1[0]-=shift;
saveCurrentSettingsVR();
VRTeleportPos[0]-=shift;
m_physicsServer.setVRTeleportPosition(VRTeleportPos);
saveCurrentSettingsVR(VRTeleportPos);
}
if (key=='a' && state)
{
gVRTeleportPos1[1]-=shift;
saveCurrentSettingsVR();
VRTeleportPos[1]-=shift;
m_physicsServer.setVRTeleportPosition(VRTeleportPos);
saveCurrentSettingsVR(VRTeleportPos);
}
if (key=='d' && state)
{
gVRTeleportPos1[1]+=shift;
saveCurrentSettingsVR();
VRTeleportPos[1]+=shift;
m_physicsServer.setVRTeleportPosition(VRTeleportPos);
saveCurrentSettingsVR(VRTeleportPos);
}
if (key=='q' && state)
{
gVRTeleportPos1[2]+=shift;
saveCurrentSettingsVR();
VRTeleportPos[2]+=shift;
m_physicsServer.setVRTeleportPosition(VRTeleportPos);
saveCurrentSettingsVR(VRTeleportPos);
}
if (key=='e' && state)
{
gVRTeleportPos1[2]-=shift;
saveCurrentSettingsVR();
VRTeleportPos[2]-=shift;
m_physicsServer.setVRTeleportPosition(VRTeleportPos);
saveCurrentSettingsVR(VRTeleportPos);
}
if (key=='z' && state)
{
gVRTeleportRotZ+=shift;
gVRTeleportOrn = btQuaternion(btVector3(0, 0, 1), gVRTeleportRotZ);
saveCurrentSettingsVR();
btQuaternion VRTeleportOrn = btQuaternion(btVector3(0, 0, 1), gVRTeleportRotZ);
m_physicsServer.setVRTeleportOrientation(VRTeleportOrn);
saveCurrentSettingsVR(VRTeleportPos);
}
return false;
}
@ -1490,33 +1426,33 @@ public:
setSharedMemoryKey(shmemKey);
}
if (args.GetCmdLineArgument("camPosX", gVRTeleportPos1[0]))
btVector3 vrTeleportPos = m_physicsServer.getVRTeleportPosition();
if (args.GetCmdLineArgument("camPosX", vrTeleportPos[0]))
{
printf("camPosX=%f\n", gVRTeleportPos1[0]);
printf("camPosX=%f\n", vrTeleportPos[0]);
}
if (args.GetCmdLineArgument("camPosY", gVRTeleportPos1[1]))
if (args.GetCmdLineArgument("camPosY", vrTeleportPos[1]))
{
printf("camPosY=%f\n", gVRTeleportPos1[1]);
printf("camPosY=%f\n", vrTeleportPos[1]);
}
if (args.GetCmdLineArgument("camPosZ", gVRTeleportPos1[2]))
if (args.GetCmdLineArgument("camPosZ", vrTeleportPos[2]))
{
printf("camPosZ=%f\n", gVRTeleportPos1[2]);
printf("camPosZ=%f\n", vrTeleportPos[2]);
}
m_physicsServer.setVRTeleportPosition(vrTeleportPos);
float camRotZ = 0.f;
if (args.GetCmdLineArgument("camRotZ", camRotZ))
{
printf("camRotZ = %f\n", camRotZ);
btQuaternion ornZ(btVector3(0, 0, 1), camRotZ);
gVRTeleportOrn = ornZ;
m_physicsServer.setVRTeleportOrientation(ornZ);
}
if (args.CheckCmdLineFlag("robotassets"))
{
gCreateDefaultRobotAssets = true;
}
if (args.CheckCmdLineFlag("realtimesimulation"))
{
@ -1524,53 +1460,17 @@ public:
m_physicsServer.enableRealTimeSimulation(true);
}
if (args.CheckCmdLineFlag("norobotassets"))
{
gCreateDefaultRobotAssets = false;
}
}
};
#ifdef B3_USE_MIDI
static bool chooseMidiPort(RtMidiIn *rtmidi)
{
/*
std::cout << "\nWould you like to open a virtual input port? [y/N] ";
std::string keyHit;
std::getline( std::cin, keyHit );
if ( keyHit == "y" ) {
rtmidi->openVirtualPort();
return true;
}
*/
std::string portName;
unsigned int i = 0, nPorts = rtmidi->getPortCount();
if (nPorts == 0) {
std::cout << "No midi input ports available!" << std::endl;
return false;
}
if (nPorts > 0) {
std::cout << "\nOpening midi input port " << rtmidi->getPortName() << std::endl;
}
// std::getline( std::cin, keyHit ); // used to clear out stdin
rtmidi->openPort(i);
return true;
}
#endif //B3_USE_MIDI
PhysicsServerExample::PhysicsServerExample(MultiThreadedOpenGLGuiHelper* helper, SharedMemoryInterface* sharedMem, int options)
PhysicsServerExample::PhysicsServerExample(MultiThreadedOpenGLGuiHelper* helper,CommandProcessorCreationInterface* commandProcessorCreator, SharedMemoryInterface* sharedMem, int options)
:SharedMemoryCommon(helper),
m_physicsServer(sharedMem),
m_physicsServer(commandProcessorCreator,sharedMem,0),
m_wantsShutdown(false),
m_isConnected(false),
m_replay(false)
@ -1579,14 +1479,7 @@ m_replay(false)
,m_tinyVrGui(0)
#endif
{
#ifdef B3_USE_MIDI
m_midi = new RtMidiIn();
chooseMidiPort(m_midi);
m_midi->setCallback(&midiCallback);
// Don't ignore sysex, timing, or active sensing messages.
m_midi->ignoreTypes(false, false, false);
#endif
m_multiThreadedHelper = helper;
// b3Printf("Started PhysicsServer\n");
}
@ -1595,10 +1488,7 @@ m_replay(false)
PhysicsServerExample::~PhysicsServerExample()
{
#ifdef B3_USE_MIDI
delete m_midi;
m_midi = 0;
#endif
#ifdef BT_ENABLE_VR
delete m_tinyVrGui;
#endif
@ -2007,7 +1897,6 @@ extern int gDroppedSimulationSteps;
extern int gNumSteps;
extern double gDtInSec;
extern double gSubStep;
extern int gVRTrackingObjectUniqueId;
extern btTransform gVRTrackingObjectTr;
@ -2206,28 +2095,15 @@ void PhysicsServerExample::drawUserDebugLines()
void PhysicsServerExample::renderScene()
{
btTransform vrTrans;
//gVRTeleportPos1 = gVRTeleportPosLocal;
//gVRTeleportOrn = gVRTeleportOrnLocal;
///little VR test to follow/drive Husky vehicle
if (gVRTrackingObjectUniqueId >= 0)
{
btTransform vrTrans;
vrTrans.setOrigin(gVRTeleportPosLocal);
vrTrans.setRotation(gVRTeleportOrnLocal);
vrTrans = vrTrans * gVRTrackingObjectTr;
gVRTeleportPos1 = vrTrans.getOrigin();
gVRTeleportOrn = vrTrans.getRotation();
}
B3_PROFILE("PhysicsServerExample::RenderScene");
drawUserDebugLines();
if (gEnableRealTimeSimVR)
if (m_physicsServer.isRealTimeSimulationEnabled())
{
static int frameCount=0;
@ -2285,8 +2161,10 @@ void PhysicsServerExample::renderScene()
{
b3Transform tr;tr.setIdentity();
tr.setOrigin(b3MakeVector3(gVRController2Pos[0],gVRController2Pos[1],gVRController2Pos[2]));
tr.setRotation(b3Quaternion(gVRController2Orn[0],gVRController2Orn[1],gVRController2Orn[2],gVRController2Orn[3]));
btVector3 VRController2Pos = m_physicsServer.getVRTeleportPosition();
btQuaternion VRController2Orn = m_physicsServer.getVRTeleportOrientation();
tr.setOrigin(b3MakeVector3(VRController2Pos[0],VRController2Pos[1],VRController2Pos[2]));
tr.setRotation(b3Quaternion(VRController2Orn[0],VRController2Orn[1],VRController2Orn[2],VRController2Orn[3]));
tr = tr*b3Transform(b3Quaternion(0,0,-SIMD_HALF_PI),b3MakeVector3(0,0,0));
b3Scalar dt = 0.01;
m_tinyVrGui->clearTextArea();
@ -2304,8 +2182,8 @@ void PhysicsServerExample::renderScene()
btTransform tr2a, tr2;
tr2a.setIdentity();
tr2.setIdentity();
tr2.setOrigin(gVRTeleportPos1);
tr2a.setRotation(gVRTeleportOrn);
tr2.setOrigin(m_physicsServer.getVRTeleportPosition());
tr2a.setRotation(m_physicsServer.getVRTeleportOrientation());
btTransform trTotal = tr2*tr2a;
btTransform trInv = trTotal.inverse();
@ -2368,9 +2246,8 @@ void PhysicsServerExample::renderScene()
if (m_guiHelper->getAppInterface()->m_renderer->getActiveCamera()->isVRCamera())
{
if (!gEnableRealTimeSimVR)
if (!m_physicsServer.isRealTimeSimulationEnabled())
{
gEnableRealTimeSimVR = true;
m_physicsServer.enableRealTimeSimulation(1);
}
}
@ -2470,35 +2347,6 @@ btVector3 PhysicsServerExample::getRayTo(int x,int y)
}
extern int gSharedMemoryKey;
class CommonExampleInterface* PhysicsServerCreateFunc(struct CommonExampleOptions& options)
{
MultiThreadedOpenGLGuiHelper* guiHelperWrapper = new MultiThreadedOpenGLGuiHelper(options.m_guiHelper->getAppInterface(),options.m_guiHelper);
PhysicsServerExample* example = new PhysicsServerExample(guiHelperWrapper,
options.m_sharedMem,
options.m_option);
if (gSharedMemoryKey>=0)
{
example->setSharedMemoryKey(gSharedMemoryKey);
}
if (options.m_option & PHYSICS_SERVER_ENABLE_COMMAND_LOGGING)
{
example->enableCommandLogging();
}
if (options.m_option & PHYSICS_SERVER_REPLAY_FROM_COMMAND_LOG)
{
example->replayFromLogFile();
}
return example;
}
void PhysicsServerExample::vrControllerButtonCallback(int controllerId, int button, int state, float pos[4], float orn[4])
@ -2511,7 +2359,6 @@ void PhysicsServerExample::vrControllerButtonCallback(int controllerId, int butt
if (gGraspingController < 0)
{
gGraspingController = controllerId;
gEnableKukaControl = true;
}
btTransform trLocal;
@ -2530,8 +2377,8 @@ void PhysicsServerExample::vrControllerButtonCallback(int controllerId, int butt
tr2.setOrigin(gVRTeleportPos1);
tr2a.setRotation(gVRTeleportOrn);
tr2.setOrigin(m_physicsServer.getVRTeleportPosition());
tr2a.setRotation(m_physicsServer.getVRTeleportOrientation());
btTransform trTotal = tr2*tr2a*trOrg*trLocal;
@ -2541,8 +2388,7 @@ void PhysicsServerExample::vrControllerButtonCallback(int controllerId, int butt
{
if (button == 1 && state == 0)
{
//gResetSimulation = true;
gVRTeleportPos1 = gLastPickPos;
}
} else
{
@ -2583,7 +2429,6 @@ void PhysicsServerExample::vrControllerButtonCallback(int controllerId, int butt
if (controllerId == gGraspingController)
{
gCreateObjectSimVR = 1;
}
else
{
@ -2599,7 +2444,7 @@ void PhysicsServerExample::vrControllerButtonCallback(int controllerId, int butt
if (controllerId == gGraspingController && (button == 33))
{
gVRGripperClosed =(state!=0);
}
else
{
@ -2669,25 +2514,18 @@ void PhysicsServerExample::vrControllerMoveCallback(int controllerId, float pos[
tr2.setOrigin(gVRTeleportPos1);
tr2a.setRotation(gVRTeleportOrn);
tr2.setOrigin(m_physicsServer.getVRTeleportPosition());
tr2a.setRotation(m_physicsServer.getVRTeleportOrientation());
btTransform trTotal = tr2*tr2a*trOrg*trLocal;
if (controllerId == gGraspingController)
{
gVRGripperAnalog = analogAxis;
gVRGripperPos = trTotal.getOrigin();
gVRGripperOrn = trTotal.getRotation();
}
else
{
gVRGripper2Analog = analogAxis;
gVRController2Pos = trTotal.getOrigin();
gVRController2Orn = trTotal.getRotation();
m_args[0].m_vrControllerPos[controllerId] = trTotal.getOrigin();
m_args[0].m_vrControllerOrn[controllerId] = trTotal.getRotation();
}
@ -2730,8 +2568,8 @@ void PhysicsServerExample::vrHMDMoveCallback(int controllerId, float pos[4], flo
tr2a.setIdentity();
btTransform tr2;
tr2.setIdentity();
tr2.setOrigin(gVRTeleportPos1);
tr2a.setRotation(gVRTeleportOrn);
tr2.setOrigin(m_physicsServer.getVRTeleportPosition());
tr2a.setRotation(m_physicsServer.getVRTeleportOrientation());
btTransform trTotal = tr2*tr2a*trOrg*trLocal;
@ -2772,8 +2610,8 @@ void PhysicsServerExample::vrGenericTrackerMoveCallback(int controllerId, float
tr2a.setIdentity();
btTransform tr2;
tr2.setIdentity();
tr2.setOrigin(gVRTeleportPos1);
tr2a.setRotation(gVRTeleportOrn);
tr2.setOrigin(m_physicsServer.getVRTeleportPosition());
tr2a.setRotation(m_physicsServer.getVRTeleportOrientation());
btTransform trTotal = tr2*tr2a*trOrg*trLocal;
m_args[0].m_csGUI->lock();
@ -2790,4 +2628,36 @@ void PhysicsServerExample::vrGenericTrackerMoveCallback(int controllerId, float
m_args[0].m_csGUI->unlock();
}
B3_STANDALONE_EXAMPLE(PhysicsServerCreateFunc)
extern int gSharedMemoryKey;
class CommonExampleInterface* PhysicsServerCreateFuncInternal(struct CommonExampleOptions& options)
{
MultiThreadedOpenGLGuiHelper* guiHelperWrapper = new MultiThreadedOpenGLGuiHelper(options.m_guiHelper->getAppInterface(),options.m_guiHelper);
PhysicsServerExample* example = new PhysicsServerExample(guiHelperWrapper,
options.m_commandProcessorCreation,
options.m_sharedMem,
options.m_option);
if (gSharedMemoryKey>=0)
{
example->setSharedMemoryKey(gSharedMemoryKey);
}
if (options.m_option & PHYSICS_SERVER_ENABLE_COMMAND_LOGGING)
{
example->enableCommandLogging();
}
if (options.m_option & PHYSICS_SERVER_REPLAY_FROM_COMMAND_LOG)
{
example->replayFromLogFile();
}
return example;
}

4
examples/SharedMemory/PhysicsServerExample.h
View File

@ -8,7 +8,9 @@ enum PhysicsServerOptions
PHYSICS_SERVER_USE_RTC_CLOCK = 4,
};
class CommonExampleInterface* PhysicsServerCreateFunc(struct CommonExampleOptions& options);
///Don't use PhysicsServerCreateFuncInternal directly
///Use PhysicsServerCreateFuncBullet2 instead, or initialize options.m_commandProcessor
class CommonExampleInterface* PhysicsServerCreateFuncInternal(struct CommonExampleOptions& options);
#endif //PHYSICS_SERVER_EXAMPLE_H

35
examples/SharedMemory/PhysicsServerExampleBullet2.cpp Normal file
View File

@ -0,0 +1,35 @@
#include "PhysicsServerExampleBullet2.h"
#include "PhysicsServerExample.h"
#include "PhysicsServerCommandProcessor.h"
#include "../CommonInterfaces/CommonExampleInterface.h"
struct Bullet2CommandProcessorCreation : public CommandProcessorCreationInterface
{
virtual class CommandProcessorInterface* createCommandProcessor()
{
PhysicsServerCommandProcessor* proc = new PhysicsServerCommandProcessor;
return proc;
}
virtual void deleteCommandProcessor(CommandProcessorInterface* proc)
{
delete proc;
}
};
static Bullet2CommandProcessorCreation sBullet2CommandCreator;
CommonExampleInterface* PhysicsServerCreateFuncBullet2(struct CommonExampleOptions& options)
{
options.m_commandProcessorCreation = &sBullet2CommandCreator;
CommonExampleInterface* example = PhysicsServerCreateFuncInternal(options);
return example;
}
B3_STANDALONE_EXAMPLE(PhysicsServerCreateFuncBullet2)

9
examples/SharedMemory/PhysicsServerExampleBullet2.h Normal file
View File

@ -0,0 +1,9 @@
#ifndef PHYSICS_SERVER_EXAMPLE_BULLET_2_H
#define PHYSICS_SERVER_EXAMPLE_BULLET_2_H
class CommonExampleInterface* PhysicsServerCreateFuncBullet2(struct CommonExampleOptions& options);
#endif //PHYSICS_SERVER_EXAMPLE_BULLET_2_H

42
examples/SharedMemory/PhysicsServerSharedMemory.cpp
View File

@ -3,7 +3,7 @@
#include "Win32SharedMemory.h"
#include "../CommonInterfaces/CommonRenderInterface.h"
#include "../CommonInterfaces/CommonExampleInterface.h"
#include "btBulletDynamicsCommon.h"
#include "LinearMath/btTransform.h"
@ -31,7 +31,8 @@ struct PhysicsServerSharedMemoryInternalData
bool m_areConnected[MAX_SHARED_MEMORY_BLOCKS];
bool m_verboseOutput;
CommandProcessorInterface* m_commandProcessor;
CommandProcessorCreationInterface* m_commandProcessorCreator;
PhysicsServerSharedMemoryInternalData()
:m_sharedMemory(0),
m_ownsSharedMemory(false),
@ -64,9 +65,10 @@ struct PhysicsServerSharedMemoryInternalData
};
PhysicsServerSharedMemory::PhysicsServerSharedMemory(SharedMemoryInterface* sharedMem)
PhysicsServerSharedMemory::PhysicsServerSharedMemory(CommandProcessorCreationInterface* commandProcessorCreator, SharedMemoryInterface* sharedMem, int bla)
{
m_data = new PhysicsServerSharedMemoryInternalData();
m_data->m_commandProcessorCreator = commandProcessorCreator;
if (sharedMem)
{
m_data->m_sharedMemory = sharedMem;
@ -81,17 +83,13 @@ PhysicsServerSharedMemory::PhysicsServerSharedMemory(SharedMemoryInterface* shar
m_data->m_ownsSharedMemory = true;
}
m_data->m_commandProcessor = new PhysicsServerCommandProcessor;
m_data->m_commandProcessor = commandProcessorCreator->createCommandProcessor();
}
PhysicsServerSharedMemory::~PhysicsServerSharedMemory()
{
//m_data->m_commandProcessor->deleteDynamicsWorld();
delete m_data->m_commandProcessor;
if (m_data->m_sharedMemory)
{
if (m_data->m_verboseOutput)
@ -105,7 +103,7 @@ PhysicsServerSharedMemory::~PhysicsServerSharedMemory()
m_data->m_sharedMemory = 0;
}
m_data->m_commandProcessorCreator->deleteCommandProcessor(m_data->m_commandProcessor);
delete m_data;
}
@ -238,6 +236,11 @@ void PhysicsServerSharedMemory::enableRealTimeSimulation(bool enableRealTimeSim)
m_data->m_commandProcessor->enableRealTimeSimulation(enableRealTimeSim);
}
bool PhysicsServerSharedMemory::isRealTimeSimulationEnabled() const
{
return m_data->m_commandProcessor->isRealTimeSimulationEnabled();
}
void PhysicsServerSharedMemory::processClientCommands()
@ -315,3 +318,24 @@ void PhysicsServerSharedMemory::replayFromLogFile(const char* fileName)
{
m_data->m_commandProcessor->replayFromLogFile(fileName);
}
const btVector3& PhysicsServerSharedMemory::getVRTeleportPosition() const
{
return m_data->m_commandProcessor->getVRTeleportPosition();
}
void PhysicsServerSharedMemory::setVRTeleportPosition(const btVector3& vrTeleportPos)
{
m_data->m_commandProcessor->setVRTeleportPosition(vrTeleportPos);
}
const btQuaternion& PhysicsServerSharedMemory::getVRTeleportOrientation() const
{
return m_data->m_commandProcessor->getVRTeleportOrientation();
}
void PhysicsServerSharedMemory::setVRTeleportOrientation(const btQuaternion& vrTeleportOrn)
{
m_data->m_commandProcessor->setVRTeleportOrientation(vrTeleportOrn);
}

14
examples/SharedMemory/PhysicsServerSharedMemory.h
View File

@ -2,7 +2,8 @@
#define PHYSICS_SERVER_SHARED_MEMORY_H
#include "PhysicsServer.h"
#include "LinearMath/btQuaternion.h"
class PhysicsServerSharedMemory : public PhysicsServer
{
struct PhysicsServerSharedMemoryInternalData* m_data;
@ -14,7 +15,7 @@ protected:
public:
PhysicsServerSharedMemory(class SharedMemoryInterface* sharedMem=0);
PhysicsServerSharedMemory(struct CommandProcessorCreationInterface* commandProcessorCreator, class SharedMemoryInterface* sharedMem, int bla);
virtual ~PhysicsServerSharedMemory();
virtual void setSharedMemoryKey(int key);
@ -29,6 +30,7 @@ public:
virtual void stepSimulationRealTime(double dtInSec,const struct b3VRControllerEvent* vrEvents, int numVREvents, const struct b3KeyboardEvent* keyEvents, int numKeyEvents);
virtual void enableRealTimeSimulation(bool enableRealTimeSim);
virtual bool isRealTimeSimulationEnabled() const;
//bool supportsJointMotor(class btMultiBody* body, int linkIndex);
@ -38,6 +40,14 @@ public:
virtual bool movePickedBody(const btVector3& rayFromWorld, const btVector3& rayToWorld);
virtual void removePickingConstraint();
virtual const btVector3& getVRTeleportPosition() const;
virtual void setVRTeleportPosition(const btVector3& vrReleportPos);
virtual const btQuaternion& getVRTeleportOrientation() const;
virtual void setVRTeleportOrientation(const btQuaternion& vrReleportOrn);
//for physicsDebugDraw and renderScene are mainly for debugging purposes
//and for physics visualization. The idea is that physicsDebugDraw can also send wireframe
//to a physics client, over shared memory

6
examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp
View File

@ -4,7 +4,9 @@
#include "PhysicsClientSharedMemory.h"
#include"../ExampleBrowser/InProcessExampleBrowser.h"
#include "PhysicsServerExample.h"
#include "PhysicsServerExampleBullet2.h"
#include "../CommonInterfaces/CommonExampleInterface.h"
#include "InProcessMemory.h"
@ -142,7 +144,7 @@ public:
CommonExampleOptions options(guiHelper);
options.m_sharedMem = m_sharedMem;
m_physicsServerExample = PhysicsServerCreateFunc(options);
m_physicsServerExample = PhysicsServerCreateFuncBullet2(options);
m_physicsServerExample ->initPhysics();
m_physicsServerExample ->resetCamera();
setSharedMemoryInterface(m_sharedMem);

5
examples/SharedMemory/main.cpp
View File

@ -14,7 +14,8 @@ subject to the following restrictions:
*/
#include "PhysicsServerExample.h"
#include "PhysicsServerExampleBullet2.h"
#include "Bullet3Common/b3CommandLineArgs.h"
@ -77,7 +78,7 @@ int main(int argc, char* argv[])
// options.m_option |= PHYSICS_SERVER_ENABLE_COMMAND_LOGGING;
// options.m_option |= PHYSICS_SERVER_REPLAY_FROM_COMMAND_LOG;
example = (SharedMemoryCommon*)PhysicsServerCreateFunc(options);
example = (SharedMemoryCommon*)PhysicsServerCreateFuncBullet2(options);
example->initPhysics();

1
examples/SharedMemory/premake4.lua
View File

@ -23,6 +23,7 @@ myfiles =
"PhysicsClientSharedMemory.cpp",
"PhysicsClientExample.cpp",
"PhysicsServerExample.cpp",
"PhysicsServerExampleBullet2.cpp",
"PhysicsServerSharedMemory.cpp",
"PhysicsServerSharedMemory.h",
"PhysicsServer.cpp",

1
examples/pybullet/CMakeLists.txt
View File

@ -33,6 +33,7 @@ SET(pybullet_SRCS
../../examples/SharedMemory/PhysicsServer.cpp
../../examples/SharedMemory/PhysicsServer.h
../../examples/SharedMemory/PhysicsServerExample.cpp
../../examples/SharedMemory/PhysicsServerExampleBullet2.cpp
../../examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp
../../examples/SharedMemory/PhysicsServerSharedMemory.cpp
../../examples/SharedMemory/PhysicsServerSharedMemory.h

1
examples/pybullet/premake4.lua
View File

@ -106,6 +106,7 @@ if not _OPTIONS["no-enet"] then
"../../examples/SharedMemory/PhysicsServer.cpp",
"../../examples/SharedMemory/PhysicsServer.h",
"../../examples/SharedMemory/PhysicsServerExample.cpp",
"../../examples/SharedMemory/PhysicsServerExampleBullet2.cpp",
"../../examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp",
"../../examples/SharedMemory/PhysicsServerSharedMemory.cpp",
"../../examples/SharedMemory/PhysicsServerSharedMemory.h",

3
setup.py
View File

@ -54,6 +54,7 @@ sources = ["examples/pybullet/pybullet.c"]\
+["examples/SharedMemory/PhysicsClient.cpp"]\
+["examples/SharedMemory/PhysicsServer.cpp"]\
+["examples/SharedMemory/PhysicsServerExample.cpp"]\
+["examples/SharedMemory/PhysicsServerExampleBullet2.cpp"]\
+["examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp"]\
+["examples/SharedMemory/PhysicsServerSharedMemory.cpp"]\
+["examples/SharedMemory/PhysicsDirect.cpp"]\
@ -417,7 +418,7 @@ else:
setup(
name = 'pybullet',
version='1.0.8',
version='1.0.9',
description='Official Python Interface for the Bullet Physics SDK Robotics Simulator',
long_description='pybullet is an easy to use Python module for physics simulation, robotics and machine learning based on the Bullet Physics SDK. With pybullet you can load articulated bodies from URDF, SDF and other file formats. pybullet provides forward dynamics simulation, inverse dynamics computation, forward and inverse kinematics and collision detection and ray intersection queries. Aside from physics simulation, pybullet supports to rendering, with a CPU renderer and OpenGL visualization and support for virtual reality headsets.',
url='https://github.com/bulletphysics/bullet3',

1
test/SharedMemory/premake4.lua
View File

@ -360,6 +360,7 @@ project ("Test_PhysicsServerInProcessExampleBrowser")
"../../examples/SharedMemory/PhysicsServer.cpp",
"../../examples/SharedMemory/PhysicsServer.h",
"../../examples/SharedMemory/PhysicsServerExample.cpp",
"../../examples/SharedMemory/PhysicsServerExampleBullet2.cpp",
"../../examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp",
"../../examples/SharedMemory/PhysicsServerSharedMemory.cpp",
"../../examples/SharedMemory/PhysicsServerSharedMemory.h",