# Multi-Segement arm Or Hilo / Arm interferences

From the discussions I’ve seen here I believe some teams are considering using arms with movable end effectors, or hilo’s with short arms on them. This introduces a unique problem: two moving mechanisms where the position of the second relies on the position of the first mechanism.

In this situation, how do you maximize the motion speeds by doing simultaneous movements, while insuring that neither device ‘crashes’ into the robot, floor, etc. The naive solution is to move your first ‘segment’ (let’s call it a hilo) into a position where the arm can’t crash into anything. Then move the arm to the position you need. Then move the hilo back to it’s final position.

There has to be a more efficient way of doing these motions than the brute force method of moving them to a safe zone like this.

If you are using CommandBasedRobot, you can run the two subsystems in parallel in a command group. Let the “slower” subsystem just do its thing, and have the command driving the “faster” subsystem track what the larger one is doing and make sure it avoids any interference or entanglement along the way.

I’ve simplified multi-joint arms by designating one master joint and slaving the positions of the other joints to the current position of the master.

last year we had a pneumatically actuated hilo, it was either down or up, and the smaller rotating thingy had it’s own separate motor and control.

This year…we haven’t got that far yet. I’m hoping we can again use pneumatics to get the shorter thing to be either up/down, and have a motor and control loop for the bigger lift.

I spent a lot of time thinking about this today, and here’s what I’ve come up with. Let’s say I have a hilo and an arm on it.

I treat the Hilo positions as variable “x”.
At different hilo positions I define minimum arm positions “y”.
I use X and y to plot in excel and generate a best fit line with X as the input and Y as the output.

This allows me to use the best fit equation to calculate a lower bound of the arm depending on the hilo position. I use the same method to generate an upper arm bound.

I do the same in reverse to insure that at a given arm position I can never travel the hilo too low, or too high.

Thus, I have constantly changing upper and lower arm bounds and upper and lower hilo bounds that are dependent on the other mechanism’s position.

1 Like

Yes. Presuming the arm is “faster” than the hilo, that’s what I was suggesting for the implementation. By “faster”, I mean that if the hilo is moving at its normal full speed, the arm is able to respond quickly enough to keep from hitting things.

Hopefully the second portion of that where we define the allowed elevator positions based on the current arm position prevents the hilo from moving too quickly.

I have to believe though that there’s still a better way yet. After all, this is for a simple 2 mechanism system. What happens when you add yet another mechanism to it, like most industrial robots?

I must be missing something, what exactly is a hilo in this context?

1 Like

Something which moves vertically; also known as an elevator or lift. First saw this a few months ago myself.

Ahh ok. Thanks for the clarification. Currently sitting 20 feet from a “Hilo” and I didn’t even know it!

This is an interesting and highly relevant topic! Any time you have a robot that can interfere with itself, you need a solution to motion planning to ensure your robot doesn’t destroy itself.

For 1 DOF systems, this is pretty straightforward. You define the minimum and maximum position constraints for your arm/elevator/whatever and make sure you always stay between them. To do so, you might use some combination of (1) rejecting, coercing, or clamping setpoints so that they are always within the valid range, (2) soft limits on the speed controller, (3) limit switches, and/or (4) physical hard stops.

For multi-DOF systems, things get fun. In the 2 DOF case (maybe an arm with a shoulder and a wrist, or an elevator and an arm), each DOF has some limits that are independent of the rest of the system (ex. min and max angles), but there are also dependent constraints (ex. if the shoulder is angled down, the wrist must be angled up to avoid hitting the ground). Examples of such constraints could include:

• Min/max angles
• Maximum extension rules
• Robot chassis
• Angles that require an infeasible amount of holding torque

As @Tom_Line alluded, one intuitive way to think about how this all works is to envision a 2D space (for 2 DOFs; generically, it is N-D for N DOFs) where the x and y axes are the positions of the degrees of freedom. A given point (x,y) can either be a valid configuration, or an invalid configuration (meaning it violates one or more constraints). So, your 2D plot may have regions (be they half-planes, rectangles, etc.) representing constraints.

First example: 2 DOFs where all of the constraints are independent (min/max limits on each axis):

``````x's represent invalid states

elevator height
^   invalid: y axis over travel
|xxxxxxxxxxxxxxxxxxxxx
|x                   x
|x   valid states    x    invalid: x axis over travel
|x                   x
|x                   x
|xxxxxxxxxxxxxxxxxxxxx
o--------------------> arm angle
``````

Motion planning/control in this example is pretty simple. Just make sure all setpoints are always inside the box of valid states.

Now let’s make it more interesting. Say that if the arm is near the middle of travel (pointing straight down), the elevator must be above a certain height, otherwise the arm intersects the robot chassis.

``````elevator height
^
|xxxxxxxxxxxxxxxxxxxxx
|x                   x
|x                   x
|x      xxxxx        x
|x      xxxxx        x
|xxxxxxxxxxxxxxxxxxxxx
o--------------------> arm angle
``````

Let’s talk about motion planning using the second example. Say the current state of our system is `s` and our goal state is `g`:

``````elevator height
^
|xxxxxxxxxxxxxxxxxxxxx
|x                   x
|x                   x
|x      xxxxx        x
|x   s  xxxxx   g    x
|xxxxxxxxxxxxxxxxxxxxx
o--------------------> arm angle
``````

Notice that the elevator height at the current state and the goal is identical. So we just have to move the arm, right…? But moving this way will sweep the system into the constraint we defined for preventing self-intersection with the chassis. Instead, we need to plan a sequence of setpoints to execute that only pass through valid states. Here’s one such example:

``````elevator height
^
|xxxxxxxxxxxxxxxxxxxxx
|x                   x
|x     a______b      x
|x    / xxxxx  \     x
|x   s  xxxxx   g    x
|xxxxxxxxxxxxxxxxxxxxx
o--------------------> arm angle
``````

`a` and `b` are intermediate waypoints we add to ensure that the whole system moves in a safe way. If you think about how this works with our toy example, the elevator goes up, then the arm rotates underneath, then the elevator goes back down. There are many possible ways to generate this path (or other valid paths), including doing some sort of search (discretize the plot into a grid and find a valid path using breadth-first search or A*) or hard-coding rules to deal with different regions of the constraint space. Our approach to the latter in 2018 is here (though largely commented out due to mechanical re-designs eliminating interferences).

Once you have a sequence of waypoints to follow, you have a few options for how to do so:

1. Generate trajectories for each DOF and time-synchronize them. This is how you can ensure your mechanism follows the “diagonal lines” in the path above, but currently there is no support for this AFAICT in any existing FRC software library. The upside of this approach is you can yield the fastest (time-optimal) coordinated movements possible. The downside is that the motion planning is “open loop” - if one of your DOFs lags behind its trajectory, it’s possible that the system might still end up in an invalid state. In practice, you always want some buffer around your setpoints to ensure that tracking error doesn’t lead to this!
2. Do moves one at a time. Move from `s` to `a`, and only start moving towards `b` once the path from the current system state to `b` is a straight line that is collision free. This gives you robustness to control error, but may result in unnecessary stops or jerky movement compared to the optimized, open-loop trajectory.

This whole approach generalizes to an arbitrary number of DOFs, and is basically how industrial 7-DOF arms work.

12 Likes