diff options
author | yunfeibai <byf1658@gmail.com> | 2016-09-21 12:08:03 -0700 |
---|---|---|
committer | yunfeibai <byf1658@gmail.com> | 2016-09-21 12:08:03 -0700 |
commit | 27fab2adb5c5c9f7b97ad50e2b98d14e0fc6598a (patch) | |
tree | f80409626fb101cbff3b6e206e23d3f624c42e16 | |
parent | bb214e823b11a6eb3b5f677ed2d9aa314e75aee2 (diff) | |
download | bullet3-27fab2adb5c5c9f7b97ad50e2b98d14e0fc6598a.tar.gz |
Create a demo for one motor gripper grasp.
-rw-r--r-- | data/gripper/wsg50_one_motor_gripper.sdf | 388 | ||||
-rw-r--r-- | examples/ExampleBrowser/ExampleEntries.cpp | 2 | ||||
-rw-r--r-- | examples/RoboticsLearning/GripperGraspExample.cpp | 131 | ||||
-rw-r--r-- | examples/RoboticsLearning/GripperGraspExample.h | 1 | ||||
-rw-r--r-- | examples/SharedMemory/PhysicsServerCommandProcessor.cpp | 12 | ||||
-rw-r--r-- | examples/SharedMemory/SharedMemoryPublic.h | 1 |
6 files changed, 528 insertions, 7 deletions
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 @@ +<?xml version="1.0" ?> +<sdf version='1.6'> + <world name='default'> + <model name='wsg50_with_gripper'> + <pose frame=''>0 0 0.26 3.14 0 0</pose> + + <link name='world'> + </link> + + <joint name='base_joint' type='prismatic'> + <parent>world</parent> + <child>base_link</child> + <axis> + <xyz>0 0 1</xyz> + <limit> + <lower>-0.5</lower> + <upper>10</upper> + <effort>1</effort> + <velocity>1</velocity> + </limit> + <dynamics> + <damping>0</damping> + <friction>0</friction> + <spring_reference>0</spring_reference> + <spring_stiffness>0</spring_stiffness> + </dynamics> + </axis> + </joint> + + <link name='base_link'> + <pose frame=''>0 0 0 0 0 0</pose> + <inertial> + <pose frame=''>0 0 0 0 0 0</pose> + <mass>1.2</mass> + <inertia> + <ixx>1</ixx> + <ixy>0</ixy> + <ixz>0</ixz> + <iyy>1</iyy> + <iyz>0</iyz> + <izz>1</izz> + </inertia> + </inertial> + + <visual name='base_link_visual'> + <pose frame=''>0 0 0 0 -0 0</pose> + <geometry> + <mesh> + <scale>1 1 1</scale> + <uri>meshes/WSG50_110.stl</uri> + </mesh> + </geometry> + <material> + + </material> + </visual> + + </link> + + <link name='motor'> + <pose frame=''>0 0 0.03 0 0 0</pose> + <inertial> + <pose frame=''>0 0 0 0 0 0</pose> + <mass>0.1</mass> + <inertia> + <ixx>0.1</ixx> + <ixy>0</ixy> + <ixz>0</ixz> + <iyy>0.1</iyy> + <iyz>0</iyz> + <izz>0.1</izz> + </inertia> + </inertial> + <visual name='motor_visual'> + <pose frame=''>0 0 0.01 0 0 0</pose> + <geometry> + <box> + <size>0.02 0.02 0.02 </size> + </box> + </geometry> + </visual> + </link> + + <joint name='base_joint_motor' type='prismatic'> + <child>motor</child> + <parent>base_link</parent> + <axis> + <xyz>0 0 1</xyz> + <limit> + <lower>-0.047</lower> + <upper>0.001</upper> + <effort>10.0</effort> + <velocity>10.0</velocity> + </limit> + <dynamics> + <damping>0</damping> + <friction>0</friction> + <spring_reference>0</spring_reference> + <spring_stiffness>0</spring_stiffness> + </dynamics> + </axis> + </joint> + + <link name='left_hinge'> + <pose frame=''>0 0 0.04 0 0 0</pose> + <inertial> + <pose frame=''>0 0 0.035 0 0 0</pose> + <mass>0.1</mass> + <inertia> + <ixx>0.1</ixx> + <ixy>0</ixy> + <ixz>0</ixz> + <iyy>0.1</iyy> + <iyz>0</iyz> + <izz>0.1</izz> + </inertia> + </inertial> + <visual name='motor_visual'> + <pose frame=''>-0.03 0 0.01 0 -1.2 0</pose> + <geometry> + <box> + <size>0.02 0.02 0.07 </size> + </box> + </geometry> + </visual> + </link> + + <joint name='motor_left_hinge_joint' type='revolute'> + <child>left_hinge</child> + <parent>motor</parent> + <axis> + <xyz>0 1 0</xyz> + <limit> + <lower>-20.0</lower> + <upper>20.0</upper> + <effort>10</effort> + <velocity>10</velocity> + </limit> + <dynamics> + <damping>0</damping> + <friction>0</friction> + <spring_reference>0</spring_reference> + <spring_stiffness>0</spring_stiffness> + </dynamics> + <use_parent_model_frame>0</use_parent_model_frame> + </axis> + </joint> + + <link name='right_hinge'> + <pose frame=''>0 0 0.04 0 0 0</pose> + <inertial> + <pose frame=''>0 0 0.035 0 0 0</pose> + <mass>0.1</mass> + <inertia> + <ixx>0.1</ixx> + <ixy>0</ixy> + <ixz>0</ixz> + <iyy>0.1</iyy> + <iyz>0</iyz> + <izz>0.1</izz> + </inertia> + </inertial> + <visual name='motor_visual'> + <pose frame=''>0.03 0 0.01 0 1.2 0</pose> + <geometry> + <box> + <size>0.02 0.02 0.07 </size> + </box> + </geometry> + </visual> + </link> + + <joint name='motor_right_hinge_joint' type='revolute'> + <child>right_hinge</child> + <parent>motor</parent> + <axis> + <xyz>0 1 0</xyz> + <limit> + <lower>-20.0</lower> + <upper>20.0</upper> + <effort>10</effort> + <velocity>10</velocity> + </limit> + <dynamics> + <damping>0</damping> + <friction>0</friction> + <spring_reference>0</spring_reference> + <spring_stiffness>0</spring_stiffness> + </dynamics> + <use_parent_model_frame>0</use_parent_model_frame> + </axis> + </joint> + + <link name='gripper_left'> + <pose frame=''>-0.055 0 0.06 0 -0 0</pose> + <inertial> + <pose frame=''>0 0 0.0115 0 -0 0</pose> + <mass>0.2</mass> + <inertia> + <ixx>0.1</ixx> + <ixy>0</ixy> + <ixz>0</ixz> + <iyy>0.1</iyy> + <iyz>0</iyz> + <izz>0.1</izz> + </inertia> + </inertial> + + <visual name='gripper_left_visual'> + <pose frame=''>0 0 -0.06 0 0 0</pose> + <geometry> + <mesh> + <scale>0.001 0.001 0.001</scale> + <uri>meshes/GUIDE_WSG50_110.stl</uri> + </mesh> + </geometry> + </visual> + <visual name='gripper_left_fixed_joint_lump__finger_left_visual_1'> + <pose frame=''>0 0 -0.037 0 0 0</pose> + <geometry> + <mesh> + <scale>0.001 0.001 0.001</scale> + <uri>meshes/WSG-FMF.stl</uri> + </mesh> + </geometry> + </visual> + + </link> + + <joint name='gripper_left_hinge_joint' type='revolute'> + <child>gripper_left</child> + <parent>left_hinge</parent> + <axis> + <xyz>0 1 0</xyz> + <limit> + <lower>-1.0</lower> + <upper>1.0</upper> + <effort>10</effort> + <velocity>10</velocity> + </limit> + <dynamics> + <damping>0.01</damping> + <friction>0</friction> + <spring_reference>0</spring_reference> + <spring_stiffness>0</spring_stiffness> + </dynamics> + <use_parent_model_frame>0</use_parent_model_frame> + </axis> + </joint> + + <link name='gripper_right'> + <pose frame=''>0.055 0 0.06 0 0 3.14159</pose> + <inertial> + <pose frame=''>0 0 0.0115 0 -0 0</pose> + <mass>0.2</mass> + <inertia> + <ixx>0.1</ixx> + <ixy>0</ixy> + <ixz>0</ixz> + <iyy>0.1</iyy> + <iyz>0</iyz> + <izz>0.1</izz> + </inertia> + </inertial> + + <visual name='gripper_right_visual'> + <pose frame=''>0 0 -0.06 0 0 0</pose> + <geometry> + <mesh> + <scale>0.001 0.001 0.001</scale> + <uri>meshes/GUIDE_WSG50_110.stl</uri> + </mesh> + </geometry> + </visual> + <visual name='gripper_right_fixed_joint_lump__finger_right_visual_1'> + <pose frame=''>0 0 -0.037 0 0 0</pose> + <geometry> + <mesh> + <scale>0.001 0.001 0.001</scale> + <uri>meshes/WSG-FMF.stl</uri> + </mesh> + </geometry> + </visual> + </link> + + <joint name='gripper_right_hinge_joint' type='revolute'> + <child>gripper_right</child> + <parent>right_hinge</parent> + <axis> + <xyz>0 1 0</xyz> + <limit> + <lower>-1.0</lower> + <upper>1.0</upper> + <effort>10</effort> + <velocity>10</velocity> + </limit> + <dynamics> + <damping>0.01</damping> + <friction>0</friction> + <spring_reference>0</spring_reference> + <spring_stiffness>0</spring_stiffness> + </dynamics> + <use_parent_model_frame>0</use_parent_model_frame> + </axis> + </joint> + + <link name='finger_right'> + <pose frame=''>0.062 0 0.145 0 0 1.5708</pose> + <inertial> + <mass>0.2</mass> + <inertia> + <ixx>0.1</ixx> + <ixy>0</ixy> + <ixz>0</ixz> + <iyy>0.1</iyy> + <iyz>0</iyz> + <izz>0.1</izz> + </inertia> + </inertial> + + <collision name='finger_right_collision'> + <pose frame=''>0 0 0.042 0 0 0 </pose> + <geometry> + <box> + <size>0.02 0.02 0.15 </size> + </box> + + </geometry> + </collision> + + <visual name='finger_right_visual'> + <pose frame=''>0 0 0 0 0 0 </pose> + <geometry> + <mesh> + <scale>1 1 1 </scale> + <uri>meshes/l_gripper_tip_scaled.stl</uri> + </mesh> + </geometry> + </visual> + </link> + + <joint name='gripper_finger_right' type='fixed'> + <parent>gripper_right</parent> + <child>finger_right</child> + </joint> + + <link name='finger_left'> + <pose frame=''>-0.062 0 0.145 0 0 4.71239</pose> + <inertial> + <mass>0.2</mass> + <inertia> + <ixx>0.1</ixx> + <ixy>0</ixy> + <ixz>0</ixz> + <iyy>0.1</iyy> + <iyz>0</iyz> + <izz>0.1</izz> + </inertia> + </inertial> + + <collision name='finger_left_collision'> + <pose frame=''>0 0 0.042 0 0 0 </pose> + <geometry> + <box> + <size>0.02 0.02 0.15 </size> + </box> + + </geometry> + </collision> + + <visual name='finger_left_visual'> + <pose frame=''>0 0 0 0 0 0 </pose> + <geometry> + <mesh> + <scale>1 1 1 </scale> + <uri>meshes/l_gripper_tip_scaled.stl</uri> + </mesh> + </geometry> + </visual> + </link> + + <joint name='gripper_finger_left' type='fixed'> + <parent>gripper_left</parent> + <child>finger_left</child> + </joint> + </model> +</world> +</sdf> 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 <string> #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;i<numJoints;i++) + { + b3JointInfo jointInfo; + m_robotSim.getJointInfo(m_gripperIndex,i,&jointInfo); + b3Printf("joint[%d].m_jointName=%s",i,jointInfo.m_jointName); + } + + for (int i=0;i<8;i++) + { + b3JointMotorArgs controlArgs(CONTROL_MODE_VELOCITY); + controlArgs.m_maxTorqueValue = 0.0; + m_robotSim.setJointMotorControl(m_gripperIndex,i,controlArgs); + } + + } + } + + if (1) + { + b3RobotSimLoadFileArgs args(""); + args.m_fileName = "plane.urdf"; + args.m_startPosition.setValue(0,0,0); + args.m_startOrientation.setEulerZYX(0,0,0); + args.m_forceOverrideFixedBase = true; + args.m_useMultiBody = true; + b3RobotSimLoadFileResults results; + m_robotSim.loadFile(args,results); + + } + m_robotSim.setGravity(b3MakeVector3(0,0,-10)); + + b3JointInfo sliderJoint1; + sliderJoint1.m_parentFrame[0] = 0; + sliderJoint1.m_parentFrame[1] = 0; + sliderJoint1.m_parentFrame[2] = 0.06; + sliderJoint1.m_parentFrame[3] = 0; + sliderJoint1.m_parentFrame[4] = 0; + sliderJoint1.m_parentFrame[5] = 0; + sliderJoint1.m_parentFrame[6] = 1.0; + sliderJoint1.m_childFrame[0] = 0; + sliderJoint1.m_childFrame[1] = 0; + sliderJoint1.m_childFrame[2] = 0; + sliderJoint1.m_childFrame[3] = 0; + sliderJoint1.m_childFrame[4] = 0; + sliderJoint1.m_childFrame[5] = 0; + sliderJoint1.m_childFrame[6] = 1.0; + sliderJoint1.m_jointAxis[0] = 1.0; + sliderJoint1.m_jointAxis[1] = 0.0; + sliderJoint1.m_jointAxis[2] = 0.0; + sliderJoint1.m_jointType = ePrismaticType; + b3JointInfo sliderJoint2; + sliderJoint2.m_parentFrame[0] = 0; + sliderJoint2.m_parentFrame[1] = 0; + sliderJoint2.m_parentFrame[2] = 0.06; + sliderJoint2.m_parentFrame[3] = 0; + sliderJoint2.m_parentFrame[4] = 0; + sliderJoint2.m_parentFrame[5] = 0; + sliderJoint2.m_parentFrame[6] = 1.0; + sliderJoint2.m_childFrame[0] = 0; + sliderJoint2.m_childFrame[1] = 0; + sliderJoint2.m_childFrame[2] = 0; + sliderJoint2.m_childFrame[3] = 0; + sliderJoint2.m_childFrame[4] = 0; + sliderJoint2.m_childFrame[5] = 1.0; + sliderJoint2.m_childFrame[6] = 0; + sliderJoint2.m_jointAxis[0] = 1.0; + sliderJoint2.m_jointAxis[1] = 0.0; + sliderJoint2.m_jointAxis[2] = 0.0; + sliderJoint2.m_jointType = ePrismaticType; + m_robotSim.createJoint(1, 0, 1, 3, &sliderJoint1); + m_robotSim.createJoint(1, 0, 1, 6, &sliderJoint2); + } } virtual void exitPhysics() @@ -241,6 +356,22 @@ public: } } + if ((m_options & eONE_MOTOR_GRASP)!=0) + { + int fingerJointIndices[2]={0,1}; + double fingerTargetVelocities[2]={sGripperVerticalVelocity,sGripperClosingTargetVelocity + }; + double maxTorqueValues[2]={50.0,50.0}; + for (int i=0;i<2;i++) + { + b3JointMotorArgs controlArgs(CONTROL_MODE_VELOCITY); + controlArgs.m_targetVelocity = fingerTargetVelocities[i]; + controlArgs.m_maxTorqueValue = maxTorqueValues[i]; + controlArgs.m_kd = 1.; + m_robotSim.setJointMotorControl(m_gripperIndex,fingerJointIndices[i],controlArgs); + } + } + m_robotSim.stepSimulation(); } virtual void renderScene() diff --git a/examples/RoboticsLearning/GripperGraspExample.h b/examples/RoboticsLearning/GripperGraspExample.h index 94835878c..37177823f 100644 --- a/examples/RoboticsLearning/GripperGraspExample.h +++ b/examples/RoboticsLearning/GripperGraspExample.h @@ -20,6 +20,7 @@ enum GripperGraspExampleOptions { eGRIPPER_GRASP=1, eTWO_POINT_GRASP=2, + eONE_MOTOR_GRASP=4, }; class CommonExampleInterface* GripperGraspExampleCreateFunc(struct CommonExampleOptions& options); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 85839e322..9217ac88c 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -2502,34 +2502,34 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm btMatrix3x3 frameInParent(btQuaternion(clientCmd.m_createJointArguments.m_parentFrame[3], clientCmd.m_createJointArguments.m_parentFrame[4], clientCmd.m_createJointArguments.m_parentFrame[5], clientCmd.m_createJointArguments.m_parentFrame[6])); btMatrix3x3 frameInChild(btQuaternion(clientCmd.m_createJointArguments.m_childFrame[3], clientCmd.m_createJointArguments.m_childFrame[4], clientCmd.m_createJointArguments.m_childFrame[5], clientCmd.m_createJointArguments.m_childFrame[6])); btVector3 jointAxis(clientCmd.m_createJointArguments.m_jointAxis[0], clientCmd.m_createJointArguments.m_jointAxis[1], clientCmd.m_createJointArguments.m_jointAxis[2]); - if (clientCmd.m_createJointArguments.m_jointType == btMultibodyLink::eFixed) + if (clientCmd.m_createJointArguments.m_jointType == eFixedType) { if (childBody->m_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 |