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

Merge pull request #886 from erwincoumans/master

Browse Source 
some VR tweaks and bugfix for issue #878
This commit is contained in:
erwincoumans
2016-12-11 12:53:47 -08:00
committed by GitHub
parent e8f38df121 436fdd5dc2
commit 0c26fee04d
14 changed files with 178 additions and 91 deletions
Download Patch File Download Diff File Expand all files Collapse all files

53
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -48,7 +48,7 @@ int gHuskyId = -1;
btTransform huskyTr = btTransform::getIdentity();
int gCreateObjectSimVR = -1;
int gEnableKukaControl = 1;
int gEnableKukaControl = 0;
btScalar simTimeScalingFactor = 1;
btScalar gRhsClamp = 1.f;
@@ -667,8 +667,6 @@ PhysicsServerCommandProcessor::PhysicsServerCommandProcessor()
m_data = new PhysicsServerCommandProcessorInternalData();
createEmptyDynamicsWorld();
m_data->m_dynamicsWorld->getSolverInfo().m_linearSlop = 0.00001;
m_data->m_dynamicsWorld->getSolverInfo().m_numIterations = 100;
}
@@ -717,6 +715,9 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld()
m_data->m_dynamicsWorld->setGravity(btVector3(0, 0, 0));
m_data->m_dynamicsWorld->getSolverInfo().m_erp2 = 0.08;
m_data->m_dynamicsWorld->getSolverInfo().m_linearSlop = 0.00001;
m_data->m_dynamicsWorld->getSolverInfo().m_numIterations = 50;
}
void PhysicsServerCommandProcessor::deleteCachedInverseKinematicsBodies()
@@ -4030,7 +4031,7 @@ void PhysicsServerCommandProcessor::replayFromLogFile(const char* fileName)
}
btVector3 gVRGripperPos(0,0,0.2);
btVector3 gVRGripperPos(0.6, 0.4, 0.7);
btQuaternion gVRGripperOrn(0,0,0,1);
btVector3 gVRController2Pos(0,0,0.2);
btQuaternion gVRController2Orn(0,0,0,1);
@@ -4044,6 +4045,12 @@ int gDroppedSimulationSteps = 0;
int gNumSteps = 0;
double gDtInSec = 0.f;
double gSubStep = 0.f;
void PhysicsServerCommandProcessor::enableRealTimeSimulation(bool enableRealTimeSim)
{
m_data->m_allowRealTimeSimulation = enableRealTimeSim;
}
void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec)
{
if (gResetSimulation)
@@ -4052,7 +4059,7 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec)
gResetSimulation = false;
}
if ((gEnableRealTimeSimVR || m_data->m_allowRealTimeSimulation) && m_data->m_guiHelper)
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...
@@ -4170,7 +4177,7 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets()
{
int bodyId = 0;
if (loadUrdf("pr2_gripper.urdf", btVector3(0, 0, 0.1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()))
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->getHandle(bodyId);
if (parentBody->m_multiBody)
@@ -4200,6 +4207,30 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets()
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;
InteralBodyData* kukaBody = m_data->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);
}
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());
@@ -4208,7 +4239,7 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets()
// Load one motor gripper for kuka
loadSdf("gripper/wsg50_one_motor_gripper_new_free_base.sdf", &gBufferServerToClient[0], gBufferServerToClient.size(), true);
m_data->m_gripperId = bodyId + 1;
InteralBodyData* kukaBody = m_data->getHandle(m_data->m_KukaId);
InteralBodyData* gripperBody = m_data->getHandle(m_data->m_gripperId);
// Reset the default gripper motor maximum torque for damping to 0
@@ -4250,6 +4281,7 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets()
m_data->m_dynamicsWorld->addMultiBodyConstraint(m_data->m_kukaGripperRevolute1);
m_data->m_dynamicsWorld->addMultiBodyConstraint(m_data->m_kukaGripperRevolute2);
kukaBody = m_data->getHandle(m_data->m_KukaId);
if (kukaBody->m_multiBody && kukaBody->m_multiBody->getNumDofs()==7)
{
gripperBody->m_multiBody->setHasSelfCollision(0);
@@ -4303,7 +4335,7 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets()
// 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("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());
@@ -4344,9 +4376,10 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets()
if (motor)
{
btScalar posTarget = (-0.048)*btMin(btScalar(0.75), gVRGripper2Analog) / 0.75;
motor->setPositionTarget(posTarget, .2);
motor->setPositionTarget(posTarget, .8);
motor->setVelocityTarget(0.0, .5);
motor->setMaxAppliedImpulse(5.0);
motor->setMaxAppliedImpulse(1.0);
}
}

1
examples/SharedMemory/PhysicsServerCommandProcessor.h
View File

@@ -85,6 +85,7 @@ public:
void replayFromLogFile(const char* fileName);
void replayLogCommand(char* bufferServerToClient, int bufferSizeInBytes );
void stepSimulationRealTime(double dtInSec);
void enableRealTimeSimulation(bool enableRealTimeSim);
void applyJointDamping(int bodyUniqueId);
};

62
examples/SharedMemory/PhysicsServerExample.cpp
View File

