diff --git a/data/gripper/wsg50_one_motor_gripper.sdf b/data/gripper/wsg50_one_motor_gripper.sdf new file mode 100644 index 000000000..46516fe1b --- /dev/null +++ b/data/gripper/wsg50_one_motor_gripper.sdf @@ -0,0 +1,388 @@ + + + + + 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.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 + + + + diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp index aa7217502..0608f0bd1 100644 --- a/examples/ExampleBrowser/ExampleEntries.cpp +++ b/examples/ExampleBrowser/ExampleEntries.cpp @@ -266,7 +266,7 @@ static ExampleEntry gDefaultExamples[]= ExampleEntry(1,"Rolling friction","Experiment on multibody rolling friction", R2D2GraspExampleCreateFunc,eROBOTIC_LEARN_ROLLING_FRICTION), ExampleEntry(1,"Gripper Grasp","Grasp experiment with a gripper to improve contact model", GripperGraspExampleCreateFunc,eGRIPPER_GRASP), ExampleEntry(1,"Two Point Grasp","Grasp experiment with two point contact to test rolling friction", GripperGraspExampleCreateFunc, eTWO_POINT_GRASP), - + ExampleEntry(1,"One Motor Gripper Grasp","Grasp experiment with a gripper with one motor to test slider constraint for closed loop structure", GripperGraspExampleCreateFunc, eONE_MOTOR_GRASP), #ifdef ENABLE_LUA diff --git a/examples/RoboticsLearning/GripperGraspExample.cpp b/examples/RoboticsLearning/GripperGraspExample.cpp index 9a7ab8e7b..7aa2f2571 100644 --- a/examples/RoboticsLearning/GripperGraspExample.cpp +++ b/examples/RoboticsLearning/GripperGraspExample.cpp @@ -10,6 +10,7 @@ #include "../SharedMemory/PhysicsServerSharedMemory.h" #include "../SharedMemory/PhysicsClientC_API.h" #include "../CommonInterfaces/CommonParameterInterface.h" +#include "../SharedMemory/SharedMemoryPublic.h" #include #include "b3RobotSimAPI.h" @@ -213,6 +214,120 @@ public: m_robotSim.setGravity(b3MakeVector3(0,0,-10)); m_robotSim.setNumSimulationSubSteps(4); } + + if ((m_options & eONE_MOTOR_GRASP)!=0) + { + + { + SliderParams slider("Vertical velocity",&sGripperVerticalVelocity); + slider.m_minVal=-2; + slider.m_maxVal=2; + m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); + } + + { + SliderParams slider("Closing velocity",&sGripperClosingTargetVelocity + ); + slider.m_minVal=-1; + slider.m_maxVal=1; + m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); + } + { + b3RobotSimLoadFileArgs args(""); + b3RobotSimLoadFileResults results; + args.m_fileName = "cube_small.urdf"; + args.m_startPosition.setValue(0, 0, .107); + args.m_startOrientation.setEulerZYX(0, 0, 0); + args.m_useMultiBody = true; + m_robotSim.loadFile(args, results); + } + + { + b3RobotSimLoadFileArgs args(""); + args.m_fileName = "gripper/wsg50_one_motor_gripper.sdf"; + args.m_fileType = B3_SDF_FILE; + args.m_useMultiBody = true; + b3RobotSimLoadFileResults results; + + if (m_robotSim.loadFile(args, results) && results.m_uniqueObjectIds.size()==1) + { + + m_gripperIndex = results.m_uniqueObjectIds[0]; + int numJoints = m_robotSim.getNumJoints(m_gripperIndex); + b3Printf("numJoints = %d",numJoints); + + for (int i=0;im_multiBody) { btMultiBodyFixedConstraint* multibodyFixed = new btMultiBodyFixedConstraint(parentBody->m_multiBody,clientCmd.m_createJointArguments.m_parentJointIndex,childBody->m_multiBody,clientCmd.m_createJointArguments.m_childJointIndex,pivotInParent,pivotInChild,frameInParent,frameInChild); - multibodyFixed->setMaxAppliedImpulse(2.0); + multibodyFixed->setMaxAppliedImpulse(500.0); m_data->m_dynamicsWorld->addMultiBodyConstraint(multibodyFixed); } else { btMultiBodyFixedConstraint* rigidbodyFixed = new btMultiBodyFixedConstraint(parentBody->m_multiBody,clientCmd.m_createJointArguments.m_parentJointIndex,childBody->m_rigidBody,pivotInParent,pivotInChild,frameInParent,frameInChild); - rigidbodyFixed->setMaxAppliedImpulse(2.0); + rigidbodyFixed->setMaxAppliedImpulse(500.0); btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld; world->addMultiBodyConstraint(rigidbodyFixed); } } - else if (clientCmd.m_createJointArguments.m_jointType == btMultibodyLink::ePrismatic) + else if (clientCmd.m_createJointArguments.m_jointType == ePrismaticType) { if (childBody->m_multiBody) { btMultiBodySliderConstraint* multibodySlider = new btMultiBodySliderConstraint(parentBody->m_multiBody,clientCmd.m_createJointArguments.m_parentJointIndex,childBody->m_multiBody,clientCmd.m_createJointArguments.m_childJointIndex,pivotInParent,pivotInChild,frameInParent,frameInChild,jointAxis); - multibodySlider->setMaxAppliedImpulse(2.0); + multibodySlider->setMaxAppliedImpulse(500.0); m_data->m_dynamicsWorld->addMultiBodyConstraint(multibodySlider); } else { btMultiBodySliderConstraint* rigidbodySlider = new btMultiBodySliderConstraint(parentBody->m_multiBody,clientCmd.m_createJointArguments.m_parentJointIndex,childBody->m_rigidBody,pivotInParent,pivotInChild,frameInParent,frameInChild,jointAxis); - rigidbodySlider->setMaxAppliedImpulse(2.0); + rigidbodySlider->setMaxAppliedImpulse(500.0); btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld; world->addMultiBodyConstraint(rigidbodySlider); } diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index b9a5c9eb5..dbb4dcd19 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -98,6 +98,7 @@ enum enum JointType { eRevoluteType = 0, ePrismaticType = 1, + eFixedType = 2, }; struct b3JointInfo