Problems implementing PID control on a KinematicRagdollControl joint

So I’m trying to implement PID controllers on the SixDofJoints created by KinematicRagdollControl. I’ve run into a number of problems that I can’t get around, and so far it has been very frustrating. I hope someone can offer some help. I’m running JME 3.0 Beta, and I believe that I switched to a nightly build or some sort at some point to get a version of the Blender imported that worked. So far, the Blender importer has been the only reliable importer for me.



First, here is a picture of my model in Blender:



https://dl.dropbox.com/u/106825941/WalkChar2.png





Problem #1: When I instantiate my model in physics space, the rigid bodies associated with each of the bones explode outward in all directions, and frequently come to rest in a state where they are locked together, and intersecting. This effect is mentioned in the tutorials about ragdolls, and the remedy according to them is to instantiate each rigid body far enough apart that none of the bone-rigid-bodies intersect. However, I don’t have that option with KinematicRagdollControl. I tried posing the model via setUserTransforms() before adding it to physics space (this didn’t work), and I cannot find a method to force the rotation of a SixDofJoint. How do I fix this?



Here is a picture of a typical scenario where my model’s leg has come to rest intersecting the head:



https://dl.dropbox.com/u/106825941/BoneIntersection.png





Problem #2: I cannot make immobile joints. If you look at the picture of the model in Blender, you can see that there are a number of bones in the “head” area. I need at least two of these (the “shoulders”) in order to place the joints I care to articulate properly. I have written a custom RagdollPreset for this model, and each of these joints within the head has a corresponding line that looks something like this:



[java]

boneMap.put(“Head”, new JointPreset(0,0,0,0,0,0));

[/java]



…Which I take to mean that the joint should not be able to flex relative to its parent. Despite this, they all flex quite a bit. There is an undocumented method for SixDofJoint called setAngularDamping() which seems to help somewhat, but it does not remove the problem entirely. Is there a way to fix this?



All of the joints I have seem a bit… Rubbery. Is Angular Damping the only tool I have to change this?





Problem #3: Running a separate PID (Or even just PD, for that matter) controller for each of the Pitch, Yaw, and Roll motors on a joint seems to be a bad idea. Quaternions would be ideal to work with, but we’re forced to use Euler angles for the RotationalMotors. I heard mention by @normen that using “direction vectors” would be better (see this post by @cmeyer), but I’m not sure what those are. Don’t all vectors have a direction? I know I have the wrong technique here, but I need a clue about how to approach the right one.





Thanks. Relevant code posted below:



[java]

@Override

