diff options
Diffstat (limited to 'examples/RoboticsLearning')
-rw-r--r-- | examples/RoboticsLearning/GripperGraspExample.cpp | 131 | ||||
-rw-r--r-- | examples/RoboticsLearning/GripperGraspExample.h | 1 |
2 files changed, 132 insertions, 0 deletions
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); |