Compliance Control of Tele-Robot Teja Swaroop Tumapala, Surendra Singh Saini, Ushnish Sarkar, Debashish Datta Ray

Division of Remote Handling & Robotics Bhabha Atomic Research Centre

Mumbai, India 400 085 E-mail: {tejswrp, sainiss, ushinish, dray} @ barc.gov.in

ABSTRACT Compliance control is essential for tasks involving interaction of the robot with the environment with contact forces e.g. sliding on an inclined plane, drawing on a rigid surface, opening the lid of a box etc. Pure position control is not sufficient for such tasks since a small variation in relative position at the contact surface may generate large contact forces which may damage the environment or manipulator itself. Compliant motion may be produced either by passive mechanical compliance built in to the manipulator, or by an active compliance implemented in the control servo loop. The second method, involving position and force control of the manipulator end-effector in mutually orthogonal directions, has been the focus of this work. In this work “Compliance Control Algorithm” module has been developed which takes the task as the input and generate Position Path and Force Profile. These aforementioned paths and profiles are further given to a Hybrid Controller to achieve compliant motion. The algorithm has been tested by developing a code for the Compliance algorithm and using a PUMA560® dynamic model in MATLAB®. Selected tasks have been performed in simulation to evaluate the performance of the algorithm. Subsequently, the algorithm has also been implemented on Tele–Robot, developed by DRHR, BARC. The results obtained from the Tele-robot system indicate the successful completion of some planned tasks which required simultaneous position and force-control. The results show that the method achieves stable and accurate control of force and position trajectories for a variety of test conditions.

Categories and Subject Descriptors I.2.9 [Robotics]: Manipulators

General Terms Algorithms, Experimentation, Verification.

Keywords Compliance Control; Compliant Motion; Hybrid Position/Force Control; Tele-Robot.

1. INTRODUCTION Compliant motion tasks involve interaction forces between the tool and the work-piece and demand certain amounts of compliance at either the tool or at the work-piece. When a

manipulator is operating in free-space (i.e., the tool is not in direct contact with any object), it is sufficient to specify and control the position of the tool. When contact occurs, the interaction forces limit or modify the free-space motion in some manner. Such tasks where the interaction forces must be accommodated rather than resisted are known as compliant motion tasks. There are two primary methods for producing compliant motion: a passive mechanical compliance built in to the manipulator, or an active compliance implemented in the software control loop i.e. force control. Passive compliance has the limitation of being suitable only for the pre-specified tasks; however, the force control method offers the advantage of programmability and hence adaptability to suite different tasks. This allows the manipulator to choose the particular form of compliance necessary for the particular application. Measurement of interaction forces of the end-effector of the robot with the environment is necessary to execute compliant motion tasks and this can be achieved by the following methods: motor currents may be measured, motor output torques may be measured, and wrist or hand mounted sensors may be used. However, for our application, the Tele-Robot environment has high radiation levels and hence the Force/ Torque sensors can’t be deployed at the end-effector. The basis for this approach and its analysis can be found in [1].

1.1 RELATED WORK Reference [2] classified the robot force control algorithms into two main groups, as fundamental and advanced force control. Fundamental force control algorithms can be categorized based on application of the relationship between position and applied force or between velocity and applied force, or the application of direct force feedback, or their combinations.Based on the fundamental force control methods, there are many advanced force control techniques, which are classified as adaptive control [3], robust control and learning control force [4].There are two main modes of fundamental force control, one involving the relation between position and applied force in the form of stiffness control, and the other employing position-force relation directly in the form of either hybrid position/force control [5]or hybrid impedance control [6].

2. COMPLIANCE CONTROL: PRINCIPLE & METHODOLOGY In this work, compliant motion has been executed through a hybrid position/ force control algorithm. The scheme has been simulated for PUMA 560 (6R) manipulator and subsequently implemented on the Tele – Robot developed by D.R.H.R., BARC. The concept of hybrid controller followed by a scheme of its software simulation is presented in subsequent subsections.

