Arm position interpolation

Hello! I’m wondering if any teams are looking at using a kinematic model of their arm to determine where the end effector will be, and using that knowledge to create different splines(like cubic hermite splines or something of the sort) in which the robot arm should move to have a continuous smooth movement of the arm and prevent collisions with other parts of the robot. Has anyone experimented with this approach? If so, are using using velocity commands over time using some type of motion profile? If you aren’t using this approach are you just creating set points for you different degrees of freedom, or are you using a kinematics model to determine your final end effector position with no interpolation? Let me know what your team is doing! Thanks!

1 Like

It’s not done yet, but our control stack for the arm looks something like this:

Target points and no-go zones → Cartesian path (via motion planning) → Joint-space path (via inverse kinematics) → Closed-loop control on joint angles (via PID+FF)

We’re hoping this will be enough to control our double arm reliably. If it isn’t we can add in the arm dynamics and use a state-space model for control, but for simplicity’s sake I’d rather avoid that if not needed.


How are you motion planning if I may ask?

Will let Ari respond with his own methods but there’s a good explanation here iMO Multi-Segement arm Or Hilo / Arm interferences - #11 by Jared_Russell

1 Like

I haven’t talked to my programming team about this specifically, so I don’t know if they have a plan for this yet. My thinking would be something similar to Akash’s quote of Jared’s post: define a 2D space of allowable cartesian coordinates or joint angles, then use some path search algorithm to traverse from your current position to the desired position. For computational reasons, rather than doing the path search on-the-fly, we might choose to define a few useful points and pre-compute the paths between them.


I’ve done some work on this subject for our own 2dof arm, and I’ve found trajectory generation to be much easier in a configuration space of joint angles rather than end effector position.

If you do this, you can treat each axis as its own function of time (and it is, being separate degrees of freedom), and generate acceleration/jerk-limited paths really easily.

I’m actually writing a whitepaper about our whole controls stack, which probably still needs time to bake, but I think it could be useful for revolute-revolute and other setups.
2_dof_Arm_Control_System_v1_4.pdf (266.3 KB)

Hope this can be of some use!

1 Like

Something I’m still trying to understand - what’s the process of generating those paths/trajectories for something like an arm/elevator? Are there FRC tools to do so?

Do you mean generating the joint configuration spaces? The way I do this is with a python script that uses inverse kinematics turn Cartesian no-go spaces into config space no-go spaces. We just manually place the nodes right now but could move to automatic grid-based placement soon enough.

The config space generator is really janky right now though.

You’ll have to change the source code if you want to use a linear mechanism like an elevator-- mostly by redefining the functions, but there are probably other things you’d have to work to get it working.

The areas it generate aren’t actually usable in code-- basically I look at the graph and then define a bunch of rectangles that cover the limited space, and our pathfinding can stay out of them.

If you mean the navigation between these nodes, we use Dijkstra’s algorithm, which, although it is guaranteed slower than things like A*, is still good enough for our limited number of nodes and a modern processor like the Rio.

1 Like

I took a look at your whitepaper, and I was wondering how effective you found the total-distance cost to be in generating good trajectories. To illustrate my concern, refer to the following diagram:


The x axis is theta1 and y axis is theta2. Red = no-go zone, green = start/end nodes, blue = estimated least-total-distance path. Looking at this path, it looks like it would “encourage” a large theta1 velocity, then a complete stop in theta1 and a sudden acceleration in theta2, and then a near complete stop in theta2 and a sudden acceleration in theta1.

I wonder if perhaps a different weighting method could yield more optimal trajectories both in terms of cost and total energy expended (maybe it could even use the dynamics model?), but that seems pretty difficult to solve. Have you put any thought to this sort of approach?


Haven’t given much thought to this actually.

I’m not entirely sure it’s worth the effort yet until we have a physical arm (simulation hasn’t showed this as a problem yet but it’s possible some parameters are wrong).

If it’s not a problem, this seems like an offseason/between-events upgrade like a lot of teams did for shoot-on-the-move last year.