diff --git a/data/gripper/wsg50_one_motor_gripper_free_base.sdf b/data/gripper/wsg50_one_motor_gripper_free_base.sdf
index 7fc3ed2b2..552c36d28 100644
--- a/data/gripper/wsg50_one_motor_gripper_free_base.sdf
+++ b/data/gripper/wsg50_one_motor_gripper_free_base.sdf
@@ -303,6 +303,10 @@
+
+
+
+
0.042 0 0.145 0 0 1.5708
0.2
@@ -343,6 +347,10 @@
+
+
+
+
-0.042 0 0.145 0 0 4.71239
0.2
diff --git a/data/gripper/wsg50_one_motor_gripper_new_free_base.sdf b/data/gripper/wsg50_one_motor_gripper_new_free_base.sdf
index 68e2a0a6e..0358f7a6a 100644
--- a/data/gripper/wsg50_one_motor_gripper_new_free_base.sdf
+++ b/data/gripper/wsg50_one_motor_gripper_new_free_base.sdf
@@ -300,6 +300,10 @@
+
+ 1.0
+ 1.5
+
0.062 0 0.145 0 0 1.5708
0.2
@@ -340,6 +344,10 @@
+
+ 1.0
+ 1.5
+
-0.062 0 0.145 0 0 4.71239
0.2
diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
index ed8d95856..2b62a92c7 100644
--- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
+++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
@@ -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;
@@ -4016,7 +4016,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);
@@ -4156,7 +4156,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)
@@ -4186,6 +4186,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 scratch_q;
+ btAlignedObjectArray 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());
@@ -4194,7 +4218,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
@@ -4236,6 +4260,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);
@@ -4289,7 +4314,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());
@@ -4330,9 +4355,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);
+
}
}
diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp
index e5b4cc3e4..0a909e938 100644
--- a/examples/SharedMemory/PhysicsServerExample.cpp
+++ b/examples/SharedMemory/PhysicsServerExample.cpp
@@ -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
+//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]);
@@ -1665,8 +1705,10 @@ void PhysicsServerExample::vrControllerButtonCallback(int controllerId, int butt
return;
if (gGraspingController < 0)
+ {
gGraspingController = controllerId;
-
+ gEnableKukaControl = true;
+ }
if (controllerId != gGraspingController)
{
if (button == 1 && state == 0)