Motion Magic with an Arm


Our team is using motion magic to control the position of an arm affected by gravity. I am fairly sure how to tune the PID aspects of it, but how should I calculate the feedforward gain (kF) for motion magic? Will the P, I, & D aspects of the loop automatically account for gravity in that the sum of the errors will build, or is an F value necessary for the arm to hold its position?


Closed Loop Control on a Rotating Arm

The cosine of the angle is proportional to the amount of force needed to counter gravity (0°→full force [cos0° = 1], 90°→no force [cos90° = 0]).
What we did was giving the arm some force and seeing to what angle it got. We did this a few times, calculated the cosine of each angle and found the proportion between the cosine and the output needed with the help of our friend, Desmos.



Here’s my answer on another thread.

1 Like


Thanks! As some others in that thread said, this was really clearly written and explained. Once we are able to calculate the percent output needed to stall at any angle using getFeedForward() , how do we go about using that for positional control? If our goal is to be able to declare a desired position (in encoder ticks), and then use the encoder as feedback for the PID loop, how would this calculated stall voltage at the desired position (in percent output) be incorporated into our use case?



You need to specify the amount of power when you’re getting the output of the Talon SRX. Using this method, specify your setpoint and the amount of power using “arbitrary feedforward”:
talon.set(ControlMode.MotionMagic, angle, DemandType.ArbitraryFeedForward, voltageNeededToHoldInPlace);



well, feed forward will help, but your PID can be set so it works without it, and then you can add the feedforward to help it. this is how we did it.

So, first we set feedforward as 0 and set the kP gain so that the arm moves to the position. Add to it until it gets there without oscillation. You can add kD to dampen it a bit, but we didn’t have to (we geared 300:1 with lots of friction in our reduction).

Then, you can add the feedforward to help overcome gravity.

The way I build the overall system was to have a variable called targetPosition and a method that runs the motion magic .set() method to go to the targetPosition. A loop runs to see if targetPosition has changed, if so, it recalls the method to set motion magic.

We have buttons that, when pressed, change the targetPosition to a different value, then the loop detects this and recalls motion magic. With this approach, the closed loop is always running and all is well.

Here’s our code if you’d like to take a look. We have a lift subsystem, which is the arm, and a wrist subsystem. both are using this targetPosition motion magic approach:



Just to be super clear, you aren’t finding the stall voltage to use with percent output, you are using a closed loop that runs in the TalonSRX. You’ll want to tune you pid so that when you tell the arm to run motion magic, it will be able to move to a targetPosition, and stay there!

here’s our code for calling motion magic:

double feedForward = getFeedForward();

motorMaster.set(ControlMode.MotionMagic, targetPosition, DemandType.ArbitraryFeedForward, feedForward);

We have this in a method that we call every time the target position changes. Then it sits there and waits for further instruction.



I took a quick look through your code and noticed the kF gain is set to zero. There’s a good chance it works well enough for what you are trying to do. But, this is not quite the textbook or intended usage of motion magic. See my post from a while ago:

You are skipping step 2 by setting the kF gain to zero. Now the closed loop has to do more work to keep the mechanism on the generated trajectory. An analogy (you are the feedback loop):

Your friend is riding a bike, and you are trying to stay beside them. Without kF, you are stuck running. You have to exert a lot of effort to keep up with your friend and prevent a gap from forming. With kF, you’ve now been given an electric bike, so most of the effort required is eliminated. You just have to modulate the throttle and brake ever so slightly as to stay right beside your friend.

In summary, by adding kF (veloctity-proportional feedforward), the output from the feedback term becomes very small compared to the total output. This means that the feedback gains can be much higher without inducing a lot of oscillation and unwanted behavior.


Closed Loop Control on a Rotating Arm

On our lift subsystem, we have feedforward set with arbitrary feedforward and kept kF at 0. I was under the impression that that was ok. I’ve seen so many conflicting accounts of how to calculate the kF gain, and a few posts saying you could keep kF at 0 if you set the arbitrary feed forward with:

