Weight Negation (Gravity Compensation) Tutorial

Connor · July 30, 2020, 3:25am

Hello everyone!
As I am working on my robot, I have to think about how to program my robot in such a manner where the robot is almost capable to hold its own position without the need of additional elements. Basically speaking, PID is built in such a manner where it doesn’t take consideration of weight, but it is essential in going from point A to point B with the extra ability to react against external forces. So, what we need to do is create a simple convenient algorithm to negate weight to be able to have a PID algorithm work precise and conveniently.

First of all, we need to understand that in order to properly negate, we must be able to get the sensor value of an arm in degrees, not in potentiometer 12_bit or any other type of sensor value. Also, as an addition, we must consider the fact that the resting(zero) position must be facing upwards (or vertical).

30deg
Yellow = upAngle perpendicular to the floor (or up)
Blue = Arm at 30 degrees from perpendicular
Black = Angle reading after conversion

I don't know how to convert it

You should convert it using the equation below:

double upAngle = (sensorValue * x) - y;

Where x is the multiplier to turn the value into degrees, and y is the offset. If the sensor value is already in degrees then just do sensorValue - y. For example:

double upAngle = (potentiometerValue * 0.064) -12;

This would convert 12_bit potentiometer value into degrees, then the -12 would be the offset (in degrees).

Next, we are going to have to include the following to be able to do weight negation:

Make sure to add C++ math library for calculations on the top of the program below the other includes

#include <cmath>

Make sure to add a function to convert degrees to radians, as cmath utilizes trig functions with radians

//Used to convert degrees to radians
double toRad(double degrees){
return ( degrees * M_PI ) / 180 ;
}

After you include the function and the other includes, you can now properly apply the weight negation using the function below:

//Runs Weight-Negation
double weightNegation(double upAngle, double weight){
return (std::sin(toRad(upAngle)) * weight);
}

The upAngle is the angle, in degrees, the arm is from being vertical.

So, how do I apply this function properly?

Its simple!

/*----------------------------------------------------------------------------*/
/*                                                                            */
/*    Module:       main.cpp                                                  */
/*    Author:       C:\Users\Connor                                           */
/*    Created:      Wed Jul 29 2020                                           */
/*    Description:  V5 project                                                */
/*                                                                            */
/*----------------------------------------------------------------------------*/

// ---- START VEXCODE CONFIGURED DEVICES ----
// ---- END VEXCODE CONFIGURED DEVICES ----

#include "vex.h"
#include <cmath>

//Settings
double weightMultiplier = 0.0;
double kP = 0.0;
double kI = 0.0;
double kD = 0.0;

using namespace vex;

//Used to convert degrees to radians
double toRad(double degrees){
  return ( degrees * M_PI ) / 180 ;
}

double weightNegation(double upAngle, double weight){
  return (std::sin(toRad(upAngle)) * weight);
}

int main() {
  // Initializing Robot Configuration. DO NOT REMOVE!
  vexcodeInit();
  
  while(true){
    
    //Insert Pid algorithm, you already know how to do it
    double PIDPower = error * kP + totalError * kI + derivative * kD;

    double upAngle = (lePot.value(analogUnits::range12bit) * 0.064) -12;
    double negatePower = weightNegation(upAngle, weightMultiplier);

    leMotor.spin(forward, PIDPower + negatePower,voltageUnits::volt);

  
    vex::task::sleep(10);
  }
}

What’s PID? (Here isn’t meant to teach PID, which is why it is vaguely added into the code for context, but click this hyperlink to learn PID)

How should I tune the arm with Weight Negation and PID?

set kP, kI, kD, and weightMultiplier to 0.0
Increase weightMultiplier until the arm can hold its position at any angle, and also if you push the arm up and down one doesn’t require more force than the other (Basically, feeling weightless)
Note: If increasing the weightMultiplier makes the arm motor push down instead of up, change leMotor.spin(forward, PIDPower + negatePower,voltageUnits::volt); to leMotor.spin(forward, PIDPower - negatePower,voltageUnits::volt);
Tune kP, kI, and kD accordingly

mvas · July 30, 2020, 2:45am

I’m a bit confused by what you’re proposing. Are you saying that a standard PID controller on a lift won’t account for the load the lift bears? I think you should be a bit more clear about what the purpose of this algorithm is doing because PID can take into account the weight of a given system (granted the controller is tuned right).

From my understanding, you’re adding supplemental power to the PID lift controller output in proportion to the angle of the arm and the additional load on the arm. In which case, this is a pretty elegant solution to the problem of oscillation on a lift with variable load.

Connor · July 30, 2020, 2:52am

PID generally isn’t too good with handling weight by itself, because if the arm is below the set point the force would be less than the force if the arm is above the set point (if that’s making sense, because one has gravity adding to the PID while another has gravity pushing against the PID) If we had integral following the most common way of just setting integral to 0 after the arm passes the point, the arm would just plop down which would cause shock loads.

