Real-Time Inverse Kinematics on a UR5

Posted on by Curt 13 min read

The UR5 is where I learned the Universal Robots stack.

I had already played with MoveIt and ROS on the lab’s Kinova Mico. Once I proved that I could handle a robot arm I was given access to the UR5. That’s the arm I really cut my teeth on. I got familiar with the UR driver and the Robotiq gripper, went further into ROS, and played with the RelaxedIK solver. The block-finding project started here too. Most of what I know about getting a UR arm to move in real time was already learned by the time the UR3e showed up and became “my” robot. The UR3e was my playground, but this post is the work that came before it.

The project context I was working within was called ITER (I’ll explain later). It grew into the experiment runner for two paper studies and a bunch of small prototype experiments. The papers are the task interdependence study from RO-MAN 2020 and the attention management study written up in Helping Operators Keep Up with Collaborative Robots. Neither study actually ran on the UR5, as I had migrated to “my” UR3e.

The Robotics Room

The UR5 lived in the robotics room in the CS building. That room was really the Graphics lab’s workspace (they also did a bunch of robotics work since there is obvious overlap between simulation, IK, etc.). There were two robots in there. The UR5 was the one I got to play with. The other was a Baxter, or maybe a Sawyer. I can’t remember now. It had the screen face and the overly compliant joints. I unfortunately never got a chance to control it. Its workcell had a multi-camera 3D tracking rig around it. The Graphics folks mostly used the UR5 for teleoperation work, exploring solvers and intent smoothing.

The UR3e lived in the HCI experimentation room. That room was better for running participants, but it was a shared space. Development meant bringing up the workcell and tearing it down again. Running a study meant booking the room for the whole period. During COVID the UR3e moved into its own little room so I could work on it in person while social distancing. The grad students, myself included, also took the opportunity to work from home instead of hiking up the stairs to the office.

There was also a period where I was using both arms. Both rooms were shared with other studies and other grad students, so I hopped between them.

The Setup

The UR5 was equipped with a Robotiq 85 gripper on its end-effector, driven by an old Dell laptop running ROS Kinetic. The gripper talked over a USB serial cable at 115200 baud. Unlike the newer UR3e, there was no URCap or tunnel through the UR controller. The cable was a 10 foot USB extension. I had to keep track of it so it did not wrap around the robot. The upside is that the gripper is independent of the arm. It can be commanded over serial while the robot is moving, if you want that. The URCap route on the UR3e was trickier because everything had to be coordinated through the UR controller.

Architecture

I developed an experiment runtime called ITER, which stands for Interdependence Task Experiment Runner. An experiment runner (within HRI) minimally needs the following:

  • The experiment needs to unfold in effectively the same way for each participant.
  • The task has to be easy to change between studies without touching the robot code.
  • The robot has to wait on the human at the right moments, and somebody has to log how long that took.

ITER’s answer is a small set of primitives. Move the end effector to a pose, grasp, release, read the current pose back, wait for the participant to press the button, and log a line. A task is a JSON plan, a list of those primitives that the runner executes in order. Task scripts build the plans. The house-building task is a Python script that spits out well over a thousand lines of JSON. While the runner walks the list, a timing node keeps track of how long the robot spent waiting on the human. That duration was important for one of my studies.

ITER was never a one-robot project. The launch files know about three arms (a UR5, a UR3e, and a Kinova Mico in three gripper configurations) and three planners (MoveIt, RelaxedIK, and the UR controller’s own solver), and you pick the pair on the command line. The Mico has its own post. The same plan runs on any of them without changing the JSON (well unless it’s UR planner specific and you have a Mico). The UR5 was one row in a table rather than a special case. That made it easier to swap the robot out as I needed to move to another room to continue testing or running an experiment.

Why RelaxedIK

MoveIt was the first planner, and it worked fine(ish) until I asked it to do the same thing twice (the docstring I left on the RelaxedIK primitives in May 2019 puts it more politely).

While MoveIt provides many nice features to get started in developing primitives,
it is lacking in several aspects. Namely, repeatability of a task plan with same
joint paths. Relaxed-IK attempts to solve this problem by treating the pose goal
as an optimization problem where trade-offs in accuracy and joint travel are
weighed.

A study needs the robot to do the same motion for every participant (or at least something relatively close if we are emulating working in a manufacturing context). MoveIt’s sampling-based planners give you a different joint path every time you call it. If the arm swings its elbow through a different arc for participant twelve than it did for participant eleven, that is a confounding variable. It is not controlled, so it could muddle the interaction effects we were actually looking for. RelaxedIK solves for joint angles as an optimization seeded from the current joints. The same sequence of pose goals produces close to the same joint path every time. Near repeatability in real time is much preferred over the arcane attempts to get something repeatable out of OMPL. Exact is not possible without literally replaying the joint path, but then we wouldn’t be able to support more dynamic experiments if we wanted to. Close to exact is fine, since the objects themselves have error in their positions. The integration leaned on the Wisc-HCI behavior execution package built for Authr, so the primitives themselves are short. I think the interesting part is how the solutions get to the arm.

Two ways to run it

The launch file has a switch called use_static. It picks between two ways of driving the robot.

The real-time mode is the one in the title. RelaxedIK publishes a stream of joint solutions. A servoing node on the ROS side forwards each one to the UR as a servoj command with a fixed timestep and gain. The arm is always chasing the latest solution. The planner steps the pose goal toward the target in 5 millimeter increments at 100 Hz. A move is really a few hundred tiny moves, and the arm glides. The solver’s own joint velocity and jerk terms smooth out the rest. Five millimeters every ten milliseconds is also a ceiling of half a meter per second at the end effector. That is quick for an arm next to a person. The same speed scaling rule from the UR3e post applied here. Develop with the pendant’s speed slider turned down.

