Motion Magic with an Arm

Hi Everyone,

Can anyone confirm or destroy my theory of how a Motion Magic go to position is actually performed on the talon. My theory is it that it is exactly the same as a motion profile execution but the trajectory points are generated by the talon instead of externally calculated. Is there anyone in this chat or reading this post that can confirm or deny this statement ?

If the above statement is true and you run motion magic with a Kf value only and no PID values (all zero), wouldn’t the controller output be zero at the end of the move (as the projected velocity is zero at the last trajectory point).?

And if the mechanism had any external force on it like gravity, wouldn’t it just drop at the end of the motion magic move? Our robot is an a bag inside a crate on the way to Houston, so this is something we can’t test.

When we are working on Motion Profiling we tweak our Kf values on the drive train using the method of zero for PID value and adjust Kf until the robot drives the approximate distance of our test profile. We always start so low that the robot doesn’t even move and then adjust up until it roughly makes the distance. Then we start ramping Kp until we get oscillations during the path and then back off Kp a bit. We mark the wheel with a highlight pen, position this mark at the top and then execute a profile that does exactly 5 turns. And with only Kf and Kp set this will execute exactly 5 turns to the mm every time. We don’t use Ki or Kd at all in drive train motion profiles. Then the team keep recompiling the motion profile to be faster and faster until the wheel slip starts to effect the repeat ability of the profile. And this gives them an indicative Max Acceleration and Max velocity to use in all of their auto paths. This work really well and I would recommend this to anyone that want to explore Motion Profiles.

We are using motion magic for our arm that goes from one side to the other, going over the top in the middle of the motion (16 different positions on a button board for all the different spots cargo and hatches can be placed), but we didn’t know about arbitrary feed forward until I read this post, so our arm is not tuned well at all and is running rather slowly in order to keep the movements smooth.

We are now in the same situation as some other people in this post. Do we pull it out of the bag at Houston and add an arbitrary feed forward that is continually updated with respect to arm angle (and I suppose you could even adjust for wrist angle because this varies the twisting force of the shoulder joint) and then re tune our Kf, Kp, Ki and Kd in the motion magic setting which could potentially destroy our shoulder mechanism and destroy gearboxes that drive it or do we just run with it as it is because it works and it got us this far. Any advise from experienced teams would be greatly appreciated here. Or if anyone wants to find us in the pits and come and offer some advise we would be forever grateful.

Cheers
Warren

1 Like

Yes. The motion profile is generated (and periodically re-generated) in real time on the talon. See the quoted post below.

The key is that the motion profile is re-generated on the fly. If you aren’t at the target, a new profile is generated to get you there, so in theory the output should never be zero unless you are on target. With only kF on an arm affected by gravity, you’d probably end up in either some weird oscillatory state around the target or a steady-state right below the target.

kF converts a velocity setpoint to motor output. If you can, you should be tuning kF such that mechanism velocity tracks the velocity setpoints over time. The appropriate Java method to read the motion magic velocity setpoint is talon.getActiveTrajectoryVelocity().

My personal take on this: If it’s working for your team, chances are it’s better to leave alone.
Focus on making sure your robot is reliable, your drive team and alliance has a strategy going into every match, etc. The marginal performance benefit from retuning motion magic in the middle of competition will probably not be worth sacrificing those other things. If you happen to have down time, keep a copy of your working gains that you can always revert back to.

1 Like

It’s been my experience even if we set and forget the subsystem using _talon.set(controlMode.motionMagic, setpoint) in the initialize() method of our command the motor continues to servo to the position. As @dardeshna very well described. If we wish to stop the closeloop we add a call to put it into percentoutput with a value of 0.0.

If we wish to change the setpoint on the fly or use arbFF we will put the _talon.set(controlMode.motionMagic, setpoint) call in the execute() method.

