Building articulated character with btMultiBody

Post Reply
roguecodemonkey
Posts: 6
Joined: Thu Mar 12, 2015 1:11 pm
Location: Dundee, Scotland, UK

Building articulated character with btMultiBody

Post by roguecodemonkey »

I'm building an articulated character using btMultiBody and using links to represent each bone in the character skeleton.

I'm going use spherical joints between each limb and use btMultiBodyJointMotor to drive each joint to create the desired pose for the character.

I was wondering what the best way to represent the root bone (pelvis) as it should have 6 degrees of freedom and be at the top of the link hierarchy.

"Practical Physics for Articulated Charaters" by Evangelos Kokkevis suggests:
http://core.ac.uk/download/pdf/21750869.pdf

"The free moving root of the original character can be modeled by adding six zero-length links at the top of the articulated body chain, connected via three prismatic and three revolute joints. Alternatively, a free rigid body can be substituted for the root link."

If I go for the first method suggested here I'm thinking each link for each degree of freedom should have the same mass and moment of inertia describing the pelvis bone. The base is fixed with this method. Does that sound right?

I'm not sure how to go about trying the second method. If I set the base to be not fixed and use a potential difference controller to calculate the appropriate force and torque to move it to it's desired location and rotation, then my initial tests show undesirable overshoot and I don't get the nice motion achieved using btMultiBodyJointMotor for the other bones in the character hierarchy.

Keeping the base fixed let's me use btMultiBodyJointMotor to pose all joints if I go ahead and create a link for degree of freedom for the root bone.

Also unless I'm mistaken, the spherical joint is basically an equivalent representation of 3 revolute joints. I'm wondering if it's worth rolling my own 6DOF joint type?

Appreciate any thoughts anyone may have regarding this.
User avatar
Erwin Coumans
Site Admin
Posts: 4221
Joined: Sun Jun 26, 2005 6:43 pm
Location: California, USA
Contact:

Re: Building articulated character with btMultiBody

Post by Erwin Coumans »

roguecodemonkey wrote:If I set the base to be not fixed and use a potential difference controller to calculate the appropriate force and torque to move it to it's desired location and rotation, then my initial tests show undesirable overshoot and I don't get the nice motion achieved using btMultiBodyJointMotor for the other bones in the character hierarchy.
I suggest using a floating base, setting the 'fixedBase' parameter to fase in the btMultiBody constructor:

Code: Select all

 btMultiBody(int n_links,             // NOT including the base
                btScalar mass,                // mass of base
                const btVector3 &inertia,    // inertia of base, in base frame; assumed diagonal
                bool fixedBase,           // whether the base is fixed (true) or can move (false)
                bool canSleep,
                bool multiDof = false
                          );
If you have any issues with this, please create a simple reproduction case in any of the Bullet examples and file the issue in github. There should be no need to use a fixed base and adding joints and motors to such fixed base, the floating base should just work fine.
Also unless I'm mistaken, the spherical joint is basically an equivalent representation of 3 revolute joints. I'm wondering if it's worth rolling my own 6DOF joint type?
No, a spherical joint (btMultiBody::setupSpherical) uses a quaternion representation and avoids Euler gimbal lock/singularities. It seems similar but it is not the same.

I plan porting the btGeneric6DofSpring2Constraint to support btMultiBody. If you want to give this porting a try you are welcome!

Erwin
roguecodemonkey
Posts: 6
Joined: Thu Mar 12, 2015 1:11 pm
Location: Dundee, Scotland, UK

Re: Building articulated character with btMultiBody

Post by roguecodemonkey »

Thanks for the very useful guidance.

Using the floating base to represent the root and then use spherical joints between the limbs is what I thought I should be using.
I just couldn't get it working on my last attempt.
I'll give it another go and post my code once I'm done.

Thanks again.
roguecodemonkey
Posts: 6
Joined: Thu Mar 12, 2015 1:11 pm
Location: Dundee, Scotland, UK

Re: Building articulated character with btMultiBody

Post by roguecodemonkey »

Here we go :)

