From 8270096fad4edbcee9e07742ba239cb271bbac09 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Mon, 25 Jul 2016 12:30:47 -0700 Subject: [PATCH] add GripperGraspExample, separate from R2D2GraspExample --- examples/ExampleBrowser/CMakeLists.txt | 2 + examples/ExampleBrowser/ExampleEntries.cpp | 3 +- .../RoboticsLearning/GripperGraspExample.cpp | 231 +++++++++++++++++ .../RoboticsLearning/GripperGraspExample.h | 23 ++ .../RoboticsLearning/R2D2GraspExample.cpp | 235 +++++------------- examples/RoboticsLearning/R2D2GraspExample.h | 1 - 6 files changed, 320 insertions(+), 175 deletions(-) create mode 100644 examples/RoboticsLearning/GripperGraspExample.cpp create mode 100644 examples/RoboticsLearning/GripperGraspExample.h diff --git a/examples/ExampleBrowser/CMakeLists.txt b/examples/ExampleBrowser/CMakeLists.txt index 0678eaf4a..38e4175b3 100644 --- a/examples/ExampleBrowser/CMakeLists.txt +++ b/examples/ExampleBrowser/CMakeLists.txt @@ -210,6 +210,8 @@ SET(BulletExampleBrowser_SRCS ../RenderingExamples/TimeSeriesCanvas.h ../RenderingExamples/TimeSeriesFontData.cpp ../RenderingExamples/TimeSeriesFontData.h + ../RoboticsLearning/GripperGraspExample.cpp + ../RoboticsLearning/GripperGraspExample.h ../RoboticsLearning/b3RobotSimAPI.cpp ../RoboticsLearning/b3RobotSimAPI.h ../RoboticsLearning/R2D2GraspExample.cpp diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp index cac015d61..60c62c43a 100644 --- a/examples/ExampleBrowser/ExampleEntries.cpp +++ b/examples/ExampleBrowser/ExampleEntries.cpp @@ -46,6 +46,7 @@ #include "../MultiThreading/MultiThreadingExample.h" #include "../InverseDynamics/InverseDynamicsExample.h" #include "../RoboticsLearning/R2D2GraspExample.h" +#include "../RoboticsLearning/GripperGraspExample.h" #include "../InverseKinematics/InverseKinematicsExample.h" #ifdef ENABLE_LUA @@ -260,7 +261,7 @@ static ExampleEntry gDefaultExamples[]= ExampleEntry(1,"R2D2 Grasp","Load the R2D2 robot from URDF file and control it to grasp objects", R2D2GraspExampleCreateFunc, eROBOTIC_LEARN_GRASP), ExampleEntry(1,"URDF Compliant Contact","Work-in-progress, experiment/improve compliant rigid contact using parameters from URDF file (contact_cfm, contact_erp, lateral_friction, rolling_friction)", R2D2GraspExampleCreateFunc,eROBOTIC_LEARN_COMPLIANT_CONTACT), - ExampleEntry(1,"Contact for Grasp","Grasp experiment to improve contact model", R2D2GraspExampleCreateFunc,eROBOTIC_LEARN_GRASP_CONTACT), + ExampleEntry(1,"Contact for Grasp","Grasp experiment to improve contact model", GripperGraspExampleCreateFunc), diff --git a/examples/RoboticsLearning/GripperGraspExample.cpp b/examples/RoboticsLearning/GripperGraspExample.cpp new file mode 100644 index 000000000..ebdc39acb --- /dev/null +++ b/examples/RoboticsLearning/GripperGraspExample.cpp @@ -0,0 +1,231 @@ + +#include "GripperGraspExample.h" + +#include "../CommonInterfaces/CommonGraphicsAppInterface.h" +#include "Bullet3Common/b3Quaternion.h" +#include "Bullet3Common/b3AlignedObjectArray.h" +#include "../CommonInterfaces/CommonRenderInterface.h" +#include "../CommonInterfaces/CommonExampleInterface.h" +#include "../CommonInterfaces/CommonGUIHelperInterface.h" +#include "../SharedMemory/PhysicsServerSharedMemory.h" +#include "../SharedMemory/PhysicsClientC_API.h" +#include "../CommonInterfaces/CommonParameterInterface.h" +#include + +#include "b3RobotSimAPI.h" +#include "../Utils/b3Clock.h" + +static btScalar sGripperVerticalVelocity = -0.2f; +static btScalar sGripperClosingTargetVelocity = 0.5f; + +class GripperGraspExample : public CommonExampleInterface +{ + CommonGraphicsApp* m_app; + GUIHelperInterface* m_guiHelper; + b3RobotSimAPI m_robotSim; + int m_options; + int m_r2d2Index; + int m_gripperIndex; + + float m_x; + float m_y; + float m_z; + b3AlignedObjectArray m_movingInstances; + enum + { + numCubesX = 20, + numCubesY = 20 + }; +public: + + GripperGraspExample(GUIHelperInterface* helper, int options) + :m_app(helper->getAppInterface()), + m_guiHelper(helper), + m_options(options), + m_r2d2Index(-1), + m_gripperIndex(-1), + m_x(0), + m_y(0), + m_z(0) + { + m_app->setUpAxis(2); + } + virtual ~GripperGraspExample() + { + m_app->m_renderer->enableBlend(false); + } + + + virtual void physicsDebugDraw(int debugDrawMode) + { + + } + virtual void initPhysics() + { + bool connected = m_robotSim.connect(m_guiHelper); + b3Printf("robotSim connected = %d",connected); + + { + + { + 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(""); + args.m_fileName = "gripper/wsg50_with_r2d2_gripper.sdf"; + args.m_fileType = B3_SDF_FILE; + 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=0)) + { + int fingerJointIndices[3]={0,1,3}; + double fingerTargetVelocities[3]={sGripperVerticalVelocity,sGripperClosingTargetVelocity + ,-sGripperClosingTargetVelocity + }; + double maxTorqueValues[3]={40.0,50.0,50.0}; + for (int i=0;i<3;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() + { + m_robotSim.renderScene(); + + //m_app->m_renderer->renderScene(); + } + + + virtual void physicsDebugDraw() + { + + } + virtual bool mouseMoveCallback(float x,float y) + { + return false; + } + virtual bool mouseButtonCallback(int button, int state, float x, float y) + { + return false; + } + virtual bool keyboardCallback(int key, int state) + { + return false; + } + + virtual void resetCamera() + { + float dist = 1.5; + float pitch = 12; + float yaw = -10; + float targetPos[3]={-0.2,0.8,0.3}; + if (m_app->m_renderer && m_app->m_renderer->getActiveCamera()) + { + m_app->m_renderer->getActiveCamera()->setCameraDistance(dist); + m_app->m_renderer->getActiveCamera()->setCameraPitch(pitch); + m_app->m_renderer->getActiveCamera()->setCameraYaw(yaw); + m_app->m_renderer->getActiveCamera()->setCameraTargetPosition(targetPos[0],targetPos[1],targetPos[2]); + } + } + +}; + + +class CommonExampleInterface* GripperGraspExampleCreateFunc(struct CommonExampleOptions& options) +{ + return new GripperGraspExample(options.m_guiHelper, options.m_option); +} + + + diff --git a/examples/RoboticsLearning/GripperGraspExample.h b/examples/RoboticsLearning/GripperGraspExample.h new file mode 100644 index 000000000..1088efffb --- /dev/null +++ b/examples/RoboticsLearning/GripperGraspExample.h @@ -0,0 +1,23 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2016 Google Inc. http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef GRIPPER_GRASP_EXAMPLE_H +#define GRIPPER_GRASP_EXAMPLE_H + + +class CommonExampleInterface* GripperGraspExampleCreateFunc(struct CommonExampleOptions& options); + + +#endif //GRIPPER_GRASP_EXAMPLE_H diff --git a/examples/RoboticsLearning/R2D2GraspExample.cpp b/examples/RoboticsLearning/R2D2GraspExample.cpp index f1fc2cbfd..4da5cc22a 100644 --- a/examples/RoboticsLearning/R2D2GraspExample.cpp +++ b/examples/RoboticsLearning/R2D2GraspExample.cpp @@ -9,15 +9,11 @@ #include "../CommonInterfaces/CommonGUIHelperInterface.h" #include "../SharedMemory/PhysicsServerSharedMemory.h" #include "../SharedMemory/PhysicsClientC_API.h" -#include "../CommonInterfaces/CommonParameterInterface.h" #include #include "b3RobotSimAPI.h" #include "../Utils/b3Clock.h" -static btScalar sGripperVerticalVelocity = -0.2f; -static btScalar sGripperClosingTargetVelocity = 0.5f; - ///quick demo showing the right-handed coordinate system and positive rotations around each axis class R2D2GraspExample : public CommonExampleInterface { @@ -26,8 +22,7 @@ class R2D2GraspExample : public CommonExampleInterface b3RobotSimAPI m_robotSim; int m_options; int m_r2d2Index; - int m_gripperIndex; - + float m_x; float m_y; float m_z; @@ -44,7 +39,6 @@ public: m_guiHelper(helper), m_options(options), m_r2d2Index(-1), - m_gripperIndex(-1), m_x(0), m_y(0), m_z(0) @@ -67,9 +61,36 @@ public: b3Printf("robotSim connected = %d",connected); - if (m_options == eROBOTIC_LEARN_GRASP) + if ((m_options & eROBOTIC_LEARN_GRASP)!=0) { - + { + b3RobotSimLoadFileArgs args(""); + args.m_fileName = "r2d2.urdf"; + args.m_startPosition.setValue(0,0,.5); + b3RobotSimLoadFileResults results; + if (m_robotSim.loadFile(args, results) && results.m_uniqueObjectIds.size()==1) + { + int m_r2d2Index = results.m_uniqueObjectIds[0]; + int numJoints = m_robotSim.getNumJoints(m_r2d2Index); + b3Printf("numJoints = %d",numJoints); + + for (int i=0;igetParameterInterface()->registerSliderFloatParameter(slider); - } - - { - SliderParams slider("Closing velocity",&sGripperClosingTargetVelocity - ); - slider.m_minVal=-1; - slider.m_maxVal=1; - m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); - } - - - { - b3RobotSimLoadFileArgs args(""); - args.m_fileName = "gripper/wsg50_with_r2d2_gripper.sdf"; - args.m_fileType = B3_SDF_FILE; - 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=0)) - { - int fingerJointIndices[3]={0,1,3}; - double fingerTargetVelocities[3]={sGripperVerticalVelocity,sGripperClosingTargetVelocity - ,-sGripperClosingTargetVelocity - }; - double maxTorqueValues[3]={40.0,50.0,50.0}; - for (int i=0;i<3;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() @@ -290,9 +179,9 @@ public: virtual void resetCamera() { - float dist = 1.5; - float pitch = 12; - float yaw = -10; + float dist = 3; + float pitch = -75; + float yaw = 30; float targetPos[3]={-0.2,0.8,0.3}; if (m_app->m_renderer && m_app->m_renderer->getActiveCamera()) { diff --git a/examples/RoboticsLearning/R2D2GraspExample.h b/examples/RoboticsLearning/R2D2GraspExample.h index 10d419269..353f684e5 100644 --- a/examples/RoboticsLearning/R2D2GraspExample.h +++ b/examples/RoboticsLearning/R2D2GraspExample.h @@ -20,7 +20,6 @@ enum RobotLearningExampleOptions { eROBOTIC_LEARN_GRASP=1, eROBOTIC_LEARN_COMPLIANT_CONTACT=2, - eROBOTIC_LEARN_GRASP_CONTACT=3, }; class CommonExampleInterface* R2D2GraspExampleCreateFunc(struct CommonExampleOptions& options);