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/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp index c5901ca34..977687da3 100644 --- a/examples/ExampleBrowser/ExampleEntries.cpp +++ b/examples/ExampleBrowser/ExampleEntries.cpp @@ -249,6 +249,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), diff --git a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp index e06d5f205..68626554c 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/R2D2GraspExample.cpp b/examples/RoboticsLearning/R2D2GraspExample.cpp index 4da5cc22a..f1fc2cbfd 100644 --- a/examples/RoboticsLearning/R2D2GraspExample.cpp +++ b/examples/RoboticsLearning/R2D2GraspExample.cpp @@ -9,11 +9,15 @@ #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 { @@ -22,7 +26,8 @@ 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; @@ -39,6 +44,7 @@ public: m_guiHelper(helper), m_options(options), m_r2d2Index(-1), + m_gripperIndex(-1), m_x(0), m_y(0), m_z(0) @@ -61,36 +67,9 @@ public: b3Printf("robotSim connected = %d",connected); - if ((m_options & eROBOTIC_LEARN_GRASP)!=0) + if (m_options == eROBOTIC_LEARN_GRASP) { - { - 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() @@ -179,9 +290,9 @@ public: virtual void resetCamera() { - float dist = 3; - float pitch = -75; - float yaw = 30; + 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()) { diff --git a/examples/RoboticsLearning/R2D2GraspExample.h b/examples/RoboticsLearning/R2D2GraspExample.h index 353f684e5..10d419269 100644 --- a/examples/RoboticsLearning/R2D2GraspExample.h +++ b/examples/RoboticsLearning/R2D2GraspExample.h @@ -20,6 +20,7 @@ enum RobotLearningExampleOptions { eROBOTIC_LEARN_GRASP=1, eROBOTIC_LEARN_COMPLIANT_CONTACT=2, + eROBOTIC_LEARN_GRASP_CONTACT=3, }; class CommonExampleInterface* R2D2GraspExampleCreateFunc(struct CommonExampleOptions& options); 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; }