I'm keeping things simple at the moment. I have the base as a 1 x 1 x 1 box with mass = 1.

My first test was to create a btMultiBody with no links and apply the appropriate force and torque [if needed] so the base has no rotation and moves at a linear speed back and forth between 2 points.

This worked as expected with the base moving smoothly between the 2 points with no rotation.

I then added a link with a revolute joint [dimensions: 1 x 1 x 1 mass: 1] with a pivot placed above the base and used btMultiBodyJointMotor to target zero rotation.

When I add the link I get some rocking motion I didn't expect.

Here's the code. Let me know if I'm doing something wrong or perhaps not understanding how all this works.

Initialisation

Code: Select all

	{
		const btVector3 base_half_extents(0.5f, 0.5f, 0.5f);
		btCollisionShape* base_shape = new btBoxShape(base_half_extents);

		const float base_mass = 1.0f;
		btVector3 base_inertia;
		base_shape->calculateLocalInertia(base_mass, base_inertia);

		multi_body_ = new btMultiBody(
			1,// number of links not including the base
			base_mass,// mass of the base
			base_inertia,// inertia of base
			false,// whether the base is fixed (true) or can move (false)
			false, // can sleep?
			true// multi dof?
			);

		// base collider
		{
			const int collider_link_num = - 1;
			btVector3 half_extents = base_half_extents;
			btCollisionShape* shape = new btBoxShape(half_extents);
			btMultiBodyLinkCollider* collider = new btMultiBodyLinkCollider(multi_body_, -1);
			collider->setCollisionShape(shape);
			multi_body_->setBaseCollider(collider);

			bullet_world_->addCollisionObject(collider, short(btBroadphaseProxy::DefaultFilter), short(btBroadphaseProxy::AllFilter));
		}

		// upper body
		{
			const float mass = 1.0f;
			const btVector3 half_extents(0.5f, 0.5f, 0.5f);
			btCollisionShape* shape = new btBoxShape(half_extents);
			btVector3 inertia;
			shape->calculateLocalInertia(mass, inertia);

//			multi_body_->setupSpherical(0, mass, inertia, -1, btQuaternion::getIdentity(), btVector3(0.0f, 1.0f, 0.0f), btVector3(0.0f, 1.0f, 0.0f), true);
			multi_body_->setupRevolute(0, mass, inertia, -1, btQuaternion::getIdentity(), btVector3(0.0f, 0.0f, 1.0f), btVector3(0.0f, 1.0f, 0.0f), btVector3(0.0f, 1.0f, 0.0f), true);

			// collider
			btMultiBodyLinkCollider* collider = new btMultiBodyLinkCollider(multi_body_, 0);
			collider->setCollisionShape(shape);
			multi_body_->getLink(0).m_collider = collider;

			bullet_world_->addCollisionObject(collider, short(btBroadphaseProxy::DefaultFilter), short(btBroadphaseProxy::AllFilter));

			// motor
			const float max_impulse = 100000.0f;
			multi_body_joint_motor_ = new btMultiBodyJointMotor(multi_body_, 0, 0.0f, max_impulse));
			((btMultiBodyDynamicsWorld*)bullet_world_)->addMultiBodyConstraint(multi_body_joint_motor_);
		}

	}

	multi_body_->finalizeMultiDof();

	// add multi body to simulation
	((btMultiBodyDynamicsWorld*)bullet_world_)->addMultiBody(multi_body_);
Update

