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

preparation for joint limit constraint for Featherstone btMultiBody

Browse Source
This commit is contained in:
erwin.coumans
2013-10-02 06:25:20 +00:00
parent d8b6a02a7a
commit 36cfbd47a6
8 changed files with 196 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files

17
Demos/FeatherstoneMultiBodyDemo/FeatherstoneMultiBodyDemo.cpp
View File

@@ -38,6 +38,7 @@ subject to the following restrictions:
#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
#include "BulletDynamics/Featherstone/btMultiBodyLink.h"
#include "BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h"
@@ -209,15 +210,15 @@ void FeatherstoneMultiBodyDemo::initPhysics()
}
createFeatherstoneMultiBody(world, 10, btVector3 (20,29.5,-2), true);
createFeatherstoneMultiBody(world, 3, btVector3 (20,29.5,-2), true, false);//true);
createFeatherstoneMultiBody(world, 5, btVector3 (0,29.5,-2), false);
createFeatherstoneMultiBody(world, 5, btVector3 (0,29.5,-2), false,false);
}
void FeatherstoneMultiBodyDemo::createFeatherstoneMultiBody(class btMultiBodyDynamicsWorld* world, int numLinks, const btVector3& basePosition,bool isFixedBase)
void FeatherstoneMultiBodyDemo::createFeatherstoneMultiBody(class btMultiBodyDynamicsWorld* world, int numLinks, const btVector3& basePosition,bool isFixedBase, bool usePrismatic)
{
{
int n_links = numLinks;
@@ -259,10 +260,11 @@ void FeatherstoneMultiBodyDemo::createFeatherstoneMultiBody(class btMultiBodyDyn
const int child_link_num = link_num_counter++;
if (0)//i==(n_links-1))
if (usePrismatic && i==(n_links-1))
{
bod->setupPrismatic(child_link_num, mass, inertia, this_link_num,
parent_to_child, joint_axis_child_prismatic, quatRotate(parent_to_child , pos));
} else
{
bod->setupRevolute(child_link_num, mass, inertia, this_link_num,parent_to_child, joint_axis_child_hinge,
@@ -271,6 +273,13 @@ void FeatherstoneMultiBodyDemo::createFeatherstoneMultiBody(class btMultiBodyDyn
bod->setJointPos(child_link_num, initial_joint_angle);
this_link_num = i;
}
//add some constraint limit
if (usePrismatic)
{
btMultiBodyConstraint* limit = new btMultiBodyJointLimitConstraint(bod,n_links-1,2,2);
world->addMultiBodyConstraint(limit);
}
}
//add a collider for the base

2
Demos/FeatherstoneMultiBodyDemo/FeatherstoneMultiBodyDemo.h
View File

@@ -53,7 +53,7 @@ class FeatherstoneMultiBodyDemo : public PlatformDemoApplication
btDefaultCollisionConfiguration* m_collisionConfiguration;
void createFeatherstoneMultiBody(class btMultiBodyDynamicsWorld* world, int numLinks, const btVector3& basePosition,bool isFixedBase);
void createFeatherstoneMultiBody(class btMultiBodyDynamicsWorld* world, int numLinks, const btVector3& basePosition,bool isFixedBase, bool usePrismatic);
public:

3
src/BulletDynamics/CMakeLists.txt
View File

@@ -26,6 +26,7 @@ SET(BulletDynamics_SRCS
Featherstone/btMultiBody.cpp
Featherstone/btMultiBodyConstraintSolver.cpp
Featherstone/btMultiBodyDynamicsWorld.cpp
Featherstone/btMultiBodyJointLimitConstraint.cpp
)
SET(Root_HDRS
@@ -72,6 +73,8 @@ SET(Featherstone_HDRS
Featherstone/btMultiBodyLink.h
Featherstone/btMultiBodyLinkCollider.h
Featherstone/btMultiBodySolverConstraint.h
Featherstone/btMultiBodyConstraint.h
Featherstone/btMultiBodyJointLimitConstraint.h
)
SET(Character_HDRS
Character/btCharacterControllerInterface.h

76
src/BulletDynamics/Featherstone/btMultiBodyConstraint.h
View File

@@ -16,19 +16,91 @@ subject to the following restrictions:
#ifndef BT_MULTIBODY_CONSTRAINT_H
#define BT_MULTIBODY_CONSTRAINT_H
#include "LinearMath/btScalar.h"
#include "LinearMath/btAlignedObjectArray.h"
#include "btMultiBody.h"
class btMultiBody;
class btMultiBodyConstraint
{
protected:
btMultiBody* m_bodyA;
btMultiBody* m_bodyB;
int m_linkA;
int m_linkB;
int m_num_rows;
int m_jac_size_A;
int m_jac_size_both;
int m_pos_offset;
// data block laid out as follows:
// cached impulses. (one per row.)
// jacobians. (interleaved, row1 body1 then row1 body2 then row2 body 1 etc)
// positions. (one per row.)
btAlignedObjectArray<btScalar> m_data;
public:
int getIslandIdA() const
btMultiBodyConstraint(btMultiBody* bodyA,btMultiBody* bodyB,int linkA, int linkB, int numRows)
:m_bodyA(bodyA),
m_bodyB(bodyB),
m_linkA(linkA),
m_linkB(linkB),
m_num_rows(numRows)
{
m_jac_size_A = (6 + bodyA->getNumLinks());
m_jac_size_both = (m_jac_size_A + (bodyB ? 6 + bodyB->getNumLinks() : 0));
m_pos_offset = ((1 + m_jac_size_both)*m_num_rows);
m_data.resize((2 + m_jac_size_both) * m_num_rows);
}
virtual int getIslandIdA() const
{
return 0;
}
int getIslandIdB() const
virtual int getIslandIdB() const
{
return 0;
}
virtual void update()=0;
// current constraint position
// constraint is pos >= 0 for unilateral, or pos = 0 for bilateral
// NOTE: position ignored for friction rows.
btScalar getPosition(int row) const
{
return m_data[m_pos_offset + row];
}
void setPosition(int row, btScalar pos)
{
m_data[m_pos_offset + row] = pos;
}
// jacobian blocks.
// each of size 6 + num_links. (jacobian2 is null if no body2.)
// format: 3 'omega' coefficients, 3 'v' coefficients, then the 'qdot' coefficients.
btScalar* jacobianA(int row)
{
return &m_data[m_num_rows + row * m_jac_size_both];
}
const btScalar* jacobianA(int row) const
{
return &m_data[m_num_rows + (row * m_jac_size_both)];
}
btScalar* jacobianB(int row)
{
return &m_data[m_num_rows + (row * m_jac_size_both) + m_jac_size_A];
}
const btScalar* jacobianB(int row) const
{
return &m_data[m_num_rows + (row * m_jac_size_both) + m_jac_size_A];
}
};
#endif //BT_MULTIBODY_CONSTRAINT_H

17
src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp
View File

@@ -18,8 +18,9 @@ subject to the following restrictions:
#include "btMultiBodyLinkCollider.h"
#include "BulletDynamics/ConstraintSolver/btSolverBody.h"
#include "btMultiBodyConstraint.h"
#include "LinearMath/btQuickprof.h"
btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
{
@@ -173,6 +174,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
bool isFriction, btScalar desiredVelocity, btScalar cfmSlip)
{
BT_PROFILE("setupMultiBodyContactConstraint");
btVector3 rel_pos1;
btVector3 rel_pos2;
@@ -484,6 +486,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodyFrictionConstraint(const btVector3& normalAxis,btPersistentManifold* manifold,int frictionIndex,btManifoldPoint& cp,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip)
{
BT_PROFILE("addMultiBodyFrictionConstraint");
btMultiBodySolverConstraint& solverConstraint = m_multiBodyFrictionContactConstraints.expandNonInitializing();
solverConstraint.m_frictionIndex = frictionIndex;
bool isFriction = true;
@@ -576,7 +579,7 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold*
/////setup the friction constraints
#define ENABLE_FRICTION
#ifdef ENABLE_FRICTION
solverConstraint.m_frictionIndex = m_tmpSolverContactFrictionConstraintPool.size();
solverConstraint.m_frictionIndex = frictionIndex;
#if ROLLING_FRICTION
btVector3 angVelA(0,0,0),angVelB(0,0,0);
if (rb0)
@@ -711,6 +714,16 @@ void btMultiBodyConstraintSolver::convertContacts(btPersistentManifold** manifol
convertMultiBodyContact(manifold,infoGlobal);
}
}
//also convert the multibody constraints, if any
for (int i=0;i<m_tmpNumMultiBodyConstraints;i++)
{
btMultiBodyConstraint* c = m_tmpMultiBodyConstraints[i];
c->update();
}
}

1
src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp
View File

@@ -393,6 +393,7 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
m_sortedConstraints.quickSort(btSortConstraintOnIslandPredicate2());
btTypedConstraint** constraintsPtr = getNumConstraints() ? &m_sortedConstraints[0] : 0;
m_sortedMultiBodyConstraints.resize(m_multiBodyConstraints.size());
for (i=0;i<m_multiBodyConstraints.size();i++)
{
m_sortedMultiBodyConstraints[i] = m_multiBodyConstraints[i];

48
src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp Normal file
View File

@@ -0,0 +1,48 @@
#include "btMultiBodyJointLimitConstraint.h"
#include "btMultiBody.h"
#include "btMultiBodyLinkCollider.h"
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
btMultiBodyJointLimitConstraint::btMultiBodyJointLimitConstraint(btMultiBody* body, int link, btScalar lower, btScalar upper)
:btMultiBodyConstraint(body,body,link,link,2),
m_lowerBound(lower),
m_upperBound(upper)
{
// the jacobians never change, so may as well
// initialize them here
// note: we rely on the fact that jacobians are
// always initialized to zero by the Constraint ctor
// row 0: the lower bound
jacobianA(0)[6 + link] = 1;
// row 1: the upper bound
jacobianB(1)[6 + link] = -1;
}
btMultiBodyJointLimitConstraint::~btMultiBodyJointLimitConstraint()
{
}
int btMultiBodyJointLimitConstraint::getIslandIdA() const
{
return m_bodyA->getLinkCollider(0)->getIslandTag();
}
int btMultiBodyJointLimitConstraint::getIslandIdB() const
{
return m_bodyB->getLinkCollider(0)->getIslandTag();
}
void btMultiBodyJointLimitConstraint::update()
{
// only positions need to be updated -- jacobians and force
// directions were set in the ctor and never change.
// row 0: the lower bound
setPosition(0, m_bodyA->getJointPos(m_linkA) - m_lowerBound);
// row 1: the upper bound
setPosition(1, m_upperBound - m_bodyA->getJointPos(m_linkA));
}

40
src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h Normal file
View File

@@ -0,0 +1,40 @@
/*
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.
*/
#ifndef BT_MULTIBODY_JOINT_LIMIT_CONSTRAINT_H
#define BT_MULTIBODY_JOINT_LIMIT_CONSTRAINT_H
#include "btMultiBodyConstraint.h"
class btMultiBodyJointLimitConstraint : public btMultiBodyConstraint
{
protected:
btScalar m_lowerBound;
btScalar m_upperBound;
public:
btMultiBodyJointLimitConstraint(btMultiBody* body, int link, btScalar lower, btScalar upper);
virtual ~btMultiBodyJointLimitConstraint();
virtual int getIslandIdA() const;
virtual int getIslandIdB() const;
virtual void update();
};
#endif //BT_MULTIBODY_JOINT_LIMIT_CONSTRAINT_H