IntegratorRot3D

  8 minute read  

IntegratorRot3D integrates an angular velocity into an orientation. It is the rotational counterpart of Integrator: where that block accumulates a number, this one composes rotations, which cannot be done by adding three angles together.

The orientation is held as a quaternion and re-normalised every cycle, so it stays a valid rotation however long the controller runs — no gimbal lock and no drift out of the valid set.

flowchart LR
    i1(["input — angular velocity, three values"]) --> B["IntegratorRot3D"]
    i2(["reset"]) --> B
    i3(["manipulabilityGain"]) --> B
    B --> o1(["output — orientation quaternion, four values"])
    B --> o2(["reference — the reset target"])
    B --> o3(["pqrLocal — applied rate, body frame"])
    B --> o4(["pqrGlobal — applied rate, world frame"])
    B --> o5(["absVelOut — rate magnitude"])
    B --> o6(["absVelDotOut — its rate of change"])
    B --> o7(["absVelLimiterActive"])
    B --> o8(["absAccLimiterActive"])

The limits act on the magnitude of the angular rate, not on each axis. When a limit bites, all three components are scaled by the same factor, so the rotation slows without changing its axis. absVelMax bounds the rate’s magnitude and absAccMax how fast that magnitude may change.

manipulabilityGain does two different things depending on absVelLimiterOn:

• On — it scales absVelMax, always, in both directions.
• Off — it scales the rate itself, but only while the gain is falling. Motion out of the region that caused the throttle is never slowed.

The acceleration limit is deliberately relaxed while slowing toward a singularity. When the throttle demands a lower rate, the block allows whatever deceleration that needs — the limit only constrains speeding up.

reference is an output, not an input. The reset target can only be set from an application, not from the parameter tree — see Limits and errors.

Signals

Inputs

Path	Unit	Range	Description
input	rad/s	unbounded	The angular velocity, three values. inputAsLocal decides whether these are in the body frame or the world frame — getting that wrong produces plausible motion about the wrong axis.
reset	-	-	Sets output back to reference and zeroes the input.
manipulabilityGain	-	0 – 1 in practice	How much rotation is allowed, typically from a measure of how close the machine is to a singularity. Negative values are used as their magnitude.

Outputs

Path	Unit	Description
output	-	The orientation, as a four-value quaternion. Always normalised. Not cleared when the controller starts — it resumes from the previous run’s attitude.
reference	-	The quaternion a reset restores. Readable only — it cannot be written from the parameter tree.
pqrLocal	rad/s	The rate actually applied, expressed in the body frame.
pqrGlobal	rad/s	The same rate in the world frame. Compare the two to confirm the frame switch is doing what you expect.
absVelOut	rad/s	The magnitude of the applied rate.
absVelDotOut	rad/s²	How fast that magnitude is changing.
absVelLimiterActive	-	True while the rate magnitude is being limited.
absAccLimiterActive	-	True while its rate of change is being limited.

Parameters

Path	Unit	Default	Range	Effect
inputAsLocal	-	false	-	True means input is in the body frame and the block rotates it into the world frame. False means it is already in the world frame.
absVelLimiterOn	-	false	-	Bounds the rate magnitude at absVelMax × manipulabilityGain. Switching it off is what enables the throttle instead.
absVelMax	rad/s	—	above 0	Maximum rate magnitude. Not checked.
absAccLimiterOn	-	false	-	Bounds how fast the rate magnitude may change.
absAccMax	rad/s²	—	above 0	That bound. Not checked, and a negative value is unsafe — see Limits and errors.

All five are persistent and survive a restart.

One sub-tree exists below this block, rateLimiter3D, which performs the acceleration limit. Do not configure it — this block overwrites its settings every cycle. Use absAccMax and absAccLimiterOn instead.

Setup

1. Decide which frame your angular velocity is in and set inputAsLocal to match. A body-frame rate fed as a world-frame one rotates about the wrong axis, and the result looks like plausible motion.

2. Set reference from your application to the orientation a reset should restore. It cannot be set from the parameter tree.

3. Pulse reset and confirm output matches reference.

4. Command a steady rate about one axis with both limiters off, and confirm the orientation rotates about that axis at the expected rate.

5. Compare pqrLocal against pqrGlobal. With the machine at its reference orientation they will agree; as it rotates they diverge, and that divergence confirms the frame handling.

   Step 5 is the check that the frames are right. If the two never diverge as the machine rotates, the frame rotation is not happening and inputAsLocal is probably wrong.

