Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

add option for rigid body/typed constraint to set target velocity

Browse Source 
compare joint feedback between multi body and rigid body. initial results are promising (not exactly the same, but reasonably close)
This commit is contained in:
Erwin Coumans
2015-06-24 23:19:00 -07:00
parent b14afba350
commit d830681674
11 changed files with 426 additions and 164 deletions
Download Patch File Download Diff File Expand all files Collapse all files

26
data/sphere2.urdf
View File

@@ -43,5 +43,31 @@
<child link="childA"/> <child link="childA"/>
<origin xyz="0 0 1.0"/> <origin xyz="0 0 1.0"/>
</joint> </joint>
<link name="childB">
<inertial>
<origin rpy="0 0 0" xyz="0 0 0.5"/>
<mass value="10.0"/>
<inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0.5"/>
<geometry>
<sphere radius="0.5"/>
</geometry>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0.5"/>
<geometry>
<sphere radius="0.5"/>
</geometry>
</collision>
</link>
<joint name="joint_childA_childB" type="continuous">
<parent link="childA"/>
<child link="childB"/>
<axis xyz="0 1 0"/>
<origin xyz="0 0 0.5"/>
</joint>
</robot> </robot>

164
examples/Constraints/TestHingeTorque.cpp
View File

@@ -5,8 +5,8 @@
#include "../CommonInterfaces/CommonParameterInterface.h" #include "../CommonInterfaces/CommonParameterInterface.h"
short collisionFilterGroup = short(btBroadphaseProxy::CharacterFilter); 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 struct TestHingeTorque : public CommonRigidBodyBase
{ {
@@ -50,7 +50,7 @@ TestHingeTorque::~ TestHingeTorque()
void TestHingeTorque::stepSimulation(float deltaTime) void TestHingeTorque::stepSimulation(float deltaTime)
{ {
if (m_once) if (0)//m_once)
{ {
m_once=false; m_once=false;
btHingeConstraint* hinge = (btHingeConstraint*)m_dynamicsWorld->getConstraint(0); btHingeConstraint* hinge = (btHingeConstraint*)m_dynamicsWorld->getConstraint(0);
@@ -63,37 +63,41 @@ void TestHingeTorque::stepSimulation(float deltaTime)
} }
m_dynamicsWorld->stepSimulation(1./60,0); m_dynamicsWorld->stepSimulation(1./240,0);
btRigidBody* base = btRigidBody::upcast(m_dynamicsWorld->getCollisionObjectArray()[0]);
static int count = 0;
b3Printf("base angvel = %f,%f,%f",base->getAngularVelocity()[0], if ((count& 0x0f)==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;i<m_jointFeedback.size();i++)
{ {
b3Printf("Applied force A:(%f,%f,%f), torque A:(%f,%f,%f)\nForce B:(%f,%f,%f), torque B:(%f,%f,%f)\n", btRigidBody* base = btRigidBody::upcast(m_dynamicsWorld->getCollisionObjectArray()[0]);
m_jointFeedback[i]->m_appliedForceBodyA.x(),
m_jointFeedback[i]->m_appliedForceBodyA.y(), b3Printf("base angvel = %f,%f,%f",base->getAngularVelocity()[0],
m_jointFeedback[i]->m_appliedForceBodyA.z(), base->getAngularVelocity()[1],
m_jointFeedback[i]->m_appliedTorqueBodyA.x(),
m_jointFeedback[i]->m_appliedTorqueBodyA.y(), base->getAngularVelocity()[2]);
m_jointFeedback[i]->m_appliedTorqueBodyA.z(),
m_jointFeedback[i]->m_appliedForceBodyB.x(), btRigidBody* child = btRigidBody::upcast(m_dynamicsWorld->getCollisionObjectArray()[1]);
m_jointFeedback[i]->m_appliedForceBodyB.y(),
m_jointFeedback[i]->m_appliedForceBodyB.z(),
m_jointFeedback[i]->m_appliedTorqueBodyB.x(), b3Printf("child angvel = %f,%f,%f",child->getAngularVelocity()[0],
m_jointFeedback[i]->m_appliedTorqueBodyB.y(), child->getAngularVelocity()[1],
m_jointFeedback[i]->m_appliedTorqueBodyB.z());
child->getAngularVelocity()[2]);
for (int i=0;i<m_jointFeedback.size();i++)
{
b3Printf("Applied force B:(%f,%f,%f), torque B:(%f,%f,%f)\n",
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());
}
} }
count++;
//CommonRigidBodyBase::stepSimulation(deltaTime); //CommonRigidBodyBase::stepSimulation(deltaTime);
} }
@@ -101,10 +105,13 @@ void TestHingeTorque::stepSimulation(float deltaTime)
void TestHingeTorque::initPhysics() void TestHingeTorque::initPhysics()
{ {
m_guiHelper->setUpAxis(1); int upAxis = 1;
m_guiHelper->setUpAxis(upAxis);
createEmptyDynamicsWorld(); 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); m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
int mode = btIDebugDraw::DBG_DrawWireframe int mode = btIDebugDraw::DBG_DrawWireframe
@@ -115,7 +122,7 @@ void TestHingeTorque::initPhysics()
{ // create a door using hinge constraint attached to the world { // 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 spherical = false; //set it ot false -to use 1DoF hinges instead of 3DoF sphericals
bool canSleep = false; bool canSleep = false;
bool selfCollide = false; bool selfCollide = false;
@@ -138,7 +145,9 @@ void TestHingeTorque::initPhysics()
m_dynamicsWorld->removeRigidBody(base); m_dynamicsWorld->removeRigidBody(base);
base->setDamping(0,0); base->setDamping(0,0);
m_dynamicsWorld->addRigidBody(base,collisionFilterGroup,collisionFilterMask); m_dynamicsWorld->addRigidBody(base,collisionFilterGroup,collisionFilterMask);
btBoxShape* linkBox = new btBoxShape(linkHalfExtents); btBoxShape* linkBox1 = new btBoxShape(linkHalfExtents);
btSphereShape* linkSphere = new btSphereShape(radius);
btRigidBody* prevBody = base; btRigidBody* prevBody = base;
for (int i=0;i<numLinks;i++) for (int i=0;i<numLinks;i++)
@@ -148,30 +157,83 @@ void TestHingeTorque::initPhysics()
linkTrans.setOrigin(basePosition-btVector3(0,linkHalfExtents[1]*2.f*(i+1),0)); linkTrans.setOrigin(basePosition-btVector3(0,linkHalfExtents[1]*2.f*(i+1),0));
btRigidBody* linkBody = createRigidBody(linkMass,linkTrans,linkBox); btCollisionShape* colOb = 0;
if (i==0)
{
colOb = linkBox1;
} else
{
colOb = linkSphere;
}
btRigidBody* linkBody = createRigidBody(linkMass,linkTrans,colOb);
m_dynamicsWorld->removeRigidBody(linkBody); m_dynamicsWorld->removeRigidBody(linkBody);
m_dynamicsWorld->addRigidBody(linkBody,collisionFilterGroup,collisionFilterMask); m_dynamicsWorld->addRigidBody(linkBody,collisionFilterGroup,collisionFilterMask);
linkBody->setDamping(0,0); linkBody->setDamping(0,0);
//create a hinge constraint btTypedConstraint* con = 0;
btVector3 pivotInA(0,-linkHalfExtents[1],0);
btVector3 pivotInB(0,linkHalfExtents[1],0); if (i==0)
btVector3 axisInA(1,0,0); {
btVector3 axisInB(1,0,0); //create a hinge constraint
bool useReferenceA = true; btVector3 pivotInA(0,-linkHalfExtents[1],0);
btHingeConstraint* hinge = new btHingeConstraint(*prevBody,*linkBody, btVector3 pivotInB(0,linkHalfExtents[1],0);
pivotInA,pivotInB, btVector3 axisInA(1,0,0);
axisInA,axisInB,useReferenceA); btVector3 axisInB(1,0,0);
btJointFeedback* fb = new btJointFeedback(); bool useReferenceA = true;
m_jointFeedback.push_back(fb); btHingeConstraint* hinge = new btHingeConstraint(*prevBody,*linkBody,
hinge->setJointFeedback(fb); 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); m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
} }

72
examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp
View File

@@ -63,11 +63,18 @@ struct ImportUrdfInternalData
ImportUrdfInternalData() ImportUrdfInternalData()
:m_numMotors(0) :m_numMotors(0)
{ {
for (int i=0;i<MAX_NUM_MOTORS;i++)
{
m_jointMotors[i] = 0;
m_generic6DofJointMotors[i] = 0;
}
} }
btScalar m_motorTargetVelocities[MAX_NUM_MOTORS]; btScalar m_motorTargetVelocities[MAX_NUM_MOTORS];
btMultiBodyJointMotor* m_jointMotors [MAX_NUM_MOTORS]; btMultiBodyJointMotor* m_jointMotors [MAX_NUM_MOTORS];
btGeneric6DofSpring2Constraint* m_generic6DofJointMotors [MAX_NUM_MOTORS];
int m_numMotors; int m_numMotors;
}; };
@@ -91,8 +98,7 @@ ImportUrdfSetup::ImportUrdfSetup(struct GUIHelperInterface* helper, int option,
} else } else
{ {
gFileNameArray.clear(); gFileNameArray.clear();
gFileNameArray.push_back("r2d2.urdf");
//load additional urdf file names from file //load additional urdf file names from file
@@ -115,6 +121,12 @@ ImportUrdfSetup::ImportUrdfSetup(struct GUIHelperInterface* helper, int option,
fclose(f); fclose(f);
} }
if (gFileNameArray.size()==0)
{
gFileNameArray.push_back("r2d2.urdf");
}
int numFileNames = gFileNameArray.size(); int numFileNames = gFileNameArray.size();
@@ -175,7 +187,7 @@ void ImportUrdfSetup::initPhysics()
btVector3 gravity(0,0,0); btVector3 gravity(0,0,0);
gravity[upAxis]=-9.8; //gravity[upAxis]=-9.8;
m_dynamicsWorld->setGravity(gravity); m_dynamicsWorld->setGravity(gravity);
@@ -210,9 +222,7 @@ void ImportUrdfSetup::initPhysics()
if (m_useMultiBody) if (m_useMultiBody)
{ {
//create motors for each btMultiBody joint
//create motors for each joint
for (int i=0;i<mb->getNumLinks();i++) for (int i=0;i<mb->getNumLinks();i++)
{ {
@@ -236,6 +246,7 @@ void ImportUrdfSetup::initPhysics()
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
float maxMotorImpulse = 0.1f; float maxMotorImpulse = 0.1f;
btMultiBodyJointMotor* motor = new btMultiBodyJointMotor(mb,mbLinkIndex,0,0,maxMotorImpulse); btMultiBodyJointMotor* motor = new btMultiBodyJointMotor(mb,mbLinkIndex,0,0,maxMotorImpulse);
//motor->setMaxAppliedImpulse(0);
m_data->m_jointMotors[m_data->m_numMotors]=motor; m_data->m_jointMotors[m_data->m_numMotors]=motor;
m_dynamicsWorld->addMultiBodyConstraint(motor); m_dynamicsWorld->addMultiBodyConstraint(motor);
m_data->m_numMotors++; m_data->m_numMotors++;
@@ -243,6 +254,44 @@ void ImportUrdfSetup::initPhysics()
} }
} }
} else
{
if (1)
{
//create motors for each generic joint
for (int i=0;i<creation.getNum6DofConstraints();i++)
{
btGeneric6DofSpring2Constraint* c = creation.get6DofConstraint(i);
if (c->getUserConstraintPtr())
{
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;i<m_data->m_numMotors;i++) for (int i=0;i<m_data->m_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 //the maximal coordinates/iterative MLCP solver requires a smallish timestep to converge

7
examples/Importers/ImportURDFDemo/MultiBodyCreationInterface.h
View File

@@ -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* 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 class btMultiBodyLinkCollider* allocateMultiBodyLinkCollider(int urdfLinkIndex, int mbLinkIndex, btMultiBody* body) = 0;
virtual void addLinkMapping(int urdfLinkIndex, int mbLinkIndex) = 0; virtual void addLinkMapping(int urdfLinkIndex, int mbLinkIndex) = 0;

137
examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp
View File

@@ -9,7 +9,7 @@
#include "btBulletDynamicsCommon.h" #include "btBulletDynamicsCommon.h"
#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h" #include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
#include "URDFJointTypes.h"
MyMultiBodyCreator::MyMultiBodyCreator(GUIHelperInterface* guiHelper) MyMultiBodyCreator::MyMultiBodyCreator(GUIHelperInterface* guiHelper)
:m_guiHelper(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) 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); btGeneric6DofSpring2Constraint* c = new btGeneric6DofSpring2Constraint(rbA,rbB,offsetInA, offsetInB, (RotateOrder)rotateOrder);
return c; 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) void MyMultiBodyCreator::addLinkMapping(int urdfLinkIndex, int mbLinkIndex)
{ {
// m_urdf2mbLink[urdfLinkIndex] = mbLinkIndex; // m_urdf2mbLink[urdfLinkIndex] = mbLinkIndex;

30
examples/Importers/ImportURDFDemo/MyMultiBodyCreator.h
View File

@@ -8,6 +8,17 @@
struct GUIHelperInterface; struct GUIHelperInterface;
class btMultiBody; 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 class MyMultiBodyCreator : public MultiBodyCreationInterface
{ {
@@ -16,6 +27,7 @@ class MyMultiBodyCreator : public MultiBodyCreationInterface
struct GUIHelperInterface* m_guiHelper; struct GUIHelperInterface* m_guiHelper;
btAlignedObjectArray<btGeneric6DofSpring2Constraint*> m_6DofConstraints;
public: 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* 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 class btMultiBodyLinkCollider* allocateMultiBodyLinkCollider(int urdfLinkIndex, int mbLinkIndex, btMultiBody* body);
virtual void addLinkMapping(int urdfLinkIndex, int mbLinkIndex); virtual void addLinkMapping(int urdfLinkIndex, int mbLinkIndex);
btMultiBody* getBulletMultiBody(); btMultiBody* getBulletMultiBody();
int getNum6DofConstraints() const
{
return m_6DofConstraints.size();
}
btGeneric6DofSpring2Constraint* get6DofConstraint(int index)
{
return m_6DofConstraints[index];
}
}; };
#endif //MY_MULTIBODY_CREATOR #endif //MY_MULTIBODY_CREATOR

93
examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
View File

@@ -295,21 +295,9 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
} else } else
{ {
printf("Fixed joint\n"); printf("Fixed joint\n");
btMatrix3x3 rm(offsetInA.getBasis()); btGeneric6DofSpring2Constraint* dof6 = creation.createFixedJoint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB);
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));
if (enableConstraints) if (enableConstraints)
world1->addConstraint(dof6,true); world1->addConstraint(dof6,true);
} }
@@ -330,51 +318,11 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
} else } 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)); btGeneric6DofSpring2Constraint* dof6 = creation.createRevoluteJoint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,jointAxisInJointSpace,jointLowerLimit, jointUpperLimit);
dof6->setAngularLowerLimit(btVector3(1,0,0));
if (enableConstraints) if (enableConstraints)
world1->addConstraint(dof6,true); 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"); printf("Revolute/Continuous joint\n");
} }
break; break;
@@ -396,33 +344,10 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
} else } else
{ {
btGeneric6DofSpring2Constraint* dof6 = creation.allocateGeneric6DofSpring2Constraint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB);
//todo(erwincoumans) for now, we only support principle axis along X, Y or Z btGeneric6DofSpring2Constraint* dof6 = creation.createPrismaticJoint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,jointAxisInJointSpace,jointLowerLimit,jointUpperLimit);
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));
if (enableConstraints) if (enableConstraints)
world1->addConstraint(dof6,true); world1->addConstraint(dof6,true);

