diff --git a/data/sphere2.urdf b/data/sphere2.urdf index 35bf786a6..891e018cb 100644 --- a/data/sphere2.urdf +++ b/data/sphere2.urdf @@ -43,5 +43,31 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/examples/Constraints/TestHingeTorque.cpp b/examples/Constraints/TestHingeTorque.cpp index 8afec175c..3e321145c 100644 --- a/examples/Constraints/TestHingeTorque.cpp +++ b/examples/Constraints/TestHingeTorque.cpp @@ -5,8 +5,8 @@ #include "../CommonInterfaces/CommonParameterInterface.h" short collisionFilterGroup = short(btBroadphaseProxy::CharacterFilter); -short collisionFilterMask = short(btBroadphaseProxy::AllFilter ^ (btBroadphaseProxy::StaticFilter|btBroadphaseProxy::CharacterFilter)); - +short collisionFilterMask = short(btBroadphaseProxy::AllFilter ^ (btBroadphaseProxy::CharacterFilter)); +static btScalar radius(0.2); struct TestHingeTorque : public CommonRigidBodyBase { @@ -50,7 +50,7 @@ TestHingeTorque::~ TestHingeTorque() void TestHingeTorque::stepSimulation(float deltaTime) { - if (m_once) + if (0)//m_once) { m_once=false; btHingeConstraint* hinge = (btHingeConstraint*)m_dynamicsWorld->getConstraint(0); @@ -63,37 +63,41 @@ void TestHingeTorque::stepSimulation(float deltaTime) } - 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]); - - for (int i=0;istepSimulation(1./240,0); + + static int count = 0; + if ((count& 0x0f)==0) { - b3Printf("Applied force A:(%f,%f,%f), torque A:(%f,%f,%f)\nForce B:(%f,%f,%f), torque B:(%f,%f,%f)\n", - m_jointFeedback[i]->m_appliedForceBodyA.x(), - m_jointFeedback[i]->m_appliedForceBodyA.y(), - m_jointFeedback[i]->m_appliedForceBodyA.z(), - m_jointFeedback[i]->m_appliedTorqueBodyA.x(), - m_jointFeedback[i]->m_appliedTorqueBodyA.y(), - m_jointFeedback[i]->m_appliedTorqueBodyA.z(), - m_jointFeedback[i]->m_appliedForceBodyB.x(), - m_jointFeedback[i]->m_appliedForceBodyB.y(), - m_jointFeedback[i]->m_appliedForceBodyB.z(), - m_jointFeedback[i]->m_appliedTorqueBodyB.x(), - m_jointFeedback[i]->m_appliedTorqueBodyB.y(), - m_jointFeedback[i]->m_appliedTorqueBodyB.z()); + 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]); + + for (int i=0;im_appliedForceBodyB.x(), + m_jointFeedback[i]->m_appliedForceBodyB.y(), + m_jointFeedback[i]->m_appliedForceBodyB.z(), + m_jointFeedback[i]->m_appliedTorqueBodyB.x(), + m_jointFeedback[i]->m_appliedTorqueBodyB.y(), + m_jointFeedback[i]->m_appliedTorqueBodyB.z()); + } } + count++; + //CommonRigidBodyBase::stepSimulation(deltaTime); } @@ -101,10 +105,13 @@ void TestHingeTorque::stepSimulation(float deltaTime) void TestHingeTorque::initPhysics() { - m_guiHelper->setUpAxis(1); + int upAxis = 1; + m_guiHelper->setUpAxis(upAxis); createEmptyDynamicsWorld(); -// m_dynamicsWorld->setGravity(btVector3(0,0,0)); + m_dynamicsWorld->getSolverInfo().m_splitImpulse = false; + + m_dynamicsWorld->setGravity(btVector3(0,-1,-10)); m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); int mode = btIDebugDraw::DBG_DrawWireframe @@ -115,7 +122,7 @@ void TestHingeTorque::initPhysics() { // create a door using hinge constraint attached to the world - int numLinks = 1; + int numLinks = 2; bool spherical = false; //set it ot false -to use 1DoF hinges instead of 3DoF sphericals bool canSleep = false; bool selfCollide = false; @@ -138,7 +145,9 @@ void TestHingeTorque::initPhysics() m_dynamicsWorld->removeRigidBody(base); base->setDamping(0,0); m_dynamicsWorld->addRigidBody(base,collisionFilterGroup,collisionFilterMask); - btBoxShape* linkBox = new btBoxShape(linkHalfExtents); + btBoxShape* linkBox1 = new btBoxShape(linkHalfExtents); + btSphereShape* linkSphere = new btSphereShape(radius); + btRigidBody* prevBody = base; for (int i=0;iremoveRigidBody(linkBody); m_dynamicsWorld->addRigidBody(linkBody,collisionFilterGroup,collisionFilterMask); 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); - btJointFeedback* fb = new btJointFeedback(); - m_jointFeedback.push_back(fb); - hinge->setJointFeedback(fb); + btTypedConstraint* con = 0; + + if (i==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); + con = hinge; + } else + { + + btTransform pivotInA(btQuaternion::getIdentity(),btVector3(0, -radius, 0)); //par body's COM to cur body's COM offset + btTransform pivotInB(btQuaternion::getIdentity(),btVector3(0, radius, 0)); //cur body's COM to cur body's PIV offset + btGeneric6DofSpring2Constraint* fixed = new btGeneric6DofSpring2Constraint(*prevBody, *linkBody, + pivotInA,pivotInB); + fixed->setLinearLowerLimit(btVector3(0,0,0)); + fixed->setLinearUpperLimit(btVector3(0,0,0)); + fixed->setAngularLowerLimit(btVector3(0,0,0)); + fixed->setAngularUpperLimit(btVector3(0,0,0)); + + con = fixed; - m_dynamicsWorld->addConstraint(hinge,true); - prevBody = linkBody; + } + btAssert(con); + if (con) + { + btJointFeedback* fb = new btJointFeedback(); + m_jointFeedback.push_back(fb); + con->setJointFeedback(fb); + + m_dynamicsWorld->addConstraint(con,true); + } + prevBody = linkBody; } } + if (1) + { + btVector3 groundHalfExtents(1,1,0.2); + groundHalfExtents[upAxis]=1.f; + btBoxShape* box = new btBoxShape(groundHalfExtents); + box->initializePolyhedralFeatures(); + + btTransform start; start.setIdentity(); + btVector3 groundOrigin(-0.4f, 3.f, 0.f); + btVector3 basePosition = btVector3(-0.4f, 3.f, 0.f); + btQuaternion groundOrn(btVector3(0,1,0),0.25*SIMD_PI); + + groundOrigin[upAxis] -=.5; + groundOrigin[2]-=0.6; + start.setOrigin(groundOrigin); + // start.setRotation(groundOrn); + btRigidBody* body = createRigidBody(0,start,box); + body->setFriction(0); + + } m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); } diff --git a/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp b/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp index fbcde7d9b..8769920e8 100644 --- a/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp +++ b/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp @@ -63,11 +63,18 @@ struct ImportUrdfInternalData ImportUrdfInternalData() :m_numMotors(0) { + for (int i=0;isetGravity(gravity); @@ -210,9 +222,7 @@ void ImportUrdfSetup::initPhysics() if (m_useMultiBody) { - - - //create motors for each joint + //create motors for each btMultiBody joint for (int i=0;igetNumLinks();i++) { @@ -236,6 +246,7 @@ void ImportUrdfSetup::initPhysics() m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); float maxMotorImpulse = 0.1f; btMultiBodyJointMotor* motor = new btMultiBodyJointMotor(mb,mbLinkIndex,0,0,maxMotorImpulse); + //motor->setMaxAppliedImpulse(0); m_data->m_jointMotors[m_data->m_numMotors]=motor; m_dynamicsWorld->addMultiBodyConstraint(motor); m_data->m_numMotors++; @@ -243,6 +254,44 @@ void ImportUrdfSetup::initPhysics() } } + } else + { + if (1) + { + //create motors for each generic joint + for (int i=0;igetUserConstraintPtr()) + { + GenericConstraintUserInfo* jointInfo = (GenericConstraintUserInfo*)c->getUserConstraintPtr(); + if ((jointInfo->m_urdfJointType ==URDFRevoluteJoint) || + (jointInfo->m_urdfJointType ==URDFPrismaticJoint) || + (jointInfo->m_urdfJointType ==URDFContinuousJoint)) + { + int urdfLinkIndex = jointInfo->m_urdfIndex; + std::string jointName = u2b.getJointName(urdfLinkIndex); + char motorName[1024]; + sprintf(motorName,"%s q'", jointName.c_str()); + btScalar* motorVel = &m_data->m_motorTargetVelocities[m_data->m_numMotors]; + + *motorVel = 0.f; + SliderParams slider(motorName,motorVel); + slider.m_minVal=-4; + slider.m_maxVal=4; + m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); + m_data->m_generic6DofJointMotors[m_data->m_numMotors]=c; + bool motorOn = true; + c->enableMotor(jointInfo->m_jointAxisIndex,motorOn); + c->setMaxMotorForce(jointInfo->m_jointAxisIndex,10000); + c->setTargetVelocity(jointInfo->m_jointAxisIndex,0); + + m_data->m_numMotors++; + } + } + } + } + } } @@ -286,7 +335,16 @@ void ImportUrdfSetup::stepSimulation(float deltaTime) { for (int i=0;im_numMotors;i++) { - m_data->m_jointMotors[i]->setVelocityTarget(m_data->m_motorTargetVelocities[i]); + if (m_data->m_jointMotors[i]) + { + m_data->m_jointMotors[i]->setVelocityTarget(m_data->m_motorTargetVelocities[i]); + } + if (m_data->m_generic6DofJointMotors[i]) + { + GenericConstraintUserInfo* jointInfo = (GenericConstraintUserInfo*)m_data->m_generic6DofJointMotors[i]->getUserConstraintPtr(); + m_data->m_generic6DofJointMotors[i]->setTargetVelocity(jointInfo->m_jointAxisIndex,m_data->m_motorTargetVelocities[i]); + //jointInfo-> + } } //the maximal coordinates/iterative MLCP solver requires a smallish timestep to converge diff --git a/examples/Importers/ImportURDFDemo/MultiBodyCreationInterface.h b/examples/Importers/ImportURDFDemo/MultiBodyCreationInterface.h index 04cd5bd93..2cbc2f5e2 100644 --- a/examples/Importers/ImportURDFDemo/MultiBodyCreationInterface.h +++ b/examples/Importers/ImportURDFDemo/MultiBodyCreationInterface.h @@ -21,6 +21,13 @@ public: virtual class btGeneric6DofSpring2Constraint* allocateGeneric6DofSpring2Constraint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB, int rotateOrder=0) = 0; + virtual class btGeneric6DofSpring2Constraint* createPrismaticJoint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB, + const btVector3& jointAxisInJointSpace,btScalar jointLowerLimit,btScalar jointUpperLimit) = 0; + virtual class btGeneric6DofSpring2Constraint* createRevoluteJoint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB, + const btVector3& jointAxisInJointSpace,btScalar jointLowerLimit,btScalar jointUpperLimit) = 0; + + virtual class btGeneric6DofSpring2Constraint* createFixedJoint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB) = 0; + virtual class btMultiBodyLinkCollider* allocateMultiBodyLinkCollider(int urdfLinkIndex, int mbLinkIndex, btMultiBody* body) = 0; virtual void addLinkMapping(int urdfLinkIndex, int mbLinkIndex) = 0; diff --git a/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp b/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp index 60226271b..aa249d538 100644 --- a/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp +++ b/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp @@ -9,7 +9,7 @@ #include "btBulletDynamicsCommon.h" #include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h" - +#include "URDFJointTypes.h" MyMultiBodyCreator::MyMultiBodyCreator(GUIHelperInterface* guiHelper) :m_guiHelper(guiHelper), @@ -42,12 +42,147 @@ class btMultiBodyLinkCollider* MyMultiBodyCreator::allocateMultiBodyLinkCollider } + + class btGeneric6DofSpring2Constraint* MyMultiBodyCreator::allocateGeneric6DofSpring2Constraint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB, int rotateOrder) { btGeneric6DofSpring2Constraint* c = new btGeneric6DofSpring2Constraint(rbA,rbB,offsetInA, offsetInB, (RotateOrder)rotateOrder); + return c; } +class btGeneric6DofSpring2Constraint* MyMultiBodyCreator::createPrismaticJoint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB, + const btVector3& jointAxisInJointSpace,btScalar jointLowerLimit,btScalar jointUpperLimit) +{ + int rotateOrder=0; + btGeneric6DofSpring2Constraint* dof6 = allocateGeneric6DofSpring2Constraint(urdfLinkIndex, rbA , rbB, offsetInA, offsetInB, rotateOrder); + //todo(erwincoumans) for now, we only support principle axis along X, Y or Z + int principleAxis = jointAxisInJointSpace.closestAxis(); + + GenericConstraintUserInfo* userInfo = new GenericConstraintUserInfo; + userInfo->m_jointAxisInJointSpace = jointAxisInJointSpace; + userInfo->m_jointAxisIndex = principleAxis; + + userInfo->m_urdfJointType = URDFPrismaticJoint; + userInfo->m_lowerJointLimit = jointLowerLimit; + userInfo->m_upperJointLimit = jointUpperLimit; + userInfo->m_urdfIndex = urdfLinkIndex; + dof6->setUserConstraintPtr(userInfo); + + + switch (principleAxis) + { + case 0: + { + dof6->setLinearLowerLimit(btVector3(jointLowerLimit,0,0)); + dof6->setLinearUpperLimit(btVector3(jointUpperLimit,0,0)); + break; + } + case 1: + { + dof6->setLinearLowerLimit(btVector3(0,jointLowerLimit,0)); + dof6->setLinearUpperLimit(btVector3(0,jointUpperLimit,0)); + break; + } + case 2: + default: + { + dof6->setLinearLowerLimit(btVector3(0,0,jointLowerLimit)); + dof6->setLinearUpperLimit(btVector3(0,0,jointUpperLimit)); + } + }; + + dof6->setAngularLowerLimit(btVector3(0,0,0)); + dof6->setAngularUpperLimit(btVector3(0,0,0)); + m_6DofConstraints.push_back(dof6); + return dof6; +} + +class btGeneric6DofSpring2Constraint* MyMultiBodyCreator::createRevoluteJoint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB, + const btVector3& jointAxisInJointSpace,btScalar jointLowerLimit,btScalar jointUpperLimit) +{ + btGeneric6DofSpring2Constraint* dof6 = 0; + + //only handle principle axis at the moment, + //@todo(erwincoumans) orient the constraint for non-principal axis + int principleAxis = jointAxisInJointSpace.closestAxis(); + switch (principleAxis) + { + case 0: + { + dof6 = allocateGeneric6DofSpring2Constraint(urdfLinkIndex,rbA, rbB, offsetInA, offsetInB,RO_ZYX); + dof6->setLinearLowerLimit(btVector3(0,0,0)); + dof6->setLinearUpperLimit(btVector3(0,0,0)); + + dof6->setAngularUpperLimit(btVector3(-1,0,0)); + dof6->setAngularLowerLimit(btVector3(1,0,0)); + + break; + } + case 1: + { + dof6 = allocateGeneric6DofSpring2Constraint(urdfLinkIndex,rbA, rbB, offsetInA, offsetInB,RO_XZY); + dof6->setLinearLowerLimit(btVector3(0,0,0)); + dof6->setLinearUpperLimit(btVector3(0,0,0)); + + dof6->setAngularUpperLimit(btVector3(0,-1,0)); + dof6->setAngularLowerLimit(btVector3(0,1,0)); + break; + } + case 2: + default: + { + dof6 = allocateGeneric6DofSpring2Constraint(urdfLinkIndex,rbA, rbB, offsetInA, offsetInB,RO_XYZ); + dof6->setLinearLowerLimit(btVector3(0,0,0)); + dof6->setLinearUpperLimit(btVector3(0,0,0)); + + dof6->setAngularUpperLimit(btVector3(0,0,-1)); + dof6->setAngularLowerLimit(btVector3(0,0,0)); + + } + }; + + GenericConstraintUserInfo* userInfo = new GenericConstraintUserInfo; + userInfo->m_jointAxisInJointSpace = jointAxisInJointSpace; + userInfo->m_jointAxisIndex = 3+principleAxis; + + if (jointLowerLimit > jointUpperLimit) + { + userInfo->m_urdfJointType = URDFContinuousJoint; + } else + { + userInfo->m_urdfJointType = URDFRevoluteJoint; + userInfo->m_lowerJointLimit = jointLowerLimit; + userInfo->m_upperJointLimit = jointUpperLimit; + userInfo->m_urdfIndex = urdfLinkIndex; + } + dof6->setUserConstraintPtr(userInfo); + m_6DofConstraints.push_back(dof6); + return dof6; +} + + +class btGeneric6DofSpring2Constraint* MyMultiBodyCreator::createFixedJoint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB) +{ + btGeneric6DofSpring2Constraint* dof6 = allocateGeneric6DofSpring2Constraint(urdfLinkIndex, rbA, rbB, offsetInA, offsetInB); + + GenericConstraintUserInfo* userInfo = new GenericConstraintUserInfo; + userInfo->m_urdfIndex = urdfLinkIndex; + userInfo->m_urdfJointType = URDFFixedJoint; + + dof6->setUserConstraintPtr(userInfo); + + dof6->setLinearLowerLimit(btVector3(0,0,0)); + dof6->setLinearUpperLimit(btVector3(0,0,0)); + + dof6->setAngularLowerLimit(btVector3(0,0,0)); + dof6->setAngularUpperLimit(btVector3(0,0,0)); + + return dof6; +} + + + void MyMultiBodyCreator::addLinkMapping(int urdfLinkIndex, int mbLinkIndex) { // m_urdf2mbLink[urdfLinkIndex] = mbLinkIndex; diff --git a/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.h b/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.h index 71b2f18f2..99dbc9fc9 100644 --- a/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.h +++ b/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.h @@ -8,6 +8,17 @@ struct GUIHelperInterface; class btMultiBody; +struct GenericConstraintUserInfo +{ + int m_urdfIndex; + int m_urdfJointType; + btVector3 m_jointAxisInJointSpace; + int m_jointAxisIndex; + btScalar m_lowerJointLimit; + btScalar m_upperJointLimit; + +}; + class MyMultiBodyCreator : public MultiBodyCreationInterface { @@ -16,6 +27,7 @@ class MyMultiBodyCreator : public MultiBodyCreationInterface struct GUIHelperInterface* m_guiHelper; + btAlignedObjectArray m_6DofConstraints; public: @@ -37,12 +49,28 @@ public: virtual class btGeneric6DofSpring2Constraint* allocateGeneric6DofSpring2Constraint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB, int rotateOrder=0); + virtual class btGeneric6DofSpring2Constraint* createPrismaticJoint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB, + const btVector3& jointAxisInJointSpace,btScalar jointLowerLimit,btScalar jointUpperLimit); + virtual class btGeneric6DofSpring2Constraint* createRevoluteJoint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB, + const btVector3& jointAxisInJointSpace,btScalar jointLowerLimit,btScalar jointUpperLimit); + + virtual class btGeneric6DofSpring2Constraint* createFixedJoint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB); + virtual class btMultiBodyLinkCollider* allocateMultiBodyLinkCollider(int urdfLinkIndex, int mbLinkIndex, btMultiBody* body); virtual void addLinkMapping(int urdfLinkIndex, int mbLinkIndex); btMultiBody* getBulletMultiBody(); - + + int getNum6DofConstraints() const + { + return m_6DofConstraints.size(); + } + + btGeneric6DofSpring2Constraint* get6DofConstraint(int index) + { + return m_6DofConstraints[index]; + } }; #endif //MY_MULTIBODY_CREATOR diff --git a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp index 92a0b24db..7b05fc7b3 100644 --- a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp +++ b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp @@ -295,21 +295,9 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat } else { printf("Fixed joint\n"); - - btMatrix3x3 rm(offsetInA.getBasis()); - btScalar y,p,r; - rm.getEulerZYX(y,p,r); - printf("y=%f,p=%f,r=%f\n", y,p,r); - - //we could also use btFixedConstraint but it has some issues - btGeneric6DofSpring2Constraint* dof6 = creation.allocateGeneric6DofSpring2Constraint(urdfLinkIndex, *parentRigidBody, *linkRigidBody, offsetInA, offsetInB); - - dof6->setLinearLowerLimit(btVector3(0,0,0)); - dof6->setLinearUpperLimit(btVector3(0,0,0)); - - dof6->setAngularLowerLimit(btVector3(0,0,0)); - dof6->setAngularUpperLimit(btVector3(0,0,0)); - + + btGeneric6DofSpring2Constraint* dof6 = creation.createFixedJoint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB); + if (enableConstraints) world1->addConstraint(dof6,true); } @@ -330,51 +318,11 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat } else { - //only handle principle axis at the moment, - //@todo(erwincoumans) orient the constraint for non-principal axis - int principleAxis = jointAxisInJointSpace.closestAxis(); - switch (principleAxis) - { - case 0: - { - btGeneric6DofSpring2Constraint* dof6 = creation.allocateGeneric6DofSpring2Constraint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,RO_ZYX); - dof6->setLinearLowerLimit(btVector3(0,0,0)); - dof6->setLinearUpperLimit(btVector3(0,0,0)); - dof6->setAngularUpperLimit(btVector3(-1,0,0)); - dof6->setAngularLowerLimit(btVector3(1,0,0)); + btGeneric6DofSpring2Constraint* dof6 = creation.createRevoluteJoint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,jointAxisInJointSpace,jointLowerLimit, jointUpperLimit); - if (enableConstraints) + if (enableConstraints) world1->addConstraint(dof6,true); - break; - } - case 1: - { - btGeneric6DofSpring2Constraint* dof6 = creation.allocateGeneric6DofSpring2Constraint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,RO_XZY); - dof6->setLinearLowerLimit(btVector3(0,0,0)); - dof6->setLinearUpperLimit(btVector3(0,0,0)); - - dof6->setAngularUpperLimit(btVector3(0,-1,0)); - dof6->setAngularLowerLimit(btVector3(0,1,0)); - - if (enableConstraints) - world1->addConstraint(dof6,true); - break; - } - case 2: - default: - { - btGeneric6DofSpring2Constraint* dof6 = creation.allocateGeneric6DofSpring2Constraint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,RO_XYZ); - dof6->setLinearLowerLimit(btVector3(0,0,0)); - dof6->setLinearUpperLimit(btVector3(0,0,0)); - - dof6->setAngularUpperLimit(btVector3(0,0,-1)); - dof6->setAngularLowerLimit(btVector3(0,0,0)); - - if (enableConstraints) - world1->addConstraint(dof6,true); - } - }; printf("Revolute/Continuous joint\n"); } break; @@ -396,33 +344,10 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat } else { - btGeneric6DofSpring2Constraint* dof6 = creation.allocateGeneric6DofSpring2Constraint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB); - //todo(erwincoumans) for now, we only support principle axis along X, Y or Z - int principleAxis = jointAxisInJointSpace.closestAxis(); - switch (principleAxis) - { - case 0: - { - dof6->setLinearLowerLimit(btVector3(jointLowerLimit,0,0)); - dof6->setLinearUpperLimit(btVector3(jointUpperLimit,0,0)); - break; - } - case 1: - { - dof6->setLinearLowerLimit(btVector3(0,jointLowerLimit,0)); - dof6->setLinearUpperLimit(btVector3(0,jointUpperLimit,0)); - break; - } - case 2: - default: - { - dof6->setLinearLowerLimit(btVector3(0,0,jointLowerLimit)); - dof6->setLinearUpperLimit(btVector3(0,0,jointUpperLimit)); - } - }; - - dof6->setAngularLowerLimit(btVector3(0,0,0)); - dof6->setAngularUpperLimit(btVector3(0,0,0)); + + btGeneric6DofSpring2Constraint* dof6 = creation.createPrismaticJoint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,jointAxisInJointSpace,jointLowerLimit,jointUpperLimit); + + if (enableConstraints) world1->addConstraint(dof6,true); diff --git a/examples/MultiBody/TestJointTorqueSetup.cpp b/examples/MultiBody/TestJointTorqueSetup.cpp index baa39e535..19a525b98 100644 --- a/examples/MultiBody/TestJointTorqueSetup.cpp +++ b/examples/MultiBody/TestJointTorqueSetup.cpp @@ -46,9 +46,12 @@ TestJointTorqueSetup::~TestJointTorqueSetup() } +///this is a temporary global, until we determine if we need the option or not +extern bool gJointFeedbackInWorldSpace; void TestJointTorqueSetup::initPhysics() { int upAxis = 1; + gJointFeedbackInWorldSpace = true; m_guiHelper->setUpAxis(upAxis); btVector4 colors[4] = @@ -90,7 +93,9 @@ void TestJointTorqueSetup::initPhysics() groundOrigin[upAxis] -=.5; groundOrigin[2]-=0.6; start.setOrigin(groundOrigin); - + btQuaternion groundOrn(btVector3(0,1,0),0.25*SIMD_PI); + + // start.setRotation(groundOrn); btRigidBody* body = createRigidBody(0,start,box); body->setFriction(0); btVector4 color = colors[curColor]; @@ -101,7 +106,7 @@ void TestJointTorqueSetup::initPhysics() { bool floating = false; - bool damping = true; + bool damping = false; bool gyro = false; int numLinks = 2; bool spherical = false; //set it ot false -to use 1DoF hinges instead of 3DoF sphericals @@ -156,7 +161,14 @@ void TestJointTorqueSetup::initPhysics() // linkMass= 1000; btVector3 linkInertiaDiag(0.f, 0.f, 0.f); - btCollisionShape* shape = new btBoxShape(btVector3(linkHalfExtents[0], linkHalfExtents[1], linkHalfExtents[2]));//new btSphereShape(linkHalfExtents[0]); + btCollisionShape* shape = 0; + if (i==0) + { + shape = new btBoxShape(btVector3(linkHalfExtents[0], linkHalfExtents[1], linkHalfExtents[2]));// + } else + { + shape = new btSphereShape(radius); + } shape->calculateLocalInertia(linkMass, linkInertiaDiag); delete shape; @@ -223,10 +235,8 @@ void TestJointTorqueSetup::initPhysics() mbC->setAngularDamping(0.9f); } // - btVector3 gravity(0,0,0); - gravity[upAxis] = -9.81; - gravity[0] = 0; - m_dynamicsWorld->setGravity(gravity); + m_dynamicsWorld->setGravity(btVector3(0,-1,-10)); + ////////////////////////////////////////////// if(0)//numLinks > 0) { @@ -288,7 +298,7 @@ void TestJointTorqueSetup::initPhysics() btVector3 color(0.0,0.0,0.5); m_guiHelper->createCollisionObjectGraphicsObject(col,color); - col->setFriction(friction); +// col->setFriction(friction); pMultiBody->setBaseCollider(col); } @@ -330,7 +340,7 @@ void TestJointTorqueSetup::initPhysics() tr.setOrigin(posr); tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3])); col->setWorldTransform(tr); - col->setFriction(friction); + // col->setFriction(friction); bool isDynamic = 1;//(linkMass > 0); short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(btBroadphaseProxy::StaticFilter); short collisionFilterMask = isDynamic? short(btBroadphaseProxy::AllFilter) : short(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter); @@ -355,7 +365,7 @@ void TestJointTorqueSetup::initPhysics() void TestJointTorqueSetup::stepSimulation(float deltaTime) { //m_multiBody->addLinkForce(0,btVector3(100,100,100)); - if (1)//m_once) + if (0)//m_once) { m_once=false; m_multiBody->addJointTorque(0, 10.0); @@ -364,9 +374,12 @@ void TestJointTorqueSetup::stepSimulation(float deltaTime) b3Printf("t = %f,%f,%f\n",torque,torque,torque);//[0],torque[1],torque[2]); } - m_dynamicsWorld->stepSimulation(1./60,0); + m_dynamicsWorld->stepSimulation(1./240,0); + + static int count = 0; + if ((count& 0x0f)==0) + { - if (1) for (int i=0;im_spatialInertia = spatInertia[i+1]; - m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec = rot_from_parent[i+1].transpose()*(spatInertia[i+1]*spatAcc[i+1]+zeroAccSpatFrc[i+1]).m_bottomVec; - m_links[i].m_jointFeedback->m_reactionForces.m_topVec = rot_from_parent[i+1].transpose()*(spatInertia[i+1]*spatAcc[i+1]+zeroAccSpatFrc[i+1]).m_topVec; - //m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec = (spatInertia[i+1]*spatAcc[i+1]+zeroAccSpatFrc[i+1]).m_bottomVec; - //m_links[i].m_jointFeedback->m_reactionForces.m_topVec = (spatInertia[i+1]*spatAcc[i+1]+zeroAccSpatFrc[i+1]).m_topVec; - } + //m_links[i].m_jointFeedback->m_spatialInertia = spatInertia[i+1]; + if (gJointFeedbackInWorldSpace) + { + m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec = m_links[i].m_cachedWorldTransform.getBasis()*(spatInertia[i+1]*spatAcc[i+1]+zeroAccSpatFrc[i+1]).m_bottomVec; + m_links[i].m_jointFeedback->m_reactionForces.m_topVec = m_links[i].m_cachedWorldTransform.getBasis()*(spatInertia[i+1]*spatAcc[i+1]+zeroAccSpatFrc[i+1]).m_topVec; + } else + { + m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec = (spatInertia[i+1]*spatAcc[i+1]+zeroAccSpatFrc[i+1]).m_bottomVec; + m_links[i].m_jointFeedback->m_reactionForces.m_topVec = (spatInertia[i+1]*spatAcc[i+1]+zeroAccSpatFrc[i+1]).m_topVec; + } + } } diff --git a/src/BulletDynamics/Featherstone/btMultiBody.h b/src/BulletDynamics/Featherstone/btMultiBody.h index cc1b3ee76..d136c7bd5 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.h +++ b/src/BulletDynamics/Featherstone/btMultiBody.h @@ -616,7 +616,6 @@ private: btAlignedObjectArray m_vectorBuf; btAlignedObjectArray m_matrixBuf; - //std::auto_ptr > > cached_imatrix_lu; btMatrix3x3 m_cachedInertiaTopLeft; btMatrix3x3 m_cachedInertiaTopRight; diff --git a/src/BulletDynamics/Featherstone/btMultiBodyJointFeedback.h b/src/BulletDynamics/Featherstone/btMultiBodyJointFeedback.h index fe2ef15e8..5c2fa8ed5 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyJointFeedback.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyJointFeedback.h @@ -22,7 +22,6 @@ subject to the following restrictions: struct btMultiBodyJointFeedback { btSpatialForceVector m_reactionForces; - btSymmetricSpatialDyad m_spatialInertia; }; #endif //BT_MULTIBODY_JOINT_FEEDBACK_H