2.1 HYBRID CONTROLLER Combining position and force information into one control scheme [5] for moving the end-effector has been introduced as

Permission to make digital or hard copies of all or part of this work for personal or classroom use is granted without fee provided that copies are not made or distributed for profit or commercial advantage and that copies bear this notice and the full citation on the first page. Copyrights for components of this work owned by others than ACM must be honored. Abstracting with credit is permitted. To copy otherwise, or republish, to post on servers or to redistribute to lists, requires prior specific permission and/or a fee. Request permissions from Permissions@acm.org. AIR '13, July 04 - 06 2013, Pune, India Copyright 2013 ACM 978-1-4503-2347-5/13/07…$15.00. http://dx.doi.org/10.1145/2506095.2506114

hybrid position/force control. The hybrid controller develops position controlled torques and force controlled torques in specified degrees of freedom and makes the manipulator to perform the required task. The Hybrid position/force controller solves the three problems as stated below:

1) Position Control of a manipulator along directions in which a natural force constraint exists,

2) Force Control of a manipulator along directions in which a natural position constraint exists,

3) A scheme to implement the arbitrary mixing of these modes along orthogonal degrees of freedom of an arbitrary frame.

The conceptual schemata of Hybrid mode of control has been shown in the Fig. 1. In Fig.1, θa, Xa, Xd describes the actual joint angles, actual position & orientation and desired position & orientation of the end effector respectively. δX, δXs, δθs are vectors describing the error in position, positional error in selected degrees of freedom (dof) and joint angle errors corresponding to the required position control in selected dof.

Figure 1. Hybrid Position/Force Control Scheme.

τp is a vector describing the required torques to be given to the corresponding joints for position control. kPx, kIx are vectors representing the proportional and integral gains of Position Controller. τa, fa, fd are vectors describing the actual joint torques of the robot, actual force experienced by the end effector and desired force that should be experienced by end effector. . fe, fes, τes, τf are vectors describing the actual force experienced by the end effector, the force error of an end effector in selected degree of freedom, joint torque errors corresponding to the required force control in selected degrees of freedom and required torques to be given to the corresponding joints for force control. kPf, kIf are vectors representing the proportional and integral gains of Force Controller. It should be mentioned here that the dimension of all the aforementioned vectors are equal to the number of degrees of freedom of the serial mechanism to be controlled. The Controller architecture essentially consists of a position control block [7] and a force control block [8]. In the Position Control block, Xd (x, y, z, α, β, γ) represents the desired position (x, y, z) and orientation (α, β, γ) of the end effector. Xa (x, y, z, α, β, γ) represents the actual position (x, y, z) and orientation (α, β, γ) of the end effector. The matrices S, 𝑆⊥ are Selection matrices [9] introduced to decouple Position/ Force in orthogonal dimensions. Once the position error matrix is multiplied with S, the vector δXs (positional error of the end effector in selected degrees of freedom) is obtained. The positional errors, in turn, can be related to individual joint angle errors (δθs) through the Inverse Jacobian (J-1) [5] of the robot. Each element of δθs corresponds to the position error of each joint and is given to the respective position

controller (PI-Controller) of that joint. The position controller of each joint produces the necessary torques and this will be added with the output of the corresponding force controller and then given to the corresponding motor drive. The Position Control Law is,

𝜏𝑝𝑚1 = 𝑘𝑃𝑥𝑚1 × 𝛿𝜃𝑠𝑚1 + 1𝑘𝐼𝑥𝑚1

∫ (𝛿𝜃𝑠𝑚1)𝑑𝑡𝑡0 for, m Є N,𝑚 ≤ 6 (1)

