Prepare/allow for non-Bullet2-based physics command processor in pybullet/Bullet-C-API
!!! 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
@@ -7,6 +7,7 @@ SET(SharedMemory_SRCS
|
||||
PhysicsClientSharedMemory.cpp
|
||||
PhysicsClientExample.cpp
|
||||
PhysicsServerExample.cpp
|
||||
PhysicsServerExampleBullet2.cpp
|
||||
PhysicsServerSharedMemory.cpp
|
||||
PhysicsServerSharedMemory.h
|
||||
PhysicsServer.cpp
|
||||
|
||||
@@ -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");
|
||||
}
|
||||
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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;
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -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
35
examples/SharedMemory/PhysicsServerExampleBullet2.cpp
Normal 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
9
examples/SharedMemory/PhysicsServerExampleBullet2.h
Normal 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
|
||||
@@ -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);
|
||||
}
|
||||
|
||||
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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);
|
||||
|
||||
@@ -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();
|
||||
|
||||
@@ -23,6 +23,7 @@ myfiles =
|
||||
"PhysicsClientSharedMemory.cpp",
|
||||
"PhysicsClientExample.cpp",
|
||||
"PhysicsServerExample.cpp",
|
||||
"PhysicsServerExampleBullet2.cpp",
|
||||
"PhysicsServerSharedMemory.cpp",
|
||||
"PhysicsServerSharedMemory.h",
|
||||
"PhysicsServer.cpp",
|
||||
|
||||
Reference in New Issue
Block a user