Hi,
I am trying to implement proportional-derivative control for physics-based animation. I started with a fixed node that is connected to a dynamic node by a SixDofJoint. My aim is to let the dynamic node follow a rotation using its RotationalLimitMotors. It works when I use the pitch or roll axis only. But if I combine the rotations of several axes the dynamic node will run out of control. The behavior is the same in jbullet and native bullet.
This is not really an engine specific problem, I guess. But I would be glad if someone could helping me solve this problem. Here is the code:
[java]
package com.jme3test.motor;
import static com.jme3.math.FastMath.*;
import java.util.Arrays;
import java.util.logging.Logger;
import jme3test.bullet.PhysicsTestHelper;
import com.jme3.app.SimpleApplication;
import com.jme3.bullet.BulletAppState;
import com.jme3.bullet.collision.shapes.BoxCollisionShape;
import com.jme3.bullet.control.RigidBodyControl;
import com.jme3.bullet.joints.SixDofJoint;
import com.jme3.bullet.joints.motors.RotationalLimitMotor;
import com.jme3.input.KeyInput;
import com.jme3.input.controls.ActionListener;
import com.jme3.input.controls.KeyTrigger;
import com.jme3.material.Material;
import com.jme3.math.ColorRGBA;
import com.jme3.math.Quaternion;
import com.jme3.math.Vector3f;
import com.jme3.scene.Geometry;
import com.jme3.scene.Node;
import com.jme3.scene.debug.Arrow;
/**
- Proportional-derivative control using a weight attached to a joint.<p>
- I/K: desired pitch velocity<br>
- U/O: desired yaw velocity<br>
- J/L: desired roll velocity<br>
- R: reset desired orientation<br>
- SPACE: enable/disable motor control<p>
- Pink arrow: desired orientation<br>
- Yellow arrow: distance between desired and current orientation<br>
- Green arrow: joint<br>
- Blue box: weight node<p>
-
@author cmeyer
/
public class TestPdControl extends SimpleApplication implements ActionListener {
private static final Logger logger =
Logger.getLogger(TestPdControl.class.getName());
protected BulletAppState bulletAppState;
protected Geometry desiredArrow;
protected Geometry distanceArrow;
// current velocity for every axis
protected float desiredPitchVelocity = 0;
protected float desiredYawVelocity = 0;
protected float desiredRollVelocity = 0;
protected boolean motorEnabled = true;
/ Angles during last frame. Used for calculating velocity */
protected final float[] lastFrameAngles = {0, 0, 0};
protected SixDofJoint joint;
protected RotationalLimitMotor pitchMotor;
protected RotationalLimitMotor yawMotor;
protected RotationalLimitMotor rollMotor;
// PD controller gain values
protected final float proportionalGain = 1;
protected final float derivativeGain = 1;
/**
- Initialization
*/
@Override
public void simpleInitApp() {
// create the bullet state for physics simulation
bulletAppState = new BulletAppState();
stateManager.attach(bulletAppState);
bulletAppState.getPhysicsSpace().enableDebug(assetManager);
setupKeys();
setupJoint();
setupArrows();
}
/**
- Key bindings
*/
private void setupKeys() {
inputManager.addMapping("PitchMinus", new KeyTrigger(KeyInput.KEY_K));
inputManager.addMapping("PitchPlus", new KeyTrigger(KeyInput.KEY_I));
inputManager.addMapping("YawMinus", new KeyTrigger(KeyInput.KEY_U));
inputManager.addMapping("YawPlus", new KeyTrigger(KeyInput.KEY_O));
inputManager.addMapping("RollMinus", new KeyTrigger(KeyInput.KEY_J));
inputManager.addMapping("RollPlus", new KeyTrigger(KeyInput.KEY_L));
inputManager.addMapping("Reset", new KeyTrigger(KeyInput.KEY_R));
inputManager.addMapping("toggleEnableMotors", new KeyTrigger(
KeyInput.KEY_SPACE));
inputManager.addListener(this, "RollMinus", "RollPlus", "PitchMinus", "PitchPlus", "YawMinus", "YawPlus", "Reset", "toggleEnableMotors");
}
/**
- Create the joint and weight node.
*/
private void setupJoint() {
// Static nodes have zero mass
Node holderNode =
PhysicsTestHelper.createPhysicsTestNode(assetManager,
new BoxCollisionShape(new Vector3f(.1f, .1f, .1f)), 0);
holderNode.getControl(RigidBodyControl.class).setPhysicsLocation(
Vector3f.ZERO);
rootNode.attachChild(holderNode);
bulletAppState.getPhysicsSpace().add(holderNode);
// dynamic node with mass of 1
Node weightNode =
PhysicsTestHelper.createPhysicsTestNode(assetManager,
new BoxCollisionShape(new Vector3f(.3f, .3f, .3f)), 1);
weightNode.getControl(RigidBodyControl.class).setPhysicsLocation(
new Vector3f(0f, 1, 0f));
rootNode.attachChild(weightNode);
bulletAppState.getPhysicsSpace().add(weightNode);
// Joint with six degrees of freedom, we will only use 3 of them
joint =
new SixDofJoint(holderNode.getControl(RigidBodyControl.class),
weightNode.getControl(RigidBodyControl.class),
Vector3f.ZERO, new Vector3f(0f, -1, 0f), true);
bulletAppState.getPhysicsSpace().add(joint);
// fetch the reference for the rotational motors and enable them
pitchMotor = joint.getRotationalLimitMotor(0);
yawMotor = joint.getRotationalLimitMotor(1);
rollMotor = joint.getRotationalLimitMotor(2);
setEnableMotors(motorEnabled);
// Default maxMotorForce is too weak to lift the weight node
pitchMotor.setMaxMotorForce(1);
yawMotor.setMaxMotorForce(1);
rollMotor.setMaxMotorForce(1);
// physics simulation puts objects to sleep when moving slower than
// sleeping threshold. We do not want that.
weightNode.getControl(RigidBodyControl.class).setAngularSleepingThreshold(0);
}
/**
- Create the arrow pointing to the desired orientation and another one for
- showing the distance between desired and current orientation.
*/
private void setupArrows() {
Arrow desiredArrowMesh = new Arrow(Vector3f.UNIT_Y);
desiredArrow = new Geometry("desired arrow", desiredArrowMesh);
Material pingMat = new Material(assetManager, "Common/MatDefs/Misc/Unshaded.j3md");
pingMat.setColor("Color", ColorRGBA.Pink);
desiredArrow.setMaterial(pingMat);
rootNode.attachChild(desiredArrow);
Arrow distanceArrowMesh = new Arrow(Vector3f.UNIT_Y);
distanceArrow = new Geometry("distance arrow", distanceArrowMesh);
Material yellowMat = new Material(assetManager, "Common/MatDefs/Misc/Unshaded.j3md");
yellowMat.setColor("Color", ColorRGBA.Yellow);
distanceArrow.setMaterial(yellowMat);
rootNode.attachChild(distanceArrow);
}
@Override
public void onAction(String name, boolean isPressed, float tpf) {
if (name.equals("PitchPlus") && isPressed) {
desiredPitchVelocity += 1;
}
if (name.equals("PitchMinus") && isPressed) {
desiredPitchVelocity -= 1;
}
if (name.equals("YawPlus") && isPressed) {
desiredYawVelocity += 1;
}
if (name.equals("YawMinus") && isPressed) {
desiredYawVelocity -= 1;
}
if (name.equals("RollPlus") && isPressed) {
desiredRollVelocity += 1;
}
if (name.equals("RollMinus") && isPressed) {
desiredRollVelocity -= 1;
}
if (name.equals("Reset") && isPressed) {
desiredPitchVelocity = 0;
desiredYawVelocity = 0;
desiredRollVelocity = 0;
desiredArrow.setLocalRotation(new Quaternion());
}
if (name.equals("toggleEnableMotors") && isPressed ) {
setEnableMotors(!motorEnabled);
logger.info("motorEnabled = " + motorEnabled);
}
}
/**
- Enable/disable all 3 motors.
-
@param enableMotors
*/
private void setEnableMotors(boolean enableMotors) {
motorEnabled = enableMotors;
pitchMotor.setEnableMotor(enableMotors);
yawMotor.setEnableMotor(enableMotors);
rollMotor.setEnableMotor(enableMotors);
}
/**
- The game loop. This method gets called every frame.
*/
@Override
public void simpleUpdate(float tpf) {
// calculate rotation step for this frame from velocities
Quaternion rotationStep = new Quaternion().fromAngleNormalAxis(-desiredPitchVelocity * tpf, Vector3f.UNIT_X);
rotationStep.multLocal(new Quaternion().fromAngleNormalAxis(-desiredYawVelocity * tpf, Vector3f.UNIT_Y));
rotationStep.multLocal(new Quaternion().fromAngleNormalAxis(-desiredRollVelocity * tpf, Vector3f.UNIT_Z));
desiredArrow.rotate(rotationStep);
// get current orientation from weight node
Quaternion currentRotation = joint.getBodyB().getPhysicsRotation();
float[] currentAngles = currentRotation.toAngles(null);
// get desired angles from desired angle’s arrow rotation
Quaternion desiredRotation = desiredArrow.getLocalRotation();
float[] desiredAngles = desiredRotation.toAngles(null);
Quaternion distanceRotation = desiredRotation.inverse().mult(currentRotation);
distanceArrow.setLocalRotation(distanceRotation);
float[] distance = distanceRotation.toAngles(null);
// set target velocity for every rotational motors based on pd control
pitchMotor.setTargetVelocity(calcMotorTargetVelocity(0, currentAngles[0], distance[0], desiredPitchVelocity));
yawMotor.setTargetVelocity(calcMotorTargetVelocity(1,currentAngles[1], distance[1], desiredYawVelocity));
rollMotor.setTargetVelocity(calcMotorTargetVelocity(2,currentAngles[2], distance[2], desiredRollVelocity));
logger.info("ndes: " + Arrays.toString(desiredAngles) + "ncur: " + Arrays.toString(currentAngles) + "ndis: " + Arrays.toString(distance) + " sum: " + (abs(distance[0]) + abs(distance[1]) + abs(distance[2])));
}
/**
- Calculate target velocity with pd control formula.
-
@param index
-
@param currentAngle
-
@param distance
-
@param desiredVelocity
-
@return target velocity for rotational motor
*/
protected float calcMotorTargetVelocity(int index, float currentAngle, float distance, float desiredVelocity) {
float angularVelocity = angleDistance(currentAngle, lastFrameAngles[index]);
lastFrameAngles[index] = currentAngle;
return proportionalGain * (distance) + derivativeGain * (desiredVelocity - angularVelocity);
}
/**
- Calculate distance between two angles taking the shortest way. Return
- value does not exceed PI and -PI.
-
@param target
-
@param source
-
@return distance angle
*/
protected float angleDistance(float target, float source) {
float result = target - source;
if (result > PI) {
return result - TWO_PI;
}
else if (result < -PI) {
return result + TWO_PI;
}
else {
return result;
}
}
public static void main(String[] args) {
TestPdControl app = new TestPdControl();
app.start();
}
}
[/java]
