ACE Project Report
May 30, 2008
Reid Simmons, Sanjiv Singh,
Bradley Hamner, Seth Koterba
Robotics Institute
CarnegieMellonUniversity
1. Introduction
This report covers the period from December 11, 2007 through May 30, 2008. During that time, we refined our method for carrying out the insertion task and extended it to include force feedback and force control. In addition, we developed, integrated and tested new mobile manipulators (based on a PowerBot base and WAM arm, Figure 1), delivered one of the robots to GM, and held a several-day tutorial on use of the robot and the software architecture. The task-refinement and extensions to use force will be described in more detail in the subsequent sections (the work on integrating the mobile manipulator is not included in this report).
Figure 1. The mobile manipulator developed by the ACE project consists of a PowerBot Base with a 7-DOF WAM arm. At the end of the arm is a spring-loaded gripper to hold a plug and cable harness. Visual sensing is provided by a Bumblebee stereo camera pair, mounted on a Directed Perception pan-tilt unit.2.Refinements to the Insertion Task
Since our last report we have made significant advances in improving the adaptability of the system, both to initial conditions and errors in execution. In the December 2007 demonstration, most of the moves were pre-scripted. The base started in a known position and moved up a fixed distance to approach the task board, ensuring that the mobile manipulator would be in range to successfully complete the plug insertion. In the current system (see Figure 2), the robot first finds the fiducial on the task board, estimates its position relative to the task board, and then moves to a fixed offset from the task board and hole. Given this change, our current insertion scenario can start with the base up to 2.5 meters away from the task board, over a reasonably wide range of angles. Delineating the starting conditions more precisely will be the subject of our upcoming deliverable.
Because the robot may start in different positions, relative to the fiducial, we have added a behavior that pans the cameras to search for the fiducial, if it is not initially in view (and continues to pan if the fiducial subsequently goes out of view as the robot moves). Also, previously after an insertion attempt, the system asked the user whether the plug had been successfully inserted. The current system uses force sensing to determine that information autonomously – if a large force in the Z direction is detected as the arm pulls back, that indicates that the plug has been pulled from the gripper and has, thus, been successfully inserted; If no large forces are detected as the arm pulls back about 10 cm, the system infers that it is still holding the plug, and thus insertion has failed.
We have also modified (by hand) the waypoint locations and tolerances, to improve performance (both speed and reliability). These locations and tolerances were chosen through trial-and-error, in an attempt to get reliable performance without too much time taken to servo exactly. Again, our work in the coming months will include refining these further, based on the results described in Section 3.
Probably the most significant modification was to the WAM controller. The code that came with the arm had moves that could not be smoothly interrupted (the arm came to an abrupt halt if a new move was received before the current one was finished). Thus, if we wanted to interrupt moves, the overall motion was very jerky; if we waited until moves finished before sending new commands, the motion was smoother, but visual servoing was very slow. We have since replaced the arm controller with code that has the performance we desire. We first had to modify the code to use a different CAN bus library, since the existing code was not compatible with our WAM hardware. We then extended the code in significant ways to provide for coordinated joint velocity control. Each joint follows a trapezoidal trajectory, but we have ensured that the transition time between the previous velocity and the commanded velocity is the same for all joints. This prevents any one joint from moving too fast or too slow relative to the other joints. The trajectory for each joint is broken up into a series of short, position-controlled waypoints for the low level PID controllers to execute. This change produced the smooth and interruptible motion that we needed for our work in force control. Finally, we added functionality to the controller code to do force sensing and force feedback control. These modifications were all essential to the success of the work described in the next section.
Figure 2.. Current task tree for the insertion task. Black arrows indicate a subtask spawned by a parent task. Blue arrows indicate that one task must occur after the previous task has finished. Dotted arrows indicate tasks that are spawned conditionally. If the “Insert Plug” task fails, the “Try Plug Insertion” task spawns another “Try Plug Insertion” task, unless the “Insert Plug” task as failed for the third time. If an “Insert Plug” task succeeds then the tree proceeds to the “Stow Arm” task.3.Extensions to use Force Feedback
Humans make great use of force feedback in tight-tolerance insertion tasks. We would like our robots to behave similarly, by augmenting visual servoing with force control strategies. Three questions are (1) how to measure and interpret forces, (2) how to do force control, and (3) how to combine visual servoing with force control.
Force measurement is an integral part of the WAM arm. When a joint of the manipulator arm is commanded to a new position, it is sent a torque. We have access to the commanded torques through the WAM arm’s control software. By multiplying these torques through the arm’s Jacobian (a matrix specifying how movement of the joints affect movements of the end-effector), our system can translate these torques into forces on the end-of-arm-tool (EOAT). This includes forces along the three axes (X, Y, Z) as well as moments (rotational forces) around those axes.
In open air situations, as the EOAT is being commanded to different points, only relatively small forces will be commanded, which are those required to move the end effector from one point to the next. If an obstacle impedes the progress of the EOAT, the control software will send larger torques to the joints in an attempt to get the EOAT to maintain the planned trajectory, and thus the system will detect noticeably larger forces. For example, the graph in Figure 3 is from a trial in which the system is told to hold the EOAT in one position as we manually push it in different directions. When the EOAT is steady, the calculated forces are near zero. As it is pushed in a direction, the system detects large forces in that direction.
Using the force data, we can control the position of the EOAT in order to achieve a desired force on the EOAT. This method is known as force control [Siciliano, 2000]. We have implemented force control using a simple proportional controller. The controller operates with a desired force in one direction relative to the orientation of the EOAT. If the instantaneous force in that direction is less than the desired force, the controller moves the EOAT in that direction. If the instantaneous force is too great, the controller moves the EOAT in the opposite direction.
Figure 3.Pushing and twisting the plug while the control program holds it steady. First, we pushed in the X (down) direction, then Y (left), then Z (forward) directions. Then we twisted about the Y-axis, then X, then Z. In the top plot, the pushes are all reflected appropriately in the force data. In the bottom plot, the moment data does not reflect the twists being applied.Figure 3 also shows a plot of the moments, which are the rotational forces or the amount of twisting applied to the EOAT. We had hoped to be able to use the moments to detect when the EOAT contacts the task board at a skewed angle and to correct for that angle using a moment controller, similar to the force controller. However, as shown in Figure 3, the moment data is very noisy, and it is impossible to tell from that data alone how the EOAT is being twisted. We are looking into ways to filter this data to get some useful information from it.
Using Force Control to Insert a Plug
Without detailed prior information about the location of the task board in relation to the mobile manipulator, inserting the plug is a difficult problem. The system has to find the task board, then find the target hole, and then insert the plug. In previous work, the insertion was done entirely by visual servoing. In this reporting period, however, we have developed an approach to do the insertion task by using the force controller and other force feedback data. See Figure 4 for an illustration of the current architectural approach.
The first step in the force insertion task is to have the EOAT touch the task board. This is a relatively simple matter. Assuming that the plug is pointed roughly perpendicular to the task board, the system commands the EOAT to move forward (which is its +Z direction) until the system detects a large force in the negative Z direction (Figure 5a). This force means something, presumably the task board, is pushing against the EOAT.
(a)(b)(c)
Figure 5. Illustration of the force insertion task. (a) The plug is commanded to move forward until the system detects a large force in the direction of motion, indicating the task board has been hit. (b) The plug is commanded in a spiral while maintaining a constant force against the task board. The plug slips in and catches when it crosses the target hole. (c) When the plug has caught, the spiral stops and the plug is moved forward again, fully inserting it into the target hole.
Once the task board has been found, the system needs to find the target hole. We designed a force-controlled spiral move to accomplish this (Figure 5b). Again, assuming the plug is pointed roughly perpendicular to the task board, the system commands the EOAT to spiral outwards from the point it hit the task board. The spiral is controlled positionally in the X (down)-Y (right) plane of the EOAT. The system ensures that the plug maintains contact with the task board by using a force controller in the Z direction of the EOAT (into the task board). When the plug crosses the target hole, the force in Z drops because it is no longer pressing against the task board, and the force controller pushes the plug slightly into the hole. With the plug caught in the hole, the spiral is no longer maintained. The system reads a large force in the plane of the spiral (the X and Y directions). So, when the system detects such large forces, it assumes it has found the hole.
When the hole has been found, the system then pushes the plug in (Figure 5c). Pushing-in is done in a manner very similar to the find-task-board step, except that the plug has some resistance with the task board. Therefore the system has to push in even when some large forces are detected, until it detects a very large force, meaning the plug is fully inserted and has bottomed out.
Finally, the EOAT is commanded to pull away from the task board. Our system uses the Z forces to determine whether the plug has actually been inserted. Our EOAT holds the cable harness with a spring-loaded gripper. When the plug has been inserted and the arm pulls back, the cable harness pulls out of the gripper, and the system detects a large force. When the plug has not been inserted and the EOAT pulls back, the system detects no such force. Therefore, the system can use the presence of that large force to determine whether the plug has been inserted into the task board and pulled out of the gripper. This step has proven to be extremely reliable.
There are many limits and tolerances in this force insertion scheme, including the following: the Z force limit used to determine when the task board has been found, the desired Z force to maintain while executing the spiral, the X/Y force limits to indicate that the hole has been found, and the Z force limit to indicate the plug has been fully inserted. All of these values were determined through inspection and trial and error. By looking at plots of the X-Y-Z force on the EOAT during these steps, we were able to arrive at what appear to be good values for the parameters. While testing, we adjusted the threshold limits by small amounts to improve performance. See Appendix A for a detailed list of the limits and tolerances.
Combining Visual Servoing and Force Control
Currently, our force insertion task assumes that the plug is roughly aligned with the task board before the task begins. The best method that we have for this alignment is the visual servo, where the system commands the EOAT to a designated pose relative to the task board by observing fiducials on the task board and the mobile manipulator’s wrist. The closer the EOAT is aligned with the hole when the force insertion begins, in general the faster and more reliable is the force insertion. However, the visual servo task can slow down the scenario, especially if the waypoint tolerances are too tight. Thus, we have a tradeoff involving the amount of visual servoing to do before switching to force insertion.
We designed and implemented three docking schemes that differ in how they combine visual servoing and force control. The first scheme (Figure 6a) uses only visual servo (abbreviated VS in the results below). It is our baseline method. The scheme commands the mobile manipulator’s wrist to a rough waypoint 7 cm from the task board, using both the base and the arm of the mobile manipulator. This waypoint has a tolerance of 3 cm. Then it commands the plug to a point 2 cm away from the hole with a tolerance of 1 cm. Finally, it attempts to insert the plug by using a visual servo waypoint that is inside the hole, also with a tolerance of 1 cm. If at any point the system detects a large force on the EOAT, meaning that the plug has been inserted or hit the task board, it stops the visual servo and begins a back-up move. After the back-up move, the system checks whether the plug has been inserted (for historical reasons, this scheme asks for user to input whether the plug has been inserted; in the other two docking schemes described below, this check is automated, based on observed forces – if a large Z force is seen, that indicates that the plug has pulled out of the EOAT, which means it has been successfully inserted).
As with the force limits, the waypoint positions and tolerances were determined through trial and error. We needed waypoints that were close enough to the task board so that subsequent visual servoing would have a good starting point, yet far enough from the task board such that the manipulator would not be likely to hit the task board by accident (if the position estimation were off). Similarly, the tolerances were chosen so that the manipulator was roughly lined up at each waypoint, with the tolerances getting tighter as the EOAT approaches the hole. If the tolerances are too loose, the plug often misses the hole; if too tight (especially near the limits of the resolution of the fiducial-finding software), the system spends lots of time servoing unnecessarily. We intend to characterize the effects of different choices of waypoints and tolerances in the coming months (see Appendix A for more details).
(a)(b)(c)
Figure 6.Illustrations of the three docking schemes. Purple arrows indicate a visual servo. Green arrows indicate force insertion. Gold boxes represent waypoints with the tolerance for each. (a) The VS scheme. A rough visual servo followed by two precise visual servos. (b) The FI scheme. A rough visual servo followed by a force insertion task. (c) The VS-FI scheme. A rough visual servo followed by two precise visual servos, followed by a force insertion task. In the timing results below, the clock starts with the completion of the first arrow.
The second docking scheme (abbreviated FI) uses visual servo only for a rough alignment, then performs the force insertion task. See Figure 6b for an illustration. The visual servo task’s rough waypoint is the same as that in the first scheme, with the same 3 cm tolerance. The force insertion proceeds in a spiral search, as described in the previous section. This scheme uses the autonomous plug-docked check, rather than asking for user input.