Redundancy Resolution and Inertia Reduction in KUKA LWR

Robot

Hugo Filipe Duarte Francohugo.franco@tecnico.ulisboa.pt

Instituto Superior Tecnico, Universidade de Lisboa, Lisboa, Portugal

November 2016

Abstract

In this master’s thesis, force feedforward control strategy and a resolution for the redundancy wereimplemented for the KUKA Ligthweight Robot 4+. When using the robot in co-manipulation, the userinteracts with the dynamics of the manipulator which includes joints friction and inertia of the roboticmanipulator which can reduce the user dexterity and increase fatigue. The feedforward force controlimplemented was tested and showed to be able to reduce the felt inertia in about 48 % in translationand up to 65 % in rotation. Since the KUKA Ligthweight Robot 4+ is a redundant robot with 7 degreesof freedom, it is possible to specify a joint pose while working in cartesian space. Redundancy has beensolved with an extra variable, the swivel angle. The user sets the desired swivel angle from which it issent to inverse kinematics to obtain a consistent joint pose with the desired swivel angle and the currentcartesian pose. In addition to the swivel angle, the user must specify a desired joint stiffness and jointdamping. The specification of joints pose has been experimentally validated, where it was found thatthe higher joints stiffness, the smaller the error obtained at the specified swivel angle.Keywords: Co-manipulation, Force Feedforward Control, Inertia Reduction, Inverse Kinematics,Redundancy Resolution

1. Introduction

In recent years, robots have been developed withthe motivation to bring them closer to people,removing their typical security cages and makinghuman-robot collaboration more accessible. Duringthe execution of these tasks, the human guidesthe robot by exerting forces on some points of therobot’s body, often the end effector, which shouldhave a compliant behaviour. This kind of manipu-lation is call co-manipulation. To be effective, thecompliant behaviour should be changed or adaptedaccording to the task and, possibly, to humanintentions [1].The impedance control imposes a desired dynamicbehaviour to the robot, described by a second-ordermechanical system of desired mass, damping andstiffness. This allows to the robot have a compli-ance behaviour which enables a safe interactionbetween the robot and humans workers [1]. Inco-manipulation, the operator interacts with thedynamics of the manipulator which includes jointsfriction and inertia. These factors may reduce thedexterity and increase operator fatigue over time. Itis desirable that these factors are reduced for a bet-ter collaboration between robot and human worker.

The KUKA Lightweight 4+, or LWR, is theresult of collaboration between KUKA RoboterGmbH and the Institute of Robotics and Mecha-tronics of the German Aerospace Center. The maincharacteristics of LWR are: ability to support itsown weight, be kinematically redundant (i.e. sevenDOF), have a total weight less than 15 kg with arange up to 1.5m and a high dynamic performance.With its seven axes, the robot has one redundantdegree of freedom (DOF), which can be exploitedto impose multi-task.It contains force/torque sensors (strain gauges),motor position sensors and links position sensors(potentiometers) which are only used for safety rea-sons (redundant sensors).There are torque sensors in each joint, coupled tothe Harmonic Drive, with measurement errors lessthan 0.5 % with a low-pass filter to 600Hz [2].The combination of position and torque sen-sors allows implementing position control but alsoimpedance control.

The Surgical Robotics Lab, at Instituto SuperiorTecnico has been on the interface between engi-neering and medicine, ranging from hardware tosoftware development of medical solutions.The objective of this work is to contribute with

1

new features to other projects of the SurgicalRobotics Lab.

The main objectives of the present work is to im-plement strategies that:

• Reduce the apparent inertia of the KUKALightweight 4+

• Solve the redundancy, in order to setup a jointconfiguration in the nullspace

2. Force feedforward control

Force feedforward control (FFC) is a controlstrategy, presented in [3], that adds a force sensorwhich measures the force exerted by the user onthe device and amplifies it by a certain gain K.In figure 1, a block diagram of this control ar-chitecture is shown, where Ze is the environmentadmittance, Zu is the operator admittance and Zmis the manipulator admittance. The correspondingtransfer function presented in equation 1.

Figure 1: Block diagram: Force Feedforward Con-trol with environment compensation[3].

X(s)

F (s)=

1

Zu(s) + Zm(s)1+K + Ze(s)

(1)

The inertia felt by the user will be reduced byapproximately 1 + K. The environment will alsobe reduced by 1 + K. To keep the environmentwithout any change, it must be multiplied by 1 + K.

It is desirable to design K as high as possible,but the system may become unstable. In [3], othersources of instability are presented, such as the forcesensor, and their measurements must be properlyfiltered. This leads to ending up with a moderategain value which represents the limitation of thiscontrol strategy.In addition to the above announced limitations, ifwe look at equation 1, the operator’s dynamics (Zu)is coupled to the dynamics of the manipulator (Zm)and thus the stability depends on this coupling.