Similarly in the Force Control block, fd represents the desired force and fa represents the actual force experienced by the end effector and this is obtained by calculating the end effector force using joint torques τa (joint torques are obtained by measuring the currents[9] in the joint motors and multiplying them with the corresponding Torque Constant of the motor). The vector fe represents the error in force experienced. fes (the vector describing the error in force in selected degrees of freedom) is obtained by multiplying fe with (S)⊥.It is known that τ = JTF therefore, we have τes= JTfes. Each element of τes corresponds to the torque error of each joint and is given to the respective force controller (PI-Controller) of that joint. The force controller of each joint produces the necessary torques and this will be added with the output of the corresponding position controller and then given to the corresponding drive. The Force Control Law is,

𝜏𝑓𝑚1 = 𝑘𝑃𝑓𝑚1 × 𝜏𝑒𝑠𝑚1 + 1𝑘𝐼𝑓𝑚1

∫ (𝜏𝑒𝑠𝑚1𝑡0 )𝑑𝑡 (JT(θ) × fd)𝑚1 (2)

The feedback to the Force Control Block is the joint torques which can be mapped to the currently applied force through the Force Transform [11] as follows: 𝑓𝑎 = [𝐽𝑇]−1 𝑋 𝜏𝑎 (3)

2.2 COMPLIANCE CONTROL ALGORITHM For a compliant motion task, the contact between manipulator’s end – effector and task environment gives rise to Natural Constraints [9] from the geometrical characteristics of the task pattern and Artificial Constraints [9] from our decision of the trajectory of position and force. The variation in the contact environment should be detectable to control the manipulator moving within natural constraints. Depending upon the task, the set of inputs and conditions are given to the algorithm module by the user. The black box model of the Compliance Control Algorithm describing its input and output has been shown in Fig. 2.

Figure 2. Input & Output of Compliance Control Algorithm.

There may be various types of tasks like drawing a known non-breaking curve on a horizontal plane, drawing a broken curve on a horizontal plane, drawing a broken/non-breaking curve on any arbitrary plane etc. The output of the algorithm will be a position path and force profile for executing the task. Different tasks may be given as input to the Compliance Control Algorithm as shown in the following examples: CCA (string taskname, float arg1, float arg2…); An illustrative set of Input arguments for the algorithm may be:

taskname = “Draw a circle on a plane”; arg1 = a; arg2 = b; arg3 = c; arg4 = d; arg5 = x1; arg6 = y1; arg7 = z1; arg8 = r; arg9 = Fx; arg10 = Fy; arg11 = Fz; arg12 = nx; arg13 = ny; arg14 = nz;

Similarly the input arguments for a broken line would comprise of: a taskname argument as “Draw broken line,” the coefficients of the equation of the plane in which lines has to be drawn, the coefficients of the equations of the line segments and the desired force components. With these arguments, the Compliance control algorithm “partitions” the degrees of freedom as per the task and generates the required position path and force profile. It is then the responsibility of the hybrid position/force controller to make the manipulator follow the position path and force profile. The flowchart in Fig. 3 depicts the process of achieving compliant motion for a given task.

Start

Read the Task(Get the Natural

Constraints)

Check for Contact

Invoke Hybrid Control Mode

Invoke Position Control Mode

Input Artificial Constraints for

the intended task

Generate the Torque in Joint Co - Ordinates,τc = τpc+τfc

Yes

Is Task Complete?

Stop

Generate S - Matrices

No

Decouple Position and Force Profiles

Convert World Co – Ordinates to

Joint Co - Ordinates

PID

Yes

No

Figure 3. Flow Chart of Compliant Motion.

For achieving Compliant motion the Compliance Control algorithm module reads the task from the user and generates S-Matrices depending upon the natural constraint geometry. With the help of Artificial Constraints, the compliance control algorithm module generates position path and force profile. These paths and profiles are given to hybrid controller which develops position controlled torques and force controlled torques to drive the manipulator as intended.

2.3 SOFTWARE SIMULATION OF COMPLAINCE CONTROL ALGORITHM The dynamic equation for a manipulator in a general form can be written as:

𝜏 = 𝑀(𝜃)�̈� + 𝑉�𝜃, �̇�� + 𝐺(𝜃) + 𝜏𝑟(𝜃, 𝜏) (4)