@@ -27,6 +27,7 @@
//@todo(erwincoumans) those globals are hacks for a VR demo, move this to Python/pybullet!
extern btVector3 gLastPickPos;
btVector3 gVRTeleportPos1(0,0,0);
btScalar gVRTeleportRotZ = 0;
btQuaternion gVRTeleportOrn(0, 0, 0,1);
extern btVector3 gVRGripperPos;
extern btQuaternion gVRGripperOrn;
@@ -46,10 +47,47 @@ extern btScalar simTimeScalingFactor;
extern bool gVRGripperClosed;
#if B3_USE_MIDI
const char* startFileNameVR = "0_VRDemoSettings.txt";
#include <vector>
//remember the settings (you don't want to re-tune again and again...)
static void saveCurrentSettingsVR()
{
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, "--camRotZ= %f\n", gVRTeleportRotZ);
fclose(f);
}
};
static void loadCurrentSettingsVR(b3CommandLineArgs& args)
{
int currentEntry = 0;
FILE* f = fopen(startFileNameVR, "r");
if (f)
{
char oneline[1024];
char* argv[] = { 0,&oneline[0] };
while (fgets(oneline, 1024, f) != NULL)
{
char *pos;
if ((pos = strchr(oneline, '\n')) != NULL)
*pos = '\0';
args.addArgs(2, argv);
}
fclose(f);
}
};
#if B3_USE_MIDI
static float getParamf(float rangeMin, float rangeMax, int midiVal)
{
float v = rangeMin + (rangeMax - rangeMin)* (float(midiVal / 127.));
@@ -70,9 +108,10 @@ void midiCallback(double deltatime, std::vector< unsigned char > *message, void
{
if (message->at(1) == 16)
{
float rotZ = getParamf(-3.1415, 3.1415, message->at(2));
gVRTeleportOrn = btQuaternion(btVector3(0, 0, 1), rotZ);
b3Printf("gVRTeleportOrn rotZ = %f\n", rotZ);
gVRTeleportRotZ= getParamf(-3.1415, 3.1415, message->at(2));
gVRTeleportOrn = btQuaternion(btVector3(0, 0, 1), gVRTeleportRotZ);
saveCurrentSettingsVR();
b3Printf("gVRTeleportOrn rotZ = %f\n", gVRTeleportRotZ);
}
if (message->at(1) == 32)
@@ -85,6 +124,7 @@ void midiCallback(double deltatime, std::vector< unsigned char > *message, void
if (message->at(1) == i)
{
gVRTeleportPos1[i] = getParamf(-2, 2, message->at(2));
saveCurrentSettingsVR();
b3Printf("gVRTeleportPos[%d] = %f\n", i,gVRTeleportPos1[i]);
}
@@ -927,7 +967,7 @@ public:
virtual void processCommandLineArgs(int argc, char* argv[])
{
b3CommandLineArgs args(argc,argv);
loadCurrentSettingsVR(args);
if (args.GetCmdLineArgument("camPosX", gVRTeleportPos1[0]))
{
printf("camPosX=%f\n", gVRTeleportPos1[0]);
@@ -1544,7 +1584,11 @@ void PhysicsServerExample::renderScene()
if (m_guiHelper->getAppInterface()->m_renderer->getActiveCamera()->isVRCamera())
{
gEnableRealTimeSimVR = true;
if (!gEnableRealTimeSimVR)
{
gEnableRealTimeSimVR = true;
m_physicsServer.enableRealTimeSimulation(1);
}
}
@@ -1665,8 +1709,10 @@ void PhysicsServerExample::vrControllerButtonCallback(int controllerId, int butt
return;
if (gGraspingController < 0)
{
gGraspingController = controllerId;
gEnableKukaControl = true;
}
if (controllerId != gGraspingController)
{
if (button == 1 && state == 0)
@@ -1777,8 +1823,6 @@ void PhysicsServerExample::vrControllerButtonCallback(int controllerId, int butt
void PhysicsServerExample::vrControllerMoveCallback(int controllerId, float pos[4], float orn[4], float analogAxis)
{
gEnableRealTimeSimVR = true;
if (controllerId <= 0 || controllerId >= MAX_VR_CONTROLLERS)
{
printf("Controller Id exceeds max: %d > %d", controllerId, MAX_VR_CONTROLLERS);

6
examples/SharedMemory/PhysicsServerSharedMemory.cpp
View File

@@ -241,6 +241,12 @@ void PhysicsServerSharedMemory::stepSimulationRealTime(double dtInSec)
m_data->m_commandProcessor->stepSimulationRealTime(dtInSec);
}
void PhysicsServerSharedMemory::enableRealTimeSimulation(bool enableRealTimeSim)
{
m_data->m_commandProcessor->enableRealTimeSimulation(enableRealTimeSim);
}
void PhysicsServerSharedMemory::processClientCommands()
{

2
examples/SharedMemory/PhysicsServerSharedMemory.h
View File

@@ -28,6 +28,8 @@ public:
virtual void stepSimulationRealTime(double dtInSec);
virtual void enableRealTimeSimulation(bool enableRealTimeSim);
//bool supportsJointMotor(class btMultiBody* body, int linkIndex);