The static mode plans first and moves second. The same solver runs over the whole path. The solutions get packaged into one joint trajectory and sent to the driver’s follow-joint-trajectory action, with a time factor to stretch or compress it. You lose the real-time servoing glide but gain an inspectable motion (something we consider further in the CoFrame project). I think this is a reasonable trade when a participant is standing next to the arm.

Two details in the real-time code only make sense once you have run it on hardware. The grasp and release primitives pause the solution stream while the gripper moves and resume it after. Otherwise the arm keeps chasing a goal while the gripper closes. The other is a small node called initialize_joints. It waits ten seconds after launch and then sends the UR5 a movej to all six joints at zero over the URScript topic, before anything else is allowed to command the arm. All zeros on a UR5 is the arm standing straight up. That was a mistake. The UR3e version sends a tucked pose instead. It reads better and is clearly less dangerous than swinging a robot arm up to reach as high as possible into the sky. The idea was originally that the robot would raise its arm to signal it was waiting for attention. Great in sim, not on a real bot.

The third planner, the UR controller’s own solver, exists because that servo stream gave me trouble. servoj did not like small steps, there was a lot of lag in the system, and no amount of tuning RelaxedIK fixed it. So I added a direct planner. It hands the UR a pose and lets the controller solve it. That was my fallback when the glide stuttered.

The UR5 launch file defaults to the Python RelaxedIK, with the Julia solver as an option. The Julia port came later. I spent a lot of time waiting on the Python solver. It ran at snail speed. Julia fixed that and brought its own problem, the JIT compile wait I complained about in the UR3e post.

The elbow-up objective

RelaxedIK is an objective function with a list of terms, and the ITER repo carries a modified copy of two of its files. The change I made for this arm is one extra term.

class Elbow_Up_UR5_Obj(Objective):
    def __init__(self, *args): pass
    def isVelObj(self): return False
    def name(self): return 'ElbowUp'

    def __call__(self, x, vars):
        elbow_pt = vars.frames[0][0][3]

        x_val = np.linalg.norm(elbow_pt - np.array([0.,0.,2.]))

        t = 0.0
        d = 2.0
        c = .2
        f = 1.0
        g = 2
        return (-math.e ** ((-(x_val - t) ** d) / (2.0 * c ** 2))) + f * (x_val - t) ** g

It takes the elbow’s position out of the forward kinematics, measures its distance from a point two meters straight above the base, and penalizes that distance. Then it goes into the objective list right between the joint limit term and the collision avoidance network, with a weight of 0.2 against 50 for the position goal. Nothing in the solver says elbow up. Solutions with the elbow high score a little better, and that is enough to keep it there.

It is a soft term. When reaching the goal and keeping the elbow up disagree, the goal wins, and the elbow comes down as far as it needs to. It cannot promise anything, so if a pose is only reachable elbow down, the arm goes elbow down.

Why bother? Elbow up reads better to the person standing next to the arm. It also means less fighting inside the solver against the neural network that monitors for collisions. Physically setting the known objective is much cheaper than letting the solver find it, even though it constrains the solution space. The solver is most comfortable inside a torus within its rough reach sphere anyway, and elbow up keeps the solutions there.

The collision map has a catch. It is not computed in real time, so it assumes the workcell does not move. The table has to be known ahead of time. In a real workcell you might get away with that. In a robotics lab shared with other grad students and undergrad kids, the robot gets jostled and the table gets swapped out.

Where block finding started

Teaching a Robot to Find Blocks describes the vision pipeline as it ended up, on the UR3e. It started inside ITER in the summer of 2019, on the UR5, and moved out to its own repository that fall.

The ITER side of it is a service. A task primitive asks for a block by type and gets back a pose in the frame it names, with or without the calibrated offset applied. My notes from two weeks in July report “still working on getting accurate and precise grasps” and “working on improving accuracy of blocks.” Most of that chase was registration. The solver’s gradient also gets messy near a collision, because the collision term is an approximate neural network weighted in with the other objectives. The calibrated offsets partly account for that, and for the quirks of the gripper, the end effector, and the solver. I hand tuned them. Today I would look into a modern deep network that “just” solves grasping for regular objects like these. Get the end effector close with the global camera, then let a depth camera and visual servoing do the rest.

Lessons Learned

The planner abstraction was worth the effort. It let me cut my teeth on different approaches without rewriting the tasks, and it was handy to have when I moved to the UR3e.

Set the known objective yourself. An elbow-up term is much, much cheaper than letting the solver fight the collision network for the same answer, even though it constrains the solution space.

Registration is most of grasping. Two weeks of chasing accurate grasps came down to registration. Bump the table and you get to do it all again.

A gripper on its own cable is simpler. Serial straight to the gripper can be commanded while the arm moves. Everything through the UR controller has to be coordinated with it, and that was clunky.

Final Thoughts

ITER has been archived owing to ROS (1) being end of life and I having no desire to port it over. That chapter has closed. It ran two studies on the UR3e. It’s still a valuable archive of using RelaxedIK on real hardware.

Thanks for reading. Stay tuned and keep building.

Posted in Grad School Tagged UR5, ROS, Motion Planning, Human-Robot Interaction, Cobots