6. Set absVelMax and switch absVelLimiterOn on. Command a rate above the limit and confirm absVelLimiterActive goes true and absVelOut settles at the limit.

7. Only if you need the singularity throttle: switch absVelLimiterOn off and link manipulabilityGain.

Tuning

1. Set absVelMax from the machine’s rated rotational speed — it is a magnitude, so it applies equally to a rotation about one axis and a combined one.
2. Set absAccMax from what the structure can take. It bounds how quickly the rotation speeds up; slowing down toward a singularity is deliberately exempt.
3. Choose per installation whether you want the limiter or the throttle. They are alternatives selected by absVelLimiterOn, exactly as in Integrator.
4. Use the throttle when a supervisor computes a closeness measure and rotation should fade out as it falls. Rotation away from the trouble is never slowed, which is the point.
5. Watch absVelOut and absVelDotOut while tuning. They are the two quantities the limits act on.
6. Nothing here needs re-checking after a task-rate change.

Read the asymmetry off the gap between the two curves during the rise.

Symptom	Cause	Action
The machine rotates about the wrong axis	inputAsLocal does not match the frame your rate is in	Switch it and re-check step 5 of Setup
pqrLocal and pqrGlobal never diverge	The frame rotation is not being applied	Check inputAsLocal
The orientation resumed from an old attitude after a restart	Expected: it is not cleared at start	Pulse reset after every start
I cannot set the reset target	Expected: reference is an output and is settable only from an application	Ask the application to set it
Rotation is slower on the way in than on the way out for the same gain	Expected: the throttle applies only while the gain is falling	This is what lets the machine rotate back out
The throttle does nothing	absVelLimiterOn is true, so the gain scales absVelMax instead	Switch the limiter off to get the throttle
The acceleration limit seems to be ignored while slowing down	Expected: it is relaxed while braking toward a singularity, so the machine may always reach the throttled rate	This is deliberate
One axis is limited and the others are not	Not possible — the limits act on the magnitude, so all three scale together	Check what consumes the output
The rotation axis changed when the limit bit	Not possible in this block	Check what follows it
Configuring the rateLimiter3D sub-tree had no effect	Expected: this block overwrites its settings every cycle	Use absAccMax and absAccLimiterOn
The rotation ran away or reversed	A negative absAccMax reaches the internal rate limiter, which then drives away from its target	Set it positive, then pulse reset
The orientation went non-numeric and stayed there	A non-numeric rate poisoned the quaternion	Pulse reset with a finite reference
The quaternion drifted out of normalisation	Not possible — it is re-normalised every cycle	Check what consumes it

A starting point for a wrist limited to 1 rad/s, with the throttle available:

inputAsLocal    = true
absVelLimiterOn = false
absVelMax       = 1.0
absAccLimiterOn = true
absAccMax       = 10.0

Limits and errors

Limit	Set by	What happens	Reported
Rate magnitude	absVelMax × manipulabilityGain, while absVelLimiterOn is set	Scaled down uniformly, preserving the axis	absVelLimiterActive, absVelOut
Rate magnitude, limiter off	manipulabilityGain, only while it is falling	Scaled proportionally. Never scaled while the gain rises	Not reported — the latch is not published
Rate of change of the magnitude	absAccMax, while absAccLimiterOn is set	Bounded — except while slowing toward a singularity, where whatever deceleration the throttle demands is allowed	absAccLimiterActive, absVelDotOut
absVelMax, absAccMax	Nothing	Not checked. A negative absAccMax reaches the internal rate limiter, which then drives the rate away from its target	Not reported
Individual axes	Nothing	Not bounded. Only the magnitude is limited	Not reported
Orientation validity	Guaranteed	The quaternion is re-normalised every cycle and stays a valid rotation indefinitely	Not applicable
reference	Not writable	It is registered as an output, so the reset target is settable only from an application	Not applicable
Orientation state	reset	Restored from reference. Not cleared by a controller stop or start	Not reported
rateLimiter3D sub-tree	Overwritten	Its enable and rateLimit are written every cycle by this block	Not reported
Non-numeric input	Nothing	Poisons the quaternion and latches. reset clears it if reference is finite	Not reported
Dimensions	Fixed	Always three rate inputs and one quaternion output	Not reported

The block raises no errors or warnings and logs nothing. Every failure above shows as a value on a trace, not as a message.

---

Verified against motorcortex-control3 3.30.0 (bc348fd).