N-Jointed Arm Control

Technical Programming
1

Arms are a versatile and elegant approach to solving design problems in FRC. Their mechanical simplicity sets them apart from other mechanisms which may require more complex design, like elevators. Despite this, programming arms with more than one joint proves to be a difficult task that steers teams away from them.

What are some methods for controlling n-jointed arms? Ranging from tasks like moving along the x or y directions, generating & following paths in (x,y) space, holding setpoints, fighting gravity, maintaining force/torque, etc. How is inverse kinematics applied when n is greater than two and simple geometry/trigonometry fails?

Two teams that have pulled it off well that come to my mind are 971 in 2018 and 4678 in 2018 and 2019. Would love if anybody on either of those teams or anyone else with knowledge on the matter pitches in.

2 Likes
2

At that point, it would be harmful to use PID since it could no longer guarantee stability. You’d need some sort of state-space approach, which involves modeling the system. But then Newton’s Laws get really annoying with n>2, so you’d need to use Lagrangian mechanics if you want to stay sane. And once that’s done, the system is so nonlinear you’d probably need to command a trajectory along different angles that would reach a final position, along with a time varying state-space controller that linearizes about the state the system is currently at. So a lot of very cool but very complicated math! Counteracting gravity would honestly be the simplest part, you just need to apply a constant voltage you calculate based on your physical model

4 Likes
3

In 2018 we had a 2dof arm (big arm + small wrist). We did some math (which eludes me right now) to calculate the angle of the wrist relative to the robot based on the angle of the arm. This allowed us to set positions for placing on the scale with just an arm angle and a wrist angle (both relative to the robot/ground). In 2018, we ran it using Motion Magic on a TalonSRX, using the built in Talon kF, P, and D gains. If we were to redo it, I’d use WPILib’s characterization tools and rio-side control loops. While this solution worked fine for our robot, I think it’d be pretty hard to generalize for something with N joints (but if you have more than 2 joints on your arm I’d start questioning your design team’s sanity)

3 Likes
4

There’s 16 slots on the PDPs, you could get away with a 4 motor drive train + N=12 !

2 Likes
5

There’s nothing wrong with using PID for the inner control system. You will want to add a feedforward based on the arm angle relative to the ground, but there’s nothing about a 2-jointed arm that’s any special- it’s just a 1 jointed arm on another 1 jointed arm.

The real problem comes in generating setpoints for each joint. There are some really neat things you can do here like configuration spaces, but the best option is usually just have a few preset positions and use known motion profiles between those.

2 Likes
6

Unfortunately, no one involved with my team’s 2015 robot (3 joints + a gripper) is still on the team, but from match video it seems like this was the case.

7

This is an interesting idea. You have a bunch of presets for the arm to be at, generate profiles for all combinations, and then load a profile based on where you are and where you want to go.

For example, you have five setpoints: ZERO (starting config), INTAKE, LOW, MID, and HIGH. Offboard (with a Desktop script, for example, or even at robot start), you generate a profile that moves you from ZERO to INTAKE, ZERO to LOW, ZERO to MID, …, moving from each state to every other state and save those to memory. During operation, you load those profiles based on what setpoint you’re at and which you want to go.

Interesting idea… But now to think how would one generate those profiles. One way I can think of now would be to give multiple x points and y points, solve for joint angles with IK at each, then interpolate between those when moving.

Another idea I had which is a little out of the box is using some sort of Pure Pursuit to follow (x,y) points. The most unintuitive part of that for me is getting the arm to arc towards the goal by commanding goal angles, but it’s an interesting thought experiment.

8

I’m actually coding a 6axis LEGO robot arm using WPILib style paradigms, so I’d like to see where this goes,

1 Like
9

For N=1 PID is definitely good, for N=2 it’s probably okay, but I’d say for N>2 the variables’ changes over time depend too much on each other for a control law for one motor to only depend on that motor’s position. I’ll take a look at the math and physics tomorrow (:crab: my APs are gone :crab:) and see how achievable control is for that case using just PD (assuming no friction + using a feedforward, no need for I)