Even if the systems are stable when separate, to-gether they may become unstable. The system mayalso become unstable when the interaction is tryingto impose a dynamic that is significantly differentfrom hardware intrinsic dynamics [1].

This strategy has already been applied previ-ously to LWR [4, 5], where it achieved a reductionof about 33 % of translational inertia and 25 % ofthe rotational inertia.

In [6] the same strategy is used to reduce theinertia felt in one direction. In free movement, theinertia was reduced up 3 times in the x direction.In [6] several aspects that may affect the value Kare analyzed. In this work, equation 2 is presentedwhere Kw represents the worst maximum gainvalue. As equation 2 shows, the higher the mass ofthe operator, mh, the lower is the value of K. In theworst case, it should be possible a 50% reduction( K = 1) with no instability occurring. Thesame equation indicates that the system stabilitydepends on how the user grasps the system.

Kw = 1 +m

mh(2)

3. Kinematic Redundancy

The redundancy can increase the ability of themanipulator and enable more complex movements,such as avoiding obstacles, singularities and jointlimits.

One simple way to control the movements ofthe nullspace is by the principle of superpositionof impedances. The principle of superposition ofimpedances allows to easily combine different be-haviors of impedances. This overlap is performed byadding the torques of different impedances (equa-tion 3, where τc is the cartesian impedance torqueand τn is the nullspace impedance torque) [7].

τ = τc + τn (3)

The desired behavior in the nullspace is charac-terized (equation 4), in joint space, by the sym-metric and positive definite matrices Kn ∈ Rn×nand Dn ∈ Rn×n representing, respectively, thedesired nullspace stiffness and damping. It isalso necessary to choose the desired equilibriumposition, qd,0 [7, 8].

τ0 = −Dnq −Kn(q − qd,0) (4)

To ensure a good impedance combination, theposition of equilibrium, qd,0, should be consistentwith the equilibrium position in cartesian space.To have a kinematic uncoupling of the joint spaceimpedance from the cartesian space impedance,

2

equation 4 cannot be applied directly because itwould affect the behavior of the desired cartesianimpedance, so it is necessary to project the binaryτ0 through a projection matrix, N(q) (equation5) [7].

τn = N(q)τ0 (5)

• Static Nullspace Projection

Consider a matrix V (q) called left annihilatorof JT (q), i.e V (q)JT (q) = 0. Therefore theprojection matrix can be

N1 = V (q)TV (q) (6)

The matrix V (q) can be calculated by singularvalue decomposition of J [8].

• Dynamically Consistent Projection

It should be noted that projection N1 is madeon kinematic level , not taking into accountthe dynamics of the manipulator. Given thisfact, a static projection is not enough toensure a decoupling of cartesian impedancefrom nullspace impedance [7].

In order to account for the dynamics, thecondition presented in equation 7 is sufficientfor a consistent nullspace mapping with theequations of motion, where J(q) is the manip-ulator jacobian and M(q)−1 is the inverse ofthe manipulator inertia matrix in joint space.

J(q)M(q)−1N(q) = 0 (7)

The projection matrix next, fulfill the previouscondition:

N2 = M(q)V (q)TV (q) (8)

Equation 8 is nothing more than the productof the inertia matrix of the manipulator, Mwith the static projection matrix N1 [8].Equation 9 presents another dynamic pro-jection which benefits from being formulatedin the cartesian space [8], where Λ(q) is themanipulator mass matrix in cartesian spaceand I represents an identity matrix.

N3 = I − J(q)TΛ(q)J(q)M(q)−1 (9)

4. Experimental Setup

In this thesis, MATLAB R©/ Simulink R© was usedto interact with the LWR using the Fast Research

Interface (FRI). FRI is a low-level communicationinterface that allows access to the LWR controllervia an external computer and its variables in realtime. The sampling time can be chosen by the userwithin a range of 1 to 100 ms. Communicationtakes place via UDP communication between thehost computer and KRC controller [9].In Figure 2 illustrates the experimental setup andthe communication between devices.

Figure 2: Scheme of comunication between devices.Adapted from [10].

The FRI-xPC server was implemented withReal-Time Simulink in the host PC (figure 2),which after being compiled in C is downloaded as astandalone application to a real-time system withthe MathWorks provided real-time kernel.For more details and a complete understandingconsult [10, 11].

4.1. Implementation

A force sensor/torque JR3 with 100N capacityin the x and y axes and 200N in the z axis. Thissensor has six embedded digital filters. Filter 3has been chosen, which has a cut-off frequencyof about 31Hz. The following filter with a lowercut-off frequency (about 7.81Hz) causes the LWRto oscillate. This oscillatory behavior may bedue to the cut-off frequency being lower than themechanical bandwidth of LWR, which based on[12] is assumed to be about 10Hz. Also accordingto [6, 13], the power spectrum of the human handachieves about 10Hz. Therefore using a filter withcut-off frequency of 31Hz is suitable.