Code: Select all

		// base
		{
			// target linear motion in the x-axis that goes back and forth between two points
			{
				const btVector3 target_pos(current_distance_, 0.f, 0.f);
				const btVector3 current_pos = multi_body_->getBasePos();
				const btVector3 target_linear_velocity = (target_pos - current_pos) / delta_time;

				const btVector3 linear_acceleration = (target_linear_velocity - multi_body_->getBaseVel()) / delta_time;

				const btVector3 force = linear_acceleration * multi_body_->getBaseMass();

				multi_body_->addBaseForce(force);
			}

			// target zero rotation
			{
				const btQuaternion& current_rot = multi_body_->getWorldToBaseRot().inverse();
				const btQuaternion& target_rot = btQuaternion::getIdentity();

				// calculate the quaternion that rotates current_base_rot into target_base_rot
				btQuaternion delta_rot = target_rot * current_rot.inverse();

				btVector3 target_angular_velocity;
				float rot_length_sqr = delta_rot.x()*delta_rot.x() + delta_rot.y()*delta_rot.y() + delta_rot.z()*delta_rot.z();
				if (rot_length_sqr > 0.0f)
				{
					float rot_length = sqrtf(rot_length_sqr);
					float rot_angle = 2.0f * acosf(delta_rot.w());

					target_angular_velocity = btVector3(delta_rot.x(), delta_rot.y(), delta_rot.z());
					target_angular_velocity /= rot_length;
					target_angular_velocity *= rot_angle / (delta_time);
				}
				else
					target_angular_velocity.setZero();

				btVector3 current_angular_velocity = multi_body_->getBaseOmega();

				// calculate desired angular acceleration to reach target angular velocity
				btVector3 angular_acceleration = (target_angular_velocity - current_angular_velocity) / delta_time;

				btVector3 inertia = multi_body_->getBaseInertia();
				btVector3 torque = inertia*angular_acceleration;

				// add torque to move to desired rotation
				multi_body_->addBaseTorque(torque);
			}
		}

		// upper link
		if (multi_body_->getNumLinks() > 0)
		{
			// make link target zero rotation
			const float target_rot = 0.0f;
			const float current_rot = multi_body_->getJointPos(0);
			float target_angular_velocity = (target_rot - current_rot) / delta_time;

			multi_body_joint_motor_->setVelocityTarget(target_angular_velocity);
		}
benelot
Posts: 350
Joined: Sat Jul 04, 2015 10:33 am
Location: Bern, Switzerland
Contact:

Re: Building articulated character with btMultiBody

Post by benelot »

Hello guys,

@roguecodemonkey:
Any success with the spherical joint? I could not control the joint properly using the motors, the joint did not move at all? Does this work with you? Also, did you try to limit any of your spherical joints, because I could not do this as well. So if you figured it out how to move the joint to a certain joint position and keep it there properly (no or minor oscillations etc), then I would really appreciate some guidance.
kingchurch
Posts: 28
Joined: Sun May 13, 2012 7:14 am

Re: Building articulated character with btMultiBody

Post by kingchurch »

Motor/limit constraints are not currently supported for spherical links of btMultiBody. However nothing prevents you from implementing your own force motor by applying a force/torque to the spherical links directly. But pay attention that the joint positions returned for spherical links is Quaternion instead of Euler angle. So if you are attempting to "control" it as Euler angles it won't work. You can either decompose the relative joint orientation into Euler angles and control Euler angles or write a controller for the relative Quaternion directly and apply torques in the direction of the rotation axis of the relative quaternion between parent and child joints. The latter way might work out cleaner since it's immune to gimbal lock.
benelot
Posts: 350
Joined: Sat Jul 04, 2015 10:33 am
Location: Bern, Switzerland
Contact:

Re: Building articulated character with btMultiBody

Post by benelot »

@Kingchurch: Thank you for your suggestion. I still do not understand why gimbal lock is a problem. To me, it is just a phenomenon that turns 3DoF into 2 DoF.

Can you give me a code example for the control via the btQuaternion? I am interested in that option but I am not sure if I understand it correctly.
BluePrintRandom
Posts: 47
Joined: Sun Oct 27, 2013 4:16 am

Re: Building articulated character with btMultiBody

Post by BluePrintRandom »

I have a working, walking ragdoll,

I use 6dof joints , and I set the angle using a angular velocity (target+current)/2
benelot
Posts: 350
Joined: Sat Jul 04, 2015 10:33 am
Location: Bern, Switzerland
Contact:

Re: Building articulated character with btMultiBody

Post by benelot »

@BluePrintRandom:
6DoF joints are not implemented in the btMultiBody, are you sure you are using it?
Post Reply