This robot is out of control!

One of the things we robo-manipulation guys have to deal with is how to design the user control for the robot arm. I'm not talking about the whole user interface, but in particular the controls to make the robot move. If there's some autonomy in there, you have a little bit more flexibility, and you can fairly effectively just use a mouse. But when you just want to allow the operator to teleoperate or "remote control" the arm, things are a little trickier.

Now, just driving a wheeled robot around is not so challenging, because people are accustomed to driving cars, and sometimes even from a remote perspective. With a robot arm, you have to make a control that operates in full 3D, and that can even include 3 axes of rotation. If you want to go really low level, then you need some way to control each joint of the arm.

My first attempt at it was to use two "thumbsticks" on a modern video game controller. The type of control I'm going for is end-effector control, where you move a virtual target point for the robot's gripper to reach. One thumbstick controls motion in one plane, and the other stick controls motion in the remaining axis. This works OK with some training, but a lot of people seemed to still struggle with it. The next attempt was to reduce the control to one stick, and change the way the controls work depending on the view. From a top-down view, the end effector moves in the XY plane (where Z is vertical), and from a side view, the end effector moves in a plane parallel to the Z axis. The tricky part is in views that are neither side or top views. When the view is halfway between those, how should the end effector move? For now I have it somewhat "remember" what control it's in, and you have to go almost all the way to the other view to switch modes. This ends up being rather confusing even for me after practicing for a while. Another option would be to simply move the end effector in the view plane. It's hard to say whether that would be good.

Some other ideas would be to make a non-standard end effector control approach. For instance, left and right on the stick could rotate the base of the arm, and forward and back would extend the arm. Instead of controlling individual joints, however, this mode would still be moving the end effector. There is a difference.

The other day one of the people on my committee offered his Novint Falcon (3D force feedback controller) for controlling the robot arm. At first I thought this was totally the way to go and would solve all of my problems including world hunger. This would mean that I wouldn't have to use two separate joysticks or different modes... with a 3D controller up is up, left is left, and back is back.

Then I remembered the whole view-dependent thing. The trouble is that the interface has head tracking to adjust the view. So it's really easy to adjust the view. It's possibly good and important, but it also makes one wonder whether the controls should always do the same thing, or depend on the view. There's a paper by Jose Macedo called "The Effect of Automated Compensation for Incongruent Axes on Teleoperator Performance" that talks about this, and they basically say that people do better with the automatic compensation (or as I say, view-dependent) control than without.

I think my situation's a little different than theirs, though. They evaluate 2D control, and it's also static. By static I mean that once they have the control axes and the display axes determined, they remain in their particular (mis)alignment for the duration of the experiment. In my case, it's 3D control, and the alignment between axes is dynamic throughout the experiment. So I think it needs to be tested. Perhaps after I get this thesis done.

Second experiment

Because the results from my first study weren't jump out of your seat exciting and significant, we're working toward a second study to get at least a shift-weight-in-chair result.

The most important thing to fix is the alignment between the 3D scan and robot arm. Just last week I managed to fully incorporate the new Videre/SRI STOC camera including calibration, and the alignment is lots better than the SwissRanger. Still, something's slightly off, so I might just cheat a little. I'm thinking of artificially inserting points in the scan for where I know the object to be. This way, the critical points will be aligned perfectly, and I won't have to spend weeks developing methods to get a better alignment. It's really not a perfect solution though, because the robot arm model could still be off, but it does get rid of one point of error.

Another thing that needs doing is smooth arm control. Currently the arm sort of jerks around while moving in a "vroom screech" fashion. Today I worked in a solution that works well for the most part, but it feels hackish and I've noticed a problem that I think is actually due to noisy readings from my servo position feedback, but I never saw it with the jerky control. So of course my adviser comments that I should be using a PD controller for this. But today I equipped my t-shirt of minimum coding effort, so I followed the path I trod earlier to get this thing whipped up quick. And now that I've got a solution mostly working, I know that my adviser's right, and I'll probably have weird problems crop up or a lot of work to remove the quirks unless I make a PD controller (sigh, grumble, sigh). It's sad to throw away code, but sometimes it just has to be done.

That's really all of the hard stuff... there are lots of little things, like improving the subjective user surveys (Likert scale and all of that), but the big stuff is getting to be under control. I can definitely see the study starting up again within a couple weeks (knock knock knock).

Seeing double

We recently got a Videre STOC stereo camera, and I've been working on integrating it with our robot arm setup. It has been somewhat of a challenge, mostly because the API provided by Videre/SRI is in C++, and our interface is in C#. A couple of my labmates mentioned that they had been able to work with C++ dlls before, so I looked into it.

SRI's API unfortunately is not just function calls, they also have custom classes that need to be created as well. So I needed to make a C++ wrapper class that would use the objects as needed to get me the information I wanted, and make everything function-based.

The next hurdle was that I wanted to get some 50,000+ 3D points from the camera, and doing a single GetPoint call for each point would be a massive bottleneck... it would be better to get the entire list of points with one call. Several hours later, I discovered that I didn't create a normal C++ wrapper class, but a C++/CLI project. That means that I have some managed code concepts to use inside C++.

After some research, I discovered that there are basically two main approaches to writing interop code with C# and C++ in windows. The more widely documented way is to do stuff with P/Invoke and marshaling data structures and write a COM dll in vanilla C++. The way I stumbled onto with CLI is to use an extended version of C++ that adds managed structures that can be used directly in C#. Basically it's a .NET-specific dll, but it really reduces the headache associated with getting funny data structures across to C++, at least if you're content with duplicating the contents of memory rather than using the existing C++ memory in-place. There are probably ways to marshal the data better than what I'm doing so far, but I will leave that to the optimization zealots. Here ( http://www.codeproject.com/KB/mcpp/quickcppcli.aspx ) is a link to the resource I used to learn what I needed for the CLI stuff. Very concise quick reference with examples, though I doubt it's complete.