The force sensor is mounted on the end-effectorof the robot. This measures the forces and mo-ments at the center of the sensor. Since the toolcenter point (TCP) was defined in the flange of thesensor an appropriate coordinate transformationwas made.

When initializing the sensor, this resets theoffsets so that any equipment mounted on topdoes not affect the measurements. In this case theoperator will be in co-manipulation with the robot,thus the sensor will vary its position over time andthis offsets will affect measurements.

To compensate for the offsets of the sensor, we

3

have to take into account the forces exerted onthe sensor. The sensor will suffer gravitationalforces caused by the weight of the assembled tool.Added to this, there is still the ”dead weight” ofthe sensor. This ”dead weight” is possibly due tosome internal electronic components. The valuewas experimentally determined and then tunedduring experiments.In the equation 11 the forces acting on the sensor,Fs, are calculated, where mtool is the tool massmounted on top and gs is the gravitational ac-celeration expressed in the tool reference frame,whose calculation is performed using equation 10,where Rbase

tool is the rotation matrix of the referenceframe of the tool expressed in the base referenceframe and g is the gravity aceleration constant.In addition to the forces, the moments, Ms, arealso compensated, which are calculated in theequation 12, where ctoolg is the tool center of grav-ity expressed in tool reference frame. Finally, inequation 13, the calculated forces and moments aresubtracted from the forces and moments measured(Fr and Mr respectively).

gs = Rbase Ttool ·

00−g

(10)

Fs = mtoolgs (11)

Ms = ctoolg × Fs (12)

[FM

]=

[FrMr

]−[FsMs

](13)

After compensating the offsets of the sensor, theforce feedforward control is easily implemented.The forces and moments are multiplied by the gainK which are then sent to the LWR via FRI. Beforethe forces are multiplied, they go to a minimumlimiter, accepting only forces greater than 1N. Thisthreshold was decided after some experience, asthe way the user must hold the end effector, makesit very difficult to exert forces smaller than 1N.

4.1.1 Redundancy Resolution

The nullspace of LWR was designed as previouslypresented, using the principle of superposition ofimpedances. For that reason it is necessary send tothe nullspace controller, a joint configuration con-sistent with the cartesian pose of LWR. In order tosend joint configuration consistent with the carte-sian position, the inverse kinematics of the LWRwas perfomed.

Figure 3 shows the coordinate axes accordingthe Denavit-Hartenberg(DH) convention and table1 presents the DH parameters.

Figure 3: DH axes.

Joints d(m) ϑ(rad) a(m) α(rad)1 0 ϑ1 0 π

2

2 0 ϑ2 0 −π23 0.4 ϑ3 0 −π24 0 ϑ4 0 π

2

5 0.39 ϑ5 0 π2

6 0 ϑ6 0 −π27 0.078 ϑ7 0 0

Table 1: DH Parameters.

Kinematic decoupling of the manipulator was ap-plied to separate the inverse kinematics of the wristfrom the arm. Despite this there is still one extradegree of freedom (DOF), which will be resolved byimposing an elbow position by introducing a swivelangle, α.

Considering the coordinate axes defined in figure3 the shoulder position, ps, the elbow position, peland wrist position, pw, are subject to the followingequalities:

ps = p0 = p1 = p2 =[0 0 0

]T(14)

pel = p3 = p4 (15)

pw = p5 = p6 (16)

4

Figure 4: Swivel angle representation.

The wrist position is easily calculated by:

pw = pc − ae · d7 (17)

where pc is the current position received from LWRvia FRI expressed in the base reference frame, aeis the third column of the current matrix rotation(Rbase

tool ) received from LWR via the FRI and d7 isthe DH parameter of joint 7 shown in table 1.It can be seen that the shoulder position, ps, isfunction of the ϑ1 and ϑ2, the wrist position, pw,is a function of ϑ1, ϑ2, ϑ3 and ϑ4. The remainingjoints only affect the orientation.

The desired swivel angle, αd, will be set by theuser and works as an input to the algorithm. Theαd is shown in Figure 4 and is considered to bezero when the triangle formed by the shoulder,elbow and wrist form a plane containing z0. Arotation is considered positive when the planerotates according to the right hand rule about thevector from shoulder to wrist (in figure 4 a negativerotation is represented).

To define the elbow position, pel, we first intro-duce the swivel reference frame, Rsw, as

Rsw =[xsw ysw zsw

](18)

withysw =

pw‖pw‖

(19)

xsw =ysw × z0‖ ysw × z0‖

(20)

zsw = xsw × ysw (21)

In order to calculate the projection of the elbowposition, psw along pw we refer to the cosine rule :

psw = d3 cos(β) =‖pw‖2 + d23 − d25