Where, M (θ) is the 6 x 6 Mass matrix of the manipulator (6 - DOF).

V�θ, θ̇� is a 6 x 1 vector of centrifugal and Coriolis terms and G (θ) is a 6 x 1 vector of gravity terms. Each element of M (θ) and G (θ) is a complex function that depends on θ (the position of all the joints of the manipulator). Each element of 𝑉(θ, θ̇) is a complex function of both θ and θ̇. For simulation of a surface, the equation of surface is taken as z = k and the reaction force from the surface is represented by

𝜏𝑟 = 𝐽𝑇(𝜃) × 𝐹𝑟 where, 𝐹𝑟 = [0 0 fr 0 0 0] T,

fr = �−((𝐽𝑇(𝜃))−1)31 × 𝜏𝑎 𝑓𝑜𝑟 𝑧 ≤ 𝑘 ,𝐹𝑧 < 00 𝑂𝑡ℎ𝑒𝑟𝑤𝑖𝑠𝑒

�

A consolidated block level description for the simulated system is shown in Fig. 4 which illustrates the simulation process of a simplistic task of controlling position in X – Y plane and force along Z-axis.

Figure 4. Computer Simulation of Compliance Control

Algorithm. As shown in Fig.4, position path and force profiles are generated by the Compliance Control Algorithm depending upon the task input given to it. These paths and profiles are then given as inputs to the hybrid controller which generates the torques required for the manipulator joints. These torques are given to the simulated dynamics model of PUMA560. The dynamics model generates the response of the manipulator (as the achieved joint angles and joint torques) under the influence of the given input torques. The simulation also calculates the torques required to compensate for gravitational and coriolis components.

3. SIMULATION RESULTS The PUMA 560 6R manipulator model described in [12], are used for computer simulations. Point type of contact and Surface simulation (as mentioned in Fig.4) are considered for the present study. The link and joint parameters of the Puma 560 used in this work are given in Table I. The tasks to be accomplished for this simulation study are, drawing a circle on a rigid plane (Figures 5, 6), drawing a broken

Equation of plane is say: ax+by+cz = d

Centre of the circle is say: (x1, y1, z1)

Radius of the Circle is say ‘r’

Desired Force is say (Fx, Fy, Fz, nx, ny, nz)

curve on a plane (Figures 7, 8) and drawing a line on an inclined plane (Figures 9, 10). The PUMA 560 manipulator has to apply a constant pressing force (when contact exists, say 15 N or 5N) while drawing the above mentioned Position paths. Table 1. Link & Joint Parameters of PUMA560 Manipulator

Joints αi(degrees) Ai (m) Di(m) θi

(degrees)

1 90 0 0 θ1

2 0 0.4318 0 θ2

3 -90 0.0203 0.15005 θ3

4 90 0 0.4318 θ4

5 -90 0 0 θ5

6 0 0 0 θ6

The Compliance Control algorithm develops decoupled Position Trajectory and Force Trajectory and is given as input to Hybrid Controller. The hybrid controller develops torque, which controls position and force according to the task given. Without loss of generality, in constrained motion experiments, the trajectories of the manipulator are always considered in the reachable workspace without singularities.

Figure 5. Circle drawn on Task Plane by the Manipulator.

Figure 6. Force applied by PUMA’s End-Effector on the Task

Plane.

4. EXPERIMENTAL RESULTS ON TELE-ROBOT For simulation experiments a dynamic model is necessary, but due to the unavailability of the Tele – Robot [1] Dynamics, the simulation experiments have not been carried out on it. Moreover the Dynamic model of PUMA560 is readily available and is kinematically similar to that of a Tele-Robot. Therefore for testing

of Compliance Algorithm, PUMA 560 model has been considered.

Figure 7. Broken Curve on Task Plane by PUMA 560.

Figure 8. Force Traced by PUMA while drawing a Broken

Curve.

Figure 9. Position Path of PUMA on an Inclined Plane.

Figure 10. Force applied by PUMA’s End-Effector on Inclined

Plane .