motorMaster.set(ControlMode.MotionMagic, targetPosition, DemandType.ArbitraryFeedForward, feedForward);

It’s really difficult to find clear documentation or an explanation of the difference between kF and ArbitraryFeedForward. Would be great if you could explain.

In our Wrist subsystem, I believe we have Arbitrary feed forward set as 0 too for this commit as we weren’t finished tuning.



Yeah for sure, good question. The output from the controller to the motors with motion magic can be expressed roughly as:

output = kP*error+kD*d_error+kI*i_error+kF*goal_velocity+arbitraryFeedForward

The functional difference between kF and arbitraryFeedForward is that kF is multiplied by the goal velocity before it is added to the final output, while arbitraryFeedForward is added directly to the output. There are also a few implementation differences (i.e. arbitraryFeedForward is set every time a new command is sent to the motor, whereas kF is a gain you can set).

What’s the purpose of having both? First, it’s important to note that for DC motors, the output speed of the motor at steady-state is roughly proportional to the applied output from the speed controller. That is, 100% output gives you full speed, 50% output gives you half speed, etc.

For motion magic (and generally any sort of motion profiling in FRC), we want to take advantage of this fact. Motion magic generates a trajectory that contains a desired velocity over time, and based on the system (motor, gear ratio, etc) we know how to figure out what output produces that desired velocity. That is precisely what kF does – it is a gain that is multiplied by the goal velocity, resulting in a corresponding output to achieve that velocity. This alleviates most of the control effort required by the feedback terms (PID), as per the bicycle analogy in my earlier post.

For the Talon SRX, kF can be calculated by the following equation kF = 1023/v_max. kF maps v_max in the Talon SRX’s native speed units (ticks / 100ms), to full output in the native output units of [-1023, 1023].

In some cases, the system may have external forces acting on it (ie gravity). In this case, additional motor output is needed to account for this external force. This is what arbitraryFeedForward is used for. For example, in your wrist code, you are calculating feedForward = horizontalHoldOutput * cos(angle). For an elevator, it’s just a constant.

(The confusion may be in part because in some motion profiling cases (primarily drivetrains), instead of using kF*goal_velocity, it has been show to be more accurate to model the system with three feedforwards: kV*velocity+kA*acceleration+kS. To implement this feedforward scheme on the Talon SRX, you have to use arbitraryFeedForward, as there is no native support for it. Furthermore, this is not possible with motion magic, as all the trajectory setpoints are calculated internally on the controller, and there is no acceleration setpoint. If you are interested in drivetrain motion profiling, I would highly recommend reading the drivetrain characterization paper written by Noah Gleason and Eli Barnett from team 449.)

Hope this helps!


Closed Loop Control on a Rotating Arm

Such an excellent response. I appreciate the practicality of this and thank you for taking the time to explain without just throwing theory around. I really can’t wait until our robot isn’t in a giant plastic bag.

One thing that puzzles me is this, how to you get that v_max? In this case, if you were doing this for an arm, would you use calculations or do you run the system and try to capture the actual target v_max to shoot for?

If you do calculations, there’s so much that effects a system such as friction, gear reduction, motor performance, etc. Is this a “close enough” sort of thing?

If you run the system and capture actual values, how do you not kill everything around you?

Also, i’m guessing we aren’t using v_max as free speed, but the target maximum speed in ticks/100ms that you want the arm to travel at it’s fastest rate during the motion profile, correct? (this is another confusing issue that seems to never be clear in these posts).

(I’ve tried to read that whitepaper on multiple occasions and it goes deeper into theory that my mind is ready to accept. I’m trying to learn all of this so I can teach it to kids and volunteer mentors, I know that depth of theory is great, but it not approachable or useful for everyone. Personally, with how I learn, it helps to have a project for direct application, then it clicks.)

Thank so much for the knowledge sharing.