public void simpleInitApp() {



//Load up the heightmap.

map.load();

map.normalizeTerrain(20.0f);



flyCam.setMoveSpeed(50);



// TERRAIN TEXTURE material

matRock = new Material(assetManager, “Common/MatDefs/Terrain/Terrain.j3md”);

matRock.setBoolean(“useTriPlanarMapping”, false);



// ALPHA map (for splat textures)

matRock.setTexture(“Alpha”, assetManager.loadTexture(“Textures/Terrain/splat/alphamap.png”));



// GRASS texture

Texture grass = assetManager.loadTexture(“Textures/Terrain/splat/grass.jpg”);

grass.setWrap(WrapMode.Repeat);

matRock.setTexture(“Tex1”, grass);

matRock.setFloat(“Tex1Scale”, grassScale);



// DIRT texture

Texture dirt = assetManager.loadTexture(“Textures/Terrain/splat/dirt.jpg”);

dirt.setWrap(WrapMode.Repeat);

matRock.setTexture(“Tex2”, dirt);

matRock.setFloat(“Tex2Scale”, dirtScale);



// ROCK texture

Texture rock = assetManager.loadTexture(“Textures/Terrain/splat/road.jpg”);

rock.setWrap(WrapMode.Repeat);

matRock.setTexture(“Tex3”, rock);

matRock.setFloat(“Tex3Scale”, rockScale);



// WIREFRAME material

matWire = new Material(assetManager, “Common/MatDefs/Misc/Unshaded.j3md”);

matWire.setColor(“Color”, ColorRGBA.Green);





terrain = new TerrainQuad(“terrain”, 65, mapSize + 1, map.getHeightMap());//, new LodPerspectiveCalculatorFactory(getCamera(), 4)); // add this in to see it use entropy for LOD calculations

List<Camera> cameras = new ArrayList<Camera>();

cameras.add(getCamera());

TerrainLodControl control = new TerrainLodControl(terrain, cameras);

terrain.addControl(control);

terrain.setMaterial(matRock);

terrain.setModelBound(new BoundingBox());

terrain.updateModelBound();

terrain.setLocalTranslation(0, -100, 0);

terrain.setLocalScale(2f, 1f, 2f);

rootNode.attachChild(terrain);



//Comment these lines out to enter debug mode.

bulletAppState = new BulletAppState();

bulletAppState.setEnabled(true);

stateManager.attach(bulletAppState);



floor_phy = new RigidBodyControl(0.0f);

terrain.addControl(floor_phy);

//Comment the next line out to enter debug state.

bulletAppState.getPhysicsSpace().add(floor_phy);



DirectionalLight light = new DirectionalLight();

light.setDirection((new Vector3f(-0.5f, -1f, -0.5f)).normalize());

rootNode.addLight(light);



flyCam.setEnabled(true);



setupWalkChar(true);



//Debug: Tether the walkchar to a physical object in space, so that it doesn’t fall.



Box box = new Box(Vector3f.ZERO, 1.0f, 1.0f, 1.0f);

box.scaleTextureCoordinates(new Vector2f(1f, .5f));

Geometry brick_geo = new Geometry(“brick”, box);

brick_geo.setMaterial(matRock);

rootNode.attachChild(brick_geo);

brick_geo.setLocalTranslation(new Vector3f(0, 10.0f, 0));

RigidBodyControl brick_phy = new RigidBodyControl(0);

brick_geo.addControl(brick_phy);

bulletAppState.getPhysicsSpace().add(brick_phy);



//Now tether the walkChar to the brick with a Point2Point joint.

//Point2PointJoint joint = new Point2PointJoint(walkChar_phy.getBoneRigidBody(“Tail”), brick_geo.getControl(RigidBodyControl.class), new Vector3f(0,5.0f,0), new Vector3f(0,-10,0));

HingeJoint joint = new HingeJoint(walkChar_phy.getBoneRigidBody(“Root”), brick_geo.getControl(RigidBodyControl.class), new Vector3f(0,5.0f,0), new Vector3f(0,-10,0), new Vector3f(1,0,0), new Vector3f(1,0,0));

joint.setLimit(0, 0);

bulletAppState.getPhysicsSpace().add(joint);









}