39
examples/MultiBody/TestJointTorqueSetup.cpp
View File

@@ -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() void TestJointTorqueSetup::initPhysics()
{ {
int upAxis = 1; int upAxis = 1;
gJointFeedbackInWorldSpace = true;
m_guiHelper->setUpAxis(upAxis); m_guiHelper->setUpAxis(upAxis);
btVector4 colors[4] = btVector4 colors[4] =
@@ -90,7 +93,9 @@ void TestJointTorqueSetup::initPhysics()
groundOrigin[upAxis] -=.5; groundOrigin[upAxis] -=.5;
groundOrigin[2]-=0.6; groundOrigin[2]-=0.6;
start.setOrigin(groundOrigin); start.setOrigin(groundOrigin);
btQuaternion groundOrn(btVector3(0,1,0),0.25*SIMD_PI);
// start.setRotation(groundOrn);
btRigidBody* body = createRigidBody(0,start,box); btRigidBody* body = createRigidBody(0,start,box);
body->setFriction(0); body->setFriction(0);
btVector4 color = colors[curColor]; btVector4 color = colors[curColor];
@@ -101,7 +106,7 @@ void TestJointTorqueSetup::initPhysics()
{ {
bool floating = false; bool floating = false;
bool damping = true; bool damping = false;
bool gyro = false; bool gyro = false;
int numLinks = 2; int numLinks = 2;
bool spherical = false; //set it ot false -to use 1DoF hinges instead of 3DoF sphericals bool spherical = false; //set it ot false -to use 1DoF hinges instead of 3DoF sphericals
@@ -156,7 +161,14 @@ void TestJointTorqueSetup::initPhysics()
// linkMass= 1000; // linkMass= 1000;
btVector3 linkInertiaDiag(0.f, 0.f, 0.f); 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); shape->calculateLocalInertia(linkMass, linkInertiaDiag);
delete shape; delete shape;
@@ -223,10 +235,8 @@ void TestJointTorqueSetup::initPhysics()
mbC->setAngularDamping(0.9f); mbC->setAngularDamping(0.9f);
} }
// //
btVector3 gravity(0,0,0); m_dynamicsWorld->setGravity(btVector3(0,-1,-10));
gravity[upAxis] = -9.81;
gravity[0] = 0;
m_dynamicsWorld->setGravity(gravity);
////////////////////////////////////////////// //////////////////////////////////////////////
if(0)//numLinks > 0) if(0)//numLinks > 0)
{ {
@@ -288,7 +298,7 @@ void TestJointTorqueSetup::initPhysics()
btVector3 color(0.0,0.0,0.5); btVector3 color(0.0,0.0,0.5);
m_guiHelper->createCollisionObjectGraphicsObject(col,color); m_guiHelper->createCollisionObjectGraphicsObject(col,color);
col->setFriction(friction); // col->setFriction(friction);
pMultiBody->setBaseCollider(col); pMultiBody->setBaseCollider(col);
} }
@@ -330,7 +340,7 @@ void TestJointTorqueSetup::initPhysics()
tr.setOrigin(posr); tr.setOrigin(posr);
tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3])); tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3]));
col->setWorldTransform(tr); col->setWorldTransform(tr);
col->setFriction(friction); // col->setFriction(friction);
bool isDynamic = 1;//(linkMass > 0); bool isDynamic = 1;//(linkMass > 0);
short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(btBroadphaseProxy::StaticFilter); short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(btBroadphaseProxy::StaticFilter);
short collisionFilterMask = isDynamic? short(btBroadphaseProxy::AllFilter) : short(btBroadphaseProxy::AllFilter ^ 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) void TestJointTorqueSetup::stepSimulation(float deltaTime)
{ {
//m_multiBody->addLinkForce(0,btVector3(100,100,100)); //m_multiBody->addLinkForce(0,btVector3(100,100,100));
if (1)//m_once) if (0)//m_once)
{ {
m_once=false; m_once=false;
m_multiBody->addJointTorque(0, 10.0); 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]); 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;i<m_jointFeedbacks.size();i++) for (int i=0;i<m_jointFeedbacks.size();i++)
{ {
b3Printf("F_reaction[%i] linear:%f,%f,%f, angular:%f,%f,%f", b3Printf("F_reaction[%i] linear:%f,%f,%f, angular:%f,%f,%f",
@@ -382,6 +395,8 @@ void TestJointTorqueSetup::stepSimulation(float deltaTime)
); );
} }
}
count++;
/* /*

20
src/BulletDynamics/Featherstone/btMultiBody.cpp
View File

@@ -30,6 +30,9 @@
#include "Bullet3Common/b3Logging.h" #include "Bullet3Common/b3Logging.h"
// #define INCLUDE_GYRO_TERM // #define INCLUDE_GYRO_TERM
///todo: determine if we need this option. If so, make a proper API, otherwise delete the global
bool gJointFeedbackInWorldSpace = false;
namespace { namespace {
const btScalar SLEEP_EPSILON = btScalar(0.05); // this is a squared velocity (m^2 s^-2) const btScalar SLEEP_EPSILON = btScalar(0.05); // this is a squared velocity (m^2 s^-2)
const btScalar SLEEP_TIMEOUT = btScalar(2); // in seconds const btScalar SLEEP_TIMEOUT = btScalar(2); // in seconds
@@ -1026,12 +1029,17 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt,
m_internalNeedsJointFeedback = true; m_internalNeedsJointFeedback = true;
m_links[i].m_jointFeedback->m_spatialInertia = spatInertia[i+1]; //m_links[i].m_jointFeedback->m_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; if (gJointFeedbackInWorldSpace)
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_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 = (spatInertia[i+1]*spatAcc[i+1]+zeroAccSpatFrc[i+1]).m_topVec; 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;
}
}
} }

1
src/BulletDynamics/Featherstone/btMultiBody.h
View File

@@ -616,7 +616,6 @@ private:
btAlignedObjectArray<btVector3> m_vectorBuf; btAlignedObjectArray<btVector3> m_vectorBuf;
btAlignedObjectArray<btMatrix3x3> m_matrixBuf; btAlignedObjectArray<btMatrix3x3> m_matrixBuf;
//std::auto_ptr<Eigen::LU<Eigen::Matrix<btScalar, 6, 6> > > cached_imatrix_lu;
btMatrix3x3 m_cachedInertiaTopLeft; btMatrix3x3 m_cachedInertiaTopLeft;
btMatrix3x3 m_cachedInertiaTopRight; btMatrix3x3 m_cachedInertiaTopRight;

1
src/BulletDynamics/Featherstone/btMultiBodyJointFeedback.h
View File

@@ -22,7 +22,6 @@ subject to the following restrictions:
struct btMultiBodyJointFeedback struct btMultiBodyJointFeedback
{ {
btSpatialForceVector m_reactionForces; btSpatialForceVector m_reactionForces;
btSymmetricSpatialDyad m_spatialInertia;
}; };
#endif //BT_MULTIBODY_JOINT_FEEDBACK_H #endif //BT_MULTIBODY_JOINT_FEEDBACK_H