Simple Pendulum: Feedforward Compensation - ihmcrobotics/ihmc-open-robotics-software-tutorials GitHub Wiki

Introduction

In the last example, we proposed a control method with a feedback PID controller that properly stabilized the pendulum at an almost horizontal position. While this feedback controller has a very low amount of steady state error (1 x 10^-6), fast responses haven't been obtained because effective control is achieved by the integral feedback that accompanies a time delay necessary for error accumulation. Notice how when running the simulation with the controller, the pendulum doesn't reach the desired position fully. (Click Simulate, then pull the black line back to the start and press Play. See how the pendulum moves in real-time).

pendulumPIDControlled

To improve this response, we can use a concept called feedforward compensation.

Feedforward Compensation

Feedforward is a predictive action based on the model we want to follow. Feedback if a reactive action that uses sensors to create a response based on the error from desired action calculated. In general, a properly designed feedforward/feedback control system will always improve performance over a simple feedback control system. For more information on feedforward control, check out this link.

The Challenge

We challenge you to figure out what to do to cause the steady-state error to become virtually 0 (the steady-state error is the difference between desired and actual value of a variable we try to control as the time goes to infinity). Before you start experimenting, set the integral gain to 0 so you aren't relying on that feedback. Then, use the information we've given you so far to edit the torque equation. If you would like some hints, check out these links.

Gravity Compensation in Robotics: Abstract giving brief overview of gravity compensation in robotics
The Simple Pendulum: Article detailing the forces acting on a pendulum
The Motion of a Pendulum: Article giving another explanation of the torque acting on a pendulum
Moment Arm and Torque: Article introducing moment arm and torque
Basic Introduction to Torque, Lever Arm, and Moment of Force: Video giving an introduction to torque, lever arm and moment of force.

Solution: Gravity Compensation

The actuator power required to resist joint torque caused by the weight of robot links can be a significant problem. Gravity compensation is a well-known technique in robot design to achieve equilibrium throughout the range of motion and as a result to reduce the loads on the actuator. (Source: Gravity Compensation in Robotics) The gravity terms, so in this case the gravitational force pulling the pendulum towards its natural equilibrium position, tend to cause static positioning errors, so some robot manufacturers include a gravity model in the control law. The complete control law takes the form: torque = PID control law + gravity model.

gravity model

For a simple pendulum, the torque from the force of gravity is:

torque on a pendulum

If we add it to our equation for torque in doControl() in simplePendulumController.java, we have that

torque = p_gain.getDoubleValue() * positionError + i_gain.getDoubleValue() * integralError + d_gain.getDoubleValue() * (0 - fulcrumJoint.getQd())
            + fulcrumJoint.getSuccessor().getInertia().getMass() * gravityZ * centerOfMassPosition.norm() * Math.sin(desiredPositionRadians.getDoubleValue());

To get the centerOfMassPosition and gravityZ you can use

double gravityZ = 9.81;
FramePoint3D centerOfMassPosition = new FramePoint3D();
fulcrumJoint.getSuccessor().getCenterOfMass(centerOfMassPosition);
centerOfMassPosition.changeFrame(fulcrumJoint.getFrameAfterJoint());

Now, when running the simulation, the pendulum should appear ever so slightly closer to the horizontal position and hesitate much less. It should have a much lower steady-state error of about -4 x 10^-14.

pendulumffwrd

If you still haven't had enough, move on to Falling Brick.