Using the WAM as a Master Controller Renbin Zhou
[email protected]
William R. Hamel A. S. Hariharan
[email protected] [email protected] 414 Dougherty Engineering Building, The University of Tennessee, Knoxville Knoxville, TN 37996-2210
Mark W. Noakes*
[email protected] *Oak Ridge National Lab One Bethel Kelly Rd. Oak Ridge, TN 37831-6305
Abstract- This paper presents technical issues related to a specific master-slave manipulator teleoperation system and its position control in the Cartesian space. The master is a whole arm manipulator-WAM™, which is a seven degrees-offreedom (7-D) manipulator by the Barrett Technology®, Inc. The slave is a Titan II 6-D manipulator by the Schilling Robotics® LLC. A distributed real-time control system involving three computers is used for the system. The communication among the computers is via a dedicated local area network (LAN). A simulation framework is also presented for system development and test. The Cartesian space position control is achieved by using inverse velocity mapping method. A heuristic technique based on singular value composition (SVD) is presented to deal with the Titan II singularity problems.
I. INTRODUCTION
II. A SYSTEM FRAMEWORK
Teleoperation is widely used in human adverse environments like nuclear waste remediation and space exploration. Teleoperation usually involves a master and a slave device. One of the commonly used teleoperation system is the one that features a master device that is suitable for joint space control, for example, a miniature of the slave. In this case, the operator will depend on video feedback in order to have a good estimate of slave position.
The proposed teleoperation system is part of dual arm teleoperation platform of the University of Tennessee, Knoxville and the Oak Ridge National Laboratory (ORNL) .
Commonly used teleoperation systems generally have one control computer, which hosts both master and slave control programs. This configuration is convenient to communicate between the two programs, for example, by using inter-process calls. In this paper, a teleoperation system is proposed by using a 7-D WAM to control a 6-D TITAN II. Due to the kinematic dissimilarity between the two, Cartesian space control must be used. Due to the fact that the master control and slave control program are running in different operating systems, a new distributed control system framework is used. This paper is organized as: In section II, the system framework will be introduced, including both the teleoperation system and the simulation system. In section III, the Cartesian space mapping based on an analytical solution will be discussed, and simulation results will be analyzed. In section IV, results based on experiments will be analyzed. Section V will outline future research.
It is a real time distributed control and simulation system which has five PCs connected on a LAN. Figure 1 shows the system hardware configuration. PC-104 QNX Titan II
Server PC Server PC Linux Linux Mapping
Develop PC QNX
Simulation PC Windows 2K Roboworks
WAM PC Linux/RTAI
Figure 1. A teleoperation system framework There are two paths in this system. The first path includes a WAM PC, a server PC, a PC-104, and a development PC. The first three PCs in this path are the actual teleoperation system. The development PC is used to develop Titan II control software and download it to the PC-104 through a Qnet™, which stands for QNX® 1 networking. The second path includes the WAM PC, the server PC, and the simulation PC; this path is used for the system design and test. All five computers are connected by a dedicated LAN. The WAM PC hosts the low level control software. It also has an Ethernet client module, which sends WAM 1
QNX is provided by QNX Software Systems , a leading provider of superior real-time operating system software.
joint angles at 20 HZ to the server PC. Communication between low level control program and Ethernet module is by a real time FIFO [1]. The entire program in the WAM PC is developed under Linux/RTAI, RTAI stands for Real time application Interface, which is hard real time Linux kernel extension founded by department of aerospace engineering of Politecnico di Milano (DIAPM). The low level WAM control loop rate is 500Hz. The PC-104 hosts the low level control program for titan II. It also has an Ethernet client module; communication between the two is by shared memory and semaphores. The Titan II low level control software is developed under QNX and its control loop update rate is 200 Hz. The server PC receives joint angles from the WAM PC and the PC-104, executes the high level position mapping, and generates the Titan II joint angle command, which is then sent to the PC-104 via Ethernet. The server PC is a Redhat Linux PC. The Simulation PC has a commercial software package, Roboworks™, for robot simulation under Windows operating system. It has a TCP/IP module to receive input via Ethernet. As will be discussed in section III, in order to have precise velocity mapping, the difference between the clock cycles in the PC-104 and the WAM PC must be sufficiently small, which is guaranteed by using real-time operating systems (RTOS). The teleoperation loop update rate is 20 Hz, which is much lower than the bandwidth of a dedicated LAN, so time delay is not an issue in this project.
III. A MAPPING METHOD For a 6-D Titan II manipulator, the Jocobian is a 6 by 6 square matrix, and mapping of 7-D master to a 6-D slave manipulator can be solved analytically. But in practice, techniques must be adopted to avoid uncontrollable slave movements that can result from numerical problems when the Titan II approaches a singularity configuration. Generally speaking, a teleoperation system can have two fundamental control schemes: open loop and close loop [2]. An open loop scheme includes a forward kinematic (FK) routine and followed by an inverse kinematic (IK) routine. Four steps are involved in this scheme, and its general procedure is outlined in Figure 2. The FK routine calculates the Cartesian space velocities from the measurement of the master joint angles; and the Cartesian space velocities are then used as command inputs to the slave IK routine, which produces the joint velocities of slave; the joint angles are finally used as slave actuator command inputs.
(1) v w a m j n t =
∆ θ wamjont ∆t
( 2 ) v car = J w a m v w a m j n t +
( 3 ) v titanjnt = J titian v car ( 4 ) ∆ θ titanjnt = v titanjnt g ∆t
Where, v v
car
-cartesian space velocity
wamjnt
, ∆θ
wamjnt
, J
wam
- wam joint velocity, angle, jacobian
+
vtitanjnt , ∆ θ titanjnt , J titan - titan joint velocity,angle,pseudo-inverse jacobian ∆ t - time step
Figure 2. Open loop Cartesian space mapping Step (1) and step (4) both have a time increment, and the two time steps must be the same in order to get exact tracking. In this system, the timers that keep track of the time steps are in two different computers, and thus make it very critical to have RTOS on the WAM PC and the PC104. The clock difference between the two timers under RTOS can be neglected, since the time delay of RTOS is in the magnitude of several microseconds, which, together with the LAN time delays, will introduce some tracking error in the system. The effects of those errors are fairly small [3, 4] and outside of the focus this paper. A close loop scheme is formed by feeding back the actual Cartesian positions of the slave. The purpose of having feedback is to reduce the tracking errors and stabilize the system [2]. Under these two schemes, various control methods have been studied in the past. The common objective of these methods is to try to attain good performance in event of redundancy or singularity, and in these cases , the Jacobian matrices are rank deficient. One of the commonly used methods was the damped least square (DSL) inverse proposed by Y. Nakamura and H. Hanafusa [5], which augments the Jacobian to a full ranked matrix when it is rank deficient. It is reported in [6] that the DSL has poor performance near the singularity. Optimization-based algorithms can be found in [6, 7], these methods generally use some kind of weighting matrix, and the selection of the weighting matrix is based on heuristic tuning method. It is also reported that the optimization based algorithm could be 20 times slower than the analytical solution [6]. This paper studied a new technique based on open loop scheme analytical solution, which has good performance near the singularities. As mentioned earlier, the Jacobian of the Titan II is a square matrix, and the analytical inverse Jacobian always
exists. By theory, the IK problem has close form analytical solution. In reality, problems like abrupt joint angle jumps due to numerical problems can happen when Titan II is near its singular point. For example, when joints 2 through 5 are lined up, the Titan II can not moves further in the direction, which is generally referred to degree-of-freedom loss. If a Cartesian space velocity command as shown in Equation (1) is used, the Titan II moves to its singular point quickly with its initial configuration shown in Figure 3.
{vx , vy , vz , ω y , ωp , ωr } = {2,3,4,0,0,0} v x , vy , vz −
(1)
velocity in x, y, and z axis (inch/sec)
ω y , ω p , ωr − angular velocity about x, y, and z axis (rad/sec)
variation in the command input will be disproportionately amplified by the inverse calculation, thus causing abrupt changes in the output joint angular velocities. This type of situation can be avoided by setting a heuristic threshold for the condition number, and if a small singular value goes below the threshold it is set to zero before the inverse routine. However this is not exactly what occurred in the simulation because the input commands are constant. Figure 5 plots the ratios of the largest singular value to each single singular value. First observation is that the condition number has abrupt changes . Further examination of Figure 4 and Figure 5 shows that the joint velocity jumps happen at the exact locations where the condition number is undergoing large changes. Condition number vs. time 1.1
z
2 1.5
1.08
1 1.06 1.04 0
Figure 3. Initial configuration of Titan II When Titan II is approaching the singularity, the simulation result shows the joint angles jump abruptly between adjacent time steps (0.05 seconds). This type of behavior will cause actuator saturation and thus should be avoided. Figure 4 shows the Titan II simulation result of joint velocit ies. The position tracking is lost due to the singularity, which is shown in Figure 6. Joint Velocity vs. time 3
Condition Number
x
0.5 1
2
3
4
0
5
2000
400
1500
300
1000
200
500 0 0
0
1
2
3
4
5
0
1
2
3
4
5
0
1
2
3
4
5
100 1
2
3
4
0
5
4000
6000
3000 4000 2000 2000
1000 0 0
1
2
3
4
0
5
time(s)
Figure 5. Ratio between largest and each singular value of Titan II Jacobian matrices
4000
2.8
position error vs. time
2000 2
2.6
x y z
0
2.4
0
0
1
2
3
4
-2000 0
5
1
2
3
4
5
4000
-2
2000
0
0 -5000 -10000
-2000 0
1
2
3
4
-4000 0
5
-2.2
1
-2.4
0.5
-2.6
0
-2.8
-0.5
-3
0
1
2
3
4
-1
5
1
2
3
4
5
Position Error(inch)
Joint Velocity (rad/sec)
2.2 5000
-4
-6
-8
-10
-12
0
1
2
3
4
5
time(s) -14
Figure 4. Titan II joint velocities From Figure 4, the Titan II joint velocity numerical results undergo large jump s under the constant Cartesian speed command. It is commonly known that when using the procedure step (2) shown in Figure 2 to solve IK problem, the Jacobian matrix must have a sufficiently small condition number, which is defined as the ratio between the largest and the smallest singular value; Otherwise, any small
0
0.5
1
1.5
2
2.5 time(s)
3
3.5
4
4.5
5
Figure 6. Titan II position error in x, y and z direction The explanation of this situation is that because of the abrupt condition number change between adjacent Jacobian matrices, especially when a large condition number is involved, some command velocities are disproportionately amplified by the inverse calculation, and thus makes very abrupt output speed changes.
Same heuristic technique can be used in this case. For example, by setting the condition number threshold to 15, and keeping everything else unchanged, a new simulation result shows a much smoother joint velocity trajectory. The results are shown in Figure 7 and Figure 8. Joint Velocity vs. time 2.5
3
2.4
2.8
2.3
2.6
2.2
Joint Velocity (rad/sec)
2.1
2.4 0
1
2
3
4
2.2
5
1.5
0
1
2
3
4
5
0.7
1.4
All errors are in similar magnitude before and after a condition number threshold is used. When the Titan II is moving away from a singular point, the performance of the analytical solution is very good without setting a condition number threshold. For example, when changing the command in Equation (1) to {-2, 3, 4, 0, 0, 0}, and the Titan II starts from the same initial configuration. The titan II will move away from the singular point. Numerical simulation results are shown in Figure 10 and Figure 11.
0.6
1.3
Joint Velocity vs. time 0.5
1.2 1.1
0
1
2
3
4
0.4
5
4 0
1
2
3
4
5
x 10
4
0
3.5
-4
0.28
10
-10 3
0.26
-20
2 0.24
0
1
2
3
4
0
5
0
1
2
3
4
5
Joint Velocity (rad/sec)
0.22
2.5
time(s)
Figure 7. Titan II joint velocities Condition number vs. time
1.5
1.06
0.5
1
2
3
4
-30
5
100
0
50
-20
0
-40
-50
2 1.08
0
0
1
2
3
4
-60
5
-2.5
1
2
3
4
4
5
0
1
2
3
4
5
2
3
4
5
-3.5
1
2
3
4
5
-0.5
100
-4
80 50
0
1
2
3
4
60
-1
5
0
time(s)
40 0
0
1
2
3
4
20 0
5
150
150
100
100
50
0
1
2
3
4
5
1
2
3
4
5
Figure 10. Titan II joint velocities position error vs. time 0
50 0 time(s)
1
2
3
4
x y z
5
-0.005
Figure 8. Ratio between largest and every single singular value of Titan II J acobian matrix position error vs. time 2 x y z
0
-0.01
-0.015
-0.02
-0.025
-2
Position Error(inch)
3
0 1
Position Error(inch)
Condition Number
100
2
0.5
-3
0 0
5
1
1
1
0
0
-4
-0.03 -6
-0.035 0
-8
0.5
1
1.5
2
2.5 time(s)
3
3.5
4
4.5
5
-10
-12
-14
0
0.5
1
1.5
2
2.5 time(s)
3
3.5
4
4.5
5
Figure 9. Titan II position error in x, y, and z axis Comparing the position errors as shown in Figure 6 and Figure 9, the errors in the x direction for both cases are very large, which is because Titan II is near singularity, while in the y and z directions, the errors are relative small.
Figure 11. Titan II position error in x, y, and z axis The joint velocities are smooth and tracking errors are small. Notice that the condition number threshold is not set. Based on the above analysis , the conclusion is that by setting an appropriate condition number threshold when the Titan II is moving into a region near a singularity point, the analytical IK solution can perform well near the singularity.
IV. EXPERIMENTAL RESULTS
0 -200 -400
0
1
2
3
4
5
6
7
8
9
10
0
1
2
3
4
5
6
7
8
9
10
0
1
2
3
4
5 time(s)
6
7
8
9
10
8 position (mm)
Currently, the real system experiment results are not completed; and in this experiment, the Titan II result is demonstrated by a model simulation in the Roboworks. The experimental results are obtained as follows: The operator moves the WAM, and the WAM joint angle values are sent to the server PC at 20 Hz for FK and IK procedure, which calculated the Titan II joint angle command, and this command is sent to Roboworks Titan II model on the simulation PC for a visual demonstration. The experiment results are also plotted using Matlab®. The WAM initial position and base frame can be shown in Figure 12 (left), and the initial position and base frame of Titan II is shown in Figure 12 (right).
Cartesian position vs. time 200
6 4 2 0
150 100 50 0 -50
z x
Figure 13. Titan II position in x, y, and z axis z
position error vs. time 20
x
15
Figure 12. WAM &Titan initial position and base frame
In the first test, the condition number threshold is not set; while in the second, the condition number threshold is set to be 10 when the titan is moving towards a singular point, 50 while it is moving away from the singular point, when Titan II is far away from the singularity, the threshold is not set. The results of the first test are shown in Figure 13 through Figure 16. The results of second tests are shown in Figure 17 through Figure 20. The selection of condition number threshold depends on the geometric construction of the manipulator; in this paper, an operational space condition number map was used to help determine an appropriate threshold, other than that, it is basically a trial and error method.
10
Position Error(mm)
5
0
-5
-10
-15
-20
0
1
2
3
4
5 time(s)
6
7
8
9
10
Figure 14. Titan II position error in x, y, and z axis Cartesian space velocity error vs. time 1
0
-1 Velocity Error (mm/sec)
Two WAM trajectories were tested. The first one is to open the elbow and then close it back to the initial position. The second one is to lift the shoulder and open the elbow simultaneously, and then move them back to the initial position. The purpose of the first test is to see the performance when the Titan II is far away from the singularity, while the second one it to check the performance wh ile the Titan II is near the singularity.
x y z
-2
-3
-4
-5
v
x
vy vz -6
0
1
2
3
4
5 time(s)
6
7
8
9
10
Figure 15. Titan II velocity errors in x, y and z axis
Joint Velocity vs. time 1
40
0.5
20
0
0
Cartesian space velocity error vs. time 100
0
-100
-0.5 0
2
4
6
8
10
-20
12
0
2
4
6
8
10
12
5
Velocity Error (mm/sec)
Joint Velocity (deg/sec)
-200
50
0 0 -5 -50
-10 0
2
4
6
8
10
12
0
0.15
1
0.1
0.5
2
4
6
8
10
12
-300
-400
-500 vx vy vz
-600
0.05
0
0
-0.5
-700
-0.05 0
2
4
6
8
10
-1
12
0
2
4
6
8
10
-800
12
0
1
2
3
4
5 time(s)
time(s)
Figure 16. Titan II joint velocities
6
7
8
9
10
Figure 19. Titan II velocity errors in x, y and z axis Joint Velocity vs. time 50
10 Cartesian position vs. time 1000
5 0
500
0 0
-5
0
1
2
3
4
5
6
7
8
9
10
position (mm)
40 20 0 -20
0
0
1
2
3
4
5
6
7
8
9
10
-100
-1
6
7
8
9
10
Figure 17. Titan II position in x, y, and z axis position error vs. time
6
8
10
4
6
8
0
2
4
6
8
10
-0.5 0
2
4
6
8
10
10
0.5
0
0
2
4
6
8
10 time(s)
Figure 20. Titan II joint Velocities Figure 14 and Figure 15 show that good position and velocity tracking are achieved when the Titan II is far away from the singularity points. Figure 18 and Figure 19 show that when the Titan II moves into a region near the singular point, before 4 seconds, position and velocity tracking are lost. When it moves out of this region, after 4.5 seconds, position and velocity tracking are achieved approximately. In all cases, the Titan II joint movements are smooth and the Titan II is in a controllable state even it is close to a singular point.
700 x y z
600
4
1
-0.5
5 time(s)
2
-20 2
1
0 4
0
0
200
3
-50
10
-10
0
2
8
0
0.5
1
6
-50
400
0
4
10
600
-200
2
50
0
-40
500
Position Error(mm)
Joint Velocity (deg/sec)
-500 -1000
400
300
200
100
0
-100
V. FUTURE WORK 0
1
2
3
4
5 time(s)
6
7
8
9
10
Figure 18. Titan II position error in x, y, and z axis
Interests in future work are: 1) Complete the system implementation; 2) Further develop an intelligent routine to chose an appropriate condition number; 3) Attack some of the control issues associated with the hydraulic actuators of Titan II;
4) 5) 6)
Develop and implement a close loop control scheme; Study redundancy allocation strategies with the 7D WAM; Study the haptic implementation and its effect on teleoperation.
ACKNOWLEDGEMENTS This research was sponsored by DOE under Grant No. DE-FG02-02ER83371. The authors would like to thanks ORNL and Barrett Technology for providing the hardware and initial control software, and QNX for provide the QNX 6.3 Neutrino professional license through its education program.
REFERENCES 1. 2.
3.
4.
5.
6.
7.
Mantegazza, P., DIAPM RTAI programming guide 1.0. 2000, Lineo, Inc.: Lindon, Utah. Sciavicco, L. and B. Sicilianno, Modeling and control of robot manipulators. 1996, New York: McGraw-Hill. Ip, B. Performance analysis of VxWorks and RTLinux. in the First Embedded Software Contest. 2003. Korea. Proctor, F.M. and W.P. Shackleford. Real time operating system timing jitter and its impact on motor control. in SPIE Conference on Sensors and Controls for Intelligent Manufacturing. 2001. Wuhan, China: SPIE. Nakamura, Y. and H. Hanafusa, Inverse kinematic solutions with singularity robustness for robot manipulator. Journal of Dynamic systems, measurement, and control, 1986(108): p. 163-171. Schreiber, G., M. Otter, and G. Hirzinger. Solving the singularity problem of non-redundant manipulators by constraint optimization. in IEEE, Conference on Intelligent Robots and Systems. 1999. Kyongju, Korea. English, J.D. and A.A. Maciejewski, On the implementation of velocity control for kinematically redundant manipulators. IEEE transactions on systems, man, and cybernetics, 2000. 30(3): p. 233-237.
