From 41aa58560bf9ed1e7a88acc5f6d8671f7701d03f Mon Sep 17 00:00:00 2001 From: = <=> Date: Mon, 15 Jun 2015 23:12:29 -0700 Subject: [PATCH] add TestHingeTorque example using btRigidBody and btHingeConstraint, setup is similar to TestJointTorque using btMultiBody and a revolute joint. --- examples/Constraints/TestHingeTorque.cpp | 153 ++++++++++++++++++ examples/Constraints/TestHingeTorque.h | 7 + examples/ExampleBrowser/CMakeLists.txt | 2 + examples/ExampleBrowser/ExampleEntries.cpp | 8 +- examples/MultiBody/TestJointTorqueSetup.cpp | 38 +++-- .../Featherstone/btMultiBodyLink.h | 9 +- 6 files changed, 203 insertions(+), 14 deletions(-) create mode 100644 examples/Constraints/TestHingeTorque.cpp create mode 100644 examples/Constraints/TestHingeTorque.h diff --git a/examples/Constraints/TestHingeTorque.cpp b/examples/Constraints/TestHingeTorque.cpp new file mode 100644 index 000000000..4842e7f45 --- /dev/null +++ b/examples/Constraints/TestHingeTorque.cpp @@ -0,0 +1,153 @@ +#include "TestHingeTorque.h" + + +#include "../CommonInterfaces/CommonRigidBodyBase.h" +#include "../CommonInterfaces/CommonParameterInterface.h" + + + +struct TestHingeTorque : public CommonRigidBodyBase +{ + bool m_once; + + TestHingeTorque(struct GUIHelperInterface* helper); + virtual ~ TestHingeTorque(); + virtual void initPhysics(); + + virtual void stepSimulation(float deltaTime); + + + virtual void resetCamera() + { + + float dist = 5; + float pitch = 270; + float yaw = 21; + float targetPos[3]={-1.34,3.4,-0.44}; + m_guiHelper->resetCamera(dist,pitch,yaw,targetPos[0],targetPos[1],targetPos[2]); + } + + +}; + +TestHingeTorque::TestHingeTorque(struct GUIHelperInterface* helper) +:CommonRigidBodyBase(helper), +m_once(true) +{ +} +TestHingeTorque::~ TestHingeTorque() +{ +} + + +void TestHingeTorque::stepSimulation(float deltaTime) +{ + if (m_once) + { + m_once=false; + btHingeConstraint* hinge = (btHingeConstraint*)m_dynamicsWorld->getConstraint(0); + + btRigidBody& bodyA = hinge->getRigidBodyA(); + btTransform trA = bodyA.getWorldTransform(); + btVector3 hingeAxisInWorld = trA.getBasis()*hinge->getFrameOffsetA().getBasis().getColumn(2); + hinge->getRigidBodyA().applyTorque(-hingeAxisInWorld*10); + hinge->getRigidBodyB().applyTorque(hingeAxisInWorld*10); + + } + + m_dynamicsWorld->stepSimulation(1./60,0); + btRigidBody* base = btRigidBody::upcast(m_dynamicsWorld->getCollisionObjectArray()[0]); + + b3Printf("base angvel = %f,%f,%f",base->getAngularVelocity()[0], + base->getAngularVelocity()[1], + + base->getAngularVelocity()[2]); + + btRigidBody* child = btRigidBody::upcast(m_dynamicsWorld->getCollisionObjectArray()[1]); + + b3Printf("child angvel = %f,%f,%f",child->getAngularVelocity()[0], + child->getAngularVelocity()[1], + + child->getAngularVelocity()[2]); + + //CommonRigidBodyBase::stepSimulation(deltaTime); +} + + + +void TestHingeTorque::initPhysics() +{ + m_guiHelper->setUpAxis(2); + + createEmptyDynamicsWorld(); + m_dynamicsWorld->setGravity(btVector3(0,0,0)); + + m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); + int mode = btIDebugDraw::DBG_DrawWireframe + +btIDebugDraw::DBG_DrawConstraints + +btIDebugDraw::DBG_DrawConstraintLimits; + m_dynamicsWorld->getDebugDrawer()->setDebugMode(mode); + + + { // create a door using hinge constraint attached to the world + + int numLinks = 1; + bool spherical = false; //set it ot false -to use 1DoF hinges instead of 3DoF sphericals + bool canSleep = false; + bool selfCollide = false; + btVector3 linkHalfExtents(0.05, 0.37, 0.1); + btVector3 baseHalfExtents(0.05, 0.37, 0.1); + + btBoxShape* baseBox = new btBoxShape(baseHalfExtents); + btVector3 basePosition = btVector3(-0.4f, 3.f, 0.f); + btTransform baseWorldTrans; + baseWorldTrans.setIdentity(); + baseWorldTrans.setOrigin(basePosition); + + //mbC->forceMultiDof(); //if !spherical, you can comment this line to check the 1DoF algorithm + //init the base + btVector3 baseInertiaDiag(0.f, 0.f, 0.f); + float baseMass = 1.f; + float linkMass = 1.f; + + btRigidBody* base = createRigidBody(baseMass,baseWorldTrans,baseBox); + m_dynamicsWorld->removeRigidBody(base); + base->setDamping(0,0); + m_dynamicsWorld->addRigidBody(base,0,0); + btBoxShape* linkBox = new btBoxShape(linkHalfExtents); + btRigidBody* prevBody = base; + + for (int i=0;iremoveRigidBody(linkBody); + m_dynamicsWorld->addRigidBody(linkBody,0,0); + linkBody->setDamping(0,0); + //create a hinge constraint + btVector3 pivotInA(0,-linkHalfExtents[1],0); + btVector3 pivotInB(0,linkHalfExtents[1],0); + btVector3 axisInA(1,0,0); + btVector3 axisInB(1,0,0); + bool useReferenceA = true; + btHingeConstraint* hinge = new btHingeConstraint(*prevBody,*linkBody, + pivotInA,pivotInB, + axisInA,axisInB,useReferenceA); + m_dynamicsWorld->addConstraint(hinge,true); + prevBody = linkBody; + + } + + } + + m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); +} + +class CommonExampleInterface* TestHingeTorqueCreateFunc(CommonExampleOptions& options) +{ + return new TestHingeTorque(options.m_guiHelper); +} diff --git a/examples/Constraints/TestHingeTorque.h b/examples/Constraints/TestHingeTorque.h new file mode 100644 index 000000000..5c2bf1d2c --- /dev/null +++ b/examples/Constraints/TestHingeTorque.h @@ -0,0 +1,7 @@ +#ifndef TEST_HINGE_TORQUE_H +#define TEST_HINGE_TORQUE_H + +class CommonExampleInterface* TestHingeTorqueCreateFunc(struct CommonExampleOptions& options); + +#endif //TEST_HINGE_TORQUE_H + diff --git a/examples/ExampleBrowser/CMakeLists.txt b/examples/ExampleBrowser/CMakeLists.txt index e07ccad94..916ec1143 100644 --- a/examples/ExampleBrowser/CMakeLists.txt +++ b/examples/ExampleBrowser/CMakeLists.txt @@ -97,6 +97,8 @@ SET(App_ExampleBrowser_SRCS ../MultiBody/TestJointTorqueSetup.h ../MultiBody/MultiDofDemo.cpp ../MultiBody/MultiDofDemo.h + ../Constraints/TestHingeTorque.cpp + ../Constraints/TestHingeTorque.h ../Constraints/ConstraintDemo.cpp ../Constraints/ConstraintDemo.h ../Constraints/Dof6Spring2Setup.cpp diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp index 8c6c51dac..a636e28dd 100644 --- a/examples/ExampleBrowser/ExampleEntries.cpp +++ b/examples/ExampleBrowser/ExampleEntries.cpp @@ -32,6 +32,8 @@ #include "../RollingFrictionDemo/RollingFrictionDemo.h" #include "../SharedMemory/PhysicsServer.h" #include "../SharedMemory/PhysicsClient.h" +#include "../Constraints/TestHingeTorque.h" + #ifdef ENABLE_LUA #include "../LuaDemo/LuaPhysicsSetup.h" @@ -79,7 +81,9 @@ static ExampleEntry gDefaultExamples[]= AllConstraintCreateFunc), 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.", @@ -94,7 +98,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).", TestJointTorqueCreateFunc), + ExampleEntry(1,"TestJointTorque","Apply a torque to a btMultiBody with 1-DOF joints (mobilizers). This setup is similar to API/TestHingeTorque.", TestJointTorqueCreateFunc), #ifdef INCLUDE_CLOTH_DEMOS diff --git a/examples/MultiBody/TestJointTorqueSetup.cpp b/examples/MultiBody/TestJointTorqueSetup.cpp index 9d14ecbfd..2927904e9 100644 --- a/examples/MultiBody/TestJointTorqueSetup.cpp +++ b/examples/MultiBody/TestJointTorqueSetup.cpp @@ -8,7 +8,7 @@ struct TestJointTorqueSetup : public CommonMultiBodyBase { btMultiBody* m_multiBody; - + bool m_once; public: TestJointTorqueSetup(struct GUIHelperInterface* helper); @@ -31,7 +31,8 @@ public: }; TestJointTorqueSetup::TestJointTorqueSetup(struct GUIHelperInterface* helper) -:CommonMultiBodyBase(helper) +:CommonMultiBodyBase(helper), +m_once(true) { } @@ -88,10 +89,10 @@ void TestJointTorqueSetup::initPhysics() } { - bool floating = false; - bool damping = true; - bool gyro = true; - int numLinks = 5; + bool floating = true; + bool damping = false; + bool gyro = false; + int numLinks = 1; bool spherical = false; //set it ot false -to use 1DoF hinges instead of 3DoF sphericals bool canSleep = false; bool selfCollide = false; @@ -173,7 +174,7 @@ void TestJointTorqueSetup::initPhysics() //gravity[upAxis] = -9.81; m_dynamicsWorld->setGravity(gravity); ////////////////////////////////////////////// - if(numLinks > 0) + if(0)//numLinks > 0) { btScalar q0 = 45.f * SIMD_PI/ 180.f; if(!spherical) @@ -275,8 +276,27 @@ void TestJointTorqueSetup::initPhysics() void TestJointTorqueSetup::stepSimulation(float deltaTime) { - m_multiBody->addJointTorque(0, 10.0); - m_dynamicsWorld->stepSimulation(deltaTime); + if (m_once) + { + m_once=false; + m_multiBody->addJointTorque(0, 10.0); + + btScalar torque = m_multiBody->getJointTorque(0); + b3Printf("t = %f,%f,%f\n",torque,torque,torque);//[0],torque[1],torque[2]); + } + + m_dynamicsWorld->stepSimulation(1./60,0); + b3Printf("base angvel = %f,%f,%f",m_multiBody->getBaseOmega()[0], + m_multiBody->getBaseOmega()[1], + m_multiBody->getBaseOmega()[2] + ); + + btScalar jointVel =m_multiBody->getJointVel(0); + + b3Printf("child angvel = %f",jointVel); + + + } diff --git a/src/BulletDynamics/Featherstone/btMultiBodyLink.h b/src/BulletDynamics/Featherstone/btMultiBodyLink.h index 1ae859aaa..93538e594 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyLink.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyLink.h @@ -120,11 +120,14 @@ struct btMultibodyLink btVector3 m_cachedRVector; // vector from COM of parent to COM of this link, in local frame. btVector3 m_appliedForce; // In WORLD frame - btVector3 m_appliedTorque; // In WORLD frame + btVector3 m_appliedTorque; // In WORLD frame btScalar m_jointPos[7]; - btScalar m_jointTorque[6]; //TODO - + + //m_jointTorque is the joint torque applied by the user using 'addJointTorque'. + //It gets set to zero after each internal stepSimulation call + btScalar m_jointTorque[6]; + class btMultiBodyLinkCollider* m_collider; int m_flags;