2‖pw‖(22)

Therefore the position of the elbow is defined as

pel = Rsw

rsw sin(αd)psw

rsw cos(αd)

=

pelxpelypelz

(23)

where rsw is the radius of swivel, given by :

rsw =√d23 − p2sw (24)

• Calculation of (ϑ1,ϑ2)

The elbow position can be solved using directkinematics which results in the following equa-tion:

pel =

−d3 cos(ϑ1) sin(ϑ2)−d3 sin(ϑ1) cos(ϑ2)

d3 cos(ϑ2)

(25)

Taking into account

cos(ϑ2) =pelzd3

(26)

sin(ϑ2) = ±√

1− cos2(ϑ2) (27)

we get two possible solutions for ϑ2, (ϑ2I ,ϑ2II).

ϑ2I = Atan2(√

1− cos2(ϑ2), cos2(ϑ2)) (28)

ϑ2II = Atan2(−√

1− cos2(ϑ2), cos2(ϑ2))(29)

The possible solutions for ϑ1, ϑ1I and ϑ1II are:

ϑ1I = Atan2(−pely, pelx) (30)

ϑ1II = Atan2(pely, pelx) (31)

• Calculation of (ϑ3,ϑ4)

With ϑ1, ϑ2 already obtained its is possibleobtain pw express in reference frame 2,2pw:

2pw = R02Tp5 =

d5 cos(ϑ3) sin(ϑ4)d5 sin(ϑ3) sin(ϑ4)d5 cos(ϑ4) + d3

(32)

Since, numerically 2pw is known, we get :

cos(ϑ4) =2pwz − d3

d5(33)

sin(ϑ4) = ±√

1− cos2(ϑ4) (34)

The two possible solution for ϑ4 are:

ϑ4I = Atan2(√

1− cos2(ϑ4), cos(ϑ4)) (35)

ϑ4II = Atan2(−√

1− cos2(ϑ4), cos(ϑ4)) (36)

Depending on the sign of sin(ϑ4) we have twosolutions for ϑ3, ϑ3I and ϑ3II

ϑ3I = Atan2(2pwy,2 pwx) (37)

ϑ3II = Atan2(−2pwy,−2pwx) (38)

5

For a given swivel angle, α, there is four possiblejoint angles sets for the arm posture.

(ϑ1I , ϑ2I , ϑ3I , ϑ4I)

(ϑ1I , ϑ2I , ϑ3II , ϑ4II)

(ϑ1II , ϑ2II , ϑ3I , ϑ4I)

(ϑ1II , ϑ2II , ϑ3II , ϑ4II)

• Calculation of (ϑ5,ϑ6,ϑ7)It is now possible calculate the remain jointsfrom

R74 = R0

4TRc =

r11 r12 r13r21 r22 r23r31 r32 r33

(39)

and taking into account

R74 =

c5c6c7 − s5s7 −c5c6s7 − s5c7 −c5s6c6c7s5 + c5s7 −c6s7s5 + c5c7 −s5s6

c7s6 −s7s6 c6

(40)