10

I had thought that we were the only team that had a three jointed trashcan pickup arm in 2015 (although we only had a trashcan pickup). Our robot from that year (Luxo) has a three jointed arm that mirrored a custom controller that looked like our robot’s arm to make control simple for anyone to pick up. I have heard that it was very difficult to use in a match but it makes for an awesome display bot. Unfortunately, while we still have the robot in our lab we somehow lost the code and no one remembers how any of it worked. So, while in concept movement mirroring is cool, for actual competition preset positions definitely sounds like the way to go (this all coming from someone who has never programmed anything remotely like that).

1 Like
11

Also, MATLAB posted this video about trajectories for robotic manipulators: https://www.youtube.com/watch?v=Fd7wjZDoh7g
I just watched it, it seems to explain different ways to generate trajectories well

1 Like
12

It depends on what your setpoints are. If it’s arm angles, it’s a simple feedforward and change of variables. If you want to control xy position, that’s much more nonlinear.

1 Like
13

Glad you brought that robot up, Nathan :grin:

I was the coder for Scorpion. I also helped out with a 2-jointed arm in 2019, which performed similarly complex motions.

If your team is looking to get into multi-jointed arms, the first thing to tackle is the mechanical design. There are a couple of tricks that will make controlling it a whole lot easier. The first is using what I can best describe as an actively controlled virtual-four bar. What this does is make it so that moving joint x does not affect the angle of all joints y>x. For example with a 2-jointed arm, if the wrist is parallel the to the ground and the motor controlling the wrist is held still, it will stay parallel to the ground even as the shoulder moves up and down. This decoupling makes control a lot easier.

The second mechanical trick is counter-balancing each joint in a way that makes it as close to being effectively weightless as possible. Because the joint is moving in a circle, the effect gravity has on it is proportional to the cosine of the angle, so the counter-balance should behave similarly. The virtual four bar makes this much more practical for multi-jointed arms.

Now on to the software. 971 does some insane work to achieve beautifully smooth motion on their mechanisms. With the resources currently available in FRC, there are only a handful of teams which could successfully pull off many of the things they do. But if your arm is designed to have the two things I mentioned above, then doing waht 971 does isn’t really necessary. PID or motion magic are both good enough for the controls side of things, especially with the 1kHz update rates on modern FRC controllers. Scorpion arm’s was running 3 PID loops at a 50Hz update rate without any feedforward.

The other aspect of this performing complex motions without breaking the arm or any extension/height rules. Running through a sequence of setpoints is the simple way to go. It can be a bit of an imprecise science to get the right setpoints, and it won’t look at pretty as fancy path planners, but it will get the job done. For determining when to consider one setpoint completed and move onto the next, look into use the root mean squared error of the joint angles. The larger the error threshold, the more fluid and less precise the motions will be.

Our 2019 code can be found here. It uses motion magic, but the setpoint sequencing is as I described. I’ll also link to Scorpion’s code, although it is a lot messier.

One last note: I love multi-jointed arms. They are a ton of fun to work with. But I’ve been starting to see more and more that they are rarely a good idea from a competitive standpoint. The complexities of which end effector positions are achievable, which aren’t, and which can only be reached through very precise motion cause a lot of headaches. This is a problem for software, but more than that it is a problem for designing and iterating the manipulator(s) at the end of the arm. Linear actuators (or simple arm designs) remove a lot of these constraints, enabling a better overall robot design.

8 Likes
14

