diff --git a/data/tray/tray.jpg b/data/tray/tray.jpg
new file mode 100644
index 000000000..2ce198fb6
Binary files /dev/null and b/data/tray/tray.jpg differ
diff --git a/data/tray/tray_textured2.mtl b/data/tray/tray_textured2.mtl
new file mode 100644
index 000000000..4689a463a
--- /dev/null
+++ b/data/tray/tray_textured2.mtl
@@ -0,0 +1,13 @@
+# Blender MTL File: 'tray_textured2.blend'
+# Material Count: 1
+
+newmtl None
+Ns 0.000000
+Ka 0.000000 0.000000 0.000000
+Kd 0.800000 0.800000 0.800000
+Ks 0.800000 0.800000 0.800000
+Ke 0.000000 0.000000 0.000000
+Ni 1.000000
+d 1.000000
+illum 2
+map_Kd tray.jpg
diff --git a/data/tray/tray_textured2.obj b/data/tray/tray_textured2.obj
new file mode 100644
index 000000000..98a8962c6
--- /dev/null
+++ b/data/tray/tray_textured2.obj
@@ -0,0 +1,255 @@
+# Blender v2.78 (sub 0) OBJ File: 'tray_textured2.blend'
+# www.blender.org
+mtllib tray_textured2.mtl
+o edge_1_Cube.003
+v 0.580000 0.590083 0.250354
+v -0.419960 0.426691 -0.001860
+v -0.580000 0.590083 0.250354
+v 0.580000 0.573309 0.261247
+v 0.420014 0.426691 -0.001059
+v -0.580000 0.573309 0.261247
+v 0.420014 0.409917 0.009834
+v -0.419960 0.409917 0.009033
+vt 0.8346 0.9187
+vt 0.2203 0.8574
+vt 0.1480 0.9187
+vt 0.8346 0.9129
+vt 0.7623 0.8574
+vt 0.1480 0.9129
+vt 0.7623 0.8511
+vt 0.2203 0.8511
+vn 0.0004 0.8386 -0.5448
+vn 0.0001 0.8391 -0.5439
+vn -0.0000 0.8393 -0.5437
+vn 0.8823 -0.2564 -0.3948
+vn -0.0004 -0.8392 0.5439
+vn -0.0001 -0.8386 0.5447
+vn 0.0000 -0.8385 0.5449
+vn -0.8826 -0.2560 -0.3942
+vn 0.0008 -0.5446 -0.8387
+vn -0.0000 0.5446 0.8387
+vn 0.0005 0.8383 -0.5452
+vn -0.0005 -0.8394 0.5435
+usemtl None
+s 1
+f 1/1/1 2/2/2 3/3/3
+f 4/4/4 5/5/4 1/1/4
+f 6/6/5 7/7/6 4/4/7
+f 3/3/8 8/8/8 6/6/8
+f 5/5/9 8/8/9 2/2/9
+f 4/4/10 3/3/10 6/6/10
+f 1/1/1 5/5/11 2/2/2
+f 4/4/4 7/7/4 5/5/4
+f 6/6/5 8/8/12 7/7/6
+f 3/3/8 2/2/8 8/8/8
+f 5/5/9 7/7/9 8/8/9
+f 4/4/10 1/1/10 3/3/10
+o edge_2_Cube
+v 0.590083 0.580000 0.250354
+v 0.409917 0.420060 0.009390
+v 0.573309 0.580000 0.261247
+v 0.590083 -0.580000 0.250354
+v 0.426691 0.420060 -0.001503
+v 0.573309 -0.580000 0.261247
+v 0.426691 -0.419158 -0.002053
+v 0.409917 -0.419158 0.008840
+vt 0.9410 0.8520
+vt 0.7523 0.8566
+vt 0.9234 0.8524
+vt 0.8896 0.1426
+vt 0.7698 0.8562
+vt 0.8721 0.1430
+vt 0.7185 0.1468
+vt 0.7009 0.1472
+vn -0.2561 0.8826 -0.3943
+vn 0.8394 0.0003 -0.5435
+vn 0.8390 0.0001 -0.5441
+vn 0.8389 0.0000 -0.5442
+vn -0.2569 -0.8818 -0.3956
+vn -0.8390 -0.0003 0.5441
+vn -0.8394 -0.0001 0.5436
+vn -0.8395 -0.0000 0.5434
+vn -0.5446 0.0005 -0.8387
+vn 0.5446 -0.0000 0.8387
+vn 0.8396 0.0004 -0.5433
+vn -0.8388 -0.0004 0.5444
+usemtl None
+s 1
+f 9/9/13 10/10/13 11/11/13
+f 12/12/14 13/13/15 9/9/16
+f 14/14/17 15/15/17 12/12/17
+f 11/11/18 16/16/19 14/14/20
+f 13/13/21 16/16/21 10/10/21
+f 12/12/22 11/11/22 14/14/22
+f 9/9/13 13/13/13 10/10/13
+f 12/12/14 15/15/23 13/13/15
+f 14/14/17 16/16/17 15/15/17
+f 11/11/18 10/10/24 16/16/19
+f 13/13/21 15/15/21 16/16/21
+f 12/12/22 9/9/22 11/11/22
+o edge_3_Cube.002
+v 0.580000 -0.573309 0.261247
+v -0.419400 -0.409917 0.008678
+v -0.580000 -0.573309 0.261247
+v 0.580000 -0.590083 0.250354
+v 0.419883 -0.409917 0.009162
+v -0.580000 -0.590083 0.250354
+v 0.419883 -0.426691 -0.001731
+v -0.419400 -0.426691 -0.002215
+vt 0.8690 0.1040
+vt 0.1365 0.1739
+vt 0.0188 0.1040
+vt 0.8690 0.0968
+vt 0.7517 0.1739
+vt 0.0188 0.0968
+vt 0.7517 0.1668
+vt 0.1365 0.1668
+vn -0.0002 0.8392 0.5438
+vn -0.0000 0.8395 0.5433
+vn -0.0000 0.8396 0.5432
+vn 0.8825 0.2562 -0.3945
+vn 0.0002 -0.8396 -0.5433
+vn 0.0000 -0.8392 -0.5438
+vn 0.0000 -0.8391 -0.5439
+vn -0.8821 0.2565 -0.3950
+vn 0.0005 0.5446 -0.8387
+vn 0.0000 -0.5446 0.8387
+vn -0.0003 0.8391 0.5440
+vn 0.0003 -0.8397 -0.5430
+usemtl None
+s 1
+f 17/17/25 18/18/26 19/19/27
+f 20/20/28 21/21/28 17/17/28
+f 22/22/29 23/23/30 20/20/31
+f 19/19/32 24/24/32 22/22/32
+f 21/21/33 24/24/33 18/18/33
+f 20/20/34 19/19/34 22/22/34
+f 17/17/25 21/21/35 18/18/26
+f 20/20/28 23/23/28 21/21/28
+f 22/22/29 24/24/36 23/23/30
+f 19/19/32 18/18/32 24/24/32
+f 21/21/33 23/23/33 24/24/33
+f 20/20/34 17/17/34 19/19/34
+o edge_5_Cube.005
+v -0.153309 0.580000 0.261247
+v -0.006691 0.419400 -0.002214
+v -0.170083 0.580000 0.250354
+v -0.153309 -0.580000 0.261247
+v 0.010083 0.419400 0.008679
+v -0.170083 -0.580000 0.250354
+v 0.010083 -0.419883 0.009732
+v -0.006691 -0.419883 -0.001161
+vt 0.0506 0.8517
+vt 0.1935 0.8492
+vt 0.0342 0.8520
+vt 0.0164 0.1914
+vt 0.2099 0.8489
+vt 0.0001 0.1917
+vt 0.1757 0.1886
+vt 0.1594 0.1889
+vn 0.2565 0.8821 -0.3950
+vn 0.8387 0.0005 0.5446
+vn 0.8394 0.0001 0.5434
+vn 0.8396 0.0000 0.5432
+vn 0.2565 -0.8822 -0.3950
+vn -0.8395 -0.0005 -0.5434
+vn -0.8388 -0.0001 -0.5445
+vn -0.8386 -0.0000 -0.5448
+vn 0.5446 -0.0011 -0.8387
+vn -0.5446 -0.0000 0.8387
+vn 0.8384 0.0007 0.5451
+vn -0.8398 -0.0007 -0.5429
+usemtl None
+s 1
+f 25/25/37 26/26/37 27/27/37
+f 28/28/38 29/29/39 25/25/40
+f 30/30/41 31/31/41 28/28/41
+f 27/27/42 32/32/43 30/30/44
+f 29/29/45 32/32/45 26/26/45
+f 28/28/46 27/27/46 30/30/46
+f 25/25/37 29/29/37 26/26/37
+f 28/28/38 31/31/47 29/29/39
+f 30/30/41 32/32/41 31/31/41
+f 27/27/42 26/26/48 32/32/43
+f 29/29/45 31/31/45 32/32/45
+f 28/28/46 25/25/46 27/27/46
+o edge_4_Cube.001
+v -0.573309 0.580000 0.261247
+v -0.426691 0.419400 -0.002214
+v -0.590083 0.580000 0.250354
+v -0.573309 -0.580000 0.261247
+v -0.409917 0.419400 0.008679
+v -0.590083 -0.580000 0.250354
+v -0.409917 -0.419400 0.009162
+v -0.426691 -0.419400 -0.001731
+vt 0.9046 0.2397
+vt 0.7929 0.2434
+vt 0.9174 0.2393
+vt 0.9537 0.7559
+vt 0.7801 0.2438
+vt 0.9664 0.7554
+vt 0.8291 0.7599
+vt 0.8419 0.7595
+vn 0.2565 0.8821 -0.3950
+vn 0.8392 0.0002 0.5438
+vn 0.8395 0.0000 0.5433
+vn 0.8396 0.0000 0.5432
+vn 0.2568 -0.8819 -0.3954
+vn -0.8396 -0.0002 -0.5433
+vn -0.8392 -0.0000 -0.5438
+vn -0.8391 -0.0000 -0.5439
+vn 0.5446 -0.0005 -0.8387
+vn -0.5446 -0.0000 0.8387
+vn 0.8391 0.0003 0.5440
+vn -0.8397 -0.0003 -0.5430
+usemtl None
+s 1
+f 33/33/49 34/34/49 35/35/49
+f 36/36/50 37/37/51 33/33/52
+f 38/38/53 39/39/53 36/36/53
+f 35/35/54 40/40/55 38/38/56
+f 37/37/57 40/40/57 34/34/57
+f 36/36/58 35/35/58 38/38/58
+f 33/33/49 37/37/49 34/34/49
+f 36/36/50 39/39/59 37/37/51
+f 38/38/53 40/40/53 39/39/53
+f 35/35/54 34/34/60 40/40/55
+f 37/37/57 39/39/57 40/40/57
+f 36/36/58 33/33/58 35/35/58
+o base_Cube.004
+v 0.420000 0.420000 0.010000
+v -0.420000 0.420000 -0.010000
+v -0.420000 0.420000 0.010000
+v 0.420000 -0.420000 0.010000
+v 0.420000 0.420000 -0.010000
+v -0.420000 -0.420000 0.010000
+v 0.420000 -0.420000 -0.010000
+v -0.420000 -0.420000 -0.010000
+vt 0.7524 0.8072
+vt -0.3038 0.8371
+vt -0.3038 0.8371
+vt 0.7012 0.1905
+vt 0.7524 0.8072
+vt -0.3550 0.2204
+vt 0.7012 0.1905
+vt -0.3550 0.2204
+vn -0.0000 1.0000 0.0000
+vn 1.0000 0.0000 0.0000
+vn 0.0000 -1.0000 0.0000
+vn -1.0000 -0.0000 0.0000
+vn -0.0000 0.0000 -1.0000
+vn 0.0000 -0.0000 1.0000
+usemtl None
+s 1
+f 41/41/61 42/42/61 43/43/61
+f 44/44/62 45/45/62 41/41/62
+f 46/46/63 47/47/63 44/44/63
+f 43/43/64 48/48/64 46/46/64
+f 45/45/65 48/48/65 42/42/65
+f 44/44/66 43/43/66 46/46/66
+f 41/41/61 45/45/61 42/42/61
+f 44/44/62 47/47/62 45/45/62
+f 46/46/63 48/48/63 47/47/63
+f 43/43/64 42/42/64 48/48/64
+f 45/45/65 47/47/65 48/48/65
+f 44/44/66 41/41/66 43/43/66
diff --git a/data/tray/tray_textured2.urdf b/data/tray/tray_textured2.urdf
new file mode 100644
index 000000000..8b89b015c
--- /dev/null
+++ b/data/tray/tray_textured2.urdf
@@ -0,0 +1,24 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
index d0a3dca38..33b178dd7 100644
--- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
+++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
@@ -43,11 +43,12 @@ bool gCloseToKuka=false;
bool gEnableRealTimeSimVR=false;
bool gCreateDefaultRobotAssets = false;
int gInternalSimFlags = 0;
+bool gResetSimulation = 0;
int gHuskyId = -1;
btTransform huskyTr = btTransform::getIdentity();
int gCreateObjectSimVR = -1;
-int gEnableKukaControl = 0;
+int gEnableKukaControl = 1;
btScalar simTimeScalingFactor = 1;
btScalar gRhsClamp = 1.f;
@@ -718,6 +719,19 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld()
}
+void PhysicsServerCommandProcessor::deleteCachedInverseKinematicsBodies()
+{
+ for (int i = 0; i < m_data->m_inverseKinematicsHelpers.size(); i++)
+ {
+ IKTrajectoryHelper** ikHelperPtr = m_data->m_inverseKinematicsHelpers.getAtIndex(i);
+ if (ikHelperPtr)
+ {
+ IKTrajectoryHelper* ikHelper = *ikHelperPtr;
+ delete ikHelper;
+ }
+ }
+ m_data->m_inverseKinematicsHelpers.clear();
+}
void PhysicsServerCommandProcessor::deleteCachedInverseDynamicsBodies()
{
for (int i = 0; i < m_data->m_inverseDynamicsBodies.size(); i++)
@@ -736,7 +750,7 @@ void PhysicsServerCommandProcessor::deleteCachedInverseDynamicsBodies()
void PhysicsServerCommandProcessor::deleteDynamicsWorld()
{
deleteCachedInverseDynamicsBodies();
-
+ deleteCachedInverseKinematicsBodies();
for (int i=0;im_multiBodyJointFeedbacks.size();i++)
{
@@ -2429,29 +2443,12 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
case CMD_RESET_SIMULATION:
{
- //clean up all data
- deleteCachedInverseDynamicsBodies();
-
- if (m_data && m_data->m_guiHelper)
- {
- m_data->m_guiHelper->removeAllGraphicsInstances();
- }
- if (m_data)
- {
- m_data->m_visualConverter.resetAll();
- }
-
- deleteDynamicsWorld();
- createEmptyDynamicsWorld();
-
- m_data->exitHandles();
- m_data->initHandles();
+
+ resetSimulation();
SharedMemoryStatus& serverCmd =serverStatusOut;
serverCmd.m_type = CMD_RESET_SIMULATION_COMPLETED;
hasStatus = true;
- m_data->m_hasGround = false;
- m_data->m_gripperRigidbodyFixed = 0;
break;
}
case CMD_CREATE_RIGID_BODY:
@@ -3940,6 +3937,12 @@ double gDtInSec = 0.f;
double gSubStep = 0.f;
void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec)
{
+ if (gResetSimulation)
+ {
+ resetSimulation();
+ gResetSimulation = false;
+ }
+
if ((gEnableRealTimeSimVR || m_data->m_allowRealTimeSimulation) && m_data->m_guiHelper)
{
@@ -3997,7 +4000,30 @@ void PhysicsServerCommandProcessor::applyJointDamping(int bodyUniqueId)
}
}
+void PhysicsServerCommandProcessor::resetSimulation()
+{
+ //clean up all data
+ deleteCachedInverseDynamicsBodies();
+ if (m_data && m_data->m_guiHelper)
+ {
+ m_data->m_guiHelper->removeAllGraphicsInstances();
+ }
+ if (m_data)
+ {
+ m_data->m_visualConverter.resetAll();
+ }
+
+ deleteDynamicsWorld();
+ createEmptyDynamicsWorld();
+
+ m_data->exitHandles();
+ m_data->initHandles();
+
+ m_data->m_hasGround = false;
+ m_data->m_gripperRigidbodyFixed = 0;
+
+}
//todo: move this to Python/scripting
void PhysicsServerCommandProcessor::createDefaultRobotAssets()
{
@@ -4141,7 +4167,7 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets()
btTransform objectLocalTr[] = {
btTransform(btQuaternion(0, 0, 0, 1), btVector3(0.0, 0.0, 0.0)),
- btTransform(btQuaternion(0, 0, 0, 1), btVector3(0.0, 0.15, 0.64)),
+ 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)),
@@ -4168,7 +4194,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.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());
diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.h b/examples/SharedMemory/PhysicsServerCommandProcessor.h
index e271cdff6..dc7992980 100644
--- a/examples/SharedMemory/PhysicsServerCommandProcessor.h
+++ b/examples/SharedMemory/PhysicsServerCommandProcessor.h
@@ -23,6 +23,8 @@ class PhysicsServerCommandProcessor : public PhysicsCommandProcessorInterface
//todo: move this to physics client side / Python
void createDefaultRobotAssets();
+ void resetSimulation();
+
protected:
@@ -37,6 +39,7 @@ protected:
int createBodyInfoStream(int bodyUniqueId, char* bufferServerToClient, int bufferSizeInBytes);
void deleteCachedInverseDynamicsBodies();
+ void deleteCachedInverseKinematicsBodies();
public:
PhysicsServerCommandProcessor();
diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp
index aff18b0f8..e5b4cc3e4 100644
--- a/examples/SharedMemory/PhysicsServerExample.cpp
+++ b/examples/SharedMemory/PhysicsServerExample.cpp
@@ -39,6 +39,7 @@ extern bool gEnableRealTimeSimVR;
extern bool gCreateDefaultRobotAssets;
extern int gInternalSimFlags;
extern int gCreateObjectSimVR;
+extern bool gResetSimulation;
extern int gEnableKukaControl;
int gGraspingController = -1;
extern btScalar simTimeScalingFactor;
@@ -273,7 +274,7 @@ void MotionThreadFunc(void* userPtr,void* lsMemory)
args->m_physicsServerPtr->removePickingConstraint();
}
- if (!gEnableKukaControl)
+// if (!gEnableKukaControl)
{
if (args->m_isVrControllerPicking[c])
{
@@ -1670,7 +1671,8 @@ void PhysicsServerExample::vrControllerButtonCallback(int controllerId, int butt
{
if (button == 1 && state == 0)
{
-// gVRTeleportPos = gLastPickPos;
+ gResetSimulation = true;
+ //gVRTeleportPos1 = gLastPickPos;
}
} else
{
@@ -1682,7 +1684,7 @@ void PhysicsServerExample::vrControllerButtonCallback(int controllerId, int butt
} else
{
gDebugRenderToggle = 0;
-
+#if 0//it confuses people, make it into a debug option in a VR GUI?
if (simTimeScalingFactor==0)
{
simTimeScalingFactor = 1;
@@ -1697,6 +1699,7 @@ void PhysicsServerExample::vrControllerButtonCallback(int controllerId, int butt
simTimeScalingFactor = 0;
}
}
+#endif
}
} else
{
@@ -1714,7 +1717,7 @@ void PhysicsServerExample::vrControllerButtonCallback(int controllerId, int butt
}
else
{
- gEnableKukaControl = !gEnableKukaControl;
+// gEnableKukaControl = !gEnableKukaControl;
}
}