private void setupWalkChar(boolean showDebugInfo) {

walkChar = (Node)assetManager.loadModel(“Models/WalkCharComplexBoneRot2/WalkCharComplexBoneRot2.j3o”);



Spatial wcSpatial = ((Node)walkChar.getChild(“Armature”)).getChild(“Cylinder”);



control = wcSpatial.getControl(AnimControl.class);











AnimControl animControl = wcSpatial.getControl(AnimControl.class);



SkeletonDebugger skeletonDebug = null;

if (showDebugInfo) {

skeletonDebug = new SkeletonDebugger(“skeleton”, animControl.getSkeleton());

Material mat2 = new Material(assetManager, “Common/MatDefs/Misc/Unshaded.j3md”);

mat2.setColor(“Color”, ColorRGBA.Green);

mat2.getAdditionalRenderState().setDepthTest(false);

skeletonDebug.setMaterial(mat2);

skeletonDebug.setLocalTranslation(walkChar.getLocalTranslation());

}









walkChar_phy = new KinematicRagdollControl(new WalkCharRagdollPreset(), 0.4f);

walkChar_phy.addBoneName(“Thigh.L”);

walkChar_phy.addBoneName(“Thigh.R”);

walkChar_phy.addBoneName(“Shin.L”);

walkChar_phy.addBoneName(“Shin.R”);

walkChar_phy.addBoneName(“Foot.L”);

walkChar_phy.addBoneName(“Foot.R”);

walkChar_phy.addBoneName(“Shoulder.L”);

walkChar_phy.addBoneName(“Shoulder.R”);

walkChar_phy.addBoneName(“Head”);

walkChar_phy.addBoneName(“Tail”);

walkChar_phy.addBoneName(“Root”);









wcSpatial.addControl(walkChar_phy);

if (showDebugInfo) ((Node)wcSpatial).attachChild(skeletonDebug);



rootNode.attachChild(walkChar);

walkChar_phy.setEnabled(true);

walkChar_phy.setRagdollMode();

bulletAppState.getPhysicsSpace().add(walkChar_phy);







chaseCam = new ChaseCamera(cam, wcSpatial, inputManager);



if (showDebugInfo) bulletAppState.getPhysicsSpace().enableDebug(assetManager);



//Get all the relevant bones.

PhysicsRigidBody head = walkChar_phy.getBoneRigidBody(“Head”);

PhysicsRigidBody tail = walkChar_phy.getBoneRigidBody(“Tail”);

PhysicsRigidBody shoulderL = walkChar_phy.getBoneRigidBody(“Shoulder.L”);

PhysicsRigidBody shoulderR = walkChar_phy.getBoneRigidBody(“Shoulder.R”);

thighL = walkChar_phy.getJoint(“Thigh.L”);

thighR = walkChar_phy.getJoint(“Thigh.R”);

shinL = walkChar_phy.getJoint(“Shin.L”);

shinR = walkChar_phy.getJoint(“Shin.R”);

footL = walkChar_phy.getJoint(“Foot.L”);

footR = walkChar_phy.getJoint(“Foot.R”);



thighR.getRotationalLimitMotor(0).setEnableMotor(true);

thighR.getRotationalLimitMotor(1).setEnableMotor(true);

thighR.getRotationalLimitMotor(2).setEnableMotor(true);

thighR.getRotationalLimitMotor(0).setMaxMotorForce(50.0f);

thighR.getRotationalLimitMotor(1).setMaxMotorForce(50.0f);

thighR.getRotationalLimitMotor(2).setMaxMotorForce(50.0f);

thighL.getRotationalLimitMotor(0).setEnableMotor(true);

thighL.getRotationalLimitMotor(1).setEnableMotor(true);

thighL.getRotationalLimitMotor(2).setEnableMotor(true);

thighL.getRotationalLimitMotor(0).setMaxMotorForce(50.0f);

thighL.getRotationalLimitMotor(1).setMaxMotorForce(50.0f);

thighL.getRotationalLimitMotor(2).setMaxMotorForce(50.0f);



shinR.getRotationalLimitMotor(0).setEnableMotor(true);

shinL.getRotationalLimitMotor(0).setEnableMotor(true);

shinL.getRotationalLimitMotor(0).setMaxMotorForce(50.0f);

shinR.getRotationalLimitMotor(0).setMaxMotorForce(50.0f);



footR.getRotationalLimitMotor(0).setEnableMotor(true);

footR.getRotationalLimitMotor(1).setEnableMotor(true);

footR.getRotationalLimitMotor(2).setEnableMotor(true);

footR.getRotationalLimitMotor(0).setMaxMotorForce(50.0f);

footR.getRotationalLimitMotor(1).setMaxMotorForce(50.0f);

footR.getRotationalLimitMotor(2).setMaxMotorForce(50.0f);

footL.getRotationalLimitMotor(0).setEnableMotor(true);

footL.getRotationalLimitMotor(1).setEnableMotor(true);

footL.getRotationalLimitMotor(2).setEnableMotor(true);

footL.getRotationalLimitMotor(0).setMaxMotorForce(50.0f);

footL.getRotationalLimitMotor(1).setMaxMotorForce(50.0f);

footL.getRotationalLimitMotor(2).setMaxMotorForce(50.0f);







head.setRestitution(0.0f);

tail.setRestitution(0.0f);

shoulderL.setRestitution(0.0f);

shoulderR.setRestitution(0.0f);

walkChar_phy.getBoneRigidBody(“Thigh.L”).setRestitution(0.0f);

walkChar_phy.getBoneRigidBody(“Thigh.R”).setRestitution(0.0f);

walkChar_phy.getBoneRigidBody(“Shin.L”).setRestitution(0.0f);

walkChar_phy.getBoneRigidBody(“Shin.R”).setRestitution(0.0f);

walkChar_phy.getBoneRigidBody(“Foot.L”).setRestitution(0.0f);

walkChar_phy.getBoneRigidBody(“Foot.R”).setRestitution(0.0f);



head.setFriction(1.0f);

tail.setFriction(1.0f);

shoulderL.setFriction(1.0f);

shoulderR.setFriction(1.0f);

walkChar_phy.getBoneRigidBody(“Thigh.L”).setFriction(1.0f);

walkChar_phy.getBoneRigidBody(“Thigh.R”).setFriction(1.0f);

walkChar_phy.getBoneRigidBody(“Shin.L”).setFriction(1.0f);

walkChar_phy.getBoneRigidBody(“Shin.R”).setFriction(1.0f);

walkChar_phy.getBoneRigidBody(“Foot.L”).setFriction(1.0f);

walkChar_phy.getBoneRigidBody(“Foot.R”).setFriction(1.0f);





shoulderL.setAngularSleepingThreshold(0);

shoulderR.setAngularSleepingThreshold(0);

walkChar_phy.getBoneRigidBody(“Thigh.R”).setAngularSleepingThreshold(0);

walkChar_phy.getBoneRigidBody(“Thigh.L”).setAngularSleepingThreshold(0);

walkChar_phy.getBoneRigidBody(“Shin.R”).setAngularSleepingThreshold(0);

walkChar_phy.getBoneRigidBody(“Shin.L”).setAngularSleepingThreshold(0);

walkChar_phy.getBoneRigidBody(“Foot.R”).setAngularSleepingThreshold(0);

walkChar_phy.getBoneRigidBody(“Foot.L”).setAngularSleepingThreshold(0);









//Don’t know if this is needed, yet.



head.setAngularDamping(1.0f);

tail.setAngularDamping(1.0f);

shoulderL.setAngularDamping(1.0f);

shoulderR.setAngularDamping(1.0f);

walkChar_phy.getBoneRigidBody(“Thigh.L”).setAngularDamping(1.0f);

walkChar_phy.getBoneRigidBody(“Thigh.R”).setAngularDamping(1.0f);

walkChar_phy.getBoneRigidBody(“Shin.L”).setAngularDamping(1.0f);

walkChar_phy.getBoneRigidBody(“Shin.R”).setAngularDamping(1.0f);

walkChar_phy.getBoneRigidBody(“Foot.L”).setAngularDamping(1.0f);

walkChar_phy.getBoneRigidBody(“Foot.R”).setAngularDamping(1.0f);



}





