diff --git a/data/gripper/wsg50_one_motor_gripper.sdf b/data/gripper/wsg50_one_motor_gripper.sdf
index 46516fe1b..1f19f174a 100644
--- a/data/gripper/wsg50_one_motor_gripper.sdf
+++ b/data/gripper/wsg50_one_motor_gripper.sdf
@@ -385,4 +385,4 @@
-
+
\ No newline at end of file
diff --git a/data/gripper/wsg50_one_motor_gripper_free.sdf b/data/gripper/wsg50_one_motor_gripper_free.sdf
new file mode 100644
index 000000000..35417f471
--- /dev/null
+++ b/data/gripper/wsg50_one_motor_gripper_free.sdf
@@ -0,0 +1,365 @@
+
+
+
+
+ 0 0 0.26 3.14 0 0
+
+
+ 0 0 0 0 0 0
+
+ 0 0 0 0 0 0
+ 1.2
+
+ 1
+ 0
+ 0
+ 1
+ 0
+ 1
+
+
+
+
+ 0 0 0 0 -0 0
+
+
+ 1 1 1
+ meshes/WSG50_110.stl
+
+
+
+
+
+
+
+
+
+
+ 0 0 0.03 0 0 0
+
+ 0 0 0 0 0 0
+ 0.1
+
+ 0.1
+ 0
+ 0
+ 0.1
+ 0
+ 0.1
+
+
+
+ 0 0 0.01 0 0 0
+
+
+ 0.02 0.02 0.02
+
+
+
+
+
+
+ motor
+ base_link
+
+ 0 0 1
+
+ -0.047
+ 0.001
+ 10.0
+ 10.0
+
+
+ 0
+ 0
+ 0
+ 0
+
+
+
+
+
+ 0 0 0.04 0 0 0
+
+ 0 0 0.035 0 0 0
+ 0.1
+
+ 0.1
+ 0
+ 0
+ 0.1
+ 0
+ 0.1
+
+
+
+ -0.03 0 0.01 0 -1.2 0
+
+
+ 0.02 0.02 0.07
+
+
+
+
+
+
+ left_hinge
+ motor
+
+ 0 1 0
+
+ -20.0
+ 20.0
+ 10
+ 10
+
+
+ 0
+ 0
+ 0
+ 0
+
+ 0
+
+
+
+
+ 0 0 0.04 0 0 0
+
+ 0 0 0.035 0 0 0
+ 0.1
+
+ 0.1
+ 0
+ 0
+ 0.1
+ 0
+ 0.1
+
+
+
+ 0.03 0 0.01 0 1.2 0
+
+
+ 0.02 0.02 0.07
+
+
+
+
+
+
+ right_hinge
+ motor
+
+ 0 1 0
+
+ -20.0
+ 20.0
+ 10
+ 10
+
+
+ 0
+ 0
+ 0
+ 0
+
+ 0
+
+
+
+
+ -0.055 0 0.06 0 -0 0
+
+ 0 0 0.0115 0 -0 0
+ 0.2
+
+ 0.1
+ 0
+ 0
+ 0.1
+ 0
+ 0.1
+
+
+
+
+ 0 0 -0.06 0 0 0
+
+
+ 0.001 0.001 0.001
+ meshes/GUIDE_WSG50_110.stl
+
+
+
+
+ 0 0 -0.037 0 0 0
+
+
+ 0.001 0.001 0.001
+ meshes/WSG-FMF.stl
+
+
+
+
+
+
+
+ gripper_left
+ left_hinge
+
+ 0 1 0
+
+ -1.0
+ 1.0
+ 10
+ 10
+
+
+ 0.01
+ 0
+ 0
+ 0
+
+ 0
+
+
+
+
+ 0.055 0 0.06 0 0 3.14159
+
+ 0 0 0.0115 0 -0 0
+ 0.2
+
+ 0.1
+ 0
+ 0
+ 0.1
+ 0
+ 0.1
+
+
+
+
+ 0 0 -0.06 0 0 0
+
+
+ 0.001 0.001 0.001
+ meshes/GUIDE_WSG50_110.stl
+
+
+
+
+ 0 0 -0.037 0 0 0
+
+
+ 0.001 0.001 0.001
+ meshes/WSG-FMF.stl
+
+
+
+
+
+
+ gripper_right
+ right_hinge
+
+ 0 1 0
+
+ -1.0
+ 1.0
+ 10
+ 10
+
+
+ 0.01
+ 0
+ 0
+ 0
+
+ 0
+
+
+
+
+ 0.062 0 0.145 0 0 1.5708
+
+ 0.2
+
+ 0.1
+ 0
+ 0
+ 0.1
+ 0
+ 0.1
+
+
+
+
+ 0 0 0.042 0 0 0
+
+
+ 0.02 0.02 0.15
+
+
+
+
+
+
+ 0 0 0 0 0 0
+
+
+ 1 1 1
+ meshes/l_gripper_tip_scaled.stl
+
+
+
+
+
+
+ gripper_right
+ finger_right
+
+
+
+ -0.062 0 0.145 0 0 4.71239
+
+ 0.2
+
+ 0.1
+ 0
+ 0
+ 0.1
+ 0
+ 0.1
+
+
+
+
+ 0 0 0.042 0 0 0
+
+
+ 0.02 0.02 0.15
+
+
+
+
+
+
+ 0 0 0 0 0 0
+
+
+ 1 1 1
+ meshes/l_gripper_tip_scaled.stl
+
+
+
+
+
+
+ gripper_left
+ finger_left
+
+
+
+
\ No newline at end of file
diff --git a/data/pr2_gripper_short.urdf b/data/pr2_gripper_short.urdf
new file mode 100644
index 000000000..e41abf926
--- /dev/null
+++ b/data/pr2_gripper_short.urdf
@@ -0,0 +1,142 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
index f6e8e987e..a3275898d 100644
--- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
+++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
@@ -388,9 +388,12 @@ struct PhysicsServerCommandProcessorInternalData
btMultiBodyFixedConstraint* m_gripperRigidbodyFixed;
btMultiBody* m_gripperMultiBody;
+ btMultiBodyFixedConstraint* m_kukaGripperFixed;
+ btMultiBody* m_kukaGripperMultiBody;
int m_huskyId;
int m_KukaId;
int m_sphereId;
+ int m_gripperId;
CommandLogger* m_commandLogger;
CommandLogPlayback* m_logPlayback;
@@ -445,6 +448,7 @@ struct PhysicsServerCommandProcessorInternalData
m_huskyId(-1),
m_KukaId(-1),
m_sphereId(-1),
+ m_gripperId(-1),
m_commandLogger(0),
m_logPlayback(0),
m_physicsDeltaTime(1./240.),
@@ -2945,13 +2949,93 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec)
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());
- loadUrdf("r2d2.urdf", btVector3(2, -2, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
-
+ loadUrdf("r2d2.urdf", btVector3(2, -2, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("kuka_iiwa/model.urdf", btVector3(2.0, 0, 0), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
m_data->m_KukaId = bodyId;
- loadUrdf("pr2_gripper.urdf", btVector3(2.2, 0.1, 0.1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
-
+
+ loadSdf("gripper/wsg50_one_motor_gripper_free.sdf", &gBufferServerToClient[0], gBufferServerToClient.size(), true);
+ m_data->m_gripperId = bodyId + 1;
+ InteralBodyData* parentBody = m_data->getHandle(m_data->m_KukaId);
+ InteralBodyData* childBody = m_data->getHandle(m_data->m_gripperId);
+
+ // Reset the default gripper motor maximum torque for damping to 0
+ for (int i = 0; i < childBody->m_multiBody->getNumLinks(); i++)
+ {
+ if (supportsJointMotor(childBody->m_multiBody, i))
+ {
+ btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)childBody->m_multiBody->getLink(i).m_userPtr;
+ if (motor)
+ {
+ motor->setMaxAppliedImpulse(0);
+ }
+ }
+ }
+
+ // Add slider joint for fingers
+ btVector3 pivotInParent1(0, 0, 0.06);
+ 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, 0, 0.06);
+ 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);
+ btMultiBodySliderConstraint* multibodySlider1 = new btMultiBodySliderConstraint(childBody->m_multiBody, 0, childBody->m_multiBody, 2, pivotInParent1, pivotInChild1, frameInParent1, frameInChild1, jointAxis1);
+ multibodySlider1->setMaxAppliedImpulse(500.0);
+ btMultiBodySliderConstraint* multibodySlider2 = new btMultiBodySliderConstraint(childBody->m_multiBody, 0, childBody->m_multiBody, 5, pivotInParent2, pivotInChild2, frameInParent2, frameInChild2, jointAxis2);
+ multibodySlider1->setMaxAppliedImpulse(500.0);
+ btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*)m_data->m_dynamicsWorld;
+ world->addMultiBodyConstraint(multibodySlider1);
+ world->addMultiBodyConstraint(multibodySlider2);
+
+ if (parentBody->m_multiBody)
+ {
+ childBody->m_multiBody->setHasSelfCollision(0);
+ btVector3 pivotInParent(0, 0, 0.02);
+ btMatrix3x3 frameInParent;
+ frameInParent.setIdentity();
+ btVector3 pivotInChild(0, 0, 0);
+ btMatrix3x3 frameInChild;
+ frameInChild.setIdentity();
+
+ m_data->m_kukaGripperFixed = new btMultiBodyFixedConstraint(parentBody->m_multiBody, 6, childBody->m_multiBody, 0, pivotInParent, pivotInChild, frameInParent, frameInChild);
+ m_data->m_kukaGripperMultiBody = childBody->m_multiBody;
+ m_data->m_kukaGripperFixed->setMaxAppliedImpulse(10000);
+ btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*)m_data->m_dynamicsWorld;
+ world->addMultiBodyConstraint(m_data->m_kukaGripperFixed);
+ }
+
+ // Load pr2 gripper for kuka
+ /*
+ loadUrdf("pr2_gripper_short.urdf", btVector3(2.2, 0.1, 0.1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
+ InteralBodyData* parentBody = m_data->getHandle(m_data->m_KukaId);
+ InteralBodyData* childBody = m_data->getHandle(bodyId);
+ if (parentBody->m_multiBody)
+ {
+ btVector3 pivotInParent(0, 0, 0.04);
+ btMatrix3x3 frameInParent;
+ frameInParent.setIdentity();
+ btVector3 pivotInChild(0, 0, 0);
+ btMatrix3x3 frameInChild;
+ frameInChild.setRotation(btQuaternion(0, 0.70711, 0, 0.70711));
+
+ m_data->m_kukaGripperFixed = new btMultiBodyFixedConstraint(parentBody->m_multiBody, 6, childBody->m_multiBody, 0, pivotInParent, pivotInChild, frameInParent, frameInChild);
+ m_data->m_kukaGripperMultiBody = childBody->m_multiBody;
+ if (m_data->m_kukaGripperMultiBody->getNumLinks() > 2)
+ {
+ m_data->m_kukaGripperMultiBody->setJointPos(0, 0);
+ m_data->m_kukaGripperMultiBody->setJointPos(2, 0);
+ }
+ m_data->m_kukaGripperFixed->setMaxAppliedImpulse(10000);
+ btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*)m_data->m_dynamicsWorld;
+ world->addMultiBodyConstraint(m_data->m_kukaGripperFixed);
+ }
+ */
+
+
loadUrdf("cube_small.urdf", btVector3(0.3, 0.6, 0.85), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());