How to apply a kinematic base to a btMultiBody?

Post Reply
pendulum
Posts: 1
Joined: Tue Dec 27, 2016 5:11 pm

How to apply a kinematic base to a btMultiBody?

Post by pendulum »

I am relatively new to Bullet and so I apologize if this is a naive question. I found a few relevant posts but couldn't find anything that seemed to address my particular question.

I would like to construct a simple model of the classic inverted pendulum on a cart (https://upload.wikimedia.org/wikipedia/ ... ndulum.svg). I created a successful model using rigid bodies and hinge constraints, but was not happy with the elasticity. Using btMultiBody seemed more appropriate for a robust simulation of this type, and the Bullet examples show many pendulum use-cases, although they all seem to be anchored.

What I can't seem to figure out is how to move the btMultiBody base kinematically. I would like to apply predictable linear motion to the base (i.e. the cart), with the links responding appropriately. When I try updating the base position manually (see code below), the whole btMultiBody system moves to the new position, but the dynamics to not respond to the movement. Should it be possible to do this? If so what might I be doing wrong?

Thanks.

Code to update the base of the btMultiBody:

Code: Select all

pMultiBody->setBasePos(pMultiBody->getBasePos() + btVector3(0, 0, direction * 0.1));
pMultiBody->setPosUpdated(true);
Code to set up the btMultiBody, mostly copied from one of the examples:

Code: Select all

btMultiBody *pMultiBody;
void InitInvertedPendulum()
{
	bool floating = false;
	bool damping = false;
	bool gyro = false;
	int numLinks = 3;
	bool canSleep = false;
	bool selfCollide = false;
	btVector3 linkHalfExtents(0.05, 0.5, 0.1);
	btVector3 baseHalfExtents(0.05, 0.5, 0.1);
	static btScalar radius(0.05);
		
	btVector3 baseInertiaDiag(0.f, 0.f, 0.f);
	float baseMass = 0.f;
		
	pMultiBody = new btMultiBody(numLinks, baseMass, baseInertiaDiag, !floating, canSleep);
	pMultiBody->setBaseWorldTransform(btTransform::getIdentity());
		
	//init the links
	btVector3 hingeJointAxis(1, 0, 0);
		
	//y-axis assumed up
	btVector3 parentComToCurrentCom(0, -linkHalfExtents[1] , 0);
	btVector3 currentPivotToCurrentCom(0, -linkHalfExtents[1], 0);
	btVector3 parentComToCurrentPivot = parentComToCurrentCom - currentPivotToCurrentCom;
		
	for(int i = 0; i < numLinks; ++i)
	{
		float linkMass = 10.f;
		btVector3 linkInertiaDiag(0.f, 0.f, 0.f);
		btCollisionShape* shape = 0;
		{
			shape = new btSphereShape(radius);
		}
		shape->calculateLocalInertia(linkMass, linkInertiaDiag);
		delete shape;
			
		pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, i - 1, 
									btQuaternion(0.f, 0.f, 0.f, 1.f), 
									hingeJointAxis, 
									parentComToCurrentPivot, 
									currentPivotToCurrentCom, false);
			
	}
	
	pMultiBody->finalizeMultiDof();
	
	dynamicsWorld2->addMultiBody(pMultiBody);
	pMultiBody->setCanSleep(canSleep);
	pMultiBody->setHasSelfCollision(selfCollide);
	pMultiBody->setUseGyroTerm(gyro);

	for (int i=0; i < pMultiBody->getNumLinks(); ++i)
	{
		btCollisionShape* shape =new btSphereShape(radius);
		btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, i);
		col->setCollisionShape(shape);
		bool isDynamic = 1;
		short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(btBroadphaseProxy::StaticFilter);
		short collisionFilterMask = isDynamic? 	short(btBroadphaseProxy::AllFilter) : 	short(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
		dynamicsWorld2->addCollisionObject(col,collisionFilterGroup,collisionFilterMask);//,2,1+2);
		btVector4 color(1,0,0,1);
		pMultiBody->getLink(i).m_collider=col;
	}

	btAlignedObjectArray<btQuaternion> scratch_q;
	btAlignedObjectArray<btVector3> scratch_m;
	pMultiBody->forwardKinematics(scratch_q,scratch_m);
	btAlignedObjectArray<btQuaternion> world_to_local;
	btAlignedObjectArray<btVector3> local_origin;
	pMultiBody->updateCollisionObjectWorldTransforms(world_to_local,local_origin);
}
Post Reply