diff --git a/data/gripper/wsg50_one_motor_gripper_new.sdf b/data/gripper/wsg50_one_motor_gripper_new.sdf new file mode 100644 index 000000000..a3a11f3b2 --- /dev/null +++ b/data/gripper/wsg50_one_motor_gripper_new.sdf @@ -0,0 +1,386 @@ + + + + + 0 0 0.26 3.14 0 0 + + + + + + world + base_link + + 0 0 1 + + -0.5 + 10 + 1 + 1 + + + 0 + 0 + 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.055 + 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 + base_link + + 1 0 0 + + -0.01 + 0.05 + 1 + 1 + + + 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 + base_link + + 1 0 0 + + -0.01 + 0.05 + 1 + 1 + + + 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/gripper/wsg50_one_motor_gripper_new_free_base.sdf b/data/gripper/wsg50_one_motor_gripper_new_free_base.sdf new file mode 100644 index 000000000..190e591a3 --- /dev/null +++ b/data/gripper/wsg50_one_motor_gripper_new_free_base.sdf @@ -0,0 +1,383 @@ + + + + + 0 -2.3 2.1 0 0 0 + + 0 0 0 0 0 0 + + 0 0 0 0 0 0 + 0.1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + + + + world + base_link + + + + 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.055 + 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 + base_link + + 1 0 0 + + -0.01 + 0.05 + 1 + 1 + + + 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 + base_link + + 1 0 0 + + -0.01 + 0.05 + 1 + 1 + + + 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/examples/RoboticsLearning/GripperGraspExample.cpp b/examples/RoboticsLearning/GripperGraspExample.cpp index 7aa2f2571..b46706e65 100644 --- a/examples/RoboticsLearning/GripperGraspExample.cpp +++ b/examples/RoboticsLearning/GripperGraspExample.cpp @@ -235,7 +235,7 @@ public: { b3RobotSimLoadFileArgs args(""); b3RobotSimLoadFileResults results; - args.m_fileName = "cube_small.urdf"; + args.m_fileName = "sphere_small.urdf"; args.m_startPosition.setValue(0, 0, .107); args.m_startOrientation.setEulerZYX(0, 0, 0); args.m_useMultiBody = true; @@ -244,7 +244,7 @@ public: { b3RobotSimLoadFileArgs args(""); - args.m_fileName = "gripper/wsg50_one_motor_gripper.sdf"; + args.m_fileName = "gripper/wsg50_one_motor_gripper_new.sdf"; args.m_fileType = B3_SDF_FILE; args.m_useMultiBody = true; b3RobotSimLoadFileResults results; @@ -287,6 +287,7 @@ public: } m_robotSim.setGravity(b3MakeVector3(0,0,-10)); + /* b3JointInfo sliderJoint1; sliderJoint1.m_parentFrame[0] = 0; sliderJoint1.m_parentFrame[1] = 0; @@ -327,6 +328,47 @@ public: sliderJoint2.m_jointType = ePrismaticType; m_robotSim.createJoint(1, 0, 1, 3, &sliderJoint1); m_robotSim.createJoint(1, 0, 1, 6, &sliderJoint2); + */ + b3JointInfo revoluteJoint1; + revoluteJoint1.m_parentFrame[0] = -0.055; + revoluteJoint1.m_parentFrame[1] = 0; + revoluteJoint1.m_parentFrame[2] = 0.02; + revoluteJoint1.m_parentFrame[3] = 0; + revoluteJoint1.m_parentFrame[4] = 0; + revoluteJoint1.m_parentFrame[5] = 0; + revoluteJoint1.m_parentFrame[6] = 1.0; + revoluteJoint1.m_childFrame[0] = 0; + revoluteJoint1.m_childFrame[1] = 0; + revoluteJoint1.m_childFrame[2] = 0; + revoluteJoint1.m_childFrame[3] = 0; + revoluteJoint1.m_childFrame[4] = 0; + revoluteJoint1.m_childFrame[5] = 0; + revoluteJoint1.m_childFrame[6] = 1.0; + revoluteJoint1.m_jointAxis[0] = 1.0; + revoluteJoint1.m_jointAxis[1] = 0.0; + revoluteJoint1.m_jointAxis[2] = 0.0; + revoluteJoint1.m_jointType = ePoint2PointType; + b3JointInfo revoluteJoint2; + revoluteJoint2.m_parentFrame[0] = 0.055; + revoluteJoint2.m_parentFrame[1] = 0; + revoluteJoint2.m_parentFrame[2] = 0.02; + revoluteJoint2.m_parentFrame[3] = 0; + revoluteJoint2.m_parentFrame[4] = 0; + revoluteJoint2.m_parentFrame[5] = 0; + revoluteJoint2.m_parentFrame[6] = 1.0; + revoluteJoint2.m_childFrame[0] = 0; + revoluteJoint2.m_childFrame[1] = 0; + revoluteJoint2.m_childFrame[2] = 0; + revoluteJoint2.m_childFrame[3] = 0; + revoluteJoint2.m_childFrame[4] = 0; + revoluteJoint2.m_childFrame[5] = 0; + revoluteJoint2.m_childFrame[6] = 1.0; + revoluteJoint2.m_jointAxis[0] = 1.0; + revoluteJoint2.m_jointAxis[1] = 0.0; + revoluteJoint2.m_jointAxis[2] = 0.0; + revoluteJoint2.m_jointType = ePoint2PointType; + m_robotSim.createJoint(1, 2, 1, 4, &revoluteJoint1); + m_robotSim.createJoint(1, 3, 1, 6, &revoluteJoint2); } } @@ -372,6 +414,7 @@ public: } } + m_robotSim.stepSimulation(); } virtual void renderScene() diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 0af01c70e..de0da7109 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -12,6 +12,7 @@ #include "BulletDynamics/Featherstone/btMultiBodyJointFeedback.h" #include "BulletDynamics/Featherstone/btMultiBodyFixedConstraint.h" #include "BulletDynamics/Featherstone/btMultiBodySliderConstraint.h" +#include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h" #include "LinearMath/btHashMap.h" #include "BulletInverseDynamics/MultiBodyTree.hpp" #include "IKTrajectoryHelper.h" @@ -390,8 +391,8 @@ struct PhysicsServerCommandProcessorInternalData btMultiBody* m_gripperMultiBody; btMultiBodyFixedConstraint* m_kukaGripperFixed; btMultiBody* m_kukaGripperMultiBody; - btMultiBodySliderConstraint* m_kukaGripperSlider1; - btMultiBodySliderConstraint* m_kukaGripperSlider2; + btMultiBodyPoint2Point* m_kukaGripperRevolute1; + btMultiBodyPoint2Point* m_kukaGripperRevolute2; int m_huskyId; int m_KukaId; int m_sphereId; @@ -2976,7 +2977,7 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec) loadUrdf("r2d2.urdf", btVector3(2, -2, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); // Load one motor gripper for kuka - loadSdf("gripper/wsg50_one_motor_gripper_free_base.sdf", &gBufferServerToClient[0], gBufferServerToClient.size(), true); + 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); @@ -3002,23 +3003,23 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec) //loadUrdf("nao/nao.urdf", btVector3(2,5, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); // Add slider joint for fingers - btVector3 pivotInParent1(0, 0, 0.06); + btVector3 pivotInParent1(-0.055, 0, 0.02); 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 pivotInParent2(0.055, 0, 0.02); 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); - m_data->m_kukaGripperSlider1 = new btMultiBodySliderConstraint(gripperBody->m_multiBody, 0, gripperBody->m_multiBody, 3, pivotInParent1, pivotInChild1, frameInParent1, frameInChild1, jointAxis1); - m_data->m_kukaGripperSlider1->setMaxAppliedImpulse(5.0); - m_data->m_kukaGripperSlider2 = new btMultiBodySliderConstraint(gripperBody->m_multiBody, 0, gripperBody->m_multiBody, 6, pivotInParent2, pivotInChild2, frameInParent2, frameInChild2, jointAxis2); - m_data->m_kukaGripperSlider2->setMaxAppliedImpulse(5.0); + m_data->m_kukaGripperRevolute1 = new btMultiBodyPoint2Point(gripperBody->m_multiBody, 2, gripperBody->m_multiBody, 4, pivotInParent1, pivotInChild1); + m_data->m_kukaGripperRevolute1->setMaxAppliedImpulse(5.0); + m_data->m_kukaGripperRevolute2 = new btMultiBodyPoint2Point(gripperBody->m_multiBody, 3, gripperBody->m_multiBody, 6, pivotInParent2, pivotInChild2); + m_data->m_kukaGripperRevolute2->setMaxAppliedImpulse(5.0); - m_data->m_dynamicsWorld->addMultiBodyConstraint(m_data->m_kukaGripperSlider1); - m_data->m_dynamicsWorld->addMultiBodyConstraint(m_data->m_kukaGripperSlider2); + m_data->m_dynamicsWorld->addMultiBodyConstraint(m_data->m_kukaGripperRevolute1); + m_data->m_dynamicsWorld->addMultiBodyConstraint(m_data->m_kukaGripperRevolute2); if (kukaBody->m_multiBody) { @@ -3075,8 +3076,8 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec) // Table area loadUrdf("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("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("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()); loadUrdf("cube_small.urdf", objectWorldTr[5].getOrigin(), objectWorldTr[5].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); loadUrdf("sphere_small.urdf", objectWorldTr[6].getOrigin(), objectWorldTr[6].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); @@ -3090,7 +3091,13 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec) // Chess area loadUrdf("table_square.urdf", btVector3(2.0, 0, 0.0), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); - + //loadUrdf("pawn.urdf", btVector3(1.8, -0.1, 0.7), btQuaternion(btVector3(1, 0, 0), SIMD_HALF_PI), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); + //loadUrdf("queen.urdf", btVector3(1.9, -0.2, 0.7), btQuaternion(btVector3(1, 0, 0), SIMD_HALF_PI), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); + //loadUrdf("king.urdf", btVector3(2.0, 0, 0.7), btQuaternion(btVector3(1, 0, 0), SIMD_HALF_PI), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); + //loadUrdf("bishop.urdf", btVector3(2.1, 0.1, 0.7), btQuaternion(btVector3(1, 0, 0), SIMD_HALF_PI), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); + //loadUrdf("rook.urdf", btVector3(2.2, 0, 0.7), btQuaternion(btVector3(1, 0, 0), SIMD_HALF_PI), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); + //loadUrdf("knight.urdf", btVector3(2.2, 0.2, 0.7), btQuaternion(btVector3(1, 0, 0), SIMD_HALF_PI), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); + loadUrdf("husky/husky.urdf", btVector3(5, 2, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); m_data->m_huskyId = bodyId; @@ -3105,7 +3112,7 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec) btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)childBody->m_multiBody->getLink(1).m_userPtr; if (motor) { - btScalar posTarget = (-0.045)*btMin(btScalar(0.75), gVRGripperAnalog) / 0.75; + btScalar posTarget = (-0.048)*btMin(btScalar(0.75), gVRGripperAnalog) / 0.75; motor->setPositionTarget(posTarget, .2); motor->setVelocityTarget(0.0, .5); motor->setMaxAppliedImpulse(5.0);