This will suffice the simulation testing of the algorithm. Actual implementation of Compliance Control was carried out on the Tele – Robot (designed in DRHR) since its required kinematics model was readily available. The D-H parameter table for the Tele-Robot is shown in Table II. In simulation various aspects e.g. friction, gear backlash and other non linear effects were not considered, however, during the actual implementation and experimental evaluation on the Tele – Robot, these effects were also present. Also, Gravity Compensation was essential in simulation of PUMA560, however, the Tele-Robot used for this work uses counter weights in the manipulator links to achieve a mechanically balanced configuration, therefore, Gravity Compensation is not required for this experimental work. Actual force experienced by the end effector is obtained by calculating the joint torques τa (joint torques are obtained by measuring the currents in the joint motors and multiplying them with the corresponding Torque Constant of the motor).Using the relation 𝑓𝑎 = [JT(θ)]−1. 𝜏𝑎, the force experienced by the end – effector is calculated and used as a feedback signal to control the force.

Table 2. Link and Joint Parameters of Tele - Robot

Joints αi(degrees) Ai (m) Di(m) θi

(degrees)

1 90 0 0 θ1

2 0 0.5 0 θ2

3 90 0.115 0 θ3

4 -90 0 0.556 θ4

5 90 0 0 θ5

6 0 0 0.17 θ6

For the task of controlling position in X, Z planes and to apply a force of 15N along the positive Y – axis, the hybrid controller develops position controlled torques along X and Z axis and tries to maintain the fixed position. The force controlled torques accelerates the manipulator’s end - effector along Y – direction as shown in the Fig.11. For the task of drawing a circle on Y – Z plane while applying a force of 15N along X – axis, the hybrid controller accelerates the end – effector along X – axis which makes the manipulator to trace the helical path as shown in the Fig. 12. The Position Path and Force Profile of a Tele – Robot drawing a circle on a plane while applying a force of 15N normal to the surface are shown in the Figures 13 and 14 respectively. The Position Path and Force Profile of a Tele– Robot drawing a broken line on a rigid plane while applying a force of 5N normal to the surface (when contact exists) are shown in Figures 15and 16 respectively. The Position Path and Force Profile of a Tele – Robot drawing a line on an inclined plane while applying a force of 5N perpendicular to the plane are shown in Figures 17 and 18 respectively. Table III summarizes the performance of the control algorithm for PUMA 560 in simulation and BARC Tele - Robot in experiment. The major component of disturbance in the position and force control of Tele – Robot are due to friction, gear back lash and other non linear effects.

Figure 11. Position Control along X, Z axis and Acceleration

along Y – axis.

Figure 12. Helical Path traced by Tele – Robot due to

acceleration along X – axis.

Figure 13. Circle drawn on the Task plane by the Tele – Robot

Figure 14. Force Applied by Tele – Robot End – Effector on Task Plane

Table 3. Performance of Controller for the different tasks for PUMA 560 (Simulation) and Tele – Robot (Experimental)

#The first entry in each cell denotes the value from Simulation of PUMA560 and the second entry denotes the experimental value obtained from Tele-Robot. * Prescribed Force – 15N, ** Prescribed Force – 5N

Figure 15. Broken Curve on task Plane by Tele – Robot

Figure 16. Force Traced by Tele - Robot while drawing a

Broken Curve

Figure 17. Position path of Tele – Robot on Inclined Plane

Figure 18. Force applied by the Tele – Robot’s End – Effector

on the Inclined Plane

Illustrative Task Mean Position Error (cm)

Peak Position Error (cm)

Mean Force Error (N) Peak Force (N)

Mean Position Settling Time

(sec)

Mean Force Settling Time

(sec)

*Drawing a Straight Line 1.51, 2.55# 0.55,1.98 01.66, 01.69 16.66, 16.69 00.07, 00.14 00.21, 00.25

**Drawing a Broken curve 1.19, 2.09 0.45, 2.52 00.69, 00.72 5.69, 5.72 00.08, 00.18 00.10, 00.17