In 2019 my team had an arm with an elbow, an extension, and a wrist that was capable of going 180 degrees, and extending outward as well. We used MotionMagic on TalonSRXs for all the motors on these parts. Tuning was quite a challenge, we started with feedforward, adjusting arbitrary feedforward as well to hold the arm in place, then tuning PID.
In more detail, here was our process for how we tuned the arm.
Our code for the subsystems making up the arm can be found here, it’s mainly a lot of tuning. For controlling the motion of the arm… more is written here, but we set up a state machine, where we represented every point we wanted the arm to reach with a node on a map with the corresponding known extension distances, wrist angle, etc (for example from 2019: “cargo ship cargo placement”, “middle level rocket cargo placement”) and transitions between these states as edges. We put this together into a map of all possible positions and transitions of the whole arm, such that paths for “impossible” transitions would not appear, then ran a search on this map to determine what motions the arm would take. Here’s the arm path generator with breadth first search and all the code for the arm. In the end it was fun to see in action, but quite a headache to tune and test, and probably not optimal – we saw problems with optimizing movement and as was said earlier, getting precise motion is real hard.

2 Likes
15

You can avoid the mechanical complexity and finicky process of counterbalancing with a feedforward though. frc-characterization includes an arm characterization tool and writing the feedforward equation is almost trivial; just because there are mechanical “hacks” doesn’t mean that fixing it in software isn’t faster for teams at the 50 percentile of software capability.

(Yes, I recognize that software is not the strong suit of every team. If you can barely manage a PID controller then feedforward is probably going to take more time than counterbalancing. Know your strengths.)

1 Like
16

Alright I said I’d do this tomorrow but time is fake right now so here’s some math! I didn’t factor in any applied voltage in the calculations. I realized that Lagrangian mechanics by itself cannot deal with non-conservative forces. If anyone has any insight about how to deal with applied forces it would be much appreciated. I could’ve worked it out with N=2 using Newtonian mechanics but wanted to demonstrate the approach you’d take for the general case (Newtonian still works for N>2 but please don’t do that to yourself!)

Let:
L_1 be the length of the first arm
L_2 be the length of the second arm
M_2 be the mass of the second arm
I_1 be the moment of inertia of the first arm
I_2 be the moment of inertia of the second arm
\bar{r_1} be the center of mass of the first arm
\bar{r_2} be the center of mass of the second arm
\theta_1 be the angle from the vertical of the first arm, increasing towards the x axis
\theta_2 be the angle from the vertical of the second arm, increasing towards the x axis
\omega_1 be the angular velocity of the first arm
\omega_2 be the angular velocity of the second arm