Thanks so much for your explanation! This cleared things up a lot for me. So just making sure I completely understand how this all works: because kF is a gain value that is simply meant to relate percentOutput to velocity in our system, it should only be calculated once, along with kP, kI, and kD. But because the ArbitraryFeedForward is meant account for the external forces acting on the system (gravity, in our case), it depends on the desired position and is represented by cos(theta) * power required at horizontal position. Also, like @ericjanofski asked, how is v_max calculated? (Also, to your question about accuracy Eric, I do think it is a “close enough” type of thing, since it only has bearing on the kF gain, a value which is corrected for by the PID loop.)



From my experience, the v_max value that results from (motor free speed)/(gear ratio) is probably close enough. If you want to develop an empirical kF gain, you can provide the mechanism with 50% output, measure the speed v, and then double it to get an estimate of v_max. If you go the empirical route, maybe consider testing at a couple of different percent outputs (i.e. 30%, 40%, 50%, etc) so you have more data points.

v_max should be inherent to the physical system, and should not depend on your target maximum speed. For example, if you scale down your v_max by half, then kF is effectively doubled. This means that for a given velocity, you are supplying twice the output necessary to achieve it, which is generally undesirable. When using motion magic, there are acceleration and velocity constraints you can use to determine the maximum parameters for the motion profile. I believe the (Java) methods are configMotionCruiseVelocity() and configMotionAcceleration().

Vishal – in terms of when different parameters are set (let me know if I am not understanding your question correctly): Generally I would recommend starting with kF and arbitraryFeedForward (if required). Those are parameters inherent to your system, depending on factors like the motors, gear ratio and mass. Once you have your feedforward terms ironed out, then you can start cranking up kP to eliminate tracking error, kD if you start to see oscillations or overshoot, and kI if the mechanism isn’t quite reaching where it is supposed to go. These feedback terms will likely take a little testing before you find an acceptable set of gains.



So I believe I understand what each of the terms is meant to control but the trouble I am still having is in calculating the kF gain, a relationship between percent output and velocity.

Why does this method work? Wouldn’t (motor free speed)/(gear ratio) give the velocity of the gearbox output shaft at 100% rather than of the whole system (an arm in our case)?

This method of supplying a certain percent output and recording the velocity of the system makes more sense to me. However, since we are using an arm, the velocity will not remain constant under a constant percent output. As the arm raises, the effect of gravity will lessen and the velocity will increase. How can I accurately measure v_max for this system?



For v_max, it depends on where your sensor is – at the motor, somewhere in the gearbox or at the arm. Depending on this, you may or may not have to scale by the gear/sprocket ratio and/or convert units. v_max should be the velocity read by the sensor at full output.

As per your question about empirically measuring, that partially why I would suggest just using a calculated kF value for elevators or arms. It’s generally close enough that the feedback terms can handle the rest.

If you do really want to use empirical data to find kF, you’ll want to measure the velocity after the arm has reached “cruise speed” (no longer accelerating). To do this, it may be helpful to log the velocity and position over time and plot it, selecting only the data once the arm’s speed has leveled off. You can also average the “up” and “down” velocity data as a rough way to compensate for the effect of gravity.



We have a mag encoder at the arm’s shaft. I do see now how calculating the kF gain may be much easier than determining it empirically. But I am still wondering how kF = (motor free speed)/(gear ratio). Isn’t kF a relation between percent output and velocity in the whole system? Dividing the motor’s free speed by the gear ratio will give the velocity of the arm shaft under no load, so the relation between motor output and the velocity of the actual arm itself will be different.



The angular velocity of the arm and output shaft should be the same, and the load (weight of the arm) is compensated for with arbitraryFeedForward. So you don’t need to account for the load on the motor with kF as well. v_max assumes no gravity and steady-state speed (the arm has stopped accelerating).

1 Like


Is the output added when using the arbitraryFeedForward term a maximum of 1023 or 1.0 output?



ArbitraryFeedForward represents a percentOutput in this case, so it has a range of [-1.0, 1.0].



Thanks again for all your help with feedforward! I managed to calculate the kF gain for our mini cim with 1:128 gearing to be 3.279 and our arbitrary feed forward to be 0.1 * cos(theta). With some tuning of P, I, and D, we got our arm to servo to a position and hold it fairly well!