*Drawing a Curved Path 1.22, 2.28 0.22, 3.08 01.62, 01.65 16.62, 16.65 00.06, 00.21 00.15, 00.19

*Circular motion along Y – Z Plane with force acting along

X – Direction 1.30, 3.39 1.25, 3.25 01.62, 01.65 16.62, 16.65 00.09, 00.16 00.23, 00.29

*Maintaining the abcissa and applicate constant and

accelerating the ordinate 1.08,2.09 0.09, 0.4 00.72, 00.81 15.72, 15.81 00.11, 00.18 00.11, 00.16

5. CONCLUSION The compliant motion algorithm takes in a task and outputs the position and force profiles for completion of the task. For the tasks demonstrated in this work, like drawing broken and non-breaking curves on planes with generic inclination, the compliant motion algorithm has produced the position/force profiles and the manipulator motion has been controlled on those command profiles by the hybrid position/force controller. The results, as demonstrated in the previous section, are in general satisfactory.

6. ACKNOWLEDGEMENTS We express our thanks to Division of Remote Handling and Robotics, Bhabha Atomic Research Centre and Homi Bhabha National Institute for supporting this work.

7. REFERENCES [1] Ray. D and Singh. M. 2010. Development of a Force

Reflecting Tele – Robot for Remote handling in Nuclear Installations, Proc of 1st International Conference on Applied Robotics for Power Industry, Montreal, (2010), Pages 1 – 6. DOI = 10.1109/CARPI.2010.5624456

[2] Zeng. G and Hemami. A. 1997. An Overview of Robot Force Control, Robotica,(1997), Vol. 15, Pages 473-482. DOI = 10.1017/S026357479700057X

[3] Vladimir F. Filaretov and Alexandr V. Zuev. 2008. Position control. Adaptive Force/Position Control of Robot Manipulators,Proc. of International Conference on Advanced Intelligent Mechatronics, (Xi'an, China, July 2008), 2-5. DOI = 10.1109/AIM.2008.4601641

[4] Kalakrishnan, Mrinal, Ludovic Righetti, Peter Pastor, and Stefan Schaal. 2012. Learning Force Control Policies for Compliant Robotic Manipulation. Proc. of International Conference on Machine Learning (ICML). (2012), 4639-4644.

[5] Raibert.M. H and Craig. J. J. 1987. Hybrid Position/Force control of manipulators, Journal of Dynamic Systems, Measurement and Control, (1981), 126-133.

[6] Robert J. Anderson and Mark W. Spong. 1988. Hybrid impedance control of robotic manipulators. IEEE Journal of Robotics and Automation, (1988) vol.4, no. 5, 549-556. DOI = 10.1115/1.3139652

[7] Paul. R. P and Shimano. B, Compliance and Control. 1976. Proc. of Joint Automatic Control Conference, (SanFrancisco, 1976).

[8] Mozaryn. J, Kurek. J. E. 2002. Design of decoupled sliding mode control for the PUMA 560 Robot Manipulator, Robot Motion and Control, (2002), 45 – 50. DOI = 10.1109/ROMOCO.2002.1177082

[9] Mason. M. T. 1981. Compliance and Force Control for computed control manipulators, IEEE Trans. on Systems, Man and Cybernetics, (1981), 418-432. DOI = 10.1109/TSMC.1981.4308708

[10] Shimano. B, Force Control, Exploratory Study of Computer Integrated Assembly Systems, Binford,Stanford Artificial Intelligence Laboratory Memo, (June, 1977), 285. 4.

[11] Ohishi, K. Miyazaki, M. Fujita, M. 1992. Hybrid Control of Force and Position without Force Sensor, Proc of IECON1992, NUMBER 2, (1992), 670. DOI = 10.1109/IECON.1992.254552

[12] Corke. P. I. 1996. A Robotics Toolbox for MATLAB, in IEEE Robotics and Automation Magazine (1996), 24 – 32. DOI = 10.1109/100.486658