The kinetic energy of the first arm is trivially given by
T_1 = \frac{1}{2} I_1 \omega_1^2
The kinetic energy of the second arm is harder. Let’s decompose it into little pieces of mass dm and find each of their kinetic energies, then add them up. Every piece dm will be a radius r_2 from the joint. Thus, its position is given by:
x_2 = L_1 \sin \theta_1 + r_2 \sin \theta_2
y_2 = L_1 \cos \theta_1 + r_2 \cos \theta_2
Then, it’s velocity is given by:
\dot{x}_2 = L_1 \omega_1 \cos \theta_1 + r_2 \omega_2 \cos \theta_2
\dot{y}_2 = -L_1 \omega_1 \sin \theta_1 - r_2 \omega_2 \sin \theta_2
The magnitude squared of the velocity is given by:
v^2 = L_1^2 \omega_1^2 + 2L_1 \omega_1 r_2 \omega_2 \cos(\theta_1 - \theta_2) + r_2^2 \omega_2^2 (derivation left to reader as an exercise)
Integrating over dm and dividing by two gives the kinetic energy of the second arm
T_2 = \frac{1}{2} L_1^2 \omega_1^2 M_2 + L_1 \omega_1 \omega_2 M_2 \bar{r_2} \cos (\theta_1 - \theta_2) + \frac{1}{2} I_2 \omega_2^2
The potential energies are much easier:
V_1 = M_1 g \bar{r_1} \cos \theta_1
V_1 = M_2 g (L_1 \cos \theta_1 + \bar{r_2} \cos \theta_2)
Putting it all together into the Lagrangian:
L = T_1 + T_2 - V_1 - V_2 = \frac{1}{2} I_1 \omega_1^2 + \frac{1}{2} L_1^2 \omega_1^2 M_2 + L_1 \omega_1 \omega_2 M_2 \bar{r_2} \cos (\theta_1 - \theta_2) + \frac{1}{2} I_2 \omega_2^2 - M_1 g \bar{r_1} \cos \theta_1 - M_2 g (L_1 \cos \theta_1 + \bar{r_2} \cos \theta_2)
Now we apply the Euler-Lagrange equation:
\frac{\partial}{\partial t} \left(\frac{\partial L}{\partial \dot{q_i}} \right) = \frac{\partial L}{\partial q_i}
For q = \theta_1 :
\frac{\partial}{\partial t} [(I_1 + L_1^2 M) \omega_1 + L_1 \omega_2 M_2 \bar{r_2} \cos(\theta_1 - \theta_2)] = -L_1 \omega_1 \omega_2 M_2 \bar{r_2} \sin(\theta_1 - \theta_2) + (M_1 \bar{r_1} + M_2 L_1) g \bar{r_1} \sin \theta_1
Evaluating gives:
(I_1 + L_1^2 M_1) \dot{w_1} + L_1 M_2 \bar{r_2} [\dot{\omega_2} \cos(\theta_1 - \theta_2) - \omega_2 (\omega_1 - \omega_2) \sin(\theta_1 - \theta_2)] = -L_1 \omega_1 \omega_2 M_2 \bar{r_2} \sin(\theta_1 - \theta_2) + (M_1 \bar{r_1} + M_2 L_1) g \bar{r_1} \sin \theta_2
For q = \theta_2
\frac{\partial}{\partial t} (L_1 \omega_2 M_2 \bar{r_2} \cos(\theta_1 - \theta_2) + I_2 \omega_2) = L_1 \omega_1 \omega_2 M \bar{r_2} \sin(\theta_1 - \theta_2) + M_2 g \bar{r_2} \sin \theta_2
L_1 M_2 \bar{r_2} [\dot{w_2} \cos(\theta_1 - \theta_2) + \omega_2 (\omega_1 - \omega_2) \cos(\theta_1 - \theta_2)] + I_2 \dot{\omega_2} = L_1 \omega_1 \omega_2 M \bar{r_2} \sin(\theta_1 - \theta_2) + M_2 g \bar{r_2} \sin \theta_2
Then you’d treat it as a system of two equations and two unknowns to solve for \dot{\omega_1} and \dot{\omega_2}

I almost surely made an error, but the moral of the story is that it is VERY nonlinear. Which explains why @ctychen had trouble tuning it. A more sophisticated approach would need path planning and would linearize the expressions for \dot{\omega_1} and \dot{\omega_2} around the current point and then create a state space system around that linearization, then create a controller.

And now that I finished typing all of that I realized I assumed the arms are along lines, which is false since they’re 3D, but the math would be similar.

One last note: I think it’s possible to add in the torques from each motor ( \tau is a linear combination of \omega and applied voltage V ) once you have the expressions without control. For example, given \dot{\omega_1} , you can write:
MOI_1 \dot{\omega_1} = MOI_1 (\text{expression for} \dot{\omega_1} \text{without voltage}) + \tau_1
and then solve for the new corrected angular acceleration. I wrote MOI_1 instead of I_1 since in this case, the moment of inertia includes the second arm, and changes with respect to \theta_2 - \theta_1 . But I’m not sure about this so please correct me.

I’d like to reiterate this definitely has some error somewhere since I did this at 12:36 AM and am too tired to proofread. But if you have any questions or would like to enlighten me about Lagrangian mechanics with non-conservative forces please go ahead I’ll read it in the morning when I am more awake!

7 Likes
17

Thank you for sharing this, MATLAB is an awesome resource.

1 Like
18

Thank you for cranking out the physics! This will definitely help people out who hope to model their arms or simulate them beforehand.

1 Like
19

I believe a team in 2011 used this “shadowing” technique as well to control their arm. Definitely an ingenious solution to the problem.

20

This is very interesting. I’m not very familiar with maps and searching but now I’m intrigued!