The end result of all of this is that I have a working integration of the new stereo camera into the interface, although there are still a few adjustments left to make.

It looks to me like the objects will be unrecognizable from a viewpoint other than the camera's, but I'm hoping that the position will be accurate. So standard video can be used to identify what objects are, and the 3D model can be used to determine where they are. Of course by the time I'm done with this research, the 3D model will be a perfect (down to the subatomic particles) representation of reality ;)

Servo feedback hacking

One of the big problems with the first run of my user study was that the base rotation joint on the arm had some backlash, so the servo was almost never where it claimed to be.  So I decided to do a little electronics hacking to get actual noisy feedback from the servo.  It's about like the quality of a baby monitor... you can tell that someone's saying something, but you can't quite make out what it is they're saying.  (Ok fine, it's not quite that bad, but anything less extreme just doesn't sound funny).

Anyway, the idea wasn't mine... I used some guidance posted here:
http://www.lynxmotion.net/viewtopic.php?t=3182

Here's a shot of what it looks like:

I struggled in thought for a few minutes about how to get a 2V power source with materials on hand, because our lab isn't really designed for electronics hacking.  After a few minutes I remembered that months ago I fried a servo and the pot wiper was still intact.  So I removed it and wired it up to my trusty servo controller.  I hooked it up to a volt meter and twisted the dial until it read 2V, then followed the rest of the guide and poof! 

I got it all in place, and even added some simple windowed running average software filtering to get rid of the major jitter in the voltages.  For 180 degrees of motion I get about 170 values... so almost-degree resolution isn't so bad.  Unfortunately, I still don't get a good enough alignment between the virtual arm display and the real world.  Something's still amiss.  I decided to wait until our stereo camera comes in (just a few weeks now) to do more work in this space of things.

Slight improvement

Instead of discarding what I've already done I decided to try getting a better calibration with linear fitting. The way it works right now is I've mounted a target on the robot arm, and using forward kinematics I know where the target is located in the robot arm's coordinate system. From the SR-3000 I can find the target (currently done by human), so I have correlated points from two coordinate systems. All that's left is to linear least-squares solve to find the transformation between the two.

Today I added some calibration targets on the ground plane (a wooden board) that correspond to a height of zero. The reason for this is that yesterday, targets low to the ground were very poorly aligned (2-3 cm off). I thought one reason might be because the target was mounted somewhat high on the robot arm, so any skew happening down low was missed.

The end result is that it works somewhat better. It's still off by an average of 0.5 cm I'd say, but it appears to be consistent. With the "guess and check" method I could align it for one setup where two blocks were about 0.2 cm off, but the third would be 2 cm off. When I changed the setup, everything ended up being off by 1 cm or more. I think if I sample just the right space, and then keep all items of interest in that space, I may not have to worry about nonlinear distortion so much. After all, everything's linear in the limit, right? :)

M(is)aligned decomposition

So I finished coding up the 3D camera calibration routines using Singular Value Decomposition and the like, and the results were better than I expected for a first attempt, but they more or less still stink. I think the problems come from three things: nonlinear distortions, inadequate sample space, and backlash in the robot arm.

I really do think the SR-3000 has some nonlinear distortion to it. It's not extreme, but it is there. So I should learn how to account for nonlinearity and calibrate accordingly. Now don't get me wrong, the SR-3000 is a pretty nice sensor, especially considering its size, power requirements, and illumination independence. It's just not quite good enough for the precision I'm looking for. If the scan is misaligned by 0.5cm in any direction, the task is extremely difficult to do.

The inadequate sample space comes from the calibration target being mounted in a place on the arm that cannot go below about 10cm (where the highest reach of the arm is 40cm). The result appears to be that objects sitting at around 15cm are aligned perfectly, while objects below 10cm are typically off by about 1 cm. Of course, this is after playing with it for only half an hour. One solution to this may be to mount the target in/on the gripper instead of the "wrist". Another possibility would be to put a few targets on the ground plane, although that could introduce some discrepancies between my robot arm forward kinematics model and the real world. In other words, where the arm really is and where it thinks it is.

Arm backlash is probably more important than I would like it to be. The base joint basically doesn't rotate for about 2-5 degrees when you change directions. I tried to hack a software solution together, but it's not good enough. To fix that I think I'll have to modify the hobbyist servos to give noisy analog position feedback, then run it through a filter to get the actual position. Not so difficult, but it takes time. Probably a better solution would be to put digital encoders on there, but while my servo controller has analog inputs, it has no digital inputs. That means I would have to get into AVR programming. While I would love to... it all comes back to time.

Gripper feedback

As I watched participants in the pilot study, the dependence on cameras is still very high. The system as it was was not developed enough to support a camera-less operation. In particular, the gripper had no feedback on whether it was actually closed all the way or just partway. The virtual display was only able to show the commanded state of the gripper, as that is all the servo controller (and in fact the servos themselves) give.

The result of this lack of feedback is that the operator must depend on other sources to examine the state of the system. One of the experiment parameters I had set was to use the 3d model by itself and a birds-eye-view camera. The up-close side-view camera was disabled. So the only way someone could check to see if they successfully grabbed an object was to lift it up until the gripper was in the camera view and see if the object was there. More often than not, they missed, and so had to try again.

As a result, I found a relatively simple way to get position feedback from these hobbyist servos. Now when the gripper misses an object, it shows up completely closed, and when it successfully grasps an object, it shows up halfway open. I think this will help quite a bit.