@Override

public void simpleUpdate(float tpf) {



walkChar.updateModelBound();



Bone thighLBone = control.getSkeleton().getBone(“Thigh.L”);

Bone thighRBone = control.getSkeleton().getBone(“Thigh.R”);

Bone shinLBone = control.getSkeleton().getBone(“Shin.L”);

Bone shinRBone = control.getSkeleton().getBone(“Shin.R”);

Bone footLBone = control.getSkeleton().getBone(“Foot.L”);

Bone footRBone = control.getSkeleton().getBone(“Foot.R”);







RotationData lastPose = receiver.getSetPose(null);

if (lastPose == null) return;











//Refactor angle values.



float elbowLeftPitch = (lastPose.elbowLeftRoll > FastMath.HALF_PI) ? -lastPose.elbowLeftPitch + FastMath.PI : lastPose.elbowLeftPitch;

elbowLeftPitch = -1;





float elbowRightPitch = (lastPose.elbowRightRoll > FastMath.HALF_PI) ? -lastPose.elbowRightPitch + FastMath.PI : lastPose.elbowRightPitch;

elbowRightPitch = -1;





float shoulderLeftPitch = (lastPose.shoulderLeftYaw < 0) ? lastPose.shoulderLeftYaw + FastMath.TWO_PI : lastPose.shoulderLeftYaw;

shoulderLeftPitch -= 135.0f
FastMath.DEG_TO_RAD;



float shoulderLeftYaw = lastPose.shoulderLeftPitch;

float shoulderLeftRoll = lastPose.shoulderLeftRoll;











//Find current bone rotations.



rightShoulderPos = thighR.getBodyB().getPhysicsRotation().inverse().mult(thighR.getBodyA().getPhysicsRotation());

leftShoulderPos = thighL.getBodyB().getPhysicsRotation().inverse().mult(thighL.getBodyA().getPhysicsRotation());

rightElbowPos = shinR.getBodyB().getPhysicsRotation().inverse().mult(shinR.getBodyA().getPhysicsRotation());

leftElbowPos = shinL.getBodyB().getPhysicsRotation().inverse().mult(shinL.getBodyA().getPhysicsRotation());





//Save current error values into the buffer.

rightShoulderBuf[0] = rightShoulderErr[0];

rightShoulderBuf[1] = rightShoulderErr[1];

rightShoulderBuf[2] = rightShoulderErr[2];

leftShoulderBuf[0] = leftShoulderErr[0];

leftShoulderBuf[1] = leftShoulderErr[1];

leftShoulderBuf[2] = leftShoulderErr[2];

rightElbowBuf[0] = rightElbowErr[0];

leftElbowBuf[0] = leftElbowErr[0];





float[] rightElbowAngles = rightElbowPos.toAngles(null);

float[] leftElbowAngles = leftElbowPos.toAngles(null);

float[] leftShoulderAngles = leftShoulderPos.toAngles(null);

float[] rightShoulderAngles = rightShoulderPos.toAngles(null);





leftShoulderErr[0] = shoulderLeftPitch - leftShoulderAngles[0];

leftShoulderErr[1] = shoulderLeftYaw - leftShoulderAngles[1];

leftShoulderErr[2] = shoulderLeftRoll - leftShoulderAngles[2];



//rightShoulderErr = shoulderRightRot.inverse().mult(rightShoulderPos).toAngles(rightShoulderErr);

leftElbowErr[0] = elbowLeftPitch - leftElbowAngles[0];

rightElbowErr[0] = elbowRightPitch - rightElbowAngles[0];





//Update integrals

shoulderRightPitchIntegral += rightShoulderErr[0] * tpf;

shoulderRightYawIntegral += rightShoulderErr[1] * tpf;

shoulderRightRollIntegral += rightShoulderErr[2] * tpf;

shoulderLeftPitchIntegral += leftShoulderErr[0] * tpf;

shoulderLeftYawIntegral += leftShoulderErr[1] * tpf;

shoulderLeftRollIntegral += leftShoulderErr[2] * tpf;

elbowRightPitchIntegral += rightElbowErr[0] * tpf;

elbowLeftPitchIntegral += leftElbowErr[0] * tpf;



//Limit integral values.

shoulderRightPitchIntegral = Math.max(-integralSaturation, Math.min(integralSaturation, shoulderRightPitchIntegral));

shoulderRightYawIntegral = Math.max(-integralSaturation, Math.min(integralSaturation, shoulderRightYawIntegral));

shoulderRightRollIntegral = Math.max(-integralSaturation, Math.min(integralSaturation, shoulderRightRollIntegral));

shoulderLeftPitchIntegral = Math.max(-integralSaturation, Math.min(integralSaturation, shoulderLeftPitchIntegral));

shoulderLeftYawIntegral = Math.max(-integralSaturation, Math.min(integralSaturation, shoulderLeftYawIntegral));

shoulderLeftRollIntegral = Math.max(-integralSaturation, Math.min(integralSaturation, shoulderLeftRollIntegral));

elbowRightPitchIntegral = Math.max(-integralSaturation, Math.min(integralSaturation, elbowRightPitchIntegral));

elbowLeftPitchIntegral = Math.max(-integralSaturation, Math.min(integralSaturation, elbowLeftPitchIntegral));



//Update derivatives.

float rightShoulderPitchDer = rightShoulderErr[0] - rightShoulderBuf[0];

float rightShoulderYawDer = rightShoulderErr[1] - rightShoulderBuf[1];

float rightShoulderRollDer = rightShoulderErr[2] - rightShoulderBuf[2];

float leftShoulderPitchDer = leftShoulderErr[0] - leftShoulderBuf[0];

float leftShoulderYawDer = leftShoulderErr[1] - leftShoulderBuf[1];

float leftShoulderRollDer = leftShoulderErr[2] - leftShoulderBuf[2];

float rightElbowPitchDer = rightElbowErr[0] - rightElbowBuf[0];

float leftElbowPitchDer = leftElbowErr[0] - leftElbowBuf[0];



//Calculate PID values.

float rightShoulderPitchVel = Kp
rightShoulderErr[0] + KishoulderRightPitchIntegral + KdrightShoulderPitchDer;

float rightShoulderYawVel = KprightShoulderErr[1] + KishoulderRightYawIntegral + KdrightShoulderYawDer;

float rightShoulderRollVel = Kp
rightShoulderErr[2] + KishoulderRightRollIntegral + KdrightShoulderRollDer;

float leftShoulderPitchVel = KpleftShoulderErr[0] + KishoulderLeftPitchIntegral + KdleftShoulderPitchDer;

float leftShoulderYawVel = Kp
leftShoulderErr[1] + KishoulderLeftYawIntegral + KdleftShoulderYawDer;

float leftShoulderRollVel = KpleftShoulderErr[2] + KishoulderLeftRollIntegral + KdleftShoulderRollDer;

float rightElbowPitchVel = Kp
rightElbowErr[0] + KielbowRightPitchIntegral + KdrightElbowPitchDer;

float leftElbowPitchVel = KpleftElbowErr[0] + KielbowLeftPitchIntegral + KdleftElbowPitchDer;



//Apply to bones.

/


thighR.getRotationalLimitMotor(0).setTargetVelocity(rightShoulderPitchVel);

thighR.getRotationalLimitMotor(1).setTargetVelocity(rightShoulderYawVel);

thighR.getRotationalLimitMotor(2).setTargetVelocity(rightShoulderRollVel);



thighL.getRotationalLimitMotor(0).setTargetVelocity(leftShoulderPitchVel);

thighL.getRotationalLimitMotor(1).setTargetVelocity(leftShoulderYawVel);

thighL.getRotationalLimitMotor(2).setTargetVelocity(leftShoulderRollVel);

*/



shinR.getRotationalLimitMotor(0).setTargetVelocity(rightElbowPitchVel);

shinL.getRotationalLimitMotor(0).setTargetVelocity(leftElbowPitchVel);



thighL.getRotationalLimitMotor(0).setTargetVelocity(leftShoulderPitchVel);

//thighL.getRotationalLimitMotor(1).setTargetVelocity(leftShoulderYawVel);

//thighL.getRotationalLimitMotor(2).setTargetVelocity(leftShoulderRollVel);





System.err.println(shoulderLeftRoll + “,” + leftShoulderAngles[2]);



}

[/java]

Regarding #1, perhaps you need to disable joints from colliding with each other? I don’t see any other way of solving that issue …

Nice to hear from your project again.

#1: PhysicsJoint.setCollisionBetweenLinkedBodys is called in for every body in KinematicRagdollController. So the connected bodies should not collide. Maybe it is not called in your setup for some reason.

#2: These presets set limits for the joints. With JointPreset(0,0,0,0,0,0) the joint will be locked and cannot rotate. Do you want to achieve this? The connected bodies will still be mobile but the joint between them will not be able to turn. I guess if you want immobile parts you need to set the bodies to kinematic. Not sure if I really understand what you mean.

#3: I am quite certain now that the motors of SixDofJoint are the wrong way to create articulated systems that can move freely. There is a limit inside the yaw motor because of the gimbal lock issue which is documented here. I am trying to use PD-controlled torques/forces directly on the rigid bodies to create an articulated structure. Seems to be more promising. But I still have problems using these together with constraints / joints. I have to figure out which forces I have to use exactly to rotate the bodies connected with constraints, without violating the limits which will result in staggering motions