diff --git a/data/cube_small.urdf b/data/cube_small.urdf new file mode 100644 index 000000000..804f7f0b1 --- /dev/null +++ b/data/cube_small.urdf @@ -0,0 +1,33 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/data/gripper/meshes/GUIDE_WSG50_110.stl b/data/gripper/meshes/GUIDE_WSG50_110.stl new file mode 100644 index 000000000..a7b584732 Binary files /dev/null and b/data/gripper/meshes/GUIDE_WSG50_110.stl differ diff --git a/data/gripper/meshes/WSG-FMF.stl b/data/gripper/meshes/WSG-FMF.stl new file mode 100644 index 000000000..f61a9cf90 Binary files /dev/null and b/data/gripper/meshes/WSG-FMF.stl differ diff --git a/data/gripper/meshes/WSG50_110.stl b/data/gripper/meshes/WSG50_110.stl new file mode 100644 index 000000000..fee723bf8 Binary files /dev/null and b/data/gripper/meshes/WSG50_110.stl differ diff --git a/data/gripper/meshes/l_gripper_tip_scaled.stl b/data/gripper/meshes/l_gripper_tip_scaled.stl new file mode 100644 index 000000000..94fb02462 Binary files /dev/null and b/data/gripper/meshes/l_gripper_tip_scaled.stl differ diff --git a/data/gripper/wsg50_with_r2d2_gripper.sdf b/data/gripper/wsg50_with_r2d2_gripper.sdf new file mode 100644 index 000000000..b022faf00 --- /dev/null +++ b/data/gripper/wsg50_with_r2d2_gripper.sdf @@ -0,0 +1,298 @@ + + + + + 0 0 0.27 3.14 0 0 + + + + + + world + base_link + + 0 0 1 + + -0.5 + 10 + 1 + 1 + + + 100 + 100 + 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 + + + 0.2 0.05 0.05 + + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/WSG50_110.stl + + + + + + + 1 + + 0 + + + + -0.055 0 0 0 -0 0 + + 0 0 0.0115 0 -0 0 + 0.2 + + 0.1 + 0 + 0 + 0.1 + 0 + 0.1 + + + + + 0 0 0 0 -0 0 + + + 0.001 0.001 0.001 + meshes/GUIDE_WSG50_110.stl + + + + + 0 0 0.023 0 -0 0 + + + 0.001 0.001 0.001 + meshes/WSG-FMF.stl + + + + + + 0 0 0 0 -0 0 + + + 0.001 0.001 0.001 + meshes/GUIDE_WSG50_110.stl + + + + + 0 0 0.023 0 -0 0 + + + 0.001 0.001 0.001 + meshes/WSG-FMF.stl + + + + + + + gripper_left + base_link + + 1 0 0 + + -0.001 + 0.055 + 1 + 1 + + + 100 + 100 + 0 + 0 + + + + + + 0.055 0 0 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 0 -0 0 + + + 0.001 0.001 0.001 + meshes/GUIDE_WSG50_110.stl + + + + + 0 0 0.023 0 -0 0 + + + 0.001 0.001 0.001 + meshes/WSG-FMF.stl + + + + + + 0 0 0 0 -0 0 + + + 0.001 0.001 0.001 + meshes/GUIDE_WSG50_110.stl + + + + + 0 0 0.023 0 -0 0 + + + 0.001 0.001 0.001 + meshes/WSG-FMF.stl + + + + + + + gripper_right + base_link + + -1 0 0 + + -0.055 + 0.001 + 1 + 1 + + + 100 + 100 + 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/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 bf4ac2233..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,6 +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", GripperGraspExampleCreateFunc), diff --git a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp index 44d0baf53..577d24c85 100644 --- a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp +++ b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp @@ -519,6 +519,8 @@ void MyStatusBarError(const char* msg) gui2->textOutput(msg); gui2->forceUpdateScrollBars(); } + btAssert(0); + } struct MyMenuItemHander :public Gwen::Event::Handler diff --git a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp index a64bf44f5..597f1a4d9 100644 --- a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp +++ b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp @@ -232,6 +232,13 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat if (mass) { compoundShape->calculateLocalInertia(mass, localInertiaDiagonal); + URDFLinkContactInfo contactInfo; + u2b.getLinkContactInfo(urdfLinkIndex,contactInfo); + //temporary inertia scaling until we load inertia from URDF + if (contactInfo.m_flags & URDF_CONTACT_HAS_INERTIA_SCALING) + { + localInertiaDiagonal*=contactInfo.m_inertiaScaling; + } } btRigidBody* linkRigidBody = 0; diff --git a/examples/Importers/ImportURDFDemo/URDFJointTypes.h b/examples/Importers/ImportURDFDemo/URDFJointTypes.h index 65d615ee3..2a6bdfef4 100644 --- a/examples/Importers/ImportURDFDemo/URDFJointTypes.h +++ b/examples/Importers/ImportURDFDemo/URDFJointTypes.h @@ -17,7 +17,8 @@ enum URDF_LinkContactFlags { URDF_CONTACT_HAS_LATERAL_FRICTION=1, URDF_CONTACT_HAS_ROLLING_FRICTION=2, - URDF_CONTACT_HAS_CONTACT_CFM=4, + URDF_CONTACT_HAS_INERTIA_SCALING=2, + URDF_CONTACT_HAS_CONTACT_CFM=4, URDF_CONTACT_HAS_CONTACT_ERP=8 }; @@ -25,6 +26,7 @@ struct URDFLinkContactInfo { btScalar m_lateralFriction; btScalar m_rollingFriction; + btScalar m_inertiaScaling; btScalar m_contactCfm; btScalar m_contactErp; int m_flags; @@ -32,6 +34,7 @@ struct URDFLinkContactInfo URDFLinkContactInfo() :m_lateralFriction(0.5), m_rollingFriction(0), + m_inertiaScaling(1), m_contactCfm(0), m_contactErp(0) { diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.cpp b/examples/Importers/ImportURDFDemo/UrdfParser.cpp index a6a5e256e..3cccbfd35 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.cpp +++ b/examples/Importers/ImportURDFDemo/UrdfParser.cpp @@ -606,6 +606,28 @@ bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *confi TiXmlElement* ci = config->FirstChildElement("contact"); if (ci) { + + TiXmlElement *damping_xml = ci->FirstChildElement("inertia_scaling"); + if (damping_xml) + { + if (m_parseSDF) + { + link.m_contactInfo.m_inertiaScaling = urdfLexicalCast(damping_xml->GetText()); + link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_INERTIA_SCALING; + } else + { + if (!damping_xml->Attribute("value")) + { + logger->reportError("Link/contact: damping element must have value attribute"); + return false; + } + + link.m_contactInfo.m_inertiaScaling = urdfLexicalCast(damping_xml->Attribute("value")); + link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_INERTIA_SCALING; + + } + } + { TiXmlElement *friction_xml = ci->FirstChildElement("lateral_friction"); if (friction_xml) { @@ -623,6 +645,7 @@ bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *confi link.m_contactInfo.m_lateralFriction = urdfLexicalCast(friction_xml->Attribute("value")); } } + } } } 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/b3RobotSimAPI.cpp b/examples/RoboticsLearning/b3RobotSimAPI.cpp index b29e0e429..ce9f032b7 100644 --- a/examples/RoboticsLearning/b3RobotSimAPI.cpp +++ b/examples/RoboticsLearning/b3RobotSimAPI.cpp @@ -560,6 +560,7 @@ void b3RobotSimAPI::setJointMotorControl(int bodyUniqueId, int jointIndex, const b3JointInfo jointInfo; b3GetJointInfo(m_data->m_physicsClient, bodyUniqueId, jointIndex, &jointInfo); int uIndex = jointInfo.m_uIndex; + b3JointControlSetKd(command,uIndex,args.m_kd); b3JointControlSetDesiredVelocity(command,uIndex,args.m_targetVelocity); b3JointControlSetMaximumForce(command,uIndex,args.m_maxTorqueValue); statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClient, command); @@ -627,6 +628,7 @@ bool b3RobotSimAPI::loadFile(const struct b3RobotSimLoadFileArgs& args, b3RobotS } statusHandle = submitClientCommandAndWaitStatusMultiThreaded(m_data->m_physicsClient, command); statusType = b3GetStatusType(statusHandle); + b3Assert(statusType==CMD_URDF_LOADING_COMPLETED); robotUniqueId = b3GetStatusBodyIndex(statusHandle); results.m_uniqueObjectIds.push_back(robotUniqueId); diff --git a/examples/RoboticsLearning/b3RobotSimAPI.h b/examples/RoboticsLearning/b3RobotSimAPI.h index 98c7e1fa0..3666700d2 100644 --- a/examples/RoboticsLearning/b3RobotSimAPI.h +++ b/examples/RoboticsLearning/b3RobotSimAPI.h @@ -61,7 +61,7 @@ struct b3JointMotorArgs m_targetPosition(0), m_kp(0.1), m_targetVelocity(0), - m_kd(0.1), + m_kd(0.9), m_maxTorqueValue(1000) { } diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 420fe37b6..469c178f8 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -1216,16 +1216,21 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } bool completedOk = loadSdf(sdfArgs.m_sdfFileName,bufferServerToClient, bufferSizeInBytes); - - //serverStatusOut.m_type = CMD_SDF_LOADING_FAILED; - serverStatusOut.m_sdfLoadedArgs.m_numBodies = m_data->m_sdfRecentLoadedBodies.size(); - int maxBodies = btMin(MAX_SDF_BODIES, serverStatusOut.m_sdfLoadedArgs.m_numBodies); - for (int i=0;im_sdfRecentLoadedBodies[i]; - } + //serverStatusOut.m_type = CMD_SDF_LOADING_FAILED; + serverStatusOut.m_sdfLoadedArgs.m_numBodies = m_data->m_sdfRecentLoadedBodies.size(); + int maxBodies = btMin(MAX_SDF_BODIES, serverStatusOut.m_sdfLoadedArgs.m_numBodies); + for (int i=0;im_sdfRecentLoadedBodies[i]; + } - serverStatusOut.m_type = CMD_SDF_LOADING_COMPLETED; + serverStatusOut.m_type = CMD_SDF_LOADING_COMPLETED; + } else + { + serverStatusOut.m_type = CMD_SDF_LOADING_FAILED; + } hasStatus = true; break; }