With some confirmation to @dardeshna advice. If it works leave it, but if you find yourself with a large window between matches and feel the 60 min wait to get on a practice field for drivers practice is not worth it. Save the current PIDF settings and look at adding the arbitraryFF. Maybe don’t apply it to the system in the first attempt but output the value to your dashboard and observe at horizontal is the value = to something that will hold the arm in position, dose the value decreases as you move from horizontal to zero when the arm no longer requires compensation for gravity? You commented the arm is rather slow. If you can gain one or two more scoring cycle that’s worth it to me.

Hi dardeshna,
After reading your response I got a bit confused and went digging for more info. I don’t know where you quoted that text from but I thing you may have misinterpreted the statement.
“This process is repeated periodically until the mechanism reaches the target.”
I think that statement refers to the step 3 only (ie tracking to a trajectory point in the profile), not the entire process of calculating the path again.

As a programmer I can’t see how the whole profile is periodically recalculated. This sort of defeats the purpose of the PID loop on the position error while the profile is executing. What would happen to the I accumulator value that has built up when the recalculation fired off again if it does this recalculation mid profile, what would fire the recalculation process, would it be done when an error is to big, on a time interval or what.

I just can’t see how the recalculation process on the fly could be implemented. I propose that the path would be calculated in full at the time the motion magic command to go to a set point is called. Allowing for the fact that the arm may be travelling in the wrong direction at that point in time, it would have to build a profile that would slow it down to a stop ( honoring the Max Acceleration) and then speed up in the direction towards the target (again honoring the max acceleration) up to the max velocity and then back down to a stop when at the target. Again, hoping someone from CTRE that rights the code for the Talon can confirm/deny this.

Note to new people reading this post. max acceleration and max velocity are honored in the calculation of the trajectory points only, not in the actual movement of the arm. If Kf is incorrectly calculated the arm can and will travel at full saturation speed ie 100% power for the entire time that the Talon thinks it should take to complete the movement. I’ve experienced this, so read the documentation carefully when calculating Kf.
Same goes for the PID constants, start small as these can cause 100% power also.

I also found a statement in the current online docs that backs up the theory about the arm falling at the end of the movement if only using Kf.

“Another note is that when the movement is finished, you can freely back-drive the mechanism without motor-response (because PID gains are zero).”

Thanks everyone for the advise on changing the code at the champs. currently leaning towards “if it aint broken, dont fix it approach”, but who knows, maybe a couple of hours between matches.

If we do make the changes, we will report back here on our finding about what happens at the end of the motion magic move.

If we don’t make the changes at the comp, I’ll definitely make the changes and report back after we get our robot back from Houston. Unfortunately that will be a couple of months away due to a boat trip.

Cheers and good luck to all the teams that are still going.
Warren

It would be fun to review this section. Do you have a link?

From your original post I thought this was a theory not a statement. Did you have an opportunity to test this? I now want to test this too by zeroing our elevators PID values and driving it from bottom to top. Then see if gravity or we can manually push it back to the bottom. My hypothesis will be the Motion Magic control mode will still be active and see the current position not equal to setpoint and start calculating trajectories.

I did email the CTRE team a few days ago before this thread asking how many trajectory points Motion Magic buffers. My assumption is 1 seeing how the sample code works using a moving target. It wouldn’t make sense if the Motion Magic calculated and buffered the entire profile but midway though seen a change to the end target so flush the buffer and recalculate.

The quoted text was an earlier post of mine giving a high-level overview of what motion magic does. You are correct in that re-generating the entire profile would likely be too resource intensive, so the Talon SRX buffers a finite number of trajectory setpoints.

The following statements are based off of (potentially incorrect) memory of a previous CD discussion I can no longer find slide 27 of the 254 champs presentation on motion profiling. If I remember correctly, the Talon SRX uses a linear filter for realtime trapezoidal profile generation. Paul Copioli initially describes the method here and there is further explanation in the ensuing discussion. This is computationally, the fastest way to generate trapezoidal motion profiles. I seem to recall the trajectory point updating every 10ms while the underlying positional control loop runs at 1ms, but this could be incorrect.

CTRE can probably share a more detailed explanation of the exact implementation, but the takeaway is that the profile is in fact regenerated in real-time throughout the motion.

