From 900fd86d58087dfe7bd913aee707562699f7eab9 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Mon, 29 Aug 2016 12:35:02 -0700 Subject: [PATCH 1/9] fix R2D2GraspExample --- data/kiva_shelf/model.sdf | 4 ++-- examples/RoboticsLearning/R2D2GraspExample.cpp | 6 ++---- examples/RoboticsLearning/b3RobotSimAPI.h | 1 + 3 files changed, 5 insertions(+), 6 deletions(-) diff --git a/data/kiva_shelf/model.sdf b/data/kiva_shelf/model.sdf index b1f49e66b..3e506a9b0 100644 --- a/data/kiva_shelf/model.sdf +++ b/data/kiva_shelf/model.sdf @@ -2,10 +2,10 @@ 1 - 0 2 1.21 0 0 0 + 0 2 0 0 0 0 - 0.0 0.0 1.2045 0 0 0 + 0.0 .0 1.2045 0 0 0 76.26 47 diff --git a/examples/RoboticsLearning/R2D2GraspExample.cpp b/examples/RoboticsLearning/R2D2GraspExample.cpp index 4da5cc22a..a83d4976f 100644 --- a/examples/RoboticsLearning/R2D2GraspExample.cpp +++ b/examples/RoboticsLearning/R2D2GraspExample.cpp @@ -94,11 +94,9 @@ public: { b3RobotSimLoadFileArgs args(""); args.m_fileName = "kiva_shelf/model.sdf"; - args.m_startPosition.setValue(0,0,.5); - args.m_startOrientation = b3Quaternion(0,B3_HALF_PI,0); - args.m_forceOverrideFixedBase = true; + args.m_forceOverrideFixedBase = true; args.m_fileType = B3_SDF_FILE; - //args.m_startOrientation = b3Quaternion() + args.m_startOrientation = b3Quaternion(0,0,0,1); b3RobotSimLoadFileResults results; m_robotSim.loadFile(args,results); } diff --git a/examples/RoboticsLearning/b3RobotSimAPI.h b/examples/RoboticsLearning/b3RobotSimAPI.h index 88506e871..5070b0155 100644 --- a/examples/RoboticsLearning/b3RobotSimAPI.h +++ b/examples/RoboticsLearning/b3RobotSimAPI.h @@ -32,6 +32,7 @@ struct b3RobotSimLoadFileArgs m_startPosition(b3MakeVector3(0,0,0)), m_startOrientation(b3Quaternion(0,0,0,1)), m_forceOverrideFixedBase(false), + m_useMultiBody(true), m_fileType(B3_URDF_FILE) { } From f86ce4d44b9a8dd01e906699d450b7b129464932 Mon Sep 17 00:00:00 2001 From: Logan Su Date: Mon, 29 Aug 2016 15:09:18 -0700 Subject: [PATCH 2/9] Add flags to load inertia values from URDF. --- .../ImportSDFDemo/ImportSDFSetup.cpp | 2 +- .../Importers/ImportURDFDemo/URDF2Bullet.cpp | 26 ++++++++++++++----- .../Importers/ImportURDFDemo/URDF2Bullet.h | 15 +++++++---- .../PhysicsServerCommandProcessor.cpp | 2 +- 4 files changed, 31 insertions(+), 14 deletions(-) diff --git a/examples/Importers/ImportSDFDemo/ImportSDFSetup.cpp b/examples/Importers/ImportSDFDemo/ImportSDFSetup.cpp index f55cc9758..6e8437950 100644 --- a/examples/Importers/ImportSDFDemo/ImportSDFSetup.cpp +++ b/examples/Importers/ImportSDFDemo/ImportSDFSetup.cpp @@ -232,7 +232,7 @@ void ImportSDFSetup::initPhysics() MyMultiBodyCreator creation(m_guiHelper); u2b.getRootTransformInWorld(rootTrans); - ConvertURDF2Bullet(u2b,creation, rootTrans,m_dynamicsWorld,m_useMultiBody,u2b.getPathPrefix(),true); + ConvertURDF2Bullet(u2b,creation, rootTrans,m_dynamicsWorld,m_useMultiBody,u2b.getPathPrefix(),CUF_USE_SDF); mb = creation.getBulletMultiBody(); diff --git a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp index 8503dd41d..c3aaee49b 100644 --- a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp +++ b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp @@ -1,4 +1,3 @@ -#include "URDFImporterInterface.h" #include #include "LinearMath/btTransform.h" #include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h" @@ -7,6 +6,7 @@ #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" #include "BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h" #include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h" +#include "URDF2Bullet.h" #include "URDFImporterInterface.h" #include "MultiBodyCreationInterface.h" #include @@ -143,7 +143,12 @@ void InitURDF2BulletCache(const URDFImporterInterface& u2b, URDF2BulletCachedDat } -void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreationInterface& creation, URDF2BulletCachedData& cache, int urdfLinkIndex, const btTransform& parentTransformInWorldSpace, btMultiBodyDynamicsWorld* world1,bool createMultiBody, const char* pathPrefix, bool useSDF = false) +void ConvertURDF2BulletInternal( + const URDFImporterInterface& u2b, MultiBodyCreationInterface& creation, + URDF2BulletCachedData& cache, int urdfLinkIndex, + const btTransform& parentTransformInWorldSpace, btMultiBodyDynamicsWorld* world1, + bool createMultiBody, const char* pathPrefix, + int flags = 0) { //b3Printf("start converting/extracting data from URDF interface\n"); @@ -200,7 +205,7 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat bool hasParentJoint = u2b.getJointInfo(urdfLinkIndex, parent2joint, linkTransformInWorldSpace, jointAxisInJointSpace, jointType,jointLowerLimit,jointUpperLimit, jointDamping, jointFriction); - if (useSDF) + if (flags & CUF_USE_SDF) { parent2joint =parentTransformInWorldSpace.inverse()*linkTransformInWorldSpace; } @@ -231,7 +236,10 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat */ if (mass) { - compoundShape->calculateLocalInertia(mass, localInertiaDiagonal); + if (!(flags & CUF_USE_URDF_INERTIA)) + { + compoundShape->calculateLocalInertia(mass, localInertiaDiagonal); + } URDFLinkContactInfo contactInfo; u2b.getLinkContactInfo(urdfLinkIndex,contactInfo); //temporary inertia scaling until we load inertia from URDF @@ -436,18 +444,22 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat { int urdfChildLinkIndex = urdfChildIndices[i]; - ConvertURDF2BulletInternal(u2b,creation, cache,urdfChildLinkIndex,linkTransformInWorldSpace,world1,createMultiBody,pathPrefix,useSDF); + ConvertURDF2BulletInternal(u2b,creation, cache,urdfChildLinkIndex,linkTransformInWorldSpace,world1,createMultiBody,pathPrefix,flags); } } -void ConvertURDF2Bullet(const URDFImporterInterface& u2b, MultiBodyCreationInterface& creation, const btTransform& rootTransformInWorldSpace, btMultiBodyDynamicsWorld* world1,bool createMultiBody, const char* pathPrefix, bool useSDF = false) +void ConvertURDF2Bullet( + const URDFImporterInterface& u2b, MultiBodyCreationInterface& creation, + const btTransform& rootTransformInWorldSpace, + btMultiBodyDynamicsWorld* world1, + bool createMultiBody, const char* pathPrefix, int flags) { URDF2BulletCachedData cache; InitURDF2BulletCache(u2b,cache); int urdfLinkIndex = u2b.getRootLinkIndex(); - ConvertURDF2BulletInternal(u2b, creation, cache, urdfLinkIndex,rootTransformInWorldSpace,world1,createMultiBody,pathPrefix,useSDF); + ConvertURDF2BulletInternal(u2b, creation, cache, urdfLinkIndex,rootTransformInWorldSpace,world1,createMultiBody,pathPrefix,flags); if (world1 && cache.m_bulletMultiBody) { diff --git a/examples/Importers/ImportURDFDemo/URDF2Bullet.h b/examples/Importers/ImportURDFDemo/URDF2Bullet.h index 148535812..dcf05c6a0 100644 --- a/examples/Importers/ImportURDFDemo/URDF2Bullet.h +++ b/examples/Importers/ImportURDFDemo/URDF2Bullet.h @@ -14,14 +14,19 @@ class MultiBodyCreationInterface; +enum ConvertURDFFlags { + CUF_USE_SDF = 1, + // Use inertia values in URDF instead of recomputing them from collision shape. + CUF_USE_URDF_INERTIA = 2 +}; -void ConvertURDF2Bullet(const URDFImporterInterface& u2b, - MultiBodyCreationInterface& creationCallback, - const btTransform& rootTransformInWorldSpace, +void ConvertURDF2Bullet(const URDFImporterInterface& u2b, + MultiBodyCreationInterface& creationCallback, + const btTransform& rootTransformInWorldSpace, btMultiBodyDynamicsWorld* world, - bool createMultiBody, + bool createMultiBody, const char* pathPrefix, - bool useSDF = false); + int flags = 0); #endif //_URDF2BULLET_H diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 3c30f30df..84d4554ca 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -776,7 +776,7 @@ bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferSe MyMultiBodyCreator creation(m_data->m_guiHelper); u2b.getRootTransformInWorld(rootTrans); - ConvertURDF2Bullet(u2b,creation, rootTrans,m_data->m_dynamicsWorld,useMultiBody,u2b.getPathPrefix(),true); + ConvertURDF2Bullet(u2b,creation, rootTrans,m_data->m_dynamicsWorld,useMultiBody,u2b.getPathPrefix(),CUF_USE_SDF); From d784c61b6184c01621c1934ceaef061944dd39fa Mon Sep 17 00:00:00 2001 From: yunfeibai Date: Tue, 30 Aug 2016 11:19:23 -0700 Subject: [PATCH 3/9] Add rolling friction, set rolling friction coefficient from urdf, and set up two point contact experiment. --- data/cube_gripper_left.urdf | 45 +++ data/cube_gripper_right.urdf | 45 +++ data/cube_small.urdf | 2 +- examples/ExampleBrowser/ExampleEntries.cpp | 3 +- .../Importers/ImportURDFDemo/UrdfParser.cpp | 21 ++ .../RoboticsLearning/GripperGraspExample.cpp | 95 ++++- .../RoboticsLearning/GripperGraspExample.h | 5 + .../btMultiBodyConstraintSolver.cpp | 334 +++++++++++++++++- .../btMultiBodyConstraintSolver.h | 10 +- 9 files changed, 536 insertions(+), 24 deletions(-) create mode 100644 data/cube_gripper_left.urdf create mode 100644 data/cube_gripper_right.urdf diff --git a/data/cube_gripper_left.urdf b/data/cube_gripper_left.urdf new file mode 100644 index 000000000..08951a2b9 --- /dev/null +++ b/data/cube_gripper_left.urdf @@ -0,0 +1,45 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/data/cube_gripper_right.urdf b/data/cube_gripper_right.urdf new file mode 100644 index 000000000..a70bcfef2 --- /dev/null +++ b/data/cube_gripper_right.urdf @@ -0,0 +1,45 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/data/cube_small.urdf b/data/cube_small.urdf index 804f7f0b1..e94692b50 100644 --- a/data/cube_small.urdf +++ b/data/cube_small.urdf @@ -3,7 +3,7 @@ - + diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp index 60c62c43a..a9fcd4bcf 100644 --- a/examples/ExampleBrowser/ExampleEntries.cpp +++ b/examples/ExampleBrowser/ExampleEntries.cpp @@ -261,7 +261,8 @@ 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), + 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), diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.cpp b/examples/Importers/ImportURDFDemo/UrdfParser.cpp index 3cccbfd35..bb0af5f79 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.cpp +++ b/examples/Importers/ImportURDFDemo/UrdfParser.cpp @@ -646,6 +646,27 @@ bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *confi } } } + + TiXmlElement *rolling_xml = ci->FirstChildElement("rolling_friction"); + if (rolling_xml) + { + if (m_parseSDF) + { + link.m_contactInfo.m_rollingFriction = urdfLexicalCast(rolling_xml->GetText()); + link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_ROLLING_FRICTION; + } else + { + if (!rolling_xml->Attribute("value")) + { + logger->reportError("Link/contact: rolling friction element must have value attribute"); + return false; + } + + link.m_contactInfo.m_rollingFriction = urdfLexicalCast(rolling_xml->Attribute("value")); + link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_ROLLING_FRICTION; + + } + } } } diff --git a/examples/RoboticsLearning/GripperGraspExample.cpp b/examples/RoboticsLearning/GripperGraspExample.cpp index a6086ada9..46db7d59c 100644 --- a/examples/RoboticsLearning/GripperGraspExample.cpp +++ b/examples/RoboticsLearning/GripperGraspExample.cpp @@ -65,6 +65,7 @@ public: bool connected = m_robotSim.connect(m_guiHelper); b3Printf("robotSim connected = %d",connected); + if ((m_options & eGRIPPER_GRASP)!=0) { { @@ -87,7 +88,7 @@ public: args.m_fileName = "cube_small.urdf"; args.m_startPosition.setValue(0, 0, .107); args.m_startOrientation.setEulerZYX(0, 0, 0); - args.m_useMultiBody = false; + args.m_useMultiBody = true; m_robotSim.loadFile(args, results); } @@ -152,10 +153,70 @@ public: } m_robotSim.setGravity(b3MakeVector3(0,0,-10)); - m_robotSim.setNumSimulationSubSteps(4); + //m_robotSim.setNumSimulationSubSteps(4); } - + if ((m_options & eTWO_POINT_GRASP)!=0) + { + { + 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(""); + b3RobotSimLoadFileResults results; + args.m_fileName = "cube_gripper_left.urdf"; + //args.m_startPosition.setValue(0.0675, 0.02, 0.13); + args.m_startPosition.setValue(0.068, 0.02, 0.11); + //args.m_startOrientation.setEulerZYX(0, 0.785398, 0.785398); + args.m_useMultiBody = true; + m_robotSim.loadFile(args, results); + + b3JointMotorArgs controlArgs(CONTROL_MODE_VELOCITY); + controlArgs.m_targetVelocity = -0.5; + controlArgs.m_maxTorqueValue = 50.0; + controlArgs.m_kd = 1.; + m_robotSim.setJointMotorControl(1,0,controlArgs); + } + + { + b3RobotSimLoadFileArgs args(""); + b3RobotSimLoadFileResults results; + args.m_fileName = "cube_gripper_right.urdf"; + //args.m_startPosition.setValue(-0.0675, 0.02, 0.13); + args.m_startPosition.setValue(-0.068, 0.02, 0.11); + //args.m_startOrientation.setEulerZYX(0, -0.785398, -0.785398); + args.m_useMultiBody = true; + m_robotSim.loadFile(args, results); + + b3JointMotorArgs controlArgs(CONTROL_MODE_VELOCITY); + controlArgs.m_targetVelocity = 0.5; + controlArgs.m_maxTorqueValue = 50.0; + controlArgs.m_kd = 1.; + m_robotSim.setJointMotorControl(2,0,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)); + m_robotSim.setNumSimulationSubSteps(4); + } } virtual void exitPhysics() @@ -164,22 +225,26 @@ public: } virtual void stepSimulation(float deltaTime) { - if ((m_gripperIndex>=0)) + if ((m_options & eGRIPPER_GRASP)!=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++) + if ((m_gripperIndex>=0)) { - 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); + 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() diff --git a/examples/RoboticsLearning/GripperGraspExample.h b/examples/RoboticsLearning/GripperGraspExample.h index 1088efffb..94835878c 100644 --- a/examples/RoboticsLearning/GripperGraspExample.h +++ b/examples/RoboticsLearning/GripperGraspExample.h @@ -16,6 +16,11 @@ subject to the following restrictions: #ifndef GRIPPER_GRASP_EXAMPLE_H #define GRIPPER_GRASP_EXAMPLE_H +enum GripperGraspExampleOptions +{ + eGRIPPER_GRASP=1, + eTWO_POINT_GRASP=2, +}; class CommonExampleInterface* GripperGraspExampleCreateFunc(struct CommonExampleOptions& options); diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp index 08411f40b..b8c2ab011 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp @@ -530,8 +530,285 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol } - - +void btMultiBodyConstraintSolver::setupMultiBodyRollingFrictionConstraint(btMultiBodySolverConstraint& solverConstraint, + const btVector3& constraintNormal, + btManifoldPoint& cp, const btContactSolverInfo& infoGlobal, + btScalar& relaxation, + bool isFriction, btScalar desiredVelocity, btScalar cfmSlip) +{ + + BT_PROFILE("setupMultiBodyRollingFrictionConstraint"); + btVector3 rel_pos1; + btVector3 rel_pos2; + + btMultiBody* multiBodyA = solverConstraint.m_multiBodyA; + btMultiBody* multiBodyB = solverConstraint.m_multiBodyB; + + const btVector3& pos1 = cp.getPositionWorldOnA(); + const btVector3& pos2 = cp.getPositionWorldOnB(); + + btSolverBody* bodyA = multiBodyA ? 0 : &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA]; + btSolverBody* bodyB = multiBodyB ? 0 : &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB]; + + btRigidBody* rb0 = multiBodyA ? 0 : bodyA->m_originalBody; + btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody; + + if (bodyA) + rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin(); + if (bodyB) + rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin(); + + relaxation = infoGlobal.m_sor; + + btScalar invTimeStep = btScalar(1)/infoGlobal.m_timeStep; + btScalar cfm = (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_CFM)?cp.m_contactCFM:infoGlobal.m_globalCfm; + cfm *= invTimeStep; + + btScalar erp = (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_ERP)?cp.m_contactERP:infoGlobal.m_erp2; + + + + + if (multiBodyA) + { + if (solverConstraint.m_linkA<0) + { + rel_pos1 = pos1 - multiBodyA->getBasePos(); + } else + { + rel_pos1 = pos1 - multiBodyA->getLink(solverConstraint.m_linkA).m_cachedWorldTransform.getOrigin(); + } + const int ndofA = multiBodyA->getNumDofs() + 6; + + solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId(); + + if (solverConstraint.m_deltaVelAindex <0) + { + solverConstraint.m_deltaVelAindex = m_data.m_deltaVelocities.size(); + multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex); + m_data.m_deltaVelocities.resize(m_data.m_deltaVelocities.size()+ndofA); + } else + { + btAssert(m_data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex+ndofA); + } + + solverConstraint.m_jacAindex = m_data.m_jacobians.size(); + m_data.m_jacobians.resize(m_data.m_jacobians.size()+ndofA); + m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size()+ndofA); + btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.size()); + + btScalar* jac1=&m_data.m_jacobians[solverConstraint.m_jacAindex]; + multiBodyA->fillContactJacobianMultiDof(solverConstraint.m_linkA, cp.getPositionWorldOnA(), constraintNormal, jac1, m_data.scratch_r, m_data.scratch_v, m_data.scratch_m); + btScalar* delta = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; + multiBodyA->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacAindex],delta,m_data.scratch_r, m_data.scratch_v); + + btVector3 torqueAxis0 = constraintNormal; + solverConstraint.m_relpos1CrossNormal = torqueAxis0; + solverConstraint.m_contactNormal1 = constraintNormal; + } else + { + btVector3 torqueAxis0 = constraintNormal; + solverConstraint.m_relpos1CrossNormal = torqueAxis0; + solverConstraint.m_contactNormal1 = constraintNormal; + solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0); + } + + + + if (multiBodyB) + { + if (solverConstraint.m_linkB<0) + { + rel_pos2 = pos2 - multiBodyB->getBasePos(); + } else + { + rel_pos2 = pos2 - multiBodyB->getLink(solverConstraint.m_linkB).m_cachedWorldTransform.getOrigin(); + } + + const int ndofB = multiBodyB->getNumDofs() + 6; + + solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId(); + if (solverConstraint.m_deltaVelBindex <0) + { + solverConstraint.m_deltaVelBindex = m_data.m_deltaVelocities.size(); + multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex); + m_data.m_deltaVelocities.resize(m_data.m_deltaVelocities.size()+ndofB); + } + + solverConstraint.m_jacBindex = m_data.m_jacobians.size(); + + m_data.m_jacobians.resize(m_data.m_jacobians.size()+ndofB); + m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size()+ndofB); + btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.size()); + + multiBodyB->fillContactJacobianMultiDof(solverConstraint.m_linkB, cp.getPositionWorldOnB(), -constraintNormal, &m_data.m_jacobians[solverConstraint.m_jacBindex], m_data.scratch_r, m_data.scratch_v, m_data.scratch_m); + multiBodyB->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacBindex],&m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],m_data.scratch_r, m_data.scratch_v); + + btVector3 torqueAxis1 = constraintNormal; + solverConstraint.m_relpos2CrossNormal = -torqueAxis1; + solverConstraint.m_contactNormal2 = -constraintNormal; + + } else + { + btVector3 torqueAxis1 = constraintNormal; + solverConstraint.m_relpos2CrossNormal = -torqueAxis1; + solverConstraint.m_contactNormal2 = -constraintNormal; + + solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0); + } + + { + + btVector3 vec; + btScalar denom0 = 0.f; + btScalar denom1 = 0.f; + btScalar* jacB = 0; + btScalar* jacA = 0; + btScalar* lambdaA =0; + btScalar* lambdaB =0; + int ndofA = 0; + if (multiBodyA) + { + ndofA = multiBodyA->getNumDofs() + 6; + jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex]; + lambdaA = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; + for (int i = 0; i < ndofA; ++i) + { + btScalar j = jacA[i] ; + btScalar l =lambdaA[i]; + denom0 += j*l; + } + } else + { + if (rb0) + { + vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1); + denom0 = rb0->getInvMass() + constraintNormal.dot(vec); + } + } + if (multiBodyB) + { + const int ndofB = multiBodyB->getNumDofs() + 6; + jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex]; + lambdaB = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; + for (int i = 0; i < ndofB; ++i) + { + btScalar j = jacB[i] ; + btScalar l =lambdaB[i]; + denom1 += j*l; + } + + } else + { + if (rb1) + { + vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2); + denom1 = rb1->getInvMass() + constraintNormal.dot(vec); + } + } + + + + btScalar d = denom0+denom1+cfm; + if (d>SIMD_EPSILON) + { + solverConstraint.m_jacDiagABInv = relaxation/(d); + } else + { + //disable the constraint row to handle singularity/redundant constraint + solverConstraint.m_jacDiagABInv = 0.f; + } + + } + + + //compute rhs and remaining solverConstraint fields + + + + btScalar restitution = 0.f; + btScalar penetration = isFriction? 0 : cp.getDistance()+infoGlobal.m_linearSlop; + + btScalar rel_vel = 0.f; + int ndofA = 0; + int ndofB = 0; + { + + btVector3 vel1,vel2; + if (multiBodyA) + { + ndofA = multiBodyA->getNumDofs() + 6; + btScalar* jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex]; + for (int i = 0; i < ndofA ; ++i) + rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i]; + } else + { + if (rb0) + { + rel_vel += rb0->getVelocityInLocalPoint(rel_pos1).dot(solverConstraint.m_contactNormal1); + } + } + if (multiBodyB) + { + ndofB = multiBodyB->getNumDofs() + 6; + btScalar* jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex]; + for (int i = 0; i < ndofB ; ++i) + rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i]; + + } else + { + if (rb1) + { + rel_vel += rb1->getVelocityInLocalPoint(rel_pos2).dot(solverConstraint.m_contactNormal2); + } + } + + solverConstraint.m_friction = cp.m_combinedRollingFriction; + + if(!isFriction) + { + restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution); + if (restitution <= btScalar(0.)) + { + restitution = 0.f; + } + } + } + + + solverConstraint.m_appliedImpulse = 0.f; + solverConstraint.m_appliedPushImpulse = 0.f; + + { + + btScalar positionalError = 0.f; + btScalar velocityError = restitution - rel_vel;// * damping; //note for friction restitution is always set to 0 (check above) so it is acutally velocityError = -rel_vel for friction + + if (penetration>0) + { + positionalError = 0; + velocityError -= penetration / infoGlobal.m_timeStep; + + } else + { + positionalError = -penetration * erp/infoGlobal.m_timeStep; + } + + btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv; + btScalar velocityImpulse = velocityError*solverConstraint.m_jacDiagABInv; + + solverConstraint.m_rhs = velocityImpulse; + solverConstraint.m_rhsPenetration = 0.f; + solverConstraint.m_lowerLimit = -solverConstraint.m_friction; + solverConstraint.m_upperLimit = solverConstraint.m_friction; + + solverConstraint.m_cfm = cfm*solverConstraint.m_jacDiagABInv; + + + + } + +} btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodyFrictionConstraint(const btVector3& normalAxis,btPersistentManifold* manifold,int frictionIndex,btManifoldPoint& cp,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip) { @@ -568,6 +845,41 @@ btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodyFrictionCo return solverConstraint; } +btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodyRollingFrictionConstraint(const btVector3& normalAxis,btPersistentManifold* manifold,int frictionIndex,btManifoldPoint& cp,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip) +{ + BT_PROFILE("addMultiBodyRollingFrictionConstraint"); + btMultiBodySolverConstraint& solverConstraint = m_multiBodyFrictionContactConstraints.expandNonInitializing(); + solverConstraint.m_orgConstraint = 0; + solverConstraint.m_orgDofIndex = -1; + + solverConstraint.m_frictionIndex = frictionIndex; + bool isFriction = true; + + const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0()); + const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1()); + + btMultiBody* mbA = fcA? fcA->m_multiBody : 0; + btMultiBody* mbB = fcB? fcB->m_multiBody : 0; + + int solverBodyIdA = mbA? -1 : getOrInitSolverBody(*colObj0,infoGlobal.m_timeStep); + int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1,infoGlobal.m_timeStep); + + solverConstraint.m_solverBodyIdA = solverBodyIdA; + solverConstraint.m_solverBodyIdB = solverBodyIdB; + solverConstraint.m_multiBodyA = mbA; + if (mbA) + solverConstraint.m_linkA = fcA->m_link; + + solverConstraint.m_multiBodyB = mbB; + if (mbB) + solverConstraint.m_linkB = fcB->m_link; + + solverConstraint.m_originalContactPoint = &cp; + + setupMultiBodyRollingFrictionConstraint(solverConstraint, normalAxis, cp, infoGlobal,relaxation,isFriction, desiredVelocity, cfmSlip); + return solverConstraint; +} + void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal) { const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0()); @@ -592,8 +904,9 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold* // if (!solverBodyA || (solverBodyA->m_invMass.isZero() && (!solverBodyB || solverBodyB->m_invMass.isZero()))) // return; - - + //only a single rollingFriction per manifold + int rollingFriction=1; + for (int j=0;jgetNumContacts();j++) { @@ -675,9 +988,9 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold* #endif //ROLLING_FRICTION ///Bullet has several options to set the friction directions - ///By default, each contact has only a single friction direction that is recomputed automatically very frame + ///By default, each contact has only a single friction direction that is recomputed automatically every frame ///based on the relative linear velocity. - ///If the relative velocity it zero, it will automatically compute a friction direction. + ///If the relative velocity is zero, it will automatically compute a friction direction. ///You can also enable two friction directions, using the SOLVER_USE_2_FRICTION_DIRECTIONS. ///In that case, the second friction direction will be orthogonal to both contact normal and first friction direction. @@ -718,6 +1031,15 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold* applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION); applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION); addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal); + + if (rollingFriction > 0) + { + addMultiBodyRollingFrictionConstraint(cp.m_normalWorldOnB,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal); + addMultiBodyRollingFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal); + addMultiBodyRollingFrictionConstraint(cp.m_lateralFrictionDir2,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal); + + rollingFriction--; + } if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) { diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h index 321ee4231..ee5cda954 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h @@ -47,8 +47,10 @@ protected: void convertContacts(btPersistentManifold** manifoldPtr,int numManifolds, const btContactSolverInfo& infoGlobal); - btMultiBodySolverConstraint& addMultiBodyFrictionConstraint(const btVector3& normalAxis,btPersistentManifold* manifold,int frictionIndex,btManifoldPoint& cp,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity=0, btScalar cfmSlip=0); + + btMultiBodySolverConstraint& addMultiBodyFrictionConstraint(const btVector3& normalAxis,btPersistentManifold* manifold,int frictionIndex,btManifoldPoint& cp,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity=0, btScalar cfmSlip=0); + btMultiBodySolverConstraint& addMultiBodyRollingFrictionConstraint(const btVector3& normalAxis,btPersistentManifold* manifold,int frictionIndex,btManifoldPoint& cp,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity=0, btScalar cfmSlip=0); void setupMultiBodyJointLimitConstraint(btMultiBodySolverConstraint& constraintRow, btScalar* jacA,btScalar* jacB, @@ -60,6 +62,12 @@ protected: btManifoldPoint& cp, const btContactSolverInfo& infoGlobal, btScalar& relaxation, bool isFriction, btScalar desiredVelocity=0, btScalar cfmSlip=0); + + void setupMultiBodyRollingFrictionConstraint(btMultiBodySolverConstraint& solverConstraint, + const btVector3& contactNormal, + btManifoldPoint& cp, const btContactSolverInfo& infoGlobal, + btScalar& relaxation, + bool isFriction, btScalar desiredVelocity=0, btScalar cfmSlip=0); void convertMultiBodyContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal); virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); From a30ff20e6b51fe908f35d2cd2a04889e17d668f0 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Tue, 30 Aug 2016 14:44:11 -0700 Subject: [PATCH 4/9] preparation for KUKA IK tracking example --- examples/ExampleBrowser/CMakeLists.txt | 4 + examples/ExampleBrowser/ExampleEntries.cpp | 2 + .../InverseKinematicsExample.cpp | 10 +- .../RoboticsLearning/IKTrajectoryHelper.cpp | 145 +++++++++++ .../RoboticsLearning/IKTrajectoryHelper.h | 30 +++ .../RoboticsLearning/KukaGraspExample.cpp | 243 ++++++++++++++++++ examples/RoboticsLearning/KukaGraspExample.h | 27 ++ examples/RoboticsLearning/b3RobotSimAPI.cpp | 54 ++++ examples/RoboticsLearning/b3RobotSimAPI.h | 14 + examples/SharedMemory/SharedMemoryPublic.h | 1 + examples/ThirdPartyLibs/BussIK/Jacobian.cpp | 14 +- examples/ThirdPartyLibs/BussIK/Jacobian.h | 6 +- 12 files changed, 535 insertions(+), 15 deletions(-) create mode 100644 examples/RoboticsLearning/IKTrajectoryHelper.cpp create mode 100644 examples/RoboticsLearning/IKTrajectoryHelper.h create mode 100644 examples/RoboticsLearning/KukaGraspExample.cpp create mode 100644 examples/RoboticsLearning/KukaGraspExample.h diff --git a/examples/ExampleBrowser/CMakeLists.txt b/examples/ExampleBrowser/CMakeLists.txt index 38e4175b3..717d57f28 100644 --- a/examples/ExampleBrowser/CMakeLists.txt +++ b/examples/ExampleBrowser/CMakeLists.txt @@ -216,6 +216,10 @@ SET(BulletExampleBrowser_SRCS ../RoboticsLearning/b3RobotSimAPI.h ../RoboticsLearning/R2D2GraspExample.cpp ../RoboticsLearning/R2D2GraspExample.h + ../RoboticsLearning/KukaGraspExample.cpp + ../RoboticsLearning/KukaGraspExample.h + ../RoboticsLearning/IKTrajectoryHelper.cpp + ../RoboticsLearning/IKTrajectoryHelper.h ../RenderingExamples/CoordinateSystemDemo.cpp ../RenderingExamples/CoordinateSystemDemo.h ../RenderingExamples/RaytracerSetup.cpp diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp index 60c62c43a..97579cbe8 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/KukaGraspExample.h" #include "../RoboticsLearning/GripperGraspExample.h" #include "../InverseKinematics/InverseKinematicsExample.h" @@ -260,6 +261,7 @@ static ExampleEntry gDefaultExamples[]= ExampleEntry(1, "Physics Client (Direct)", "Create a physics client that can communicate with a physics server directly in-process.", PhysicsClientCreateFunc,eCLIENTEXAMPLE_DIRECT), ExampleEntry(1,"R2D2 Grasp","Load the R2D2 robot from URDF file and control it to grasp objects", R2D2GraspExampleCreateFunc, eROBOTIC_LEARN_GRASP), + ExampleEntry(1,"Kuka IK","Control a Kuka IIWA robot to follow a target using IK. This IK is not setup properly yet.", KukaGraspExampleCreateFunc,0), 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/InverseKinematics/InverseKinematicsExample.cpp b/examples/InverseKinematics/InverseKinematicsExample.cpp index 4c36037a6..388826752 100644 --- a/examples/InverseKinematics/InverseKinematicsExample.cpp +++ b/examples/InverseKinematics/InverseKinematicsExample.cpp @@ -21,7 +21,7 @@ #define MAX_NUM_EFFECT 100 double T = 0; -VectorR3 target1[MAX_NUM_EFFECT]; +VectorR3 targetaa[MAX_NUM_EFFECT]; @@ -85,7 +85,7 @@ void Reset(Tree &tree, Jacobian* m_ikJacobian) void UpdateTargets( double T2, Tree & treeY) { double T = T2 / 5.; - target1[0].Set(0.6*b3Sin(0), 0.6*b3Cos(0), 0.5+0.4*b3Sin(3 * T)); + targetaa[0].Set(0.6*b3Sin(0), 0.6*b3Cos(0), 0.5+0.4*b3Sin(3 * T)); } @@ -103,7 +103,7 @@ void DoUpdateStep(double Tstep, Tree & treeY, Jacobian *jacob, int ikMethod) { else { jacob->SetJendActive(); } - jacob->ComputeJacobian(); // Set up Jacobian and deltaS vectors + jacob->ComputeJacobian(targetaa); // Set up Jacobian and deltaS vectors // Calculate the change in theta values switch (ikMethod) { @@ -129,7 +129,7 @@ void DoUpdateStep(double Tstep, Tree & treeY, Jacobian *jacob, int ikMethod) { if ( SleepCounter==0 ) { jacob->UpdateThetas(); // Apply the change in the theta values - jacob->UpdatedSClampValue(); + jacob->UpdatedSClampValue(targetaa); SleepCounter = SleepsPerStep; } else { @@ -290,7 +290,7 @@ public: getLocalTransform(m_ikTree.GetRoot(), act); MyDrawTree(m_ikTree.GetRoot(), act); - b3Vector3 pos = b3MakeVector3(target1[0].x, target1[0].y, target1[0].z); + b3Vector3 pos = b3MakeVector3(targetaa[0].x, targetaa[0].y, targetaa[0].z); b3Quaternion orn(0, 0, 0, 1); m_app->m_renderer->writeSingleInstanceTransformToCPU(pos, orn, m_targetInstance); diff --git a/examples/RoboticsLearning/IKTrajectoryHelper.cpp b/examples/RoboticsLearning/IKTrajectoryHelper.cpp new file mode 100644 index 000000000..2e97653f6 --- /dev/null +++ b/examples/RoboticsLearning/IKTrajectoryHelper.cpp @@ -0,0 +1,145 @@ +#include "IKTrajectoryHelper.h" +#include "BussIK/Node.h" +#include "BussIK/Tree.h" +#include "BussIK/Jacobian.h" +#include "BussIK/VectorRn.h" +#include "Bullet3Common/b3AlignedObjectArray.h" + + +#define RADIAN(X) ((X)*RadiansToDegrees) + + +//use BussIK and Reflexxes to convert from Cartesian endeffector future target to +//joint space positions at each real-time (simulation) step +struct IKTrajectoryHelperInternalData +{ + VectorR3 m_endEffectorTargetPosition; + + Tree m_ikTree; + b3AlignedObjectArray m_ikNodes; + Jacobian* m_ikJacobian; + + IKTrajectoryHelperInternalData() + { + m_endEffectorTargetPosition.SetZero(); + } +}; + + +IKTrajectoryHelper::IKTrajectoryHelper() +{ + m_data = new IKTrajectoryHelperInternalData; +} + +IKTrajectoryHelper::~IKTrajectoryHelper() +{ + delete m_data; +} + + +void IKTrajectoryHelper::createKukaIIWA() +{ + const VectorR3& unitx = VectorR3::UnitX; + const VectorR3& unity = VectorR3::UnitY; + const VectorR3& unitz = VectorR3::UnitZ; + const VectorR3 unit1(sqrt(14.0) / 8.0, 1.0 / 8.0, 7.0 / 8.0); + const VectorR3& zero = VectorR3::Zero; + + float minTheta = -4 * PI; + float maxTheta = 4 * PI; + + m_data->m_ikNodes.resize(8);//7DOF+additional endeffector + + m_data->m_ikNodes[0] = new Node(VectorR3(0.100000, 0.000000, 0.087500), unitz, 0.08, JOINT, -1e30, 1e30, RADIAN(0.)); + m_data->m_ikTree.InsertRoot(m_data->m_ikNodes[0]); + + m_data->m_ikNodes[1] = new Node(VectorR3(0.100000, -0.000000, 0.290000), unity, 0.08, JOINT, -0.5, 0.4, RADIAN(0.)); + m_data->m_ikTree.InsertLeftChild(m_data->m_ikNodes[0], m_data->m_ikNodes[1]); + + m_data->m_ikNodes[2] = new Node(VectorR3(0.100000, -0.000000, 0.494500), unitz, 0.08, JOINT, minTheta, maxTheta, RADIAN(0.)); + m_data->m_ikTree.InsertLeftChild(m_data->m_ikNodes[1], m_data->m_ikNodes[2]); + + m_data->m_ikNodes[3] = new Node(VectorR3(0.100000, 0.000000, 0.710000), -unity, 0.08, JOINT, minTheta, maxTheta, RADIAN(0.)); + m_data->m_ikTree.InsertLeftChild(m_data->m_ikNodes[2], m_data->m_ikNodes[3]); + + m_data->m_ikNodes[4] = new Node(VectorR3(0.100000, 0.000000, 0.894500), unitz, 0.08, JOINT, minTheta, maxTheta, RADIAN(0.)); + m_data->m_ikTree.InsertLeftChild(m_data->m_ikNodes[3], m_data->m_ikNodes[4]); + + m_data->m_ikNodes[5] = new Node(VectorR3(0.100000, 0.000000, 1.110000), unity, 0.08, JOINT, minTheta, maxTheta, RADIAN(0.)); + m_data->m_ikTree.InsertLeftChild(m_data->m_ikNodes[4], m_data->m_ikNodes[5]); + + m_data->m_ikNodes[6] = new Node(VectorR3(0.100000, 0.000000, 1.191000), unitz, 0.08, JOINT, minTheta, maxTheta, RADIAN(0.)); + m_data->m_ikTree.InsertLeftChild(m_data->m_ikNodes[5], m_data->m_ikNodes[6]); + + m_data->m_ikNodes[7] = new Node(VectorR3(0.100000, 0.000000, 1.20000), zero, 0.08, EFFECTOR); + m_data->m_ikTree.InsertLeftChild(m_data->m_ikNodes[6], m_data->m_ikNodes[7]); + + m_data->m_ikJacobian = new Jacobian(&m_data->m_ikTree); +// Reset(m_ikTree,m_ikJacobian); + + m_data->m_ikTree.Init(); + m_data->m_ikTree.Compute(); + m_data->m_ikJacobian->Reset(); + + +} + +bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3], + const double* q_current, int numQ, + double* q_new, int ikMethod) +{ + + if (numQ != 7) + { + return false; + } + + for (int i=0;im_ikNodes[i]->SetTheta(q_current[i]); + } + bool UseJacobianTargets1 = false; + + if ( UseJacobianTargets1 ) { + m_data->m_ikJacobian->SetJtargetActive(); + } + else { + m_data->m_ikJacobian->SetJendActive(); + } + VectorR3 targets; + targets.Set(endEffectorTargetPosition[0],endEffectorTargetPosition[1],endEffectorTargetPosition[2]); + m_data->m_ikJacobian->ComputeJacobian(&targets); // Set up Jacobian and deltaS vectors + + // Calculate the change in theta values + switch (ikMethod) { + case IK2_JACOB_TRANS: + m_data->m_ikJacobian->CalcDeltaThetasTranspose(); // Jacobian transpose method + break; + case IK2_DLS: + m_data->m_ikJacobian->CalcDeltaThetasDLS(); // Damped least squares method + break; + case IK2_DLS_SVD: + m_data->m_ikJacobian->CalcDeltaThetasDLSwithSVD(); + break; + case IK2_PURE_PSEUDO: + m_data->m_ikJacobian->CalcDeltaThetasPseudoinverse(); // Pure pseudoinverse method + break; + case IK2_SDLS: + m_data->m_ikJacobian->CalcDeltaThetasSDLS(); // Selectively damped least squares method + break; + default: + m_data->m_ikJacobian->ZeroDeltaThetas(); + break; + } + + m_data->m_ikJacobian->UpdateThetas(); + + // Apply the change in the theta values + m_data->m_ikJacobian->UpdatedSClampValue(&targets); + + for (int i=0;im_ikNodes[i]->GetTheta(); + } + return true; +} \ No newline at end of file diff --git a/examples/RoboticsLearning/IKTrajectoryHelper.h b/examples/RoboticsLearning/IKTrajectoryHelper.h new file mode 100644 index 000000000..5b436057f --- /dev/null +++ b/examples/RoboticsLearning/IKTrajectoryHelper.h @@ -0,0 +1,30 @@ +#ifndef IK_TRAJECTORY_HELPER_H +#define IK_TRAJECTORY_HELPER_H + +enum IK2_Method +{ + IK2_JACOB_TRANS=0, + IK2_PURE_PSEUDO, + IK2_DLS, + IK2_SDLS , + IK2_DLS_SVD +}; + + +class IKTrajectoryHelper +{ + struct IKTrajectoryHelperInternalData* m_data; + +public: + IKTrajectoryHelper(); + virtual ~IKTrajectoryHelper(); + + ///todo: replace createKukaIIWA with a generic way of create an IK tree + void createKukaIIWA(); + + bool computeIK(const double endEffectorTargetPosition[3], + const double* q_old, int numQ, + double* q_new, int ikMethod); + +}; +#endif //IK_TRAJECTORY_HELPER_H diff --git a/examples/RoboticsLearning/KukaGraspExample.cpp b/examples/RoboticsLearning/KukaGraspExample.cpp new file mode 100644 index 000000000..52cf68160 --- /dev/null +++ b/examples/RoboticsLearning/KukaGraspExample.cpp @@ -0,0 +1,243 @@ + +#include "KukaGraspExample.h" +#include "IKTrajectoryHelper.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 + +#include "b3RobotSimAPI.h" +#include "../Utils/b3Clock.h" + +///quick demo showing the right-handed coordinate system and positive rotations around each axis +class KukaGraspExample : public CommonExampleInterface +{ + CommonGraphicsApp* m_app; + GUIHelperInterface* m_guiHelper; + b3RobotSimAPI m_robotSim; + int m_kukaIndex; + + IKTrajectoryHelper m_ikHelper; + int m_targetSphereInstance; + b3Vector3 m_targetPos; + double m_time; + int m_options; + + b3AlignedObjectArray m_movingInstances; + enum + { + numCubesX = 20, + numCubesY = 20 + }; +public: + + KukaGraspExample(GUIHelperInterface* helper, int options) + :m_app(helper->getAppInterface()), + m_guiHelper(helper), + m_options(options), + m_kukaIndex(-1), + m_time(0) + { + m_targetPos.setValue(0.5,0,1); + m_app->setUpAxis(2); + } + virtual ~KukaGraspExample() + { + m_app->m_renderer->enableBlend(false); + } + + + virtual void physicsDebugDraw(int debugDrawMode) + { + + } + virtual void initPhysics() + { + + ///create some graphics proxy for the tracking target + ///the endeffector tries to track it using Inverse Kinematics + { + int sphereId = m_app->registerGraphicsUnitSphereShape(SPHERE_LOD_MEDIUM); + b3Quaternion orn(0, 0, 0, 1); + b3Vector4 color = b3MakeVector4(1., 0.3, 0.3, 1); + b3Vector3 scaling = b3MakeVector3(.02, .02, .02); + m_targetSphereInstance = m_app->m_renderer->registerGraphicsInstance(sphereId, m_targetPos, orn, color, scaling); + } + m_app->m_renderer->writeTransforms(); + + + + + m_ikHelper.createKukaIIWA(); + + bool connected = m_robotSim.connect(m_guiHelper); + b3Printf("robotSim connected = %d",connected); + + + { + b3RobotSimLoadFileArgs args(""); + args.m_fileName = "kuka_iiwa/model.urdf"; + args.m_startPosition.setValue(0,0,0); + b3RobotSimLoadFileResults results; + if (m_robotSim.loadFile(args, results) && results.m_uniqueObjectIds.size()==1) + { + m_kukaIndex = results.m_uniqueObjectIds[0]; + int numJoints = m_robotSim.getNumJoints(m_kukaIndex); + b3Printf("numJoints = %d",numJoints); + + for (int i=0;im_renderer->writeSingleInstanceTransformToCPU(m_targetPos, orn, m_targetSphereInstance); + m_app->m_renderer->writeTransforms(); + + //draw the end-effector target sphere + + //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 = 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()) + { + 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* KukaGraspExampleCreateFunc(struct CommonExampleOptions& options) +{ + return new KukaGraspExample(options.m_guiHelper, options.m_option); +} + + + diff --git a/examples/RoboticsLearning/KukaGraspExample.h b/examples/RoboticsLearning/KukaGraspExample.h new file mode 100644 index 000000000..8275adba8 --- /dev/null +++ b/examples/RoboticsLearning/KukaGraspExample.h @@ -0,0 +1,27 @@ +/* +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 KUKA_GRASP_EXAMPLE_H +#define KUKA_GRASP_EXAMPLE_H + +enum KukaGraspExampleOptions +{ + eKUKA_GRASP_DLS_IK=1, +}; + +class CommonExampleInterface* KukaGraspExampleCreateFunc(struct CommonExampleOptions& options); + + +#endif //KUKA_GRASP_EXAMPLE_H diff --git a/examples/RoboticsLearning/b3RobotSimAPI.cpp b/examples/RoboticsLearning/b3RobotSimAPI.cpp index 686eeecdf..e7daae828 100644 --- a/examples/RoboticsLearning/b3RobotSimAPI.cpp +++ b/examples/RoboticsLearning/b3RobotSimAPI.cpp @@ -615,6 +615,60 @@ void b3RobotSimAPI::createJoint(int parentBodyIndex, int parentJointIndex, int c } } + +bool b3RobotSimAPI::getJointStates(int bodyUniqueId, b3JointStates& state) +{ + b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(m_data->m_physicsClient,bodyUniqueId); + b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClient, command); + + if (statusHandle) + { + double rootInertialFrame[7]; + const double* rootLocalInertialFrame; + const double* actualStateQ; + const double* actualStateQdot; + const double* jointReactionForces; + + int stat = b3GetStatusActualState(statusHandle, + &state.m_bodyUniqueId, + &state.m_numDegreeOfFreedomQ, + &state.m_numDegreeOfFreedomU, + &rootLocalInertialFrame, + &actualStateQ, + &actualStateQdot, + &jointReactionForces); + if (stat) + { + state.m_actualStateQ.resize(state.m_numDegreeOfFreedomQ); + state.m_actualStateQdot.resize(state.m_numDegreeOfFreedomU); + + for (int i=0;i @@ -17,6 +18,17 @@ enum b3RigidSimFileType B3_AUTO_DETECT_FILE//todo based on extension }; +struct b3JointStates +{ + int m_bodyUniqueId; + int m_numDegreeOfFreedomQ; + int m_numDegreeOfFreedomU; + b3Transform m_rootLocalInertialFrame; + b3AlignedObjectArray m_actualStateQ; + b3AlignedObjectArray m_actualStateQdot; + b3AlignedObjectArray m_jointReactionForces; +}; + struct b3RobotSimLoadFileArgs { std::string m_fileName; @@ -91,6 +103,8 @@ public: void createJoint(int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, b3JointInfo* jointInfo); + bool getJointStates(int bodyUniqueId, b3JointStates& state); + void setJointMotorControl(int bodyUniqueId, int jointIndex, const struct b3JointMotorArgs& args); void stepSimulation(); diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 87efcb7dd..9b50e4d17 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -28,6 +28,7 @@ enum EnumSharedMemoryClientCommand CMD_REQUEST_CAMERA_IMAGE_DATA, CMD_APPLY_EXTERNAL_FORCE, CMD_CALCULATE_INVERSE_DYNAMICS, + CMD_CALCULATE_INVERSE_KINEMATICS, CMD_MAX_CLIENT_COMMANDS, CMD_CREATE_JOINT }; diff --git a/examples/ThirdPartyLibs/BussIK/Jacobian.cpp b/examples/ThirdPartyLibs/BussIK/Jacobian.cpp index 2b0454ea8..bd8f49e67 100644 --- a/examples/ThirdPartyLibs/BussIK/Jacobian.cpp +++ b/examples/ThirdPartyLibs/BussIK/Jacobian.cpp @@ -36,7 +36,7 @@ using namespace std; void Arrow(const VectorR3& tail, const VectorR3& head); //extern RestPositionOn; -extern VectorR3 target1[]; +//extern VectorR3 target1[]; // Optimal damping values have to be determined in an ad hoc manner (Yuck!) const double Jacobian::DefaultDampingLambda = 0.6; // Optimal for the "Y" shape (any lower gives jitter) @@ -96,7 +96,7 @@ void Jacobian::Reset() // Compute the deltaS vector, dS, (the error in end effector positions // Compute the J and K matrices (the Jacobians) -void Jacobian::ComputeJacobian() +void Jacobian::ComputeJacobian(VectorR3* targets) { // Traverse tree to find all end effectors VectorR3 temp; @@ -104,7 +104,7 @@ void Jacobian::ComputeJacobian() while ( n ) { if ( n->IsEffector() ) { int i = n->GetEffectorNum(); - const VectorR3& targetPos = target1[i]; + const VectorR3& targetPos = targets[i]; // Compute the delta S value (differences from end effectors to target positions. temp = targetPos; @@ -418,7 +418,7 @@ void Jacobian::CalcdTClampedFromdS() } } -double Jacobian::UpdateErrorArray() +double Jacobian::UpdateErrorArray(VectorR3* targets) { double totalError = 0.0; @@ -428,7 +428,7 @@ double Jacobian::UpdateErrorArray() while ( n ) { if ( n->IsEffector() ) { int i = n->GetEffectorNum(); - const VectorR3& targetPos = target1[i]; + const VectorR3& targetPos = targets[i]; temp = targetPos; temp -= n->GetS(); double err = temp.Norm(); @@ -440,7 +440,7 @@ double Jacobian::UpdateErrorArray() return totalError; } -void Jacobian::UpdatedSClampValue() +void Jacobian::UpdatedSClampValue(VectorR3* targets) { // Traverse tree to find all end effectors VectorR3 temp; @@ -448,7 +448,7 @@ void Jacobian::UpdatedSClampValue() while ( n ) { if ( n->IsEffector() ) { int i = n->GetEffectorNum(); - const VectorR3& targetPos = target1[i]; + const VectorR3& targetPos = targets[i]; // Compute the delta S value (differences from end effectors to target positions. // While we are at it, also update the clamping values in dSclamp; diff --git a/examples/ThirdPartyLibs/BussIK/Jacobian.h b/examples/ThirdPartyLibs/BussIK/Jacobian.h index b1c683f8a..502ba0413 100644 --- a/examples/ThirdPartyLibs/BussIK/Jacobian.h +++ b/examples/ThirdPartyLibs/BussIK/Jacobian.h @@ -55,7 +55,7 @@ class Jacobian { public: Jacobian(Tree*); - void ComputeJacobian(); + void ComputeJacobian(VectorR3* targets); const MatrixRmn& ActiveJacobian() const { return *Jactive; } void SetJendActive() { Jactive = &Jend; } // The default setting is Jend. void SetJtargetActive() { Jactive = &Jtarget; } @@ -69,9 +69,9 @@ public: void CalcDeltaThetasSDLS(); void UpdateThetas(); - double UpdateErrorArray(); // Returns sum of errors + double UpdateErrorArray(VectorR3* targets); // Returns sum of errors const VectorRn& GetErrorArray() const { return errorArray; } - void UpdatedSClampValue(); + void UpdatedSClampValue(VectorR3* targets); void SetCurrentMode( UpdateMode mode ) { CurrentUpdateMode = mode; } UpdateMode GetCurrentMode() const { return CurrentUpdateMode; } From 9c124b5896b3e86eb38d3b2b47ebaf6d3210100a Mon Sep 17 00:00:00 2001 From: yunfeibai Date: Tue, 30 Aug 2016 17:50:37 -0700 Subject: [PATCH 5/9] Rolling friction demo for sphere and torsional friction demo for two point contact. --- data/cube_gripper_left.urdf | 2 +- data/cube_gripper_right.urdf | 2 +- data/cube_small.urdf | 2 +- data/plane.urdf | 3 ++ data/sphere2_rolling_friction.urdf | 29 +++++++++++++++++ examples/ExampleBrowser/ExampleEntries.cpp | 1 + .../RoboticsLearning/GripperGraspExample.cpp | 12 +++---- .../RoboticsLearning/R2D2GraspExample.cpp | 31 +++++++++++++++++++ examples/RoboticsLearning/R2D2GraspExample.h | 1 + .../btMultiBodyConstraintSolver.cpp | 14 ++++----- 10 files changed, 79 insertions(+), 18 deletions(-) create mode 100644 data/sphere2_rolling_friction.urdf diff --git a/data/cube_gripper_left.urdf b/data/cube_gripper_left.urdf index 08951a2b9..f46f69bcf 100644 --- a/data/cube_gripper_left.urdf +++ b/data/cube_gripper_left.urdf @@ -6,7 +6,7 @@ - + diff --git a/data/cube_gripper_right.urdf b/data/cube_gripper_right.urdf index a70bcfef2..68a202175 100644 --- a/data/cube_gripper_right.urdf +++ b/data/cube_gripper_right.urdf @@ -6,7 +6,7 @@ - + diff --git a/data/cube_small.urdf b/data/cube_small.urdf index e94692b50..23ae7fa3b 100644 --- a/data/cube_small.urdf +++ b/data/cube_small.urdf @@ -3,7 +3,7 @@ - + diff --git a/data/plane.urdf b/data/plane.urdf index 57b746104..fb7c124af 100644 --- a/data/plane.urdf +++ b/data/plane.urdf @@ -1,6 +1,9 @@ + + + diff --git a/data/sphere2_rolling_friction.urdf b/data/sphere2_rolling_friction.urdf new file mode 100644 index 000000000..e4ff66341 --- /dev/null +++ b/data/sphere2_rolling_friction.urdf @@ -0,0 +1,29 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp index a9fcd4bcf..9a98f5e0c 100644 --- a/examples/ExampleBrowser/ExampleEntries.cpp +++ b/examples/ExampleBrowser/ExampleEntries.cpp @@ -261,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,"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), diff --git a/examples/RoboticsLearning/GripperGraspExample.cpp b/examples/RoboticsLearning/GripperGraspExample.cpp index 46db7d59c..434454985 100644 --- a/examples/RoboticsLearning/GripperGraspExample.cpp +++ b/examples/RoboticsLearning/GripperGraspExample.cpp @@ -172,15 +172,13 @@ public: b3RobotSimLoadFileArgs args(""); b3RobotSimLoadFileResults results; args.m_fileName = "cube_gripper_left.urdf"; - //args.m_startPosition.setValue(0.0675, 0.02, 0.13); args.m_startPosition.setValue(0.068, 0.02, 0.11); - //args.m_startOrientation.setEulerZYX(0, 0.785398, 0.785398); args.m_useMultiBody = true; m_robotSim.loadFile(args, results); b3JointMotorArgs controlArgs(CONTROL_MODE_VELOCITY); - controlArgs.m_targetVelocity = -0.5; - controlArgs.m_maxTorqueValue = 50.0; + controlArgs.m_targetVelocity = -0.1; + controlArgs.m_maxTorqueValue = 10.0; controlArgs.m_kd = 1.; m_robotSim.setJointMotorControl(1,0,controlArgs); } @@ -189,15 +187,13 @@ public: b3RobotSimLoadFileArgs args(""); b3RobotSimLoadFileResults results; args.m_fileName = "cube_gripper_right.urdf"; - //args.m_startPosition.setValue(-0.0675, 0.02, 0.13); args.m_startPosition.setValue(-0.068, 0.02, 0.11); - //args.m_startOrientation.setEulerZYX(0, -0.785398, -0.785398); args.m_useMultiBody = true; m_robotSim.loadFile(args, results); b3JointMotorArgs controlArgs(CONTROL_MODE_VELOCITY); - controlArgs.m_targetVelocity = 0.5; - controlArgs.m_maxTorqueValue = 50.0; + controlArgs.m_targetVelocity = 0.1; + controlArgs.m_maxTorqueValue = 10.0; controlArgs.m_kd = 1.; m_robotSim.setJointMotorControl(2,0,controlArgs); } diff --git a/examples/RoboticsLearning/R2D2GraspExample.cpp b/examples/RoboticsLearning/R2D2GraspExample.cpp index 4da5cc22a..3a33ba653 100644 --- a/examples/RoboticsLearning/R2D2GraspExample.cpp +++ b/examples/RoboticsLearning/R2D2GraspExample.cpp @@ -142,6 +142,37 @@ public: } } + if ((m_options & eROBOTIC_LEARN_ROLLING_FRICTION)!=0) + { + b3RobotSimLoadFileArgs args(""); + b3RobotSimLoadFileResults results; + { + args.m_fileName = "sphere2_rolling_friction.urdf"; + args.m_startPosition.setValue(0,0,2.5); + args.m_startOrientation.setEulerZYX(0,0,0); + args.m_useMultiBody = true; + m_robotSim.loadFile(args,results); + } + { + args.m_fileName = "sphere2.urdf"; + args.m_startPosition.setValue(0,2,2.5); + args.m_startOrientation.setEulerZYX(0,0,0); + args.m_useMultiBody = true; + m_robotSim.loadFile(args,results); + } + { + b3RobotSimLoadFileArgs args(""); + args.m_fileName = "plane.urdf"; + args.m_startPosition.setValue(0,0,0); + args.m_startOrientation.setEulerZYX(0,0.2,0); + args.m_useMultiBody = true; + args.m_forceOverrideFixedBase = true; + b3RobotSimLoadFileResults results; + m_robotSim.loadFile(args,results); + m_robotSim.setGravity(b3MakeVector3(0,0,-10)); + } + } + } virtual void exitPhysics() diff --git a/examples/RoboticsLearning/R2D2GraspExample.h b/examples/RoboticsLearning/R2D2GraspExample.h index 353f684e5..1861b4607 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_ROLLING_FRICTION=4, }; class CommonExampleInterface* R2D2GraspExampleCreateFunc(struct CommonExampleOptions& options); diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp index b8c2ab011..161b1b7ed 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp @@ -598,18 +598,18 @@ void btMultiBodyConstraintSolver::setupMultiBodyRollingFrictionConstraint(btMult btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.size()); btScalar* jac1=&m_data.m_jacobians[solverConstraint.m_jacAindex]; - multiBodyA->fillContactJacobianMultiDof(solverConstraint.m_linkA, cp.getPositionWorldOnA(), constraintNormal, jac1, m_data.scratch_r, m_data.scratch_v, m_data.scratch_m); + multiBodyA->fillConstraintJacobianMultiDof(solverConstraint.m_linkA, cp.getPositionWorldOnA(), constraintNormal, btVector3(0,0,0), jac1, m_data.scratch_r, m_data.scratch_v, m_data.scratch_m); btScalar* delta = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; multiBodyA->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacAindex],delta,m_data.scratch_r, m_data.scratch_v); btVector3 torqueAxis0 = constraintNormal; solverConstraint.m_relpos1CrossNormal = torqueAxis0; - solverConstraint.m_contactNormal1 = constraintNormal; + solverConstraint.m_contactNormal1 = btVector3(0,0,0); } else { btVector3 torqueAxis0 = constraintNormal; solverConstraint.m_relpos1CrossNormal = torqueAxis0; - solverConstraint.m_contactNormal1 = constraintNormal; + solverConstraint.m_contactNormal1 = btVector3(0,0,0); solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0); } @@ -641,18 +641,18 @@ void btMultiBodyConstraintSolver::setupMultiBodyRollingFrictionConstraint(btMult m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size()+ndofB); btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.size()); - multiBodyB->fillContactJacobianMultiDof(solverConstraint.m_linkB, cp.getPositionWorldOnB(), -constraintNormal, &m_data.m_jacobians[solverConstraint.m_jacBindex], m_data.scratch_r, m_data.scratch_v, m_data.scratch_m); + multiBodyB->fillConstraintJacobianMultiDof(solverConstraint.m_linkB, cp.getPositionWorldOnB(), -constraintNormal, btVector3(0,0,0), &m_data.m_jacobians[solverConstraint.m_jacBindex], m_data.scratch_r, m_data.scratch_v, m_data.scratch_m); multiBodyB->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacBindex],&m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],m_data.scratch_r, m_data.scratch_v); btVector3 torqueAxis1 = constraintNormal; solverConstraint.m_relpos2CrossNormal = -torqueAxis1; - solverConstraint.m_contactNormal2 = -constraintNormal; + solverConstraint.m_contactNormal2 = -btVector3(0,0,0); } else { btVector3 torqueAxis1 = constraintNormal; solverConstraint.m_relpos2CrossNormal = -torqueAxis1; - solverConstraint.m_contactNormal2 = -constraintNormal; + solverConstraint.m_contactNormal2 = -btVector3(0,0,0); solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0); } @@ -762,7 +762,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyRollingFrictionConstraint(btMult rel_vel += rb1->getVelocityInLocalPoint(rel_pos2).dot(solverConstraint.m_contactNormal2); } } - + solverConstraint.m_friction = cp.m_combinedRollingFriction; if(!isFriction) From 7790ee2f029cc8133cb5166e648b5c4348441a23 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Wed, 31 Aug 2016 16:05:42 -0700 Subject: [PATCH 6/9] fix compile issue with pybullet.c on Visual Studio --- examples/pybullet/pybullet.c | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index fdc753443..f7f062073 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -617,14 +617,16 @@ static int pybullet_internalGetBasePositionAndOrientation(int bodyIndex, double b3SubmitClientCommandAndWaitStatus(sm, cmd_handle); const int status_type = b3GetStatusType(status_handle); + const double* actualStateQ; + // const double* jointReactionForces[]; + int i; + if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED) { PyErr_SetString(SpamError, "getBasePositionAndOrientation failed."); return 0; } - const double* actualStateQ; - // const double* jointReactionForces[]; - int i; + b3GetStatusActualState(status_handle, 0/* body_unique_id */, 0/* num_degree_of_freedom_q */, 0/* num_degree_of_freedom_u */, 0 /*root_local_inertial_frame*/, From 1c65cae6d0b0e91faf93ca5bc2a06cf44b64152d Mon Sep 17 00:00:00 2001 From: Jeffrey Bingham Date: Thu, 1 Sep 2016 10:45:14 -0700 Subject: [PATCH 7/9] [SharedMemory] Calculate inverse dynamics now uses world gravity. Small change that takes the world gravity and applies it to the body during the call to inverse dynamics in the shared memory interface. Otherwise the gravity vector is left at the default value and there is currently no interface to set the gravity for the inverse dynamics system. --- .../PhysicsServerCommandProcessor.cpp | 48 ++++++++++--------- 1 file changed, 25 insertions(+), 23 deletions(-) diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 84d4554ca..914cce573 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -507,7 +507,7 @@ void PhysicsServerCommandProcessor::setGuiHelper(struct GUIHelperInterface* guiH } m_data->m_guiHelper = guiHelper; - + } @@ -518,7 +518,7 @@ PhysicsServerCommandProcessor::PhysicsServerCommandProcessor() createEmptyDynamicsWorld(); m_data->m_dynamicsWorld->getSolverInfo().m_linearSlop = 0.0001; - + } PhysicsServerCommandProcessor::~PhysicsServerCommandProcessor() @@ -887,7 +887,7 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto btMultiBody* mb = creation.getBulletMultiBody(); btRigidBody* rb = creation.getRigidBody(); - + if (useMultiBody) { @@ -1024,7 +1024,7 @@ int PhysicsServerCommandProcessor::createBodyInfoStream(int bodyUniqueId, char* bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes ) { - + bool hasStatus = false; { @@ -1154,7 +1154,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm int height = clientCmd.m_requestPixelDataArguments.m_pixelHeight; int numPixelsCopied = 0; - + if ((clientCmd.m_updateFlags & ER_BULLET_HARDWARE_OPENGL)!=0) { @@ -1481,10 +1481,10 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] & SIM_DESIRED_STATE_HAS_KD)!=0) { kd = clientCmd.m_sendDesiredStateCommandArgument.m_Kd[dofIndex]; - } - + } + motor->setVelocityTarget(desiredVelocity,kd); - + btScalar kp = 0.f; motor->setPositionTarget(0,kp); hasDesiredVelocity = true; @@ -1565,7 +1565,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm motor->setVelocityTarget(desiredVelocity,kd); motor->setPositionTarget(desiredPosition,kp); - + btScalar maxImp = 1000000.f*m_data->m_physicsDeltaTime; if ((clientCmd.m_updateFlags & SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0) @@ -1805,7 +1805,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } case CMD_STEP_FORWARD_SIMULATION: { - + if (m_data->m_verboseOutput) { b3Printf("Step simulation request"); @@ -1897,7 +1897,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm clientCmd.m_initPoseArgs.m_initialStateQ[4], clientCmd.m_initPoseArgs.m_initialStateQ[5], clientCmd.m_initPoseArgs.m_initialStateQ[6]); - + mb->setWorldToBaseRot(invOrn.inverse()); } if (clientCmd.m_updateFlags & INIT_POSE_HAS_JOINT_STATE) @@ -1936,10 +1936,10 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { m_data->m_visualConverter.resetAll(); } - + deleteDynamicsWorld(); createEmptyDynamicsWorld(); - + m_data->exitHandles(); m_data->initHandles(); @@ -2128,7 +2128,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm InternalBodyHandle* bodyHandle = m_data->getHandle(clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId); if (bodyHandle && bodyHandle->m_multiBody) { - btInverseDynamics::MultiBodyTree** treePtrPtr = + btInverseDynamics::MultiBodyTree** treePtrPtr = m_data->m_inverseDynamicsBodies.find(bodyHandle->m_multiBody); btInverseDynamics::MultiBodyTree* tree = 0; serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_FAILED; @@ -2140,12 +2140,12 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm else { btInverseDynamics::btMultiBodyTreeCreator id_creator; - if (-1 == id_creator.createFromBtMultiBody(bodyHandle->m_multiBody, false)) + if (-1 == id_creator.createFromBtMultiBody(bodyHandle->m_multiBody, false)) { b3Error("error creating tree\n"); serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_FAILED; } - else + else { tree = btInverseDynamics::CreateMultiBodyTree(id_creator); m_data->m_inverseDynamicsBodies.insert(bodyHandle->m_multiBody, tree); @@ -2163,8 +2163,10 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm qdot[i + baseDofs] = clientCmd.m_calculateInverseDynamicsArguments.m_jointVelocitiesQdot[i]; nu[i+baseDofs] = clientCmd.m_calculateInverseDynamicsArguments.m_jointAccelerations[i]; } - - if (-1 != tree->calculateInverseDynamics(q, qdot, nu, &joint_force)) + // Set the gravity to correspond to the world gravity + btInverseDynamics::vec3 id_grav(m_data->m_dynamicsWorld->getGravity()); + if (-1 != tree->calculateInverseDynamics(q, qdot, nu, &joint_force) && + -1 != tree->setGravityInWorldFrame(id_grav)) { serverCmd.m_inverseDynamicsResultArgs.m_bodyUniqueId = clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId; serverCmd.m_inverseDynamicsResultArgs.m_dofCount = num_dofs; @@ -2179,7 +2181,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_FAILED; } } - + } else { @@ -2203,7 +2205,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { btMultiBody* mb = body->m_multiBody; bool isLinkFrame = ((clientCmd.m_externalForceArguments.m_forceFlags[i] & EF_LINK_FRAME)!=0); - + if ((clientCmd.m_externalForceArguments.m_forceFlags[i] & EF_FORCE)!=0) { btVector3 forceLocal(clientCmd.m_externalForceArguments.m_forcesAndTorques[i*3+0], @@ -2213,7 +2215,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm clientCmd.m_externalForceArguments.m_positions[i*3+0], clientCmd.m_externalForceArguments.m_positions[i*3+1], clientCmd.m_externalForceArguments.m_positions[i*3+2]); - + if (clientCmd.m_externalForceArguments.m_linkIds[i] == -1) { btVector3 forceWorld = isLinkFrame ? forceLocal : mb->getBaseWorldTransform().getBasis()*forceLocal; @@ -2236,7 +2238,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm btVector3 torqueLocal(clientCmd.m_externalForceArguments.m_forcesAndTorques[i*3+0], clientCmd.m_externalForceArguments.m_forcesAndTorques[i*3+1], clientCmd.m_externalForceArguments.m_forcesAndTorques[i*3+2]); - + if (clientCmd.m_externalForceArguments.m_linkIds[i] == -1) { btVector3 torqueWorld = isLinkFrame ? torqueLocal : mb->getBaseWorldTransform().getBasis()*torqueLocal; @@ -2252,7 +2254,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } } } - + SharedMemoryStatus& serverCmd =serverStatusOut; serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED; hasStatus = true; From 85fd7f560c64cf90ad7a30a57cae103708ef9f69 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Thu, 1 Sep 2016 13:30:07 -0700 Subject: [PATCH 8/9] add first draft of contact point query in shared memory API b3SharedMemoryCommandHandle b3InitRequestContactPointInformation(b3PhysicsClientHandle physClient); void b3SetContactFilterBodyA(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdA); void b3SetContactFilterBodyB(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdB); void b3GetContactPointInformation(b3PhysicsClientHandle physClient, struct b3ContactInformation* contactPointData); Implemented for PhysicsClientSharedMemory, not for PhysicsDirect yet. Add btCollisionObject::setUserIndex2 --- examples/SharedMemory/PhysicsClient.h | 2 + examples/SharedMemory/PhysicsClientC_API.cpp | 45 ++++++ examples/SharedMemory/PhysicsClientC_API.h | 5 + .../SharedMemory/PhysicsClientExample.cpp | 23 ++- .../PhysicsClientSharedMemory.cpp | 54 +++++++ .../SharedMemory/PhysicsClientSharedMemory.h | 2 + examples/SharedMemory/PhysicsDirect.cpp | 9 ++ examples/SharedMemory/PhysicsDirect.h | 2 + examples/SharedMemory/PhysicsLoopBack.cpp | 7 + examples/SharedMemory/PhysicsLoopBack.h | 2 + .../PhysicsServerCommandProcessor.cpp | 136 +++++++++++++++++- examples/SharedMemory/SharedMemoryCommands.h | 15 ++ examples/SharedMemory/SharedMemoryPublic.h | 38 ++++- examples/TinyRenderer/tgaimage.h | 6 +- .../CollisionDispatch/btCollisionObject.cpp | 1 + .../CollisionDispatch/btCollisionObject.h | 17 ++- .../Featherstone/btMultiBody.cpp | 4 +- src/BulletDynamics/Featherstone/btMultiBody.h | 36 +++++ 18 files changed, 395 insertions(+), 9 deletions(-) diff --git a/examples/SharedMemory/PhysicsClient.h b/examples/SharedMemory/PhysicsClient.h index 07b10aa13..b76e7e18c 100644 --- a/examples/SharedMemory/PhysicsClient.h +++ b/examples/SharedMemory/PhysicsClient.h @@ -40,6 +40,8 @@ public: virtual void getCachedCameraImage(struct b3CameraImageData* cameraData)=0; + virtual void getCachedContactPointInformation(struct b3ContactInformation* contactPointData)=0; + }; #endif // BT_PHYSICS_CLIENT_API_H diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index b0f66c6db..d57113e3e 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -1103,6 +1103,51 @@ void b3GetCameraImageData(b3PhysicsClientHandle physClient, struct b3CameraImage } } +///request an contact point information +b3SharedMemoryCommandHandle b3InitRequestContactPointInformation(b3PhysicsClientHandle physClient) +{ + PhysicsClient* cl = (PhysicsClient* ) physClient; + b3Assert(cl); + b3Assert(cl->canSubmitCommand()); + struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); + b3Assert(command); + command->m_type =CMD_REQUEST_CONTACT_POINT_INFORMATION; + command->m_requestContactPointArguments.m_startingContactPointIndex = 0; + command->m_requestContactPointArguments.m_objectAIndexFilter = -1; + command->m_requestContactPointArguments.m_objectBIndexFilter = -1; + command->m_updateFlags = 0; + return (b3SharedMemoryCommandHandle) command; +} + +void b3SetContactFilterBodyA(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdA) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_REQUEST_CONTACT_POINT_INFORMATION); + command->m_requestContactPointArguments.m_objectAIndexFilter = bodyUniqueIdA; +} + +void b3SetContactFilterBodyB(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdB) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_REQUEST_CONTACT_POINT_INFORMATION); + command->m_requestContactPointArguments.m_objectBIndexFilter = bodyUniqueIdB; +} + +void b3SetContactFilterBodyB(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdB); + + +void b3GetContactPointInformation(b3PhysicsClientHandle physClient, struct b3ContactInformation* contactPointData) +{ + PhysicsClient* cl = (PhysicsClient* ) physClient; + if (cl) + { + cl->getCachedContactPointInformation(contactPointData); + } +} + + b3SharedMemoryCommandHandle b3ApplyExternalForceCommandInit(b3PhysicsClientHandle physClient) { PhysicsClient* cl = (PhysicsClient* ) physClient; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 351c87677..4d6522576 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -80,6 +80,11 @@ void b3RequestCameraImageSetPixelResolution(b3SharedMemoryCommandHandle command, void b3RequestCameraImageSelectRenderer(b3SharedMemoryCommandHandle commandHandle, int renderer); void b3GetCameraImageData(b3PhysicsClientHandle physClient, struct b3CameraImageData* imageData); +///request an contact point information +b3SharedMemoryCommandHandle b3InitRequestContactPointInformation(b3PhysicsClientHandle physClient); +void b3SetContactFilterBodyA(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdA); +void b3SetContactFilterBodyB(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdB); +void b3GetContactPointInformation(b3PhysicsClientHandle physClient, struct b3ContactInformation* contactPointData); b3SharedMemoryCommandHandle b3InitPhysicsParamCommand(b3PhysicsClientHandle physClient); int b3PhysicsParamSetGravity(b3SharedMemoryCommandHandle commandHandle, double gravx,double gravy, double gravz); diff --git a/examples/SharedMemory/PhysicsClientExample.cpp b/examples/SharedMemory/PhysicsClientExample.cpp index 034215bd2..07ba317a4 100644 --- a/examples/SharedMemory/PhysicsClientExample.cpp +++ b/examples/SharedMemory/PhysicsClientExample.cpp @@ -274,7 +274,7 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId) case CMD_CREATE_BOX_COLLISION_SHAPE: { b3SharedMemoryCommandHandle commandHandle = b3CreateBoxShapeCommandInit(m_physicsClientHandle); - b3CreateBoxCommandSetStartPosition(commandHandle,0,0,-3); + b3CreateBoxCommandSetStartPosition(commandHandle,0,0,-1.5); b3CreateBoxCommandSetColorRGBA(commandHandle,0,0,1,1); b3SubmitClientCommand(m_physicsClientHandle, commandHandle); break; @@ -415,6 +415,7 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId) b3SubmitClientCommand(m_physicsClientHandle, commandHandle); break; } + case CMD_CALCULATE_INVERSE_DYNAMICS: { if (m_selectedBody >= 0) @@ -442,6 +443,14 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId) } break; } + case CMD_REQUEST_CONTACT_POINT_INFORMATION: + { + b3SharedMemoryCommandHandle commandHandle = b3InitRequestContactPointInformation(m_physicsClientHandle); + b3SetContactFilterBodyA(commandHandle,0); + b3SetContactFilterBodyB(commandHandle,1); + b3SubmitClientCommand(m_physicsClientHandle, commandHandle); + break; + } default: { b3Error("Unknown buttonId"); @@ -530,6 +539,7 @@ void PhysicsClientExample::createButtons() createButton("Initialize Pose",CMD_INIT_POSE, isTrigger); createButton("Set gravity", CMD_SEND_PHYSICS_SIMULATION_PARAMETERS, isTrigger); createButton("Compute Inverse Dynamics", CMD_CALCULATE_INVERSE_DYNAMICS, isTrigger); + createButton("Get Contact Point Info", CMD_REQUEST_CONTACT_POINT_INFORMATION, isTrigger); if (m_bodyUniqueIds.size()) { @@ -946,6 +956,17 @@ void PhysicsClientExample::stepSimulation(float deltaTime) } } + if (statusType == CMD_CONTACT_POINT_INFORMATION_FAILED) + { + b3Warning("Cannot get contact information"); + } + if (statusType == CMD_CONTACT_POINT_INFORMATION_COMPLETED) + { + b3ContactInformation contactPointData; + b3GetContactPointInformation(m_physicsClientHandle, &contactPointData); + b3Printf("Num Contacts: %d\n", contactPointData.m_numContactPoints); + + } } } if (b3CanSubmitCommand(m_physicsClientHandle)) diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index 85c90bbd7..317902254 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -38,6 +38,9 @@ struct PhysicsClientSharedMemoryInternalData { btAlignedObjectArray m_cachedCameraDepthBuffer; btAlignedObjectArray m_cachedSegmentationMaskBuffer; + btAlignedObjectArray m_cachedContactPoints; + btAlignedObjectArraym_cachedContactPointDynamics; + btAlignedObjectArray m_bodyIdsRequestInfo; SharedMemoryStatus m_tempBackupServerStatus; @@ -58,6 +61,7 @@ struct PhysicsClientSharedMemoryInternalData { m_counter(0), m_cachedCameraPixelsWidth(0), m_cachedCameraPixelsHeight(0), + m_isConnected(false), m_waitingForServer(false), m_hasLastServerStatus(false), @@ -558,6 +562,33 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { b3Warning("Inverse Dynamics computations failed"); break; } + case CMD_CONTACT_POINT_INFORMATION_COMPLETED: + { + if (m_data->m_verboseOutput) + { + b3Printf("Contact Point Information Request OK\n"); + } + int startContactIndex = serverCmd.m_sendContactPointArgs.m_startingContactPointIndex; + int numContactsCopied = serverCmd.m_sendContactPointArgs.m_numContactPointsCopied; + + m_data->m_cachedContactPoints.resize(startContactIndex+numContactsCopied); + m_data->m_cachedContactPointDynamics.resize(0); + + b3ContactPointData* contactData = (b3ContactPointData*)m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor; + + for (int i=0;im_cachedContactPoints[startContactIndex+i] = contactData[i]; + } + + break; + } + case CMD_CONTACT_POINT_INFORMATION_FAILED: + { + b3Warning("Contact Point Information Request failed"); + break; + } + default: { b3Error("Unknown server status\n"); btAssert(0); @@ -620,6 +651,21 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { } } + if (serverCmd.m_type == CMD_CONTACT_POINT_INFORMATION_COMPLETED) + { + //todo: request remaining points, if needed + SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0]; + if (serverCmd.m_sendContactPointArgs.m_numRemainingContactPoints>0 && serverCmd.m_sendContactPointArgs.m_numContactPointsCopied) + { + command.m_type = CMD_REQUEST_CONTACT_POINT_INFORMATION; + command.m_requestContactPointArguments.m_startingContactPointIndex = serverCmd.m_sendContactPointArgs.m_startingContactPointIndex+serverCmd.m_sendContactPointArgs.m_numContactPointsCopied; + command.m_requestContactPointArguments.m_objectAIndexFilter = -1; + command.m_requestContactPointArguments.m_objectBIndexFilter = -1; + submitClientCommand(command); + return 0; + } + } + if (serverCmd.m_type == CMD_CAMERA_IMAGE_COMPLETED) { SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0]; @@ -716,6 +762,14 @@ void PhysicsClientSharedMemory::getCachedCameraImage(struct b3CameraImageData* c cameraData->m_segmentationMaskValues = m_data->m_cachedSegmentationMaskBuffer.size()?&m_data->m_cachedSegmentationMaskBuffer[0] : 0; } +void PhysicsClientSharedMemory::getCachedContactPointInformation(struct b3ContactInformation* contactPointData) +{ + contactPointData->m_numContactPoints = m_data->m_cachedContactPoints.size(); + contactPointData->m_contactDynamicsData = 0; + contactPointData->m_contactPointData = contactPointData->m_numContactPoints? &m_data->m_cachedContactPoints[0] : 0; + +} + const float* PhysicsClientSharedMemory::getDebugLinesFrom() const { if (m_data->m_debugLinesFrom.size()) { return &m_data->m_debugLinesFrom[0].m_x; diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.h b/examples/SharedMemory/PhysicsClientSharedMemory.h index a47fc551a..69e5ed64d 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.h +++ b/examples/SharedMemory/PhysicsClientSharedMemory.h @@ -48,6 +48,8 @@ public: virtual const float* getDebugLinesTo() const; virtual const float* getDebugLinesColor() const; virtual void getCachedCameraImage(struct b3CameraImageData* cameraData); + + virtual void getCachedContactPointInformation(struct b3ContactInformation* contactPointData); }; #endif // BT_PHYSICS_CLIENT_API_H diff --git a/examples/SharedMemory/PhysicsDirect.cpp b/examples/SharedMemory/PhysicsDirect.cpp index 948b90b90..e465aa6d9 100644 --- a/examples/SharedMemory/PhysicsDirect.cpp +++ b/examples/SharedMemory/PhysicsDirect.cpp @@ -487,3 +487,12 @@ void PhysicsDirect::getCachedCameraImage(b3CameraImageData* cameraData) cameraData->m_segmentationMaskValues = m_data->m_cachedSegmentationMask.size()? &m_data->m_cachedSegmentationMask[0] : 0; } } + +void PhysicsDirect::getCachedContactPointInformation(struct b3ContactInformation* contactPointData) +{ + if (contactPointData) + { + contactPointData->m_numContactPoints = 0; + } +} + diff --git a/examples/SharedMemory/PhysicsDirect.h b/examples/SharedMemory/PhysicsDirect.h index 7e3ae2c94..ee3709ecf 100644 --- a/examples/SharedMemory/PhysicsDirect.h +++ b/examples/SharedMemory/PhysicsDirect.h @@ -64,6 +64,8 @@ public: virtual void getCachedCameraImage(b3CameraImageData* cameraData); + virtual void getCachedContactPointInformation(struct b3ContactInformation* contactPointData); + //those 2 APIs are for internal use for visualization virtual bool connect(struct GUIHelperInterface* guiHelper); virtual void renderScene(); diff --git a/examples/SharedMemory/PhysicsLoopBack.cpp b/examples/SharedMemory/PhysicsLoopBack.cpp index ada37ce88..91ac0d2cc 100644 --- a/examples/SharedMemory/PhysicsLoopBack.cpp +++ b/examples/SharedMemory/PhysicsLoopBack.cpp @@ -105,10 +105,12 @@ const float* PhysicsLoopBack::getDebugLinesFrom() const { return m_data->m_physicsClient->getDebugLinesFrom(); } + const float* PhysicsLoopBack::getDebugLinesTo() const { return m_data->m_physicsClient->getDebugLinesTo(); } + const float* PhysicsLoopBack::getDebugLinesColor() const { return m_data->m_physicsClient->getDebugLinesColor(); @@ -118,3 +120,8 @@ void PhysicsLoopBack::getCachedCameraImage(struct b3CameraImageData* cameraData) { return m_data->m_physicsClient->getCachedCameraImage(cameraData); } + +void PhysicsLoopBack::getCachedContactPointInformation(struct b3ContactInformation* contactPointData) +{ + return m_data->m_physicsClient->getCachedContactPointInformation(contactPointData); +} diff --git a/examples/SharedMemory/PhysicsLoopBack.h b/examples/SharedMemory/PhysicsLoopBack.h index 8c0e187b6..326e21128 100644 --- a/examples/SharedMemory/PhysicsLoopBack.h +++ b/examples/SharedMemory/PhysicsLoopBack.h @@ -53,6 +53,8 @@ public: virtual const float* getDebugLinesTo() const; virtual const float* getDebugLinesColor() const; virtual void getCachedCameraImage(struct b3CameraImageData* cameraData); + + virtual void getCachedContactPointInformation(struct b3ContactInformation* contactPointData); }; diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 84d4554ca..daccc500c 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -403,6 +403,8 @@ struct PhysicsServerCommandProcessorInternalData btDefaultCollisionConfiguration* m_collisionConfiguration; btMultiBodyDynamicsWorld* m_dynamicsWorld; SharedMemoryDebugDrawer* m_remoteDebugDrawer; + + btAlignedObjectArray m_cachedContactPoints; btAlignedObjectArray m_sdfRecentLoadedBodies; @@ -759,6 +761,8 @@ bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferSe int bodyUniqueId = m_data->allocHandle(); InternalBodyHandle* bodyHandle = m_data->getHandle(bodyUniqueId); + + u2b.setBodyUniqueId(bodyUniqueId); { btScalar mass = 0; @@ -782,9 +786,16 @@ bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferSe mb = creation.getBulletMultiBody(); rb = creation.getRigidBody(); + if (rb) + rb->setUserIndex2(bodyUniqueId); + + if (mb) + mb->setUserIndex2(bodyUniqueId); + if (mb) { bodyHandle->m_multiBody = mb; + m_data->m_sdfRecentLoadedBodies.push_back(bodyUniqueId); @@ -856,6 +867,8 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto u2b.setBodyUniqueId(bodyUniqueId); InternalBodyHandle* bodyHandle = m_data->getHandle(bodyUniqueId); + + { btScalar mass = 0; @@ -887,6 +900,7 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto btMultiBody* mb = creation.getBulletMultiBody(); btRigidBody* rb = creation.getRigidBody(); + if (useMultiBody) { @@ -894,7 +908,7 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto if (mb) { - + mb->setUserIndex2(bodyUniqueId); bodyHandle->m_multiBody = mb; createJointMotors(mb); @@ -958,6 +972,7 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto if (rb) { bodyHandle->m_rigidBody = rb; + rb->setUserIndex2(bodyUniqueId); return true; } } @@ -2079,6 +2094,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm int bodyUniqueId = m_data->allocHandle(); InternalBodyHandle* bodyHandle = m_data->getHandle(bodyUniqueId); serverCmd.m_rigidBodyCreateArgs.m_bodyUniqueId = bodyUniqueId; + rb->setUserIndex2(bodyUniqueId); bodyHandle->m_rootLocalInertialFrame.setIdentity(); bodyHandle->m_rigidBody = rb; hasStatus = true; @@ -2121,6 +2137,124 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED; hasStatus = true; break; + } + case CMD_REQUEST_CONTACT_POINT_INFORMATION: + { + SharedMemoryStatus& serverCmd =serverStatusOut; + serverCmd.m_sendContactPointArgs.m_numContactPointsCopied = 0; + + //make a snapshot of the contact manifolds into individual contact points + if (clientCmd.m_requestContactPointArguments.m_startingContactPointIndex==0) + { + int numContactManifolds = m_data->m_dynamicsWorld->getDispatcher()->getNumManifolds(); + m_data->m_cachedContactPoints.resize(0); + m_data->m_cachedContactPoints.reserve(numContactManifolds*4); + for (int i=0;im_dynamicsWorld->getDispatcher()->getInternalManifoldPointer()[i]; + + int objectIndexB = -1; + + const btRigidBody* bodyB = btRigidBody::upcast(manifold->getBody1()); + if (bodyB) + { + objectIndexB = bodyB->getUserIndex2(); + } + const btMultiBodyLinkCollider* mblB = btMultiBodyLinkCollider::upcast(manifold->getBody1()); + if (mblB && mblB->m_multiBody) + { + objectIndexB = mblB->m_multiBody->getUserIndex2(); + } + + int objectIndexA = -1; + const btRigidBody* bodyA = btRigidBody::upcast(manifold->getBody0()); + if (bodyA) + { + objectIndexA = bodyA->getUserIndex2(); + } + const btMultiBodyLinkCollider* mblA = btMultiBodyLinkCollider::upcast(manifold->getBody0()); + if (mblA && mblA->m_multiBody) + { + objectIndexA = mblA->m_multiBody->getUserIndex2(); + } + + btAssert(bodyA || mblA); + + //apply the filter, if any + if (clientCmd.m_requestContactPointArguments.m_objectAIndexFilter>=0) + { + if ((clientCmd.m_requestContactPointArguments.m_objectAIndexFilter != objectIndexA) && + (clientCmd.m_requestContactPointArguments.m_objectAIndexFilter != objectIndexB)) + continue; + } + + if (clientCmd.m_requestContactPointArguments.m_objectBIndexFilter>=0) + { + if ((clientCmd.m_requestContactPointArguments.m_objectBIndexFilter != objectIndexA) && + (clientCmd.m_requestContactPointArguments.m_objectBIndexFilter != objectIndexB)) + continue; + } + + for (int p=0;pgetNumContacts();p++) + { + //if the point passes the optional filter, add it + + if (clientCmd.m_requestContactPointArguments.m_objectAIndexFilter>=0) + { + //one of the two unique Ids has to match... + + } + + b3ContactPointData pt; + pt.m_bodyUniqueIdA = -1; + pt.m_bodyUniqueIdB = -1; + const btManifoldPoint& srcPt = manifold->getContactPoint(p); + pt.m_contactDistance = srcPt.getDistance(); + pt.m_contactFlags = 0; + pt.m_contactPointDynamicsIndex = -1; + pt.m_linkIndexA = -1; + pt.m_linkIndexB = -1; + for (int j=0;j<3;j++) + { + pt.m_contactNormalOnBInWS[j] = srcPt.m_normalWorldOnB[j]; + pt.m_positionOnAInWS[j] = srcPt.getPositionWorldOnA()[j]; + pt.m_positionOnBInWS[j] = srcPt.getPositionWorldOnB()[j]; + } + m_data->m_cachedContactPoints.push_back (pt); + } + } + } + + int numContactPoints = m_data->m_cachedContactPoints.size(); + + + //b3ContactPoint + //struct b3ContactPointDynamics + + int totalBytesPerContact = sizeof(b3ContactPointData); + int contactPointStorage = bufferSizeInBytes/totalBytesPerContact-1; + + b3ContactPointData* contactData = (b3ContactPointData*)bufferServerToClient; + + int startContactPointIndex = clientCmd.m_requestContactPointArguments.m_startingContactPointIndex; + int numContactPointBatch = btMin(numContactPoints,contactPointStorage); + + int endContactPointIndex = startContactPointIndex+numContactPointBatch; + + for (int i=startContactPointIndex;im_cachedContactPoints[i]; + b3ContactPointData& destPt = contactData[serverCmd.m_sendContactPointArgs.m_numContactPointsCopied]; + destPt = srcPt; + serverCmd.m_sendContactPointArgs.m_numContactPointsCopied++; + } + + serverCmd.m_sendContactPointArgs.m_startingContactPointIndex = clientCmd.m_requestContactPointArguments.m_startingContactPointIndex; + serverCmd.m_sendContactPointArgs.m_numRemainingContactPoints = numContactPoints - clientCmd.m_requestContactPointArguments.m_startingContactPointIndex - serverCmd.m_sendContactPointArgs.m_numContactPointsCopied; + + serverCmd.m_type = CMD_CONTACT_POINT_INFORMATION_COMPLETED; //CMD_CONTACT_POINT_INFORMATION_FAILED, + hasStatus = true; + break; } case CMD_CALCULATE_INVERSE_DYNAMICS: { diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index ccbfb36fe..b718d40c7 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -144,6 +144,12 @@ enum EnumRequestPixelDataUpdateFlags }; +struct RequestContactDataArgs +{ + int m_startingContactPointIndex; + int m_objectAIndexFilter; + int m_objectBIndexFilter; +}; struct SendDebugLinesArgs @@ -406,6 +412,7 @@ struct SharedMemoryCommand struct ExternalForceArgs m_externalForceArguments; struct CalculateInverseDynamicsArgs m_calculateInverseDynamicsArguments; struct CreateJointArgs m_createJointArguments; + struct RequestContactDataArgs m_requestContactPointArguments; }; }; @@ -414,6 +421,13 @@ struct RigidBodyCreateArgs int m_bodyUniqueId; }; +struct SendContactDataArgs +{ + int m_startingContactPointIndex; + int m_numContactPointsCopied; + int m_numRemainingContactPoints; +}; + struct SharedMemoryStatus { int m_type; @@ -430,6 +444,7 @@ struct SharedMemoryStatus struct SendPixelDataArgs m_sendPixelDataArguments; struct RigidBodyCreateArgs m_rigidBodyCreateArgs; struct CalculateInverseDynamicsResultArgs m_inverseDynamicsResultArgs; + struct SendContactDataArgs m_sendContactPointArgs; }; }; diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 9b50e4d17..231929ab6 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -29,8 +29,11 @@ enum EnumSharedMemoryClientCommand CMD_APPLY_EXTERNAL_FORCE, CMD_CALCULATE_INVERSE_DYNAMICS, CMD_CALCULATE_INVERSE_KINEMATICS, + CMD_CREATE_JOINT, + CMD_REQUEST_CONTACT_POINT_INFORMATION, + //don't go beyond this command! CMD_MAX_CLIENT_COMMANDS, - CMD_CREATE_JOINT + }; enum EnumSharedMemoryServerStatus @@ -64,6 +67,8 @@ enum EnumSharedMemoryServerStatus CMD_INVALID_STATUS, CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED, CMD_CALCULATED_INVERSE_DYNAMICS_FAILED, + CMD_CONTACT_POINT_INFORMATION_COMPLETED, + CMD_CONTACT_POINT_INFORMATION_FAILED, CMD_MAX_SERVER_COMMANDS }; @@ -131,6 +136,37 @@ struct b3CameraImageData const int* m_segmentationMaskValues;//m_pixelWidth*m_pixelHeight ints }; + +struct b3ContactPointData +{ + int m_contactFlags;//flag wether certain fields below are valid + int m_bodyUniqueIdA; + int m_bodyUniqueIdB; + int m_linkIndexA; + int m_linkIndexB; + double m_positionOnAInWS[3];//contact point location on object A, in world space coordinates + double m_positionOnBInWS[3];//contact point location on object A, in world space coordinates + double m_contactNormalOnBInWS[3];//the separating contact normal, pointing from object B towards object A + double m_contactDistance;//negative number is penetration, positive is distance. + int m_contactPointDynamicsIndex; +}; + +struct b3ContactPointDynamicsData +{ + double m_normalForce; + double m_linearFrictionDirection[3]; + double m_linearFrictionForce; + double m_angularFrictionDirection[3]; + double m_angularFrictionForce; +}; + +struct b3ContactInformation +{ + int m_numContactPoints; + struct b3ContactPointData* m_contactPointData; + struct b3ContactPointDynamicsData* m_contactDynamicsData; +}; + ///b3LinkState provides extra information such as the Cartesian world coordinates ///center of mass (COM) of the link, relative to the world reference frame. ///Orientation is a quaternion x,y,z,w diff --git a/examples/TinyRenderer/tgaimage.h b/examples/TinyRenderer/tgaimage.h index 9e2cbf877..80818dda7 100644 --- a/examples/TinyRenderer/tgaimage.h +++ b/examples/TinyRenderer/tgaimage.h @@ -30,20 +30,20 @@ struct TGAColor { bgra[i] = 0; } - TGAColor(unsigned char R, unsigned char G, unsigned char B, unsigned char A=255) : bgra(), bytespp(4) { + TGAColor(unsigned char R, unsigned char G, unsigned char B, unsigned char A=255) : bytespp(4) { bgra[0] = B; bgra[1] = G; bgra[2] = R; bgra[3] = A; } - TGAColor(unsigned char v) : bgra(), bytespp(1) { + TGAColor(unsigned char v) : bytespp(1) { for (int i=0; i<4; i++) bgra[i] = 0; bgra[0] = v; } - TGAColor(const unsigned char *p, unsigned char bpp) : bgra(), bytespp(bpp) { + TGAColor(const unsigned char *p, unsigned char bpp) : bytespp(bpp) { for (int i=0; i<(int)bpp; i++) { bgra[i] = p[i]; } diff --git a/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp b/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp index 395df3a55..032a5828d 100644 --- a/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp +++ b/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp @@ -35,6 +35,7 @@ btCollisionObject::btCollisionObject() m_rollingFriction(0.0f), m_internalType(CO_COLLISION_OBJECT), m_userObjectPointer(0), + m_userIndex2(-1), m_userIndex(-1), m_hitFraction(btScalar(1.)), m_ccdSweptSphereRadius(btScalar(0.)), diff --git a/src/BulletCollision/CollisionDispatch/btCollisionObject.h b/src/BulletCollision/CollisionDispatch/btCollisionObject.h index c68402418..a96809f91 100644 --- a/src/BulletCollision/CollisionDispatch/btCollisionObject.h +++ b/src/BulletCollision/CollisionDispatch/btCollisionObject.h @@ -93,8 +93,10 @@ protected: ///users can point to their objects, m_userPointer is not used by Bullet, see setUserPointer/getUserPointer - void* m_userObjectPointer; - + void* m_userObjectPointer; + + int m_userIndex2; + int m_userIndex; ///time of impact calculation @@ -476,6 +478,12 @@ public: { return m_userIndex; } + + int getUserIndex2() const + { + return m_userIndex2; + } + ///users can point to their objects, userPointer is not used by Bullet void setUserPointer(void* userPointer) { @@ -487,6 +495,11 @@ public: { m_userIndex = index; } + + void setUserIndex2(int index) + { + m_userIndex2 = index; + } int getUpdateRevisionInternal() const { diff --git a/src/BulletDynamics/Featherstone/btMultiBody.cpp b/src/BulletDynamics/Featherstone/btMultiBody.cpp index edef315b3..76903be98 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBody.cpp @@ -106,7 +106,9 @@ btMultiBody::btMultiBody(int n_links, m_awake(true), m_canSleep(canSleep), m_sleepTimer(0), - + m_userObjectPointer(0), + m_userIndex2(-1), + m_userIndex(-1), m_linearDamping(0.04f), m_angularDamping(0.04f), m_useGyroTerm(true), diff --git a/src/BulletDynamics/Featherstone/btMultiBody.h b/src/BulletDynamics/Featherstone/btMultiBody.h index a4510d2ca..2b387df1d 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.h +++ b/src/BulletDynamics/Featherstone/btMultiBody.h @@ -577,6 +577,38 @@ void addJointTorque(int i, btScalar Q); m_baseName = name; } + ///users can point to their objects, userPointer is not used by Bullet + void* getUserPointer() const + { + return m_userObjectPointer; + } + + int getUserIndex() const + { + return m_userIndex; + } + + int getUserIndex2() const + { + return m_userIndex2; + } + ///users can point to their objects, userPointer is not used by Bullet + void setUserPointer(void* userPointer) + { + m_userObjectPointer = userPointer; + } + + ///users can point to their objects, userPointer is not used by Bullet + void setUserIndex(int index) + { + m_userIndex = index; + } + + void setUserIndex2(int index) + { + m_userIndex2 = index; + } + private: btMultiBody(const btMultiBody &); // not implemented void operator=(const btMultiBody &); // not implemented @@ -653,6 +685,10 @@ private: bool m_canSleep; btScalar m_sleepTimer; + void* m_userObjectPointer; + int m_userIndex2; + int m_userIndex; + int m_companionId; btScalar m_linearDamping; btScalar m_angularDamping; From e98fca1e5e396fa6755966c6421e52d09ba87652 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Thu, 1 Sep 2016 18:28:39 -0700 Subject: [PATCH 9/9] implement pybullet.getContactPointData(), two optional object unique ids as filter returns a pylist of contact points. Each point has the following data: 0 int m_contactFlags;//unused for now 1 int m_bodyUniqueIdA; 2 int m_bodyUniqueIdB; 3 int m_linkIndexA; 4 int m_linkIndexB; 5-6-7 double m_positionOnAInWS[3];//contact point location on object A, in world space coordinates 8-9-10 double m_positionOnBInWS[3];//contact point location on object A, in world space coordinates 11-12-13 double m_contactNormalOnBInWS[3];//the separating contact normal, pointing from object B towards object A 14 double m_contactDistance;//negative number is penetration, positive is distance. 15 double m_normalForce; --- .../PhysicsClientSharedMemory.cpp | 6 +- examples/SharedMemory/PhysicsDirect.cpp | 58 ++++++++- examples/SharedMemory/PhysicsDirect.h | 2 + .../PhysicsServerCommandProcessor.cpp | 35 +++--- examples/SharedMemory/SharedMemoryPublic.h | 23 ++-- examples/pybullet/pybullet.c | 114 ++++++++++++++++++ .../btSequentialImpulseConstraintSolver.cpp | 4 +- .../btMultiBodyConstraintSolver.cpp | 4 +- 8 files changed, 205 insertions(+), 41 deletions(-) diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index 317902254..e622cf716 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -39,7 +39,6 @@ struct PhysicsClientSharedMemoryInternalData { btAlignedObjectArray m_cachedSegmentationMaskBuffer; btAlignedObjectArray m_cachedContactPoints; - btAlignedObjectArraym_cachedContactPointDynamics; btAlignedObjectArray m_bodyIdsRequestInfo; SharedMemoryStatus m_tempBackupServerStatus; @@ -572,7 +571,6 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { int numContactsCopied = serverCmd.m_sendContactPointArgs.m_numContactPointsCopied; m_data->m_cachedContactPoints.resize(startContactIndex+numContactsCopied); - m_data->m_cachedContactPointDynamics.resize(0); b3ContactPointData* contactData = (b3ContactPointData*)m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor; @@ -653,8 +651,7 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { if (serverCmd.m_type == CMD_CONTACT_POINT_INFORMATION_COMPLETED) { - //todo: request remaining points, if needed - SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0]; + SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0]; if (serverCmd.m_sendContactPointArgs.m_numRemainingContactPoints>0 && serverCmd.m_sendContactPointArgs.m_numContactPointsCopied) { command.m_type = CMD_REQUEST_CONTACT_POINT_INFORMATION; @@ -765,7 +762,6 @@ void PhysicsClientSharedMemory::getCachedCameraImage(struct b3CameraImageData* c void PhysicsClientSharedMemory::getCachedContactPointInformation(struct b3ContactInformation* contactPointData) { contactPointData->m_numContactPoints = m_data->m_cachedContactPoints.size(); - contactPointData->m_contactDynamicsData = 0; contactPointData->m_contactPointData = contactPointData->m_numContactPoints? &m_data->m_cachedContactPoints[0] : 0; } diff --git a/examples/SharedMemory/PhysicsDirect.cpp b/examples/SharedMemory/PhysicsDirect.cpp index e465aa6d9..6ac5d6212 100644 --- a/examples/SharedMemory/PhysicsDirect.cpp +++ b/examples/SharedMemory/PhysicsDirect.cpp @@ -39,6 +39,8 @@ struct PhysicsDirectInternalData btAlignedObjectArray m_cachedCameraDepthBuffer; btAlignedObjectArray m_cachedSegmentationMask; + btAlignedObjectArray m_cachedContactPoints; + PhysicsServerCommandProcessor* m_commandProcessor; @@ -193,6 +195,51 @@ bool PhysicsDirect::processDebugLines(const struct SharedMemoryCommand& orgComma return m_data->m_hasStatus; } +bool PhysicsDirect::processContactPointData(const struct SharedMemoryCommand& orgCommand) +{ + SharedMemoryCommand command = orgCommand; + + const SharedMemoryStatus& serverCmd = m_data->m_serverStatus; + + do + { + bool hasStatus = m_data->m_commandProcessor->processCommand(command,m_data->m_serverStatus,&m_data->m_bulletStreamDataServerToClient[0],SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); + m_data->m_hasStatus = hasStatus; + if (hasStatus) + { + if (m_data->m_verboseOutput) + { + b3Printf("Contact Point Information Request OK\n"); + } + int startContactIndex = serverCmd.m_sendContactPointArgs.m_startingContactPointIndex; + int numContactsCopied = serverCmd.m_sendContactPointArgs.m_numContactPointsCopied; + + m_data->m_cachedContactPoints.resize(startContactIndex+numContactsCopied); + + b3ContactPointData* contactData = (b3ContactPointData*)&m_data->m_bulletStreamDataServerToClient[0]; + + for (int i=0;im_cachedContactPoints[startContactIndex+i] = contactData[i]; + } + + if (serverCmd.m_sendContactPointArgs.m_numRemainingContactPoints>0 && serverCmd.m_sendContactPointArgs.m_numContactPointsCopied) + { + command.m_type = CMD_REQUEST_CONTACT_POINT_INFORMATION; + command.m_requestContactPointArguments.m_startingContactPointIndex = serverCmd.m_sendContactPointArgs.m_startingContactPointIndex+serverCmd.m_sendContactPointArgs.m_numContactPointsCopied; + command.m_requestContactPointArguments.m_objectAIndexFilter = -1; + command.m_requestContactPointArguments.m_objectBIndexFilter = -1; + + } + + } + } while (serverCmd.m_sendContactPointArgs.m_numRemainingContactPoints > 0 && serverCmd.m_sendContactPointArgs.m_numContactPointsCopied); + + return m_data->m_hasStatus; + +} + + bool PhysicsDirect::processCamera(const struct SharedMemoryCommand& orgCommand) { SharedMemoryCommand command = orgCommand; @@ -326,6 +373,10 @@ bool PhysicsDirect::submitClientCommand(const struct SharedMemoryCommand& comman { return processCamera(command); } + if (command.m_type == CMD_REQUEST_CONTACT_POINT_INFORMATION) + { + return processContactPointData(command); + } bool hasStatus = m_data->m_commandProcessor->processCommand(command,m_data->m_serverStatus,&m_data->m_bulletStreamDataServerToClient[0],SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); m_data->m_hasStatus = hasStatus; @@ -490,9 +541,8 @@ void PhysicsDirect::getCachedCameraImage(b3CameraImageData* cameraData) void PhysicsDirect::getCachedContactPointInformation(struct b3ContactInformation* contactPointData) { - if (contactPointData) - { - contactPointData->m_numContactPoints = 0; - } + contactPointData->m_numContactPoints = m_data->m_cachedContactPoints.size(); + contactPointData->m_contactPointData = contactPointData->m_numContactPoints? &m_data->m_cachedContactPoints[0] : 0; + } diff --git a/examples/SharedMemory/PhysicsDirect.h b/examples/SharedMemory/PhysicsDirect.h index ee3709ecf..eae128998 100644 --- a/examples/SharedMemory/PhysicsDirect.h +++ b/examples/SharedMemory/PhysicsDirect.h @@ -21,6 +21,8 @@ protected: bool processCamera(const struct SharedMemoryCommand& orgCommand); + bool processContactPointData(const struct SharedMemoryCommand& orgCommand); + void processBodyJointInfo(int bodyUniqueId, const struct SharedMemoryStatus& serverCmd); public: diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 7bfd03cc1..03357e59f 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -2151,7 +2151,9 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm for (int i=0;im_dynamicsWorld->getDispatcher()->getInternalManifoldPointer()[i]; - + int linkIndexA = -1; + int linkIndexB = -1; + int objectIndexB = -1; const btRigidBody* bodyB = btRigidBody::upcast(manifold->getBody1()); @@ -2162,6 +2164,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm const btMultiBodyLinkCollider* mblB = btMultiBodyLinkCollider::upcast(manifold->getBody1()); if (mblB && mblB->m_multiBody) { + linkIndexB = mblB->m_link; objectIndexB = mblB->m_multiBody->getUserIndex2(); } @@ -2174,12 +2177,14 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm const btMultiBodyLinkCollider* mblA = btMultiBodyLinkCollider::upcast(manifold->getBody0()); if (mblA && mblA->m_multiBody) { + linkIndexA = mblA->m_link; + objectIndexA = mblA->m_multiBody->getUserIndex2(); } btAssert(bodyA || mblA); - //apply the filter, if any + //apply the filter, if the user provides it if (clientCmd.m_requestContactPointArguments.m_objectAIndexFilter>=0) { if ((clientCmd.m_requestContactPointArguments.m_objectAIndexFilter != objectIndexA) && @@ -2187,6 +2192,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm continue; } + //apply the second object filter, if the user provides it if (clientCmd.m_requestContactPointArguments.m_objectBIndexFilter>=0) { if ((clientCmd.m_requestContactPointArguments.m_objectBIndexFilter != objectIndexA) && @@ -2196,29 +2202,23 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm for (int p=0;pgetNumContacts();p++) { - //if the point passes the optional filter, add it - - if (clientCmd.m_requestContactPointArguments.m_objectAIndexFilter>=0) - { - //one of the two unique Ids has to match... - - } b3ContactPointData pt; - pt.m_bodyUniqueIdA = -1; - pt.m_bodyUniqueIdB = -1; + pt.m_bodyUniqueIdA = objectIndexA; + pt.m_bodyUniqueIdB = objectIndexB; const btManifoldPoint& srcPt = manifold->getContactPoint(p); pt.m_contactDistance = srcPt.getDistance(); pt.m_contactFlags = 0; - pt.m_contactPointDynamicsIndex = -1; - pt.m_linkIndexA = -1; - pt.m_linkIndexB = -1; + pt.m_linkIndexA = linkIndexA; + pt.m_linkIndexB = linkIndexB; for (int j=0;j<3;j++) { pt.m_contactNormalOnBInWS[j] = srcPt.m_normalWorldOnB[j]; pt.m_positionOnAInWS[j] = srcPt.getPositionWorldOnA()[j]; pt.m_positionOnBInWS[j] = srcPt.getPositionWorldOnB()[j]; } + pt.m_normalForce = srcPt.getAppliedImpulse()/m_data->m_physicsDeltaTime; +// pt.m_linearFrictionForce = srcPt.m_appliedImpulseLateral1; m_data->m_cachedContactPoints.push_back (pt); } } @@ -2298,8 +2298,9 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } // Set the gravity to correspond to the world gravity btInverseDynamics::vec3 id_grav(m_data->m_dynamicsWorld->getGravity()); - if (-1 != tree->calculateInverseDynamics(q, qdot, nu, &joint_force) && - -1 != tree->setGravityInWorldFrame(id_grav)) + + if (-1 != tree->setGravityInWorldFrame(id_grav) && + -1 != tree->calculateInverseDynamics(q, qdot, nu, &joint_force)) { serverCmd.m_inverseDynamicsResultArgs.m_bodyUniqueId = clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId; serverCmd.m_inverseDynamicsResultArgs.m_dofCount = num_dofs; @@ -2649,7 +2650,7 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec) loadUrdf("plane.urdf", btVector3(0, 0, 0), btQuaternion(0, 0, 0, 1), true, true, &bodyId, &bufferServerToClient[0], bufferServerToClient.size()); } - m_data->m_dynamicsWorld->stepSimulation(dtInSec,10,1./240.); + m_data->m_dynamicsWorld->stepSimulation(dtInSec,10,m_data->m_physicsDeltaTime); } } diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 231929ab6..28038a3ad 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -139,7 +139,8 @@ struct b3CameraImageData struct b3ContactPointData { - int m_contactFlags;//flag wether certain fields below are valid +//todo: expose some contact flags, such as telling which fields below are valid + int m_contactFlags; int m_bodyUniqueIdA; int m_bodyUniqueIdB; int m_linkIndexA; @@ -148,23 +149,23 @@ struct b3ContactPointData double m_positionOnBInWS[3];//contact point location on object A, in world space coordinates double m_contactNormalOnBInWS[3];//the separating contact normal, pointing from object B towards object A double m_contactDistance;//negative number is penetration, positive is distance. - int m_contactPointDynamicsIndex; + + double m_normalForce; + +//todo: expose the friction forces as well +// double m_linearFrictionDirection0[3]; +// double m_linearFrictionForce0; +// double m_linearFrictionDirection1[3]; +// double m_linearFrictionForce1; +// double m_angularFrictionDirection[3]; +// double m_angularFrictionForce; }; -struct b3ContactPointDynamicsData -{ - double m_normalForce; - double m_linearFrictionDirection[3]; - double m_linearFrictionForce; - double m_angularFrictionDirection[3]; - double m_angularFrictionForce; -}; struct b3ContactInformation { int m_numContactPoints; struct b3ContactPointData* m_contactPointData; - struct b3ContactPointDynamicsData* m_contactDynamicsData; }; ///b3LinkState provides extra information such as the Cartesian world coordinates diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index f7f062073..75e4a02a5 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -1113,6 +1113,116 @@ static int pybullet_internalSetVector(PyObject* objMat, float vector[3]) return 0; } + + +static PyObject* pybullet_getContactPointData(PyObject* self, PyObject* args) +{ + int size= PySequence_Size(args); + int objectUniqueIdA = -1; + int objectUniqueIdB = -1; + b3SharedMemoryCommandHandle commandHandle; + struct b3ContactInformation contactPointData; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + + PyObject* pyResultList=0; + + if (size==1) + { + if (!PyArg_ParseTuple(args, "i", &objectUniqueIdA)) + { + PyErr_SetString(SpamError, "Error parsing object unique id"); + return NULL; + } + } + if (size==2) + { + if (!PyArg_ParseTuple(args, "ii", &objectUniqueIdA,&objectUniqueIdB)) + { + PyErr_SetString(SpamError, "Error parsing object unique id"); + return NULL; + } + } + + commandHandle = b3InitRequestContactPointInformation(sm); + b3SetContactFilterBodyA(commandHandle,objectUniqueIdA); + b3SetContactFilterBodyB(commandHandle,objectUniqueIdB); + b3SubmitClientCommand(sm, commandHandle); + + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + statusType = b3GetStatusType(statusHandle); + if (statusType==CMD_CONTACT_POINT_INFORMATION_COMPLETED) + { + + /* + 0 int m_contactFlags; + 1 int m_bodyUniqueIdA; + 2 int m_bodyUniqueIdB; + 3 int m_linkIndexA; + 4 int m_linkIndexB; + 5-6-7 double m_positionOnAInWS[3];//contact point location on object A, in world space coordinates + 8-9-10 double m_positionOnBInWS[3];//contact point location on object A, in world space coordinates + 11-12-13 double m_contactNormalOnBInWS[3];//the separating contact normal, pointing from object B towards object A + 14 double m_contactDistance;//negative number is penetration, positive is distance. + + 15 double m_normalForce; + */ + + b3GetContactPointInformation(sm, &contactPointData); + pyResultList = PyTuple_New(contactPointData.m_numContactPoints); + for (int i=0;i0.f) && (rollingFriction>0)) { - //only a single rollingFriction per manifold - rollingFriction--; +//disabled: only a single rollingFriction per manifold +// rollingFriction--; if (relAngVel.length()>infoGlobal.m_singleAxisRollingFrictionThreshold) { relAngVel.normalize(); diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp index 08bee49c4..b40c6d72b 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp @@ -963,8 +963,8 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold* if ((cp.m_combinedRollingFriction>0.f) && (rollingFriction>0)) { - //only a single rollingFriction per manifold - rollingFriction--; +//disabled: only a single rollingFriction per manifold +//rollingFriction--; if (relAngVel.length()>infoGlobal.m_singleAxisRollingFrictionThreshold) { relAngVel.normalize();