From 46f2f3db4ef8e7de42fccc8dfb8018b61a3b55a0 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Wed, 7 Jun 2017 16:22:02 -0700 Subject: [PATCH] implement 'mimic' joint constraint or 'gear' constraint for btMultiBody, add example in pybullet/examples/mimicJointConstraint.py --- .../PhysicsServerCommandProcessor.cpp | 43 ++++- .../pybullet/examples/mimicJointConstraint.py | 19 +- setup.py | 1 + src/BulletDynamics/CMakeLists.txt | 2 + .../Featherstone/btMultiBodyConstraint.h | 1 + .../btMultiBodyGearConstraint.cpp | 169 ++++++++++++++++++ .../Featherstone/btMultiBodyGearConstraint.h | 103 +++++++++++ 7 files changed, 329 insertions(+), 9 deletions(-) create mode 100644 src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp create mode 100644 src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.h diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 85b75acbb..9103e4384 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -10,6 +10,10 @@ #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" #include "BulletDynamics/Featherstone/btMultiBodyJointFeedback.h" #include "BulletDynamics/Featherstone/btMultiBodyFixedConstraint.h" +#include "BulletDynamics/Featherstone/btMultiBodyGearConstraint.h" + + + #include "BulletDynamics/Featherstone/btMultiBodySliderConstraint.h" #include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h" #include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" @@ -5759,7 +5763,31 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm btMatrix3x3 frameInParent(btQuaternion(clientCmd.m_userConstraintArguments.m_parentFrame[3], clientCmd.m_userConstraintArguments.m_parentFrame[4], clientCmd.m_userConstraintArguments.m_parentFrame[5], clientCmd.m_userConstraintArguments.m_parentFrame[6])); btMatrix3x3 frameInChild(btQuaternion(clientCmd.m_userConstraintArguments.m_childFrame[3], clientCmd.m_userConstraintArguments.m_childFrame[4], clientCmd.m_userConstraintArguments.m_childFrame[5], clientCmd.m_userConstraintArguments.m_childFrame[6])); btVector3 jointAxis(clientCmd.m_userConstraintArguments.m_jointAxis[0], clientCmd.m_userConstraintArguments.m_jointAxis[1], clientCmd.m_userConstraintArguments.m_jointAxis[2]); - if (clientCmd.m_userConstraintArguments.m_jointType == eFixedType) + + + + if (clientCmd.m_userConstraintArguments.m_jointType == eGearType) + { + if (childBody && childBody->m_multiBody) + { + if ((clientCmd.m_userConstraintArguments.m_childJointIndex>=-1) && (clientCmd.m_userConstraintArguments.m_childJointIndex m_multiBody->getNumLinks())) + { + btMultiBodyGearConstraint* multibodyGear = new btMultiBodyGearConstraint(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,childBody->m_multiBody,clientCmd.m_userConstraintArguments.m_childJointIndex,pivotInParent,pivotInChild,frameInParent,frameInChild); + multibodyGear->setMaxAppliedImpulse(defaultMaxForce); + m_data->m_dynamicsWorld->addMultiBodyConstraint(multibodyGear); + InteralUserConstraintData userConstraintData; + userConstraintData.m_mbConstraint = multibodyGear; + int uid = m_data->m_userConstraintUIDGenerator++; + serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid; + serverCmd.m_userConstraintResultArgs.m_maxAppliedForce = defaultMaxForce; + userConstraintData.m_userConstraintData = serverCmd.m_userConstraintResultArgs; + m_data->m_userConstraints.insert(uid,userConstraintData); + serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED; + } + + } + } + else if (clientCmd.m_userConstraintArguments.m_jointType == eFixedType) { if (childBody && childBody->m_multiBody) { @@ -5959,9 +5987,22 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm userConstraintPtr->m_userConstraintData.m_maxAppliedForce = clientCmd.m_userConstraintArguments.m_maxAppliedForce; userConstraintPtr->m_mbConstraint->setMaxAppliedImpulse(maxImp); } + + if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_GEAR_RATIO) + { + userConstraintPtr->m_mbConstraint->setGearRatio(clientCmd.m_userConstraintArguments.m_gearRatio); + } + } if (userConstraintPtr->m_rbConstraint) { + if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_MAX_FORCE) + { + btScalar maxImp = clientCmd.m_userConstraintArguments.m_maxAppliedForce*m_data->m_physicsDeltaTime; + userConstraintPtr->m_userConstraintData.m_maxAppliedForce = clientCmd.m_userConstraintArguments.m_maxAppliedForce; + //userConstraintPtr->m_rbConstraint->setMaxAppliedImpulse(maxImp); + } + if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_GEAR_RATIO) { if (userConstraintPtr->m_rbConstraint->getObjectType()==GEAR_CONSTRAINT_TYPE) diff --git a/examples/pybullet/examples/mimicJointConstraint.py b/examples/pybullet/examples/mimicJointConstraint.py index a92edc130..af615e02f 100644 --- a/examples/pybullet/examples/mimicJointConstraint.py +++ b/examples/pybullet/examples/mimicJointConstraint.py @@ -1,16 +1,19 @@ -#mimic joint currently only works for 'maximal coordinate' rigid bodies -#one way to use it would be to attach maximal coordinate rigid bodies to multibody links using -#fixed constraints +#a mimic joint can act as a gear between two joints +#you can control the gear ratio in magnitude and sign (>0 reverses direction) import pybullet as p import time p.connect(p.GUI) -wheelA = p.loadURDF("wheel.urdf",[0,0,0],useMaximalCoordinates=1) -wheelB = p.loadURDF("wheel.urdf",[0,0,1],useMaximalCoordinates=1) -c = p.createConstraint(wheelA,-1,wheelB,-1,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) -p.changeConstraint(c,gearRatio=-0.1) +wheelA = p.loadURDF("wheel.urdf",[0,0,0]) +wheelB = p.loadURDF("wheel.urdf",[0,0,1]) +p.setJointMotorControl2(wheelA,0,p.VELOCITY_CONTROL,targetVelocity=0,force=0) +p.setJointMotorControl2(wheelB,0,p.VELOCITY_CONTROL,targetVelocity=1,force=1) + +c = p.createConstraint(wheelA,0,wheelB,0,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) +p.changeConstraint(c,gearRatio=-0.1, maxForce=10000) + p.setRealTimeSimulation(1) while(1): time.sleep(0.01) -p.removeConstraint(c) +#p.removeConstraint(c) \ No newline at end of file diff --git a/setup.py b/setup.py index ecd77a484..11f5ed71c 100644 --- a/setup.py +++ b/setup.py @@ -253,6 +253,7 @@ sources = ["examples/pybullet/pybullet.c"]\ +["src/BulletDynamics/Featherstone/btMultiBody.cpp"]\ +["src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp"]\ +["src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp"]\ ++["src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp"]\ +["src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp"]\ +["src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp"]\ +["src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp"]\ diff --git a/src/BulletDynamics/CMakeLists.txt b/src/BulletDynamics/CMakeLists.txt index 4023d721e..f8a6f34ba 100644 --- a/src/BulletDynamics/CMakeLists.txt +++ b/src/BulletDynamics/CMakeLists.txt @@ -37,6 +37,7 @@ SET(BulletDynamics_SRCS Featherstone/btMultiBodyFixedConstraint.cpp Featherstone/btMultiBodySliderConstraint.cpp Featherstone/btMultiBodyJointMotor.cpp + Featherstone/btMultiBodyGearConstraint.cpp MLCPSolvers/btDantzigLCP.cpp MLCPSolvers/btMLCPSolver.cpp MLCPSolvers/btLemkeAlgorithm.cpp @@ -98,6 +99,7 @@ SET(Featherstone_HDRS Featherstone/btMultiBodyFixedConstraint.h Featherstone/btMultiBodySliderConstraint.h Featherstone/btMultiBodyJointMotor.h + Featherstone/btMultiBodyGearConstraint.h ) SET(MLCPSolvers_HDRS diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h index 7204f2785..27e1ccc05 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h @@ -183,6 +183,7 @@ public: virtual void debugDraw(class btIDebugDraw* drawer)=0; + virtual void setGearRatio(btScalar ratio) {} }; #endif //BT_MULTIBODY_CONSTRAINT_H diff --git a/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp b/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp new file mode 100644 index 000000000..2e242b1f7 --- /dev/null +++ b/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp @@ -0,0 +1,169 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2013 Erwin Coumans 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. +*/ + +///This file was written by Erwin Coumans + +#include "btMultiBodyGearConstraint.h" +#include "btMultiBody.h" +#include "btMultiBodyLinkCollider.h" +#include "BulletCollision/CollisionDispatch/btCollisionObject.h" + +btMultiBodyGearConstraint::btMultiBodyGearConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB) + :btMultiBodyConstraint(bodyA,bodyB,linkA,linkB,1,false), + m_gearRatio(1) +{ + +} + +void btMultiBodyGearConstraint::finalizeMultiDof() +{ + + allocateJacobiansMultiDof(); + + m_numDofsFinalized = m_jacSizeBoth; +} + +btMultiBodyGearConstraint::~btMultiBodyGearConstraint() +{ +} + + +int btMultiBodyGearConstraint::getIslandIdA() const +{ + + if (m_bodyA) + { + btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider(); + if (col) + return col->getIslandTag(); + for (int i=0;igetNumLinks();i++) + { + if (m_bodyA->getLink(i).m_collider) + return m_bodyA->getLink(i).m_collider->getIslandTag(); + } + } + return -1; +} + +int btMultiBodyGearConstraint::getIslandIdB() const +{ + if (m_bodyB) + { + btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider(); + if (col) + return col->getIslandTag(); + + for (int i=0;igetNumLinks();i++) + { + col = m_bodyB->getLink(i).m_collider; + if (col) + return col->getIslandTag(); + } + } + return -1; +} + + +void btMultiBodyGearConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows, + btMultiBodyJacobianData& data, + const btContactSolverInfo& infoGlobal) +{ + // only positions need to be updated -- data.m_jacobians and force + // directions were set in the ctor and never change. + + if (m_numDofsFinalized != m_jacSizeBoth) + { + finalizeMultiDof(); + } + + //don't crash + if (m_numDofsFinalized != m_jacSizeBoth) + return; + + + if (m_maxAppliedImpulse==0.f) + return; + + // note: we rely on the fact that data.m_jacobians are + // always initialized to zero by the Constraint ctor + int linkDoF = 0; + unsigned int offsetA = 6 + (m_bodyA->getLink(m_linkA).m_dofOffset + linkDoF); + unsigned int offsetB = 6 + (m_bodyB->getLink(m_linkB).m_dofOffset + linkDoF); + + // row 0: the lower bound + jacobianA(0)[offsetA] = 1; + jacobianB(0)[offsetB] = m_gearRatio; + + const btScalar posError = 0; + const btVector3 dummy(0, 0, 0); + btScalar erp = infoGlobal.m_erp; + btScalar kp = 1; + btScalar kd = 1; + int numRows = getNumRows(); + + for (int row=0;rowgetJointPosMultiDof(m_linkA)[dof]; + btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof]; + + //btScalar positionStabiliationTerm = erp*(m_desiredPosition-currentPosition)/infoGlobal.m_timeStep; + //btScalar velocityError = (m_desiredVelocity - currentVelocity); + + btScalar desiredRelativeVelocity = 0; + + fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,dummy,posError,infoGlobal,-m_maxAppliedImpulse,m_maxAppliedImpulse,false,1,false,desiredRelativeVelocity); + + constraintRow.m_orgConstraint = this; + constraintRow.m_orgDofIndex = row; + { + //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) + { + case btMultibodyLink::eRevolute: + { + constraintRow.m_contactNormal1.setZero(); + constraintRow.m_contactNormal2.setZero(); + btVector3 revoluteAxisInWorld = quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(),m_bodyA->getLink(m_linkA).m_axes[0].m_topVec); + constraintRow.m_relpos1CrossNormal=revoluteAxisInWorld; + constraintRow.m_relpos2CrossNormal=-revoluteAxisInWorld; + + break; + } + case btMultibodyLink::ePrismatic: + { + btVector3 prismaticAxisInWorld = quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(),m_bodyA->getLink(m_linkA).m_axes[0].m_bottomVec); + constraintRow.m_contactNormal1=prismaticAxisInWorld; + constraintRow.m_contactNormal2=-prismaticAxisInWorld; + constraintRow.m_relpos1CrossNormal.setZero(); + constraintRow.m_relpos2CrossNormal.setZero(); + break; + } + default: + { + btAssert(0); + } + }; + + } + + } + +} + diff --git a/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.h b/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.h new file mode 100644 index 000000000..947166ae7 --- /dev/null +++ b/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.h @@ -0,0 +1,103 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2013 Erwin Coumans 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. +*/ + +///This file was written by Erwin Coumans + +#ifndef BT_MULTIBODY_GEAR_CONSTRAINT_H +#define BT_MULTIBODY_GEAR_CONSTRAINT_H + +#include "btMultiBodyConstraint.h" + +class btMultiBodyGearConstraint : public btMultiBodyConstraint +{ +protected: + + btRigidBody* m_rigidBodyA; + btRigidBody* m_rigidBodyB; + btVector3 m_pivotInA; + btVector3 m_pivotInB; + btMatrix3x3 m_frameInA; + btMatrix3x3 m_frameInB; + btScalar m_gearRatio; + +public: + + btMultiBodyGearConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB); + btMultiBodyGearConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB); + + virtual ~btMultiBodyGearConstraint(); + + virtual void finalizeMultiDof(); + + virtual int getIslandIdA() const; + virtual int getIslandIdB() const; + + virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows, + btMultiBodyJacobianData& data, + const btContactSolverInfo& infoGlobal); + + const btVector3& getPivotInA() const + { + return m_pivotInA; + } + + void setPivotInA(const btVector3& pivotInA) + { + m_pivotInA = pivotInA; + } + + const btVector3& getPivotInB() const + { + return m_pivotInB; + } + + virtual void setPivotInB(const btVector3& pivotInB) + { + m_pivotInB = pivotInB; + } + + const btMatrix3x3& getFrameInA() const + { + return m_frameInA; + } + + void setFrameInA(const btMatrix3x3& frameInA) + { + m_frameInA = frameInA; + } + + const btMatrix3x3& getFrameInB() const + { + return m_frameInB; + } + + virtual void setFrameInB(const btMatrix3x3& frameInB) + { + m_frameInB = frameInB; + } + + virtual void debugDraw(class btIDebugDraw* drawer) + { + //todo(erwincoumans) + } + + virtual void setGearRatio(btScalar gearRatio) + { + m_gearRatio = gearRatio; + } + +}; + +#endif //BT_MULTIBODY_GEAR_CONSTRAINT_H