Motion Magic with an Arm

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: https://github.com/team6637/Robot2019/tree/master/src/main/java/frc/robot

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);
motorSlave.follow(motorMaster);

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.

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);
motorSlave.follow(motorMaster);

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!

7 Likes

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!

2 Likes

Hi Vishal,
We have also gone down the same road as you (ie using Motion Magic with arms effected by gravity, I completely understood every question you asked when I read this post). Ours was even more complicated because our arm went right over the top and down the other side. I wasn’t aware of the arbitrary feed forward term at all until reading this post.

We calculated our Kf by applying 75% power and using phoenix tuner to capture the speeds on the mag encoders at about the top of the travel and then try and stop/catch the arm before it slammed into the bottom on the other side. (A slightly risky process).

When we do Kf for Motion Profiling on the drive train, we get the robot in a big hall and do the same process. We used to do it at 100% power but once we realised we were never going to go this fast in an actual motion profile, we then calculated our Kf at a power value that was closer to the speeds that we wanted our profile to max out at.

You talk about setting the arbitary feed forward to be 0.1 * cos (Theta). Do you re-calculate this and set it every time you target to a new position, or are you continually adjusting it throughout the motion of the arm in the execute method of the command?
Also, did you find that enabling/ disabling the arbitrary FF made any considerable difference to the smoothness of the profile?

I’d love to be able to log the output of the inner control loop against the position measurement during the movement of the arm. This way you would know how well the Abitrary Feed Forward and Kf component of the control loop are doing at hitting the desired encoder position at that instant in time. Then you could also see how much the PID controller is contributing to the correction.

Also have to agree with you on the whole documentations (or lack there of) on motion magic and motion profiling. I feel its like learning magic in the Harry Potter movies. At first every goes horrible wrong and it seems that you will never get it to actually work. But finally after you have reversed engineered heaps of sample codes including the actual code that runs on the talons and then read 50 articles from all over the internet, you have then proven yourself worthy of being in the elite club of teams that actually know how to make it work and this is when the magic happens and your arm, elevator or drivetrain slides perfectly smoothly to its exact setpoint as if its being controlled by magic.

So much to learn and so little space left in my head to hold it long enough to re-teach it to the team.

Cheers
Warren

We used a very similar method to calculate kf by applying a ~100% power to our arm and stopping before the crash logging all the data using the Phoenix Tuner to calculate the initial motion magic variables.

Yes Arbitrary Feed Forward is poorly documented at this time but i believe the people over at CTRE are working on it along with some other advanced option they offer. But to answer the question of setting the Arbitrary feed forward yes on each program scan execute() method we call these

“Sudo code”…
_arbFF = percentOutput to hold arm at horizontal * cos(current angle from horizontal)
_talon.set(controlMode.MotionMagic, setpoint, demon.arbitraryFeedForward, _arbFF)

It stared to make sense when we quantified arbFF as an adjustable spring or counterbalance that is adjusted through the arch.

My goal now that our season is complete is to try and record a few short videos of our process that is hopefully more right than wrong to start and demystify Motion Magic and its options.

I just want to say that I really appreciate this post and will be following along closely. We have a 2-stage cable-driven elevator, the winch motor is powered by a Talon SRX and we’re using motion magic for setpoints. We’ve got a P value set (0.9), I and D and F are all 0. It works… but I know it could be better.

Our students tried to calibrate a feedforward value but it had the effect of running the lift up at full speed and (apparently) crashing through the soft limit. The lift stages jammed, our upper pulley bracket got bent, and the fabrication team gave us a really big frownie face after fixing it all. Thus we set the lift at essentially the minimum that gets it working and have left it alone since…

The software students want to try recalibrating, but we need to weigh that against “if it ain’t broke” and the possibility of breaking something before our District Championship.

On the one hand… I’m glad to see it’s not just us. On the other hand… I join the chorus of those wishing for more beginner-friendly documentation. This thread has been very helpful.

I’ve PM’d @Warren.Reynolds directly, but I’ll open it up to everyone in this thread so we can get as much feedback as possible.

Which portion of the existing documentation gave folks the most trouble when trying to use Motion Magic?
https://phoenix-documentation.readthedocs.io/en/latest/ch16_ClosedLoop.html

Specifically, is there any step where:

  • You didn’t know what to do next
  • Your robot/system behaved in a manner you didn’t expect
  • The step was confusing

Other useful information:

Were you using our Motion Magic example project to go through the documentation steps?

For those that had any of the three items above happen but were later able to use MM successfully, what did you do to get MM working in your system? What piece of information made it “click”?

It’s obvious from reading this thread that we have room to improve our MM documentation but whether that’s tweaking flow/wording or re-writing major portions, it’s critical for us to understand where the difficulties are happening with the existing documentation.

1 Like