This is correct, If you’re not holding anything then you can have like a weightMultiplier of like 0.5, but once you add a cube you can have a sensor sense that a cube is added and change weightMultiplier to like 1.3. This, in turn, would result in the PID requiring very little extra tuning for the difference in weight, if any at all.

mvas · July 30, 2020, 3:00am

I’m curious what the difference in performance is, maybe you can post a video comparing the behavior of the lift with and without the weight negation. Otherwise, nice job explaining the concept

rjacoby442 · July 30, 2020, 3:00am

Couldn’t you multiply the previous integral term by something like 0.95 or 0.9 to prevent integral windup(this makes the effect as if older values fade away), and make kI higher to adapt to different weights? I’m not really an expert on PID, so I’m not really sure if that would work.

mvas · July 30, 2020, 3:07am

The thing is that the angle of the arm changes the force distribution components acting against the motor so tuning a controller without considering those two variables (weight and angle) independently will likely lead to oscillation.

From my understanding, the PID controller in this overall algorithm controls the lift without regard to the load (making it much easier to tune) and the weight negation supplements power output by factoring in the angle of the lift and weight of the load independently.

Separate question, how do you know what the load is on the lift? In autonomous you can pre-calculate these values but not in driver control.

rjacoby442 · July 30, 2020, 3:11am

Maybe you could measure motor current? Or maybe in driver control the driver switches it to holding something mode.

Connor · July 30, 2020, 3:13am

I guess I can make a video that visually shows the difference. When I tried it with weight negation, the difference was astounding to a point where I wondered why this wasn’t an aspect of PID yet. Low and behold, the proper term is called feed forward which is an addition to power but I realized feed forward was a mere constant and not a continuous changing variable based upon the angle. That’s why I called this “Weight Negation”.

if(cubeLimitSwitch.value())
weightMultiplier = 1.5;
else
weightMultiplier  = 0.7;

Maybe another sensor that detects an object intaked?

mvas · July 30, 2020, 3:16am

Yep, I was gonna say this is something like a feedforward controller (a motion profile is a good example of feedforward) but it’s not quite the same

tabor473 · July 30, 2020, 3:19am

This is totally a common thing in real systems. It is called Gravity Compensation. On simple glance I only see research papers explaining it, I will see if I can find some more approachable resources. But you are totally correct in everything you said. PID/PD only handles linear systems, up and down being equivalent is one of the the properties of linear systems.
PD just happens to be so robust it can convince you nothing more is necessary.

Drew2158U · July 30, 2020, 3:57am

upAngle can be replaced with the angle from the horizontal if you use std::cos instead of std::sin, if for whatever reason you have your pot set like that.

tabor473 · July 30, 2020, 6:54am

Here are some examples of PD vs PD + gravity compensation from a homework assignment I did a while ago. (It is a 2 link arm tracing a very slow circle. Fast circles look bad because there are other physics going on then just gravity)

2775Josh · July 30, 2020, 12:17pm

This is cool but what kind of homework do you have??

tabor473 · July 30, 2020, 6:52pm

Not totally sure what the question is. But that is the kind of homework you can expect from a “robot control” course. This style course would likely be graduate level, as mine was, but that is mostly because most schools don’t go into robot specific stuff until graduate school.

Here is a list of the courses required for a robotics graduate degree at the University of Utah if you are curious. All of which you can take as an undergrad if you wish. You could find similar courses at a dozen universities around the country.

http://robotics.coe.utah.edu/robotics-track/#tab-2

Connor · July 31, 2020, 8:36am

This video was some time ago before I added anti-backlash into the program, but you can get the gist about just how powerful this algorithm is. This is the gravity compensation without PID:

mvas · July 31, 2020, 5:05pm

Oh wow, the weight negation alone has that much control? I’m curious to see the behavior with a rubber banded lift because the rubber bands add much more dynamic behavior. Maybe it dampens it oscillation even more which is a good thing (or so I’d assume).

coachdoug80516 · August 2, 2020, 10:38pm

Griffin did a nice job of explaining ideas with gravity control issues. You know something about your system, and thus you don’t have to wait for feedback to start your control effort. You might have a model of your system and use that model to drive outputs before the desired position term in your PID would move it otherwise. This model could be math, or it could be taken from data.

Think about testing your arm in a number of positions, making a table of angle versus hold force, and then putting that force in as a “hold drive” outside of the PID control path. This prevents the PID system from having to wait for an error before reacting. It also means you don’t have an integrator that is already wound up when you want to start your next move.

In real world systems you might see acceleration or deceleration feed forward working with a PID, where your math model predicts how your robot would move. It might drive a “no slip” acceleration profile to drive the motors. The programmed acceleration knows the desired position at each time interval. This is fed as desired position to the PID. The PID then helps correct for errors when the predicted motion doesn’t match measured motion. In this sort of system, any work done by the PID is actually bad – your model didn’t match behavior. If you were perfect, the PID output would be zero all the time.

For your robot, this means your model doesn’t have to be perfect. A rough approximation of hold current by position will help the PID, and if you’re a bit wrong, you still have that loop to help you maintain desired position.