From b803f4e7cf3df277a18437aaff268dfdff84eed1 Mon Sep 17 00:00:00 2001 From: YunfeiBai Date: Sun, 25 Sep 2016 16:05:53 -0700 Subject: [PATCH] Attach gripper to kuka arm. --- data/gripper/wsg50_one_motor_gripper.sdf | 2 +- data/gripper/wsg50_one_motor_gripper_free.sdf | 365 ++++++++++++++++++ data/pr2_gripper_short.urdf | 142 +++++++ .../PhysicsServerCommandProcessor.cpp | 92 ++++- 4 files changed, 596 insertions(+), 5 deletions(-) create mode 100644 data/gripper/wsg50_one_motor_gripper_free.sdf create mode 100644 data/pr2_gripper_short.urdf 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());