From 34317738001d0aa0042a975e3f886c481055661d Mon Sep 17 00:00:00 2001 From: = <=> Date: Mon, 6 Jul 2015 23:00:46 -0700 Subject: [PATCH] expose btMultiBodyConstraint applied impulse (force) on its degree of freedom(s), only tested for btMultiBodyJointMotor for now. See also MultiBody/MultiBodyConstraintFeedback example --- examples/ExampleBrowser/CMakeLists.txt | 1 + examples/ExampleBrowser/ExampleEntries.cpp | 3 ++- examples/ExampleBrowser/premake4.lua | 1 + examples/MultiBody/TestJointTorqueSetup.cpp | 10 ++++++++-- .../Featherstone/btMultiBodyConstraint.h | 14 ++++++++++++++ .../Featherstone/btMultiBodyConstraintSolver.cpp | 12 ++++++++++-- .../btMultiBodyJointLimitConstraint.cpp | 4 +++- .../Featherstone/btMultiBodyJointMotor.cpp | 4 ++-- .../Featherstone/btMultiBodyPoint2Point.cpp | 5 ++++- .../Featherstone/btMultiBodySolverConstraint.h | 7 +++++-- 10 files changed, 50 insertions(+), 11 deletions(-) diff --git a/examples/ExampleBrowser/CMakeLists.txt b/examples/ExampleBrowser/CMakeLists.txt index edfa38828..b657938e3 100644 --- a/examples/ExampleBrowser/CMakeLists.txt +++ b/examples/ExampleBrowser/CMakeLists.txt @@ -98,6 +98,7 @@ SET(App_ExampleBrowser_SRCS ../Vehicles/Hinge2Vehicle.h ../MultiBody/TestJointTorqueSetup.cpp ../MultiBody/TestJointTorqueSetup.h + ../MultiBody/MultiBodyConstraintFeedback.cpp ../MultiBody/MultiDofDemo.cpp ../MultiBody/MultiDofDemo.h ../Constraints/TestHingeTorque.cpp diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp index a636e28dd..6fa29abf6 100644 --- a/examples/ExampleBrowser/ExampleEntries.cpp +++ b/examples/ExampleBrowser/ExampleEntries.cpp @@ -20,6 +20,7 @@ #include "../Constraints/Dof6Spring2Setup.h" #include "../Constraints/ConstraintPhysicsSetup.h" #include "../MultiBody/TestJointTorqueSetup.h" +#include "../MultiBody/MultiBodyConstraintFeedback.h" #include "../MultiBody/MultiDofDemo.h" #include "../VoronoiFracture/VoronoiFractureDemo.h" #include "../SoftDemo/SoftDemo.h" @@ -83,7 +84,6 @@ static ExampleEntry gDefaultExamples[]= ExampleEntry(1,"Motorized Hinge","Use of a btHingeConstraint. You can adjust the first slider to change the target velocity, and the second slider to adjust the maximum impulse applied to reach the target velocity. Note that the hinge angle can reach beyond -360 and 360 degrees.", ConstraintCreateFunc), ExampleEntry(1,"TestHingeTorque", "Apply a torque in the hinge axis. This example uses a btHingeConstraint and btRigidBody. The setup is similar to the multi body example TestJointTorque.", TestHingeTorqueCreateFunc), - // ExampleEntry(0,"What's new in 2.83"), ExampleEntry(1,"6DofSpring2","Show the use of the btGeneric6DofSpring2Constraint. This is a replacement of the btGeneric6DofSpringConstraint, it has various improvements. This includes improved spring implementation and better control over the restitution (bounce) when the constraint hits its limits.", @@ -99,6 +99,7 @@ static ExampleEntry gDefaultExamples[]= ExampleEntry(0,"MultiBody"), ExampleEntry(1,"MultiDofCreateFunc","Create a basic btMultiBody with 3-DOF spherical joints (mobilizers). The demo uses a fixed base or a floating base at restart.", MultiDofCreateFunc), ExampleEntry(1,"TestJointTorque","Apply a torque to a btMultiBody with 1-DOF joints (mobilizers). This setup is similar to API/TestHingeTorque.", TestJointTorqueCreateFunc), + ExampleEntry(1,"Constraint Feedback", "The example shows how to receive joint reaction forces in a btMultiBody. Also the applied impulse is available for a btMultiBodyJointMotor", MultiBodyConstraintFeedbackCreateFunc), #ifdef INCLUDE_CLOTH_DEMOS diff --git a/examples/ExampleBrowser/premake4.lua b/examples/ExampleBrowser/premake4.lua index 038e3b3eb..dde3d4b34 100644 --- a/examples/ExampleBrowser/premake4.lua +++ b/examples/ExampleBrowser/premake4.lua @@ -73,6 +73,7 @@ "../Raycast/*", "../MultiBody/MultiDofDemo.cpp", "../MultiBody/TestJointTorqueSetup.cpp", + "../MultiBody/MultiBodyConstraintFeedback.cpp", "../ThirdPartyLibs/stb_image/*", "../ThirdPartyLibs/Wavefront/tiny_obj_loader.*", "../ThirdPartyLibs/tinyxml/*", diff --git a/examples/MultiBody/TestJointTorqueSetup.cpp b/examples/MultiBody/TestJointTorqueSetup.cpp index 300ced7db..e8dd19ae7 100644 --- a/examples/MultiBody/TestJointTorqueSetup.cpp +++ b/examples/MultiBody/TestJointTorqueSetup.cpp @@ -6,7 +6,7 @@ #include "../CommonInterfaces/CommonMultiBodyBase.h" -btScalar radius(0.2); +static btScalar radius(0.2); struct TestJointTorqueSetup : public CommonMultiBodyBase { @@ -48,10 +48,16 @@ TestJointTorqueSetup::~TestJointTorqueSetup() ///this is a temporary global, until we determine if we need the option or not extern bool gJointFeedbackInWorldSpace; +extern bool gJointFeedbackInJointFrame; + + + void TestJointTorqueSetup::initPhysics() { int upAxis = 1; gJointFeedbackInWorldSpace = true; + gJointFeedbackInJointFrame = true; + m_guiHelper->setUpAxis(upAxis); btVector4 colors[4] = @@ -417,4 +423,4 @@ void TestJointTorqueSetup::stepSimulation(float deltaTime) class CommonExampleInterface* TestJointTorqueCreateFunc(struct CommonExampleOptions& options) { return new TestJointTorqueSetup(options.m_guiHelper); -} \ No newline at end of file +} diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h index 2c78373f5..3b32b46e9 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h @@ -58,6 +58,7 @@ protected: btScalar m_maxAppliedImpulse; + // warning: the data block lay out is not consistent for all constraints // data block laid out as follows: // cached impulses. (one per row.) // jacobians. (interleaved, row1 body1 then row1 body2 then row2 body 1 etc) @@ -108,6 +109,19 @@ public: return m_bodyB; } + void internalSetAppliedImpulse(int dof, btScalar appliedImpulse) + { + btAssert(dof>=0); + btAssert(dof < getNumRows()); + m_data[dof] = appliedImpulse; + } + + btScalar getAppliedImpulse(int dof) + { + btAssert(dof>=0); + btAssert(dof < getNumRows()); + return m_data[dof]; + } // current constraint position // constraint is pos >= 0 for unilateral, or pos = 0 for bilateral // NOTE: ignored position for friction rows. diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp index 3e7853ead..1f355eee6 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp @@ -612,7 +612,9 @@ btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodyFrictionCo { BT_PROFILE("addMultiBodyFrictionConstraint"); btMultiBodySolverConstraint& solverConstraint = m_multiBodyFrictionContactConstraints.expandNonInitializing(); - solverConstraint.m_useJointForce = false; + solverConstraint.m_orgConstraint = 0; + solverConstraint.m_orgDofIndex = -1; + solverConstraint.m_frictionIndex = frictionIndex; bool isFriction = true; @@ -680,10 +682,11 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold* int frictionIndex = m_multiBodyNormalContactConstraints.size(); btMultiBodySolverConstraint& solverConstraint = m_multiBodyNormalContactConstraints.expandNonInitializing(); - solverConstraint.m_useJointForce = false; // btRigidBody* rb0 = btRigidBody::upcast(colObj0); // btRigidBody* rb1 = btRigidBody::upcast(colObj1); + solverConstraint.m_orgConstraint = 0; + solverConstraint.m_orgDofIndex = -1; solverConstraint.m_solverBodyIdA = solverBodyIdA; solverConstraint.m_solverBodyIdB = solverBodyIdB; solverConstraint.m_multiBodyA = mbA; @@ -891,6 +894,11 @@ void btMultiBodyConstraintSolver::writeBackSolverBodyToMultiBody(btMultiBodySolv //bod->addBaseForce(m_gravity * bod->getBaseMass()); //bod->addLinkForce(j, m_gravity * bod->getLinkMass(j)); + + if (c.m_orgConstraint) + { + c.m_orgConstraint->internalSetAppliedImpulse(c.m_orgDofIndex,c.m_appliedImpulse); + } if (c.m_multiBodyA) diff --git a/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp b/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp index db4778917..b2d864139 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp @@ -114,6 +114,9 @@ void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraint btScalar direction = row? -1 : 1; btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing(); + constraintRow.m_orgConstraint = this; + constraintRow.m_orgDofIndex = row; + constraintRow.m_multiBodyA = m_bodyA; constraintRow.m_multiBodyB = m_bodyB; const btScalar posError = 0; //why assume it's zero? @@ -123,7 +126,6 @@ void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraint if (m_bodyA->isMultiDof()) { - constraintRow.m_useJointForce = false; //expect either prismatic or revolute joint type for now btAssert((m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eRevolute)||(m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::ePrismatic)); switch (m_bodyA->getLink(m_linkA).m_jointType) diff --git a/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp b/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp index be7aff045..fc718fd0d 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp @@ -116,10 +116,10 @@ void btMultiBodyJointMotor::createConstraintRows(btMultiBodyConstraintArray& con fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,posError,infoGlobal,-m_maxAppliedImpulse,m_maxAppliedImpulse,1,false,m_desiredVelocity); - + constraintRow.m_orgConstraint = this; + constraintRow.m_orgDofIndex = row; if (m_bodyA->isMultiDof()) { - constraintRow.m_useJointForce = false; //expect either prismatic or revolute joint type for now btAssert((m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eRevolute)||(m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::ePrismatic)); switch (m_bodyA->getLink(m_linkA).m_jointType) diff --git a/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp b/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp index 4e5911e00..2ccb9827d 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp @@ -33,6 +33,7 @@ btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* body, int link, btRi m_pivotInA(pivotInA), m_pivotInB(pivotInB) { + m_data.resize(BTMBP2PCONSTRAINT_DIM);//at least store the applied impulses } btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB) @@ -42,6 +43,7 @@ btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* bodyA, int linkA, bt m_pivotInA(pivotInA), m_pivotInB(pivotInB) { + m_data.resize(BTMBP2PCONSTRAINT_DIM);//at least store the applied impulses } void btMultiBodyPoint2Point::finalizeMultiDof() @@ -108,7 +110,8 @@ int numDim = BTMBP2PCONSTRAINT_DIM; btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing(); //memset(&constraintRow,0xffffffff,sizeof(btMultiBodySolverConstraint)); - constraintRow.m_useJointForce = false; + constraintRow.m_orgConstraint = this; + constraintRow.m_orgDofIndex = i; constraintRow.m_relpos1CrossNormal.setValue(0,0,0); constraintRow.m_contactNormal1.setValue(0,0,0); constraintRow.m_relpos2CrossNormal.setValue(0,0,0); diff --git a/src/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h b/src/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h index 5a8580913..6fa1550e9 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h +++ b/src/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h @@ -20,6 +20,7 @@ subject to the following restrictions: #include "LinearMath/btAlignedObjectArray.h" class btMultiBody; +class btMultiBodyConstraint; #include "BulletDynamics/ConstraintSolver/btSolverBody.h" #include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h" @@ -28,7 +29,7 @@ ATTRIBUTE_ALIGNED16 (struct) btMultiBodySolverConstraint { BT_DECLARE_ALIGNED_ALLOCATOR(); - btMultiBodySolverConstraint() : m_solverBodyIdA(-1), m_multiBodyA(0), m_linkA(-1), m_solverBodyIdB(-1), m_multiBodyB(0), m_linkB(-1),m_useJointForce(false) + btMultiBodySolverConstraint() : m_solverBodyIdA(-1), m_multiBodyA(0), m_linkA(-1), m_solverBodyIdB(-1), m_multiBodyB(0), m_linkB(-1),m_orgConstraint(0), m_orgDofIndex(-1) {} int m_deltaVelAindex;//more generic version of m_relpos1CrossNormal/m_contactNormal1 @@ -73,7 +74,9 @@ ATTRIBUTE_ALIGNED16 (struct) btMultiBodySolverConstraint btMultiBody* m_multiBodyB; int m_linkB; - bool m_useJointForce;//needed for write-back of joint versus link/base force/torque + //for writing back applied impulses + btMultiBodyConstraint* m_orgConstraint; + int m_orgDofIndex; enum btSolverConstraintType {