it is possible to obtain the two possible solu-tions for the wrist configuration. If ϑ6 ∈]0, π[we get:

ϑ5I = Atan2(−r23,−r13) (41)

ϑ6I = Atan2(√r231 + r232, r33) (42)

ϑ7I = Atan2(−r32, r31) (43)

otherwise, i.e ϑ6 ∈]− π, 0[, we get:

ϑ5II = Atan2(r23, r13) (44)

ϑ6II = Atan2(−√r231 + r232, r33) (45)

ϑ7II = Atan2(r32,−r31) (46)

When the pulse is on top of the shoulder (i.e ontop of the z0) a singularity occurs. In this case,the swivel angle, α, becomes undefined and ϑ1 cantake any value. Another singularity occurs whenthe manipulator is fully extended, i.e the wrist andelbow are aligned.When the elbow is in line with z0 another singu-larity occurs, since ϑ1 can take any value. Finallywe have the singularity of the wrist when z4 andz6 are aligned.The implemented algorithm detects the singu-larities announced above and will return thecurrent value of the appropriate joint to resolve thesingularity. This strategy to solve singularities waschosen for simplicity.

In a normal situation, without singularities, thealgorithm must choose between the two existingsolutions. To make this choice, it is first checked

if any solution is outside the joints limits. If asolution doesn’t respect the joints limits, it isdiscarded and another solution is automaticallychosen.In case the joint limits are respected, the solutionnearer to the current joint position is chosen.

The inverse kinematics was constructed based onthe coordinate axes shown in Fig 3, where z0 is theshoulder and z7 is the end effector of the LWR. Thefact that the last coordinate axis is at the end ef-fector will affect the inverse kinematics when a toolis defined in LWR controller.For simplicity, the tool will still be set in the LWRcontroller, however it is necessary calculate the endeffector rotation matrix and position (R0

7 e p07 re-spectively) from the rotation matrix Rbase

tool given byFRI.The tool in LWR controller is defined by six param-eters. The X, Y and Z parameters are distances inrelation to the end effector of the LWR and A, B, Care the parameters that corresponds to rotations, inthis case the rotation ZYX (R7

8). So we can easilycalculate the matrix rotation R0

7:

R07 = R0

8R78T

(47)

For the position, we subtract X, Y from the tool(equation 48).

p07 = p08 −[|X| |Y | 0

](48)

The Z parameter is included in the d7 parameter.This way no more changes are needed in the inversekinematics.

Two solutions were implemented in order toallow the user to specify or change the desiredswivel angle. In the first one, the user just sendsthe desired swivel angle to the inverse kinematicsand this sends the join configuration to LWR.The second one is based on trajectory planning.The user specify the trajectory duration and thedesired swivel angle. This solution offers moresafety to the user and avoids abrupt changes onthe joint configuration. For the trajectory to beacomplished, the end effector needs to be fixed incartesian space. When the trajectory ends, thefinal swivel angle is sent constantly to allow aco-manipulation with the swivel angle prescribed.

A good robot calibration is essential for a gooddecoupling between the nullspace and the cartesianimpedance. The LWR was calibrated in order toavoid drift when it wasn’t in use and a swivel anglewas prescribed. The calibration only decreased thedrift, but didn’t eliminated it. For that reason,the JR3 sensor was used to avoid the LWR drift.

6

When the sensor detects forces (i.e someone ismaneuvering the LWR) the nullspace configurationis sent, otherwise the actual joint configuration issent.

5. Results

To validate the effective reduction of the felt in-ertia experimental tests with LWR with a samplerate of 500Hz. These experiments consisted in mak-ing oscillatory movements in each axis (figure 5), inwhich the position and force/moment exerted bythe user were measured. For the translation in they axis it was necessary to change the configurationof the LWR because it was not possible to approx-imate the desired equations to the data obtained.This may be due to non-linearities.To avoid noise amplification, data acquired from theLWR position sensors was approximated by a cubicspline. Since the LWR only possesses position sen-sors, velocities and accelerations were obtained bytaking the first and second derivatives, respectively,of the (approximated) position.

(a) Translation: x and zaxes.

(b) Translation: y axis.

(c) Rotation Configura-tion.

(d) Rotation movements.

Figure 5: LWR Configuration and Indication of themovements during the tests.

5.1. Free MovementTests were performed without applying a virtual

spring stiffness, i.e. Kcart = 0. In this case, theLWR behavior is similar to when in gravity com-pensation but still has a damping component (β).Therefore the data will be approached to equation49.

f = mx+ βx (49)

The tests were performed also with the nullspaceresolution active with αd = 0, with a joint stiffnessof kj = 250N/m and with a damping joint coef-ficient of dj = 1. In cartesian space the dampingcoefficient was dcart = 1.

Axes K f = mx+ bx Error† [%] Reduction [%] Theoretical reduction [%]0 11.894x+ 23.086x 13.60%

0.25 10.509x+ 17.803x 12.71% 11.6% 20.00%0.5 8.682x+ 16.863x 12.40% 27.01% 33.33%0.75 7.545x+ 12.153x 9.89% 36.56% 42.86%

x

1 6.294x+ 8.777x 8.76% 47.08% 50.00%0 13.152x+ 24.873x 10.52%

0.25 11.620x+ 15.480x 8.82% 11.7% 20.00%0.5 9.687x+ 11.618x 7.00% 26.35% 33.33%0.75 7.979x+ 10.277x 8.53% 39.33% 42.86%

y

1 6.783x+ 8.567x 5.81% 48.43% 50.00%0 5.300x+ 18.537x 11.28%

0.25 4.949x+ 12.099x 8.66% 6.62% 20.00%0.5 4.432x+ 10.031x 5.96% 16.38% 33.33%0.75 3.473x+ 8.045x 11.06% 34.47% 42.86%

z

1 2.754x+ 6.986x 16.56% 48.04% 50.00%†Relative error of root-mean-square-error(Rmse) to the absolute maximum value of the measured force.

Table 2: Results: Translation.

As it can be seen from the table 2, as expected,the higher the gains, the greater the reduction ofthe inertia felt by the user.As it can be seen when K = 0.25 the reductionfalls short of what was expected to achieve, only11% (x and y axis) and 7% (z axis ) when it shouldbe close to 20%. In z axis for gains K = 0.25and K = 0.5 the reduction is well below the ex-pected performance and the reduction in other axes.

The same type of test conducted for the transla-tion was performed for rotation. The user appliedmoments so that the orientation oscillated arounda point.

As we can seen, in table 3, the larger the gain,K, the greater reduction we get. In all axes, withK = 0.25, the reduction was higher than thetheoretical. The same happened when K = 1 inrotation around x.

5.2. Influence of environment impedanceIn this section we study the influence of the

application of force feedforward control has on theimpedance of the environment.

Given that the inertia is reduced by 1 + K,the damping and stiffness must also be reduced

7

Axes K M = Iϑ+ bϑ Error† [%] Reduction [%] Theoretical reduction [%]

0 0.0679ϑ+ 0.4397ϑ 13.78%

0.25 0.0493ϑ+ 0.3727ϑ 17.38% 27.39% 20.00%

0.5 0.0495ϑ+ 0.3884ϑ 13.47% 27.10% 33.33%

0.75 0.0397ϑ+ 0.2697ϑ 14.95% 41.53% 42.86%

1 0.0315ϑ+ 0.2238ϑ 18.32% 53.61% 50.00%

1.5 0.0280ϑ+ 0.1373ϑ 16.48% 58.76% 60.00%

x

2 0.0237ϑ+ 0.1136ϑ 12.04% 65.10% 66.67%

0 0.0606ϑ+ 0.3356ϑ 12.36%

0.25 0.0450ϑ+ 0.2234ϑ 13.88% 25.74% 20.00%

0.5 0.0397ϑ+ 0.1666ϑ 15.21% 34.49% 33.33%

0.75 0.0414ϑ+ 0.2798ϑ 14.95% 31.68% 42.86%

1 0.0367ϑ+ 0.2090ϑ 10.78% 39.44% 50.00%

1.5 0.0325ϑ+ 0.2256ϑ 10.34% 46.37% 60.00%

y

2 0.0267ϑ+ 0.1495ϑ 10.96% 55.94% 66.67%

0 0.0848ϑ+ 0.6172ϑ 6.78%

0.25 0.0603ϑ+ 0.3696ϑ 6.05% 28.89% 20.00%

0.5 0.0586ϑ+ 0.2832ϑ 7.37% 30.90% 33.33%

0.75 0.0511ϑ+ 0.2632ϑ 4.59% 39.74% 42.86%

1 0.0463ϑ+ 0.2160ϑ 6.42% 45.40% 50.00%

1.5 0.0387ϑ+ 0.1758ϑ 4.32% 54.36% 60.00%

z

2 0.0332ϑ+ 0.1405ϑ 12.67% 60.85% 66.67%†Relative error of root-mean-square-error(Rmse) to the absolute maximum value of the measured force.

Table 3: Results: Rotation.

by 1 + K. To study this hypothesis, tests sim-ilar to those performed for free movement wereconducted. An initial position, xo, was fixedand constantly sent to LWR. The stiffness usedwas Kcart =

[300 300 300 2 2 2

], being

Kcart =[kx ky kz krotz kroty krotx

]. In ad-

dition to the stiffness, the cartesian damping co-efficient was dcart = 1. The obtained data wasapproximated using least squares to equation 50.

f = mx+ βx+Kcart(x− x0) (50)

ReductionAxes K f = mx+ bx+Kcart(x− x0) Error† [%] Mass [%] Damping [%] Stiffness [%]

0 15.32x+ 110.57x+ 300.07(x− x0) 6.02%x

1 8.23x+ 52.37x+ 136.29(x− x0) 4.71% 46.28% 52.64% 54.58%0 10.72x+ 113.11x+ 252.36(x− x0) 4.85%

y1 5.44x+ 54.90x+ 103.57(x− x0) 3.61% 49.25% 51.46% 58.96%0 5.23x+ 83.87x+ 358.36(x− x0) 3.66%

z1 2.70x+ 43.66x+ 152.65(x− x0) 4.02% 48.37% 47.94% 57.40%

†Relative error of root-mean-square-error(Rmse) to the absolute maximum value of the measured force.

Table 4: Results: Influence of EnvironmentImpedance

Table 4 presents the results of the tests carriedout. With K = 1 it is expected a 50 % reductionin all components. In terms of felt mass, valuesclose to 50 % were achieved. For the damping inthe x and y translation the theoretical value is notmuch exceeded , achieving a reduction also around50 %. The stiffness is more strongly affected, withreductions in the order of 55 % to 59 %. Thisresults that in order for the user to feel the desiredstiffness, it is necessary to multiply it by 1 +K.In translation on the z axis, the stiffness estimatedwith K = 0 is higher than the specified stiffness.This estimate is higher due to the LWR configura-tion chosen in this test, that makes it difficult tomove on the z axis and easy to stay in the initialposition.

5.3. Reduction limitation

As we can see from the tables 2 and 3 differentmaximum gains for the translation (K = 1) and

rotation (K = 2) were chosen. The choice of differ-ent gains was due to the occurrence of vibrations.Other sensor filters were tested, plus other filterswere designed to try to solve the matter without anysuccess. One possible reason may be that the dy-namics imposed on LWR may be very different fromthe intrinsic hardware dynamics, therefore reachingthe limit of this control technique. It may also bedue to the configuration of the LWR, since somevibrations occur with different configurations whileusing the same gain of the FFC. That fact may becorrelated with the damping matrix, which dependson the mass matrix and this depends on the config-uration of the LWR. Figure 6 shows the spectrumof the Fast Fourier Transform (FFT) of the torqueof one joint, which shows a peak at 12-13Hz. Theocurrence of vibrations was tested in another LWR(working at sample rate of 1KHz ) from differentgeneration and these vibrations occurred. This wasalso tested with the estimation of forces and mo-ments provided from the LWR and the vibrationsstill occurred.

0 50 100 150 200 250

Frequency

-70

-60

-50

-40

-30

-20

-10

0FFT Spectrum Estimate

Marker 1 Marker 2

Signal:Fx

5696-by-1 real

Fs = 500

Parameters

Revert Apply

Method FFT

nherit from

Nfft 1024

x:

y:

x:

y:

dx:

dy:

12.695313 166.50391

-5.372633 -56.572072

153.80859

-51.199439

Figure 6: FFT Spectrum.

5.4. Nullspace configuration

The LWR, using its internal controller, appliesthe principle of superposition of the impedances. Ituses torques that come from the FRI control laws,where only their parameters can be changed. Inequations 51 and 52 is presented the FRI controllaws. Since we are working in Cartesian impedancecontrol, the terms fdynamics and τd presented inthe equation 52 are not applicable. To changethe nullspace configuration, the user must specifythe swivel angle, α, joint stiffness (kj) and thejoint damping coefficient (dj ). As for the desiredjoint configuration(qd) is the configuration that isobtained from the inverse kinematics.τcart = J

T(Kcart(Xd −Xmsr)+Fcmd)+Dcart(dcart)+fdynamics

(51)

τjoint = kj(qd − qmsr) +Dj(dj) + fdynamics + τd (52)

Using a cubic polynominal for position and veloc-ity, a path for the swivel angle, α, was planned, thatstarts with α = 0 rad (figure 7(a)) and ends with adesired swivel angle, αd = 1 rad (figure 7(b)). Thesame path was tested for different values of joint

8

stiffness in order to determine its influence on theability of the LWR to follow the reference. Theplanned trajectory lasted 20s, but after that time aconstant reference (i.e αd = 1) was sent constantlyduring ten seconds to determine if the LWR couldreach the desired swivel angle.

(a) Initial configu-ration with α = 0

(b) Final configu-ration with α = 1

Figure 7: LWR nullspace configuration.

0 5 10 15 20 25 30

Time [s]

-0.2

0

0.2

0.4

0.6

0.8

1

Sw

ivel

ang

le [r

ad]

Referencekj = 50

kj = 100

kj = 250

kj = 500

kj = 1000

kj = 2000

Figure 8: Swivel angle trajectory.

From figure 8 it is easy to conclude that theLWR cannot follow the path perfectly. It is alsopossible to conclude the higher the joint stiffness,the better the LWR can follow the path. That wasthe expected result since the higher the kj , thestiffer is the LWR’s behavior.

Table 5 presents the error when the path finishes(t = 20s) and the error after ten more seconds (t =30s). Except for kj = 250, the higher the stiffness,the smaller the error. It is also observed that theLWR, after the trajectory ends, tries to achieve thereference as expected.

6. Conclusions

This work aimed to reduce the apparent inertia ofthe LWR and implement a redundancy resolution in

kj α (t=20s) α (t=30s) Error(t=20s) Error (t=30s)50 0.9637 0.9736 0.0363 0.0264100 0.9810 0.9880 0.0190 0.0120250 0.9782 0.9833 0.0218 0.0167500 0.9926 0.9978 0.0074 0.00221000 0.9945 0.9989 0.0056 0.00112000 0.9948 0.9994 0.0052 0.0006

Table 5: Swivel angle trajectory’s errors.

order to enable setting a joint configuration in thenullspace.

To implement the redundancy resolution, it wasnecessary to perform the inverse kinematics of theLWR, and in order to solve the redundancy it wasintroduced an additional variable, the swivel angle.To set the desired configuration joints, it is alsonecessary to impose a stiffness in the nullspace,since it behaves as an impedance. Trajectory plan-ning was implemented by using cubic polynomialsfor position and velocity. The user can define theduration of the trajectory and the final swivelangle. The trajectory can also be changed duringexecution.It was found that the greater the stiffness, thelower the error of the swivel angle. However, thehigher the nullspace stiffness, less compliant theLWR will be.

To reduce inertia, a force sensor JR3 wasmounted in the end effector of the LWR. Forcefeedforward control (FFC) was applied, whichis based on taking the measurements of forcesand moments exerted by the user to the robot,multiplying them by a certain value and sendingthem to the robot. It was possible to achieve areduction about of 48 % in translation and between55 % to 65 % in rotation. It is also important tomention that, by using FFC, it was possible todecouple rotation from translation, since before itwas very difficult to make a pure rotation in one ofthe axes.

Tests were also made in order to understand howthe FFC affects the impedance of the environment.It was expected a reduction of about 50% in mass,damping and stiffness, which has been achieved formass and damping, whereas for stiffness this valuewas exceeded. The equation of the mass-spring-damper system does not include non-linearities,which may explain why the stiffness reductionof the theoretical value has been exceeded bya large difference. Although, in x and y axes,damping exceeded the theoretical value for a smalldifference, which can also show the influence ofnonlinearities. In any case, taking into account theresults when working with stiffness, the stiffnessmust be compensated in order to the user feel the

9

desired stiffness. In this test it was not possibleto assess the influence on the rotational stiffness,since it wasn’t possible to aproximate the data tothe second order equation.

In FFC strategy different maximum gains for thetranslation (K = 1) and rotation (K = 2) werechosen. The choice of different gains was due to theoccurrence of vibrations from a certain value. Itwas found that rotation was not as easily affectedby vibrations as translation. The reason behind thevibration occurrence was not determined.

7. Future Work

This work had as objective to contribute featuresto other projects of Surgical Robotics Lab, so themost natural step for the future is the integrationof the inertia reduction and redundancy resolutionin these same projects.In the nullspace configuration a simple solution tohandle with singularities was implemented. How-ever a more robust algorithm should be developedto avoid reaching singularities and also to avoid apossible collision with objects and/or people.Finally, it should be further investigated why the vi-brations occurs when the gain of the FFC achievescertain values and ways of solving these vibrations.

References[1] Fanny Ficuciello, Amedeo Romano, Luigi

Villani, and Bruno Siciliano. CartesianImpedance Control of Redundant Manip-ulators for Human-Robot Co-Manipulation.IEEE/RSJ International Conference on Intel-ligent Robots and Systems, pages 2120–2125,2014.

[2] A. Albu-Schaffer, S. Haddadin, Ch. Ott,A. Stemmer, T. Wimbock, and G. Hirzinger.The DLR lightweight robot: design and controlconcepts for robots in human environments.Industrial Robot: An International Journal,34(5):376–385, 2007.

[3] JJ Gil and E Sanchez. Control algorithms forhaptic interaction and modifying the dynami-cal behavior of the interface. International con-ference on Enactive Interfaces, 2005.

[4] Thomas Hulin, Katharina Hertkorn, PhilippKremer, Simon Schatzle, Jordi Artigas, MikelSagardia, Franziska Zacharias, and CarstenPreusche. The DLR bimanual haptic devicewith optimized workspace. Proceedings - IEEEInternational Conference on Robotics and Au-tomation, pages 3441–3442, 2011.

[5] Thomas Hulin, Mikel Sagardia, Jordi Artigas,Simon Schaetzle, Philipp Kremer, and Carsten

Preusche. Human-Scale Bimanual Haptic In-terface. 5th International Conference on En-active Interfaces, 2008.

[6] Jorge Juan Gil, Angel Rubio, and JoanSavall. Decreasing the Apparent Inertia ofan Impedance Haptic by Using Force Feedfor-ward. IEEE Transactions on Control SystemsTecnhology, 17(4):833–838, 2009.

[7] Christian Ott. Cartesian Impedance Controlof Redundant and Flexible-Joint Robots, vol-ume 49. Springer, 2008.

[8] A. Albu-Schaffer. Cartesian Impedance Con-trol of Redundant Robots : Recent Resultswith the DLR-Light-Weight-Arms. Proceedingsof the 2003 IEEE International Conference onRobotics and Automation, 2003.

[9] Kuka Systems Tecnology.KUKA.FastResearchInterface 1.0. pages1–57, 2011.

[10] Pedro Roios. A Co-manipulation RoboticSystem for Brain Biopsies. Dissertacao deMestrado em Engenharia Mecanica, InstitutoSuperior Tecnico, 2016.

[11] Leon Zlajpah. FRI-xPC server for KUKA FRIcontroller. Ljubljana, Slovenia, 2014.

[12] Gunter Schreiber, Andreas Stemmer, andRainer Bischoff. The Fast Research Interfacefor the KUKA Lightweight Robot. pages 15–21.

[13] Dale A Lawrence, Lucy Y Pao, Mark A Salada,and Anne M Dougherty. Quantitative Exper-imental Analysis of Transparency. Proc.fifthAnnual Symposium on Haptic Interfaces forVirtual Environment and Teleoperator Sys-tems, 1996.

10