The statement you found from the online documentation is interesting, as I was almost certain Motion Magic does not finish unless you change control modes. See Omar’s post below:

@capapplefamily
The link to the documentation is
https://phoenix-documentation.readthedocs.io/en/latest/ch16_ClosedLoop.html#motion-magic-position-velocity-current-closed-loop-closed-loop

The paragraph before the first image contains the statement that I’m referring to.

@dardeshna
I agree that it stays in motion magic mode at the end of the profile. But because it has reached the last point in the profile and the last point has a velocity of zero, that means that the Kf has no effect on the motor output (Kf * 0 = 0). The reason that the arm/elevator stays in position is because of the effect of the remaining steady state error * Kp (as long as Kp is set) plus any accumulated values that have been built up by the Ki over time (as long as Ki has been set). Ultimately Motion Magic is just a PID position hold that is always running once it has been told to go to a position. The motion magic part is just providing continually updated positions that it should target to and a guess at how much power is needed to get to these positions. Once the profile has finished (ie the last point has been pulled from the buffer) the last position in still there and active, it just that it is not changing any more.

I also agree that it probably starts moving after the first point is calculated and loaded into the buffer, and it wouldn’t wait to load the whole profile. (I know we start our motion profiles after 5 points are loaded and then continue to push points into the buffer at the same time the executor is pulling them out of the buffer to target them.) The generating part of the algorithm on the Talon would then continue to create points and load them into the buffer, the execution part of the algorithm pulls them from the buffer and targets to them. So it would appear that it is constantly being updated, but I think that it would create the path based on where is was when the go to position call was made and where is has to get to, otherwise the PID controller that is trying to push the error to zero would be all over the shop.

Yes, you are also correct that this is my theory, I don’t represent CTRE, this is our first year in the competition and there is every chance that I’m wrong and giving out the wrong information. That happens all the time in these types of forums. That’s why I’m trying to get to the bottom of it, when I read a statement that doesn’t make sense to me I question it so that other readers that read this post are going to walk away with the right information. When a statement I make is proven incorrect everyone benefits.

All my statements in this post are based on the extensive testing and implementation we have done on Motion Profiling, and there is a big assumption on my part that they are implemented the same way at the execution level in the Talon. When I get a chance I’ll go through the source code on the Talon and see if I can work out exactly what is going on and report back here for the benefit of all the readers. I was hoping a source coder from CTRE would jump in here and set us all straight.

Lets face it, I’m glad we are having these conversations. Other people will be reading this with all the same questions and everyone including me will have a far better understanding by the time they get to last post in the topic.
Cheers
Warren

When the mechanism is at the goal location, it is definitely the positional PID that is keeping the arm in place as the contribution of feedforward is zero. However, if there is a disturbance that moves the mechanism off of the target, then a trapezoidal motion profile will be used to get back to the final goal. I believe your original scenario posed was based on using motion magic with only a kF value.

Regarding profile re-generation, see the linked post from @Jacob_C from another thread (sorry, it took me a few searches to find otherwise I would have posted it earlier):

The entire trajectory is not generated and buffered. Based on the provided constraints, only the next setpoint is generated. This occurs in real time at the 10ms rate I mentioned earlier. This is all happening on the Talon SRX in firmware.

I wonder if the paragraph in question was using controlmode.velocity seeing that this entire section is for all modes " Motion Magic / Position / Velocity / Current Closed Loop Closed Loop" and also the next section adds Motion Magic Cruise Velocity And Acceleration. The two variable unique to motion magic.

I’m tempted to goto the school tonight to test this but i might just wait till tomorrow.

If anyone got a robot not in a bag I Just thought of a way to test this, to see if the path is being generated based on start and end points only or if its being generated and adapted on the fly based on real time feedback of where the arm is through out the motion of the move. (ie a different path is being generated depending on how well the movement is going.)

Setup will be a simple command that does one turn of a PG188 using motion magic. (Go to position 4 x 7 x 188= 5264, ) And continually query and log the active trajectory point in the commands execute method and loop count until the ActTraj_Velocity is zero.

Run once with an excessive Kf and again with a minimal Kf. If the same number of loop counts is reported in the both cases, we will know that it only being based on start and end points, if its a completely different number then we will know if it effectively be adapted/ re-generated throughout the move based on sensor position. I’m pretty sure there was a buffer count method you could query in Motion Profiling. This would also be interesting to log to see how far in front the buffer is to the motion profile executor.

BTW the ActTraj_Velocity == zero test is a great way to implement the isfinished method in a motion magic move command. So much better than the endpoint testing that we use at the moment. Especially when you are building sequential command groups with multiple moves in them.
See you learn something new every day.

In that same paragraph they refer to active trajectory point. A velocity control mode wouldn’t be using this term. This only applies to Motion Magic and Motion Profiling.

Funny, this entire conversation basically started when I said the documentation wasn’t clear. And yet again we are having issues interrupting the documentation again.

2 Likes

Yes we have come full Circle.

@ozrien and @Jacob_C must be volunteering this weekend. They are normally very quick to respond to support emails and threads like this.

Sorry, I’m questioning this one. I think the only thing that is returning it to the end position is the PID loop on the position error. If your statement was true, what would the definition of a disturbance be that would trigger the regeneration? We don’t use a Ki in our motion magic moves so we always have an error at the end of the move so it’s never on target. And if you move the arm ever so slightly the error would change. If an error change (“disturbance”) was the key to reproducing a new trapezoidal motion magic profile again to get to the final position it would generate points with velocities and the arm would jump because of the effect of Kf. That doesn’t happen. So my money is still on the fact that the path is only generated as a result of the initial motion magic go to position. (NB: I don’t dispute that it is generated on the fly point by point, I’m only disputing the resultant path is entirely based on the velocity and position at the start and end point and nothing in between would result in a different path being produced) This could easily be tested if someone can continuously log the ActTraj_Velocity values in a motion magic command and then after the arm/motor has come to rest, apply a disturbance to the arm and see if the ActTraj_Velocity values change from zero (ie are re-generated) or remain at zero (ie the path was generated once and won’t be regenerated until another go to position is called).

It could depend on how much the error is. If it is small enough that the profile is triangular and peaks at a low velocity, then the contribution from feedforward would be very small and may not do much. I understand what you are proposing though.

Regardless of whether the profile is recalculated based on the current position or initial position at time of method invocation, I guess this is only relevant in a situation where .set() is not being called periodically. Nope, you have to explicitly exit and reinvoke motion magic.

Yup, you seem to be correct that after the profile hits the target (getActiveTrajectoryVelocity() is zero), then only positional PID is used unless motion magic is explicitly exited and reinvoked. Seems like this discussion has already happened this season.

I am a bit concerned about this behavior to be quite honest. It seems if there is a large disturbance you would want your mechanism to honor the motion magic constraints. For example, the disable-enable described in that thread.

Additionally, because the tracking error is low while profiling with motion magic, you can ratchet up gains quite a bit. Using these same gains to hold at the end of the profile could result in pretty different responses to disturbances. It does seem that many teams switch over to position PID with separate gains in their mechanism hold state.

Yep,
Now that you mention that, we have experienced that full speed take you head off movement that happens after you disable the robot and then move the arm to the other side of the robot to work on something and then re-enable 5 minutes later. It definitely doesn’t honor max velocity or max acceleration in that circumstance. Its a just a big position error times a large Kp which result in a 100% full speed get out of the road very quickly movement.

A simple fix here might be to use the active traj velocity ==0 check in the isFinished() Method of the motion magic go to position command and then a call to change to position hold mode in the end() method.

Is there a method that returns the set point that you sent to motion magic when you first called it? If there is, it would be just a case of reading this and changing modes with the same set point. Otherwise you would have to keep an instance of a class variable to remember what the last set point was so that you could re-use it again in position hold.

Only thing I would be careful of here is that having a non zero value in Kf while in position servo mode seems to scale the set point position by a factor of Kf when in this mode. I can’t really see why the Kf isn’t completely ignored when in position servo mode. I can see a need for it in velocity hold mode but can’t think of a case where it is a good idea in position servo mode. Anyone know why Kf isn’t ignored when is this mode?

Just thought of another problem with the changing modes at the end of the move, the accumulator that has been built up to hold the position at the end of the motion magic move might get cleared when you change from Motion Magic to Position servo. Can anyone confirm or deny this one?

Seeing as this post is fast becoming the answer to all Motion Magic questions known to man kind, I’ll offer another question that someone might answer here. Does calling the motion magic go to position again with the exact same value that is was called with last time cause a new profile to be created or is this called ignored and it is just left there happily holding position due to the PID component that is always running when in motion magic mode?

Cheers
Warren

After doing more reading I can answer/ correct some of my own statements. Motion Magic trajectory points are generated 10ms apart. The saw tooth that I refer to would not exist because

Starting in 2019, the Talon and Victor will linearly interpolate targets between two buffer points every 1ms. This means you can send points with larger time durations, but still have a smooth continuous motion. This features default on.

Might be easier to just do a call to a set to percentage output at 0% in the disabled init method of our robot project. The motor is dead when disabled anyway so setting to 0% output will take it out of motion magic mode so it won’t take your head off when you re-enable.

There will certainly be a few changes implemented in our base code for next year comp thanks to this thread.

In theory getClosedLoopTarget() should get the goal position for Motion Magic. I have no idea why kF is used in position mode and wish it was just ignored as well. Not sure on the integral accumulator, but I could imagine funny behavior if it were generally persistent across modes.

It seems from what we have gathered, that if the active trajectory velocity is zero (when exactly does this happen?), then the profile following component of Motion Magic is considered complete. It would be good know the conditions that lead to the active trajectory velocity being zero. This would tell us when invoking motion magic would just fall through to positional PID.

For me, ideal Motion Magic behavior might be either:

  1. trapezoidal profiling is always used, even when holding at the end
  2. trapezoidal profiling terminates when the error between the current position and goal position falls below a threshold, and then there is user adjustable hysteresis for when profiling kicks back in

Basically I think this is going to happen at the end of the back side of the trapezoidal profile speed curve. The generator is trying to create MP points that get you to the target as quickly as its allowed to do. That is, stay at the maximum allowed speed for as long as possible (flat part of the curve) and then start to slow down towards zero speed at a rate that doesn’t exceed the max de-acceleration rate as you approach the final set point (Back side of the curve). What we have to remember is that it is always effectively in “PID position mode” the entire time, its just that the goal post is constantly being changed every 10ms. Eventually the goal post stops moving and at the same point in time the feed forward velocity will be zero also, because this is the last point in the profile.

Going back to high school physics this will be when the area under the speed curve is equal to the distance needed to be covered (ie difference between encoder reading at the time motion magic was started, to the set point that was called in the motion magic go to position call). ie the Integral of speed curve equals distance.

working backwards with a bit of simple maths should be able to give you the exact time taken for a MM move to finish if you know all of the parameters.

At least for the simple case where the arm/mechanism is at rest when you initiate the motion magic move and assuming the generator algorithm used by Talons internal firmware generates a perfect trapezoid.

The slope on each side of the trapezoid is determined by the max acceleration value and the height of the horizontal flat section is determined by max velocity value.

Add the two triangle together from each end into a rectangle, add this rectangle to the rectangle in the middle and this total area needs to equal the distance to cover and hey presto you can work out how long the motion magic move will take to complete. Sounds like a good problem to give to my son tonight. I’ll report back later with the magic formula.

I wonder if this value is continually changing as the profile is being executed and only settles at the destination position after the profile following part of MM has finished. More testing to be done!!

Maybe everyone in this conversation needs to co-author a document titled “Motion Magic under the hood” once we work out all the answers to these questions so that it can be referred at the top of this post so everyone doesn’t have to read all of my babel.