Since this is the meaty bits where the robot is configured, we need to understand links and joints first. Here is the bit from the docs:
Ok so that’s like the basics, which is pretty confusingly worded here. But basically, because the body isn’t counted as a link, you have the same number of links as joints, since you add a joint and link, each time you add a new body part.
Maybe good to recall the 6 degrees of freedom:
So the one function is localCreateRigidBody,
you can follow along in the code (We’re looking at TestRig) here:
https://github.com/bulletphysics/bullet3/blob/master/examples/DynamicControlDemo/MotorDemo.cpp
Takes mass, startTransform, and shape.
If mass is not 0, it is ‘dynamic’. If it’s dynamic, calculate the local inertia on the shape, using the mass and local inertia.
The default motion state uses the startTransform
The rigid body construction info takes mass, motion state, shape and local inertia. body is then pointing to the rigid body created using the construction info.
The rigid body is added to the world, and returned.
So the Test Rig takes the world, a vector position offset and a boolean of whether it is fixed or not.
The comment says // Setup geometry
Body size is 0.25, legs length are 0.45, foreleg lengths are 0.75 long.
shapes[0] is a capsule shape of ‘body size’ radius, and 0.1 scalar height. Then it iterates through the number of legs, adding in shapes[i] as leg capsules with .1 radius and leg length, and shapes[i+1] as foreleg capsules with .08 radius and foreleg length.
Next is // Setup rigid bodies
It just sets up the offset origin.
Next is // root
Root’s vector is set as 0.5 height, and a transform set up with origin at root. If the ‘fixed’ variable is set, the root body is set up with (mass, startTransform, and shape) of 0, offset*transform, and the capsule shape set up just now. If ‘fixed’ variable is not set, the mass is 1, same place, same shape.
Next is // legs
This bit is complicated. Angle is 2*PI*i/NUM_LEGS, to evenly distribute the legs around the root body. fSin is the sin (opposite over hypotenuse), and fCos is the cos (adjacent over hypotenuse).
So, set transform to identity. That just means like 1,1,1. Multiply a vector by identity and you get the vector, I think.
vBoneOrigin is a vector:
X: cos (bodysize + 0.5*leg length),
Y: 0.5,
Z: sin (bodysize + 0.5*leg length)
then set the transform to this vector. So, this is to find where the joint is, between the body, and the thigh. Same starting height for all legs, but X and Z plane goes around in circle.
// thigh
vToBone = (vBoneOrigin – vRoot).normalize()
Normalizing makes a unit vector (changes length to 1). So Bone Origin minus Root, normalized, means a unit vector in the direction of root, from the new thigh joint.
vAxis = vToBone.cross(vUp)
where vUp is a unit vector on the y axis.
transform.setRotation(btQuaternion(vAxis, M_PI_2));
Ok dunno. So… cross product is like a normal vector, perpendicular to both vToBone, and vUp, using a right hand rule. Both are unit vectors. So it’s like a weird sideways vector. Ok but that is called vAxis. AH. ok, so the axis is the freaking axis to rotate around. Duh. And it rotates on that axis, by PI/2. By 90 degrees.
The quaternion is just the mathematical system rotating it by 90 degrees. It must have rotated the transform to point downwards.
m_bodies[1 + 2 * i] = localCreateRigidBody(btScalar(1.), offset * transform, m_shapes[1 + 2 * i]);
Ok so that makes the thigh leg bits I guess.
// shin
transform.setIdentity();
transform.setOrigin(btVector3(btScalar(fCos * (fBodySize + fLegLength)), btScalar(fHeight – 0.5 * fForeLegLength), btScalar(fSin * (fBodySize + fLegLength))));
m_bodies[2 + 2 * i] = localCreateRigidBody(btScalar(1.), offset * transform, m_shapes[2 + 2 * i]);
So transform is moved to
X: cos (body + leg)
Y: height – 0.5*foreleg,
Z: sin (body + leg),
Ok so we already rotated 90 degrees, and now we have to move the transform to the axis for the foreleg. So the Y axis is the same for all of them, and then when you have cos and sin, that makes a circle around the y axis, at the (body+leg) distance. That might just make sense.
// Setup some damping on the m_bodies
setDamping, setDeactivationTime, setSleepingThresholds must be so the robot chills for a bit and acts a bit more earth-like.
Ok and finally
// Setup the constraints
Iterates through NUM_LEGS, same angle iteration.
//Hip joints first. (i.e. Between Body and Thigh)
localA.setIdentity();localB.setIdentity();
localA.getBasis().setEulerZYX(0, -fAngle, 0);
localA.setOrigin(btVector3(btScalar(fCos * fBodySize), btScalar(0.), btScalar(fSin * fBodySize)));
localB = m_bodies[1 + 2 * i]->getWorldTransform().inverse() * m_bodies[0]->getWorldTransform() * localA;
hingeC = new btHingeConstraint(*m_bodies[0], *m_bodies[1 + 2 * i], localA, localB);
hingeC->setLimit(btScalar(-0.75 * M_PI_4), btScalar(M_PI_8));
m_joints[2 * i] = hingeC;
m_ownerWorld->addConstraint(m_joints[2 * i], true);
Ok so A is rotated around Y axis by the current angle, and A is moved to bodySize outwards.
OK so for B… the inverse of the leg’s ‘world transform’, times the body’s ‘world transform’, times A. The body is presumably like sitting up straight, and A rotates it about Y, and then the inverse of the leg’s transform makes it point back towards origin.
And then the hinge constraint is set up with body and thigh, A and B. Limits set to -.75*PI/4 to PI/8. Apparently -33.75 degrees to 22.5 degrees.
//knee joints
localA.setIdentity();localB.setIdentity();localC.setIdentity();
localA.getBasis().setEulerZYX(0, -fAngle, 0);
localA.setOrigin(btVector3(btScalar(fCos * (fBodySize + fLegLength)), btScalar(0.), btScalar(fSin * (fBodySize + fLegLength))));
localB = m_bodies[1 + 2 * i]->getWorldTransform().inverse() * m_bodies[0]->getWorldTransform() * localA;
localC = m_bodies[2 + 2 * i]->getWorldTransform().inverse() * m_bodies[0]->getWorldTransform() * localA;
hingeC = new btHingeConstraint(*m_bodies[1 + 2 * i], *m_bodies[2 + 2 * i], localB, localC);
hingeC->setLimit(btScalar(-M_PI_8), btScalar(0.2));
m_joints[1 + 2 * i] = hingeC;
m_ownerWorld->addConstraint(m_joints[1 + 2 * i], true);
And now, same story but between leg and foreleg, from -22.5 degrees to 11.4 degrees.
Anyway, I get the idea.