diff --git a/data/cube_gripper_left.urdf b/data/cube_gripper_left.urdf new file mode 100644 index 000000000..f46f69bcf --- /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..68a202175 --- /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..23ae7fa3b 100644 --- a/data/cube_small.urdf +++ b/data/cube_small.urdf @@ -3,7 +3,7 @@ - + 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/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/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..aa7217502 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,8 +261,11 @@ 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), + 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/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/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/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/GripperGraspExample.cpp b/examples/RoboticsLearning/GripperGraspExample.cpp index af7211f39..434454985 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); } @@ -155,7 +156,63 @@ public: //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.068, 0.02, 0.11); + args.m_useMultiBody = true; + m_robotSim.loadFile(args, results); + + b3JointMotorArgs controlArgs(CONTROL_MODE_VELOCITY); + controlArgs.m_targetVelocity = -0.1; + controlArgs.m_maxTorqueValue = 10.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.068, 0.02, 0.11); + args.m_useMultiBody = true; + m_robotSim.loadFile(args, results); + + b3JointMotorArgs controlArgs(CONTROL_MODE_VELOCITY); + controlArgs.m_targetVelocity = 0.1; + controlArgs.m_maxTorqueValue = 10.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 +221,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/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/R2D2GraspExample.cpp b/examples/RoboticsLearning/R2D2GraspExample.cpp index 4da5cc22a..e4a412d73 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); } @@ -142,6 +140,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/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; @@ -32,6 +44,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) { } @@ -90,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/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..e622cf716 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -38,6 +38,8 @@ struct PhysicsClientSharedMemoryInternalData { btAlignedObjectArray m_cachedCameraDepthBuffer; btAlignedObjectArray m_cachedSegmentationMaskBuffer; + btAlignedObjectArray m_cachedContactPoints; + btAlignedObjectArray m_bodyIdsRequestInfo; SharedMemoryStatus m_tempBackupServerStatus; @@ -58,6 +60,7 @@ struct PhysicsClientSharedMemoryInternalData { m_counter(0), m_cachedCameraPixelsWidth(0), m_cachedCameraPixelsHeight(0), + m_isConnected(false), m_waitingForServer(false), m_hasLastServerStatus(false), @@ -558,6 +561,32 @@ 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); + + 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 +649,20 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { } } + if (serverCmd.m_type == CMD_CONTACT_POINT_INFORMATION_COMPLETED) + { + 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 +759,13 @@ 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_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..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; @@ -487,3 +538,11 @@ 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) +{ + 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 7e3ae2c94..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: @@ -64,6 +66,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 3c30f30df..03357e59f 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; @@ -507,7 +509,7 @@ void PhysicsServerCommandProcessor::setGuiHelper(struct GUIHelperInterface* guiH } m_data->m_guiHelper = guiHelper; - + } @@ -518,7 +520,7 @@ PhysicsServerCommandProcessor::PhysicsServerCommandProcessor() createEmptyDynamicsWorld(); m_data->m_dynamicsWorld->getSolverInfo().m_linearSlop = 0.0001; - + } PhysicsServerCommandProcessor::~PhysicsServerCommandProcessor() @@ -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; @@ -776,15 +780,22 @@ 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); 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,14 +900,14 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto btMultiBody* mb = creation.getBulletMultiBody(); btRigidBody* rb = creation.getRigidBody(); - + if (useMultiBody) { if (mb) { - + mb->setUserIndex2(bodyUniqueId); bodyHandle->m_multiBody = mb; createJointMotors(mb); @@ -958,6 +971,7 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto if (rb) { bodyHandle->m_rigidBody = rb; + rb->setUserIndex2(bodyUniqueId); return true; } } @@ -1024,7 +1038,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 +1168,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 +1495,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 +1579,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 +1819,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } case CMD_STEP_FORWARD_SIMULATION: { - + if (m_data->m_verboseOutput) { b3Printf("Step simulation request"); @@ -1897,7 +1911,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 +1950,10 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { m_data->m_visualConverter.resetAll(); } - + deleteDynamicsWorld(); createEmptyDynamicsWorld(); - + m_data->exitHandles(); m_data->initHandles(); @@ -2079,6 +2093,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 +2136,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 linkIndexA = -1; + int linkIndexB = -1; + + 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) + { + linkIndexB = mblB->m_link; + 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) + { + linkIndexA = mblA->m_link; + + objectIndexA = mblA->m_multiBody->getUserIndex2(); + } + + btAssert(bodyA || mblA); + + //apply the filter, if the user provides it + if (clientCmd.m_requestContactPointArguments.m_objectAIndexFilter>=0) + { + if ((clientCmd.m_requestContactPointArguments.m_objectAIndexFilter != objectIndexA) && + (clientCmd.m_requestContactPointArguments.m_objectAIndexFilter != objectIndexB)) + 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) && + (clientCmd.m_requestContactPointArguments.m_objectBIndexFilter != objectIndexB)) + continue; + } + + for (int p=0;pgetNumContacts();p++) + { + + b3ContactPointData pt; + 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_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); + } + } + } + + 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: { @@ -2128,7 +2261,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 +2273,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 +2296,11 @@ 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->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; @@ -2179,7 +2315,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_FAILED; } } - + } else { @@ -2203,7 +2339,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 +2349,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 +2372,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 +2388,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } } } - + SharedMemoryStatus& serverCmd =serverStatusOut; serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED; hasStatus = true; @@ -2514,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/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 87efcb7dd..28038a3ad 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -28,8 +28,12 @@ enum EnumSharedMemoryClientCommand CMD_REQUEST_CAMERA_IMAGE_DATA, 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 @@ -63,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 }; @@ -130,6 +136,38 @@ struct b3CameraImageData const int* m_segmentationMaskValues;//m_pixelWidth*m_pixelHeight ints }; + +struct b3ContactPointData +{ +//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; + 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. + + 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 b3ContactInformation +{ + int m_numContactPoints; + struct b3ContactPointData* m_contactPointData; +}; + ///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/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; } 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/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index fdc753443..75e4a02a5 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*/, @@ -1111,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/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; diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp index a4636c3c0..b40c6d72b 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp @@ -534,8 +534,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->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 = btVector3(0,0,0); + } else + { + btVector3 torqueAxis0 = constraintNormal; + solverConstraint.m_relpos1CrossNormal = torqueAxis0; + solverConstraint.m_contactNormal1 = btVector3(0,0,0); + 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->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 = -btVector3(0,0,0); + + } else + { + btVector3 torqueAxis1 = constraintNormal; + solverConstraint.m_relpos2CrossNormal = -torqueAxis1; + solverConstraint.m_contactNormal2 = -btVector3(0,0,0); + + 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) { @@ -572,6 +849,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()); @@ -596,8 +908,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++) { @@ -650,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(); @@ -679,9 +992,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. @@ -722,6 +1035,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);