Problems using RotationalLimitMotor for PD-Control

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() {

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);

// 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);

// 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);

// 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]

Use direction vectors for your computations, not rotations. They will give you headaches with gimbal lock, euler angle translation and other things. Its way easier to just imagine direction vectors and rotating them. The math for dummies tutorial shows how that works.

Sounds convincing. I will try that

It seems to me that direction vectors are not helping me at this point. Problems start before doing these calculations. I am unable to synchronize the rotations of the joint and a spatial. These are things that I find weird or do not understand:

• If I rotate around the y-axis from the initial orientation on the joint will stop after a quarter revolution. Changing hi/lo limit doesn't have an effect on this.

• Rotating around Z by PI radians will invert the rotational direction around X if done after that. This is the correct behaviour. But if I do it the other way around (first X and then Z) the rotation around Z will not be inverted. Doing the same on a spatial would lead to different behaviour.
You can see it using the code. Arrow and joint are moving into opposite directions then. Just rotate using pitch (I/K) by PI radians so that the joint and arrow are pointing downwards. Stop the rotation and use J/L for roll rotation. Arrow and joint will rotate in opposite directions.

• Sometimes the joint continues to rotate after setting all target velocities to zero. I cannot say when this does happen exactly. But it seems to me there are some forces applied I do not know of. Gravity is set to (0,0,0). Maybe it has something to do with the limits. Probably a connection to the first problem.

Here is the code I used to learn about the joint rotations. My aim was to synchronize the rotation of joint and spatial (red arrow), which only works to some extent.

[java]
package com.jme3test.motor;

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;

/**
* Joint vs. spatial rotational behaviour.<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: spatial<br>
* Green arrow: joint<br>
* Blue box: weight node<p>
* @author cmeyer
*
*/
public class TestRotationalMotor extends SimpleApplication implements ActionListener {
private static final Logger logger =
Logger.getLogger(TestRotationalMotor.class.getName());

protected BulletAppState bulletAppState;
protected Geometry arrow;

// current velocity for every axis
protected float desiredPitchVelocity = 0;
protected float desiredYawVelocity = 0;
protected float desiredRollVelocity = 0;

protected boolean motorEnabled = true;

protected SixDofJoint joint;
protected RotationalLimitMotor pitchMotor;
protected RotationalLimitMotor yawMotor;
protected RotationalLimitMotor rollMotor;

/**
* 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() {
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);

// dynamic node with mass of 1
Node weightNode =
PhysicsTestHelper.createPhysicsTestNode(assetManager,
new BoxCollisionShape(new Vector3f(.3f, .3f, .3f)), 1);
RigidBodyControl weightNodeControl = weightNode.getControl(RigidBodyControl.class);
weightNodeControl.setPhysicsLocation(
new Vector3f(0f, 1, 0f));
rootNode.attachChild(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);

// 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.
weightNodeControl.setAngularSleepingThreshold(0);
weightNodeControl.setGravity(Vector3f.ZERO);
}

private void setupArrows() {
Arrow desiredArrowMesh = new Arrow(Vector3f.UNIT_Y);
arrow = new Geometry("arrow", desiredArrowMesh);

Material pinkMat = new Material(assetManager, "Common/MatDefs/Misc/Unshaded.j3md");
pinkMat.setColor("Color", ColorRGBA.Pink);
arrow.setMaterial(pinkMat);

rootNode.attachChild(arrow);
}

@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;
arrow.setLocalRotation(new Quaternion());

joint.getBodyB().clearForces();
joint.getBodyB().setPhysicsLocation(Vector3f.UNIT_Y);
joint.getBodyB().setPhysicsRotation(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);
}

@Override
public void simpleUpdate(float tpf) {
// calculate rotation step for this frame from velocities
Quaternion rotationStep = new Quaternion();
rotationStep.multLocal(new Quaternion().fromAngleNormalAxis(-desiredYawVelocity * tpf, Vector3f.UNIT_Y));
rotationStep.multLocal(new Quaternion().fromAngleNormalAxis(-desiredRollVelocity * tpf, Vector3f.UNIT_Z));
rotationStep.multLocal(new Quaternion().fromAngleNormalAxis(-desiredPitchVelocity * tpf, Vector3f.UNIT_X));
arrow.rotate(rotationStep);

// set motors to desired velocity
yawMotor.setTargetVelocity(desiredYawVelocity);
rollMotor.setTargetVelocity(desiredRollVelocity);
pitchMotor.setTargetVelocity(desiredPitchVelocity);
}

/**
* @param args
*/
public static void main(String[] args) {
TestRotationalMotor app = new TestRotationalMotor();
app.start();
}

}
[/java]

The joints also have limits, did you check/set these?

I checked them. They are set to 1.0 (LoLimit) and -1.0 (HiLimit) by default in native bullet. It seems to me that they are turned off with these values. On the X and Z axes there is clearly no limit, I can turn full revolutions. This changes if I set low limit to a negative value and high limit to a positive value smaller than PI. Then the joint stops after reaching the limit.

But it is different with the Y-axis. There it stops after a quarter revolution in both directions. I am only able to make it stop earlier by changing the limits.

Weird, there is clearly a limit on the Y-axis although the values are the same as the ones for the X and Z axes. My idea was that there is a limit set by the joint. But I looked into the bullet code and limit values set at the joint via setAngular*Limit will just be passed on to the motor.

The limit will lead to rotations that occur with all target velocities set to 0. If I yaw rotate until the limit is reached the movement will become uncontrolled until I yaw rotate the joint back away from the limit.

That joint rotation really bugs me. I do not understand how to write code that makes the joint rotate in the way I want, like I can do it with a spatial. Without that I am unable to create working pd-control for physics-based animation