Regular Paper On Inverse Kinematics with Inequality Constraints: New

June 9, 2016

Advanced Robotics

ADR-SI-2015-W.Suleiman

To appear in Advanced Robotics Vol. 00, No. 00, xx xx, 1–15

Regular Paper On Inverse Kinematics with Inequality Constraints: New Insights Into Minimum Jerk Trajectory Generation Wael Suleiman∗ Electrical and Computer Engineering Department, Faculty of Engineering, University of Sherbrooke, 2500, boul. Universite, Sherbrooke, Quebec (Canada) J1K 2R1. (Received 00 Month 201X; accepted 00 Month 201X)

The problem of inverse kinematics is revisited in the present paper. The paper is focusing on the problem of solving the inverse kinematics problem while minimizing the jerk of the joint trajectories. Even-though the conventional inverse kinematics algorithms have been proven to be efficient in many applications, it has been proven that constraints on the accelerations or the jerk cannot be guaranteed, and even yields to divergence or makes the problem unsolvable. The proposed algorithm yields smooth velocity and acceleration trajectories, which are highly desired features for industrial robots. The algorithm uses the joint jerk as the control parameter instead of the classical use of the joint velocity, as a result constraints on the jerk function can be easily incorporated. To validate the proposed approach, we have conducted several simulations scenarios. The simulation results have revealed that the proposed method can efficiently solve the inverse kinematics problem while considering constraints on the joint acceleration and jerk. Keywords: Inverse kinematics; Acceleration limits; Smooth motions; Minimum jerk; Quadratic Programming

1.

Introduction

The inverse kinematics problem is one of the most studied problems in robotics, and many methods to solve it have been proposed in the literature. Those methods are extensively used in robotics, they can be also combined with other techniques, e.g. force control, to cope with the imperfect modelling of the robot and its dynamic parameters. Generally speaking, the objective is to find a vector in the configuration space that satisfies constraints in the operational space, in other words the values of the robot’s joints to make the end-effector reaches a desired goal (position and orientation) in the Cartesian space. Although in some special cases the problem can be solved analytically [1], [2], [3] it is generally solved numerically [4, 5]. Many efficient and robust numerical algorithms have been proposed for solving the inverse kinematics problem [6], [7], [8], [4], [5], [9], [10], only to cite few. 2.

Inverse Kinematics Problem: Revisited

The inverse kinematics problem was originally formulated as follows: min q˙t T Qq˙t q˙t

∗ A preliminary version of the paper has been presented at the Workshop on Whole-Body Control for Robots in the Real World, IROS 2014. Corresponding author. Email: [email protected]

June 9, 2016

Advanced Robotics

ADR-SI-2015-W.Suleiman

subject to J q˙t = r˙t

(1)

where Q is a diagonal and positive semi-definite matrix, J is the Jacobian matrix, q˙ ∈ Rn is the joint velocity and r˙t is the linear and angular velocity of the end-effector. The optimization problem (1) can be efficiently solved using the pseudo-inverse technique [4, 5] as follows: q˙t = Jˆ† r˙t + (I − Jˆ† J)z

(2)


−1 where Jˆ† = Q−1 J T J Q−1 J T and z is an arbitrary vector. If Q is the identity matrix, Jˆ† becomes the well-known Moore-Penrose pseudo-inverse. It is worth mentioning that the optimization problem (1) is time-invariant as the time derivative can be simply replaced by the difference. Thus, we obtain the following equivalent optimization problem: min ∆q T Q∆q ∆q

subject to J∆q = ∆r

(3)

The solution of the above problem is given by: ∆qt = Jˆ† ∆rt + (I − Jˆ† J)ˆ z

(4)

However, in order to consider the velocity and joints limits as well as to avoid obstacles, inequality constraints should be added. There, many research in robotics have been conducted to solve the following general inverse kinematics problem: min q˙t T Qq˙t q˙t

subject to J q˙t = r˙t b− ≤ Aq˙t ≤ b+

(5)

q˙ − ≤ q˙t ≤ q˙ +

q˙ − and q˙ + are respectively the lower and upper limits of the velocity. A, b− and b+ represent the additional constraints that result from considering geometric constraints, such as collision avoidance (for more details, the reader is referred to the Appendix and Scenario 4 in Section 8), or physical constraints, such as joint limits. For more details and examples regarding the above-mentioned general formulation, the reader is referred to [11],[12], [13], [14] [15]. The main differences between (1) and (5) are: • There is no closed-form expression for the solution of (5). However, it is a convex optimization problem if Q is a positive semi-definite matrix, and in this case, the problem can be efficiently

June 9, 2016

Advanced Robotics

ADR-SI-2015-W.Suleiman

solved by a Quadratic Programming (QP) solver [12], [13],[14],[15]. • Problem (5) is time-variant.

3.

Motivation

The integration of acceleration limits in the inverse kinematics problem of an industrial robot manipulator is motivated by the following reasons: • Accelerations are proportional to the exerted torques by the joints actuators, therefore ensuring acceleration limits prevents that high torques are applied on the joints and thus prevents damaging the actuators. • Minimizing the jerk of joints yields to smooth and human-like motions, that is mainly interesting while interacting with humans.

A first attempt to integrate constraints on the accelerations would be using the following finite difference approach: q¨t =

q˙t − q˙t−1 T

(6)

where T is the sampling period. Then the optimization problem (5) can be modified as follows: min q˙t T Qq˙t q˙t

subject to J q˙t = r˙t b− ≤ Aq˙t ≤ b+

q˙ − ≤ q˙t ≤ q˙ +

(7)

T q¨− + q˙t−1 ≤ q˙t ≤ T q¨+ + q˙t−1

where q¨− and q¨+ are respectively the lower and upper limits of the accelerations. Recall that q˙t−1 is a constant known vector resulting from solving the inverse kinematics problem at the instant t − 1. It is clear that when T → 0, the additional constraint on the acceleration leads to q˙t = q˙t−1 , which yields to an infeasible optimization problem. In practice, even when the sampling frequency is 1 KHz , that means T = 10−3 s, the optimization problem (7) often becomes infeasible. A second attempt would be to differentiate the equality constraint in (5), thus:

˙ t , q˙t )q˙t = r¨t J(qt )q¨t + J(q

The generalized inverse kinematics problem becomes: min q¨t T Qq¨t q¨t

(8)

June 9, 2016

Advanced Robotics

ADR-SI-2015-W.Suleiman

subject to ˙ t , q˙t )q˙t J(qt )q¨t = r¨t −J(q b− ≤ Aq¨t ≤ b+

(9)

q¨− ≤ q¨t ≤ q¨+

The following assumptions are usually made: • Assumption 1: As the problem is formulated in function of the acceleration q¨t , the velocity q˙t is then supposed to be constant and equals to q˙t−1 . ˙ t , q˙t ) in a closed form, an approximation • Assumption 2: As it is very complex to compute J(q using finite difference is deployed.

Even-though the Jacobian matrix J(qt ) can be computed in an efficient way using only the generalized ˙ t , q˙t ) (Assumption 2) leads, in coordinates vector qt , using a finite deference approach to compute J(q practice, to ill-conditioning and numerical instability, this is because J(qt ) is a complex and highly nonlinear function, that yields to divergence or infeasibility of the optimization problem (9). The main contribution of the paper is providing a general inverse kinematics framework that can efficiently handle constraints on the acceleration or the jerk.

4.

Problem Formulation

In order to efficiently consider constraints on the accelerations, one would integrate the acceleration variation (commonly called jerk) into (5). The main idea is to introduce the jerk as the control parameter of the inverse kinematics problem instead of the joint velocity. The relationship between the position, velocity, acceleration and the jerk can be expressed by the following equation:     2 qt In T In T2 In q˙t  =  0 In T In  q¨t 0 0 In Where Xt =

 T q Tt

  T3  qt−1 6 In q˙t−1  +  T 2 In  q...t 2 q¨t−1 T In

(10)

T

∈ R3n is the vector that we need to compute, Xt−1 =  T T T is the actual (known) configuration values. T is the sampling control period, and q¨t−1 qt−1 q˙t−1 In is identity matrix of size n (number of degrees of freedom). The unknown parameter in (10) is the ... jerk vector qt . It is worth to mention that most of modern manipulators have a fixed sampling control period, therefore the inverse kinematics problem should be discretized in consequence. ... The objective is therefore to transform the inverse kinematic problem (5) into a function of ut = qt instead of q˙t . To this end, we can use the following formula: q˙t T

q¨t T

q˙t = q˙t−1 + T q¨t−1 +

The inverse kinematic problem becomes: min uTt Qut ut

T2 ut 2

(11)

June 9, 2016

Advanced Robotics

ADR-SI-2015-W.Suleiman

subject to Jut = r˜˙t ˜b− ≤ A ut ≤ ˜b+

(12)

+ u− q˙ ≤ ut ≤ uq˙

where: 2 r˜˙t = 2 (r˙t − J q˙t−1 − J T q¨t−1 ) T
˜b− = 2 b− − Aq˙t−1 − A T q¨t−1 2 T
˜b+ = 2 b+ − Aq˙t−1 − A T q¨t−1 2 T
2 q˙ − − q˙t−1 − T q¨t−1 u− q˙ = 2 T
2 + ˙ ˙ ¨ u+ q − q − T q t−1 t−1 q˙ = T2

(13)

4.1 Incorporating Acceleration and Jerk Constraints Let us consider the following constraint on the acceleration: q¨− ≤ q¨t ≤ q¨+

(14)

q¨t = q¨t−1 + T ut

(15)

Using the following equation:

The constraint (14) becomes:

1 − 1 + q¨ − q¨t−1 ≤ ut ≤ q¨ − q¨t−1 T T

(16)

Contrary to the acceleration constraints in (7), the above constraints will not yield to infeasibility as the limits become unbounded (−∞ ≤ ut ≤ +∞) when T → 0. Moreover, in order to generate smooth acceleration trajectories, limits can be added on the jerk as follows: u− ≤ ut ≤ u+

(17)

where u− and u+ are the lower and upper limits of the jerk vector. 4.2 Incorporating Joint Limits A compact and efficient way for dealing with both of velocity and joint limits has been originally proposed in [11]. The constraints on the joint and velocity constraints are replaced by the following con-

June 9, 2016

Advanced Robotics

ADR-SI-2015-W.Suleiman

straints: ˆ˙ − ≤ q˙t ≤ q ˆ˙ + q

(18)

ˆ˙ − and q ˆ˙ + are the generalized joint velocity limits defined as follows: where q =

(

ˆ˙ − = q j

(

ˆ˙ + q j

q˙j+ q˙j+

(qj+ −qj )−ρs ρi −ρs

if qj+ − qj ≤ ρi otherwise

q˙j− q˙j−

(qj −qj− )−ρs ρi −ρs

if qj − qj− ≤ ρi otherwise

(19)

ˆ˙ ∗ is the j element of the vector q ˆ˙ ∗ , qj is the value of joint j , q + and q − are the upper and lower limits q j j j of the joint j and q˙j+ and q˙j− are the upper and lower velocity limits of the joint j . ρi and ρs are user-defined positive constants, and ρi is usually called the influence distance. It can be easily proven that the equalities constraints in (19), not only yield a motion within the humanoid’s velocity limits, but also the joints limits are respected as well with a safety margin equals to ρs : qj− + ρs ≤ qj ≤ qj+ − ρs

5.

Practical Implementation

An issue that has been arisen in practice is that the limits on the jerk (u− ≤ ut ≤ u+ ) might yield in some special cases to divergence, a way to solve this issue is introducing a slack variable in order to relax the equality constraint, thus the optimization problem becomes: min uTt Qut + δ T Qδ δ ut ,δ

subject to Jut + δ = r˜˙t

(20)

˜b− ≤ A ut ≤ ˜b+

(21)

u− ˆ˙ q

u+ ˆ˙ q

(22)

+ u− q¨ ≤ ut ≤ uq¨

(23)

u− ≤ ut ≤ u+

(24)

≤ ut ≤

where: • δ is a slack variable that has the same dimension as r˜˙t . The matrix Qδ is a positive matrix defined in such a way that kQk  kQδ k. • (22) designs the generalized velocity constraints, where: 2 q T2 2 u+ ˆ˙ = T 2 q

u− ˆ˙ =

 

ˆ˙ − − q˙t−1 − T q¨t−1 q ˆ˙ + − q˙t−1 − T q¨t−1 q

 

June 9, 2016

Advanced Robotics

ADR-SI-2015-W.Suleiman

• (23) designs the accelerations constraints, where:


1 − q¨ − q¨t−1 T
1 + ¨ ¨ q − q u+ = t−1 q¨ T u− q¨ =

To reduce the required time to solve the above-mentioned optimization problem, one might consider reducing the number of constraints. In our case, it can be easily noticed that the constraints (22), (23) and (24) are lower and upper bounds on the variable ut , they can be therefore transformed into a more compact form as follows: u ˆ− ≤ ut ≤ u ˆ+

where: n o − − u ˆ− = max u− , u , u q¨ ˆ˙ q o n + + , u , u u ˆ+ = min u+ q¨ ˆ˙

(25)

q

The above-mentioned operators min and max are defined over the rows of the vectors. Let us introduce the following parameters: Z=

  ut , δ

  A= A 0 ,

    Q 0 , J = J I 0 Qδ  +  − u ˆ u ˆ + − Z = + , Z = − δ δ

QZ =

(26)

where δ − and δ + are user-defined constants as lower and upper limits of the slack variable δ . Thus the optimization problem is transformed into the following classical QP problem: min Z T QZ Z Z

subject to J Z = r˜˙t

˜b− ≤ A Z ≤ ˜b+

(27)

Z − ≤Z ≤ Z +

6.

Complexity Analysis

In order to compare the complexity of the new algorithm with the conventional general inverse kinematics algorithms, one can compare the QP problems (5) and (27): • On one hand, the dimension of the optimization variable in the proposed algorithm (Z in (27)) is bigger than the conventional velocity variable (q˙t in (5)). On the other hand, adding the slack variable in (27) has the advantage of increasing the robustness of the QP problem by relaxing the hard equality constraint. • The number of equality and inequality constraints is the same.

Advanced Robotics

ADR-SI-2015-W.Suleiman

As a result, the complexity of the QP problems is theoretically quite similar. The numerical simulations (Scenario 3) in Section 8 also confirmed the above conclusion. October 27, 2015 Advanced However, it Robotics is worthADR-SI-2015-W.Suleiman to note that the new algorithm can efficiently handle constraints on the acceleration and the jerk that conventional inverse kinematics algorithms simply cannot handle. As a result, the complexity of the QP problems is quite similar, the numerical simulations also con-

7.

Implementation Algorithm firmed the above conclusion.

However, it is worth to note that the new algorithm can efficiently handle constraints on the acceleration and the jerk that conventional inverse kinematics algorithms simply cannot handle.

The implementation algorithm can be summarized as follows:  T (1) Initial values: X0 = q0 0 0 and t = T 7. Implementation Algorithm (2) Compute the jacobian matrix using the actual configuration (Xt−T ) (3) Compute the upperalgorithm and lower of the jerk (25) The implementation can limits be summarized as follows: ... ⇥ ⇤ (4) Solve the optimization problem (27) and obtain ut = qt T (1) Initial values: X0 = q0 0 0 and t = T (5) Compute the configuration vector Xthe t using (2) Compute the jacobian matrix using actual(10) configuration (Xt T ) (6) t=t+T (3) Compute the upper and lower limits of the jerk (25) ... (4) Solve the 2 optimization problem (27) and obtain ut = qt (7) Return to step (5) Compute the configuration vector Xt using (10) (6) t=t+T (7) Return to step 2

8.

Experimental Results

Experimental Results method, we have conducted three simulations scenarios: In order8.to validate the proposed In order to validate the proposed method, we have conducted three simulations scenarios:

8.1

Scenario 1: acceleration limits 8.1

Scenario 1: acceleration limits 1

Joint 1

0.5 0

Y (m)

June 9, 2016

0.5 1

Joint 2 1.5

Joint 3

2 2.5

Joint 4 0

0.5

1

1.5

2 X (m)

2.5

3

3.5

4

Planar redundant the end-effectorisisdesigned designed by circle and and the robot red circles, and the end-effector Figure 1. Figure Planar1.redundant robot:robot: the end-effector bygreen green circle the joints robotbyjoints by red circles, and thetrajectory end-effector trajectory by solid cyan line. by solid cyan line.

We first considered a planar redundant manipulator (Fig. 1). The objective is to move the end-effector from an initial to a goal position while following a predefined Bezier trajectory. We firstThe considered a planar redundant manipulator (Fig. 1). The objective is to move the end-effector inequality constraints that have been considered are:

from an initial to a goal position while following a predefined Bezier trajectory. The inequality constraints that have been considered are: (1) Velocity limits (22). (2) Acceleration constraints (23).

8

Advanced Robotics Advanced Robotics ADR-SI-2015-W.Suleiman ADR-SI-2015-W.Suleiman Advanced Robotics ADR-SI-2015-W.Suleiman

June 6, 2016

ADR-SI-2015-W.Suleiman

For this scenario, wewe chose thethe following parameters: For this scenario, we chose the following parameters: For this scenario, chose following parameters: ADR-SI-2015-W.Suleiman For this scenario, we chose the following parameters: T TT • ••q˙ + q˙ qq˙˙−== 0.4 ⇥× [1[1 qq˙˙++== 0.4 =− = 0.4 ⇥ [11 111 111] 1] 1] + T TTT • ••q¨•q¨q¨+q+ = q¨ q¨q¨q−˙== 5=⇥ [1[1 550.4 × 1] ==− = ⇥ [11[11111 111] 1]1] ˙ += ⇥ 1 3 + T • •T•TT=scenario, 1010 q¨= = 5 ⇥ the [1 following 1 1 1]parameters: = 10−33q¨we=chose For •this 3 • T = 10 The trajectories the and joint acceleration areare given in in Fig. 2. 2. The tracking trajectories of joints, joint velocity and joint acceleration given tracking •The q˙ + = q˙ = of 0.4 ⇥the [1joints, 1 1joint 1]Tvelocity The trajectories of the joints, joint velocity and joint acceleration are given inFig. Fig. 2.The The tracking + T error between the desired trajectory and the executed trajectory is given in Fig. 2(d). joints, velocity and joint acceleration are in given in2(d). Fig. 2. The tracking error between and the trajectory isisgiven • q¨The =trajectories q¨ the = 5desired ⇥of[1the1trajectory 1 1]joint error between the desired trajectory and theexecuted executed trajectory given inFig. Fig. 2(d). 3 Fig. Fig. 2(c) point outout that the constraints onon thethe joint velocity and acceleration areare fully between desired trajectory and the executed trajectory isjoint given in Fig. 2(d). •error T 2(b) =2(b) 10and Fig. and Fig. 2(c) point that the constraints velocity and acceleration Fig. 2(b) andthe Fig. 2(c) point out that the constraints on the joint velocity and acceleration arefully fully respected, and Fig. 2(d) shows that the error between the executed trajectory and the desired one is less Fig. 2(b) and Fig. 2(c) point out that the constraints on the joint velocity and acceleration are fully respected, and Fig. 2(d) shows that the error between the executed trajectory and the desired one isisless respected, and Fig. 2(d) shows that the error between the executed trajectory and the desired one less 3−3 4 −4 The trajectories joints, joint velocity and joint acceleration given in 2. tracking than 1.5 ⇥× 1010 m. Itthe to mention the tracking is are less than 10Fig. mthe if we do not consider respected, and Fig. 2(d) shows that the that error between theerror executed and desired one isconsider less 3of 4 than 1.5 m. Itis isworth worth to mention that the tracking error isistrajectory less than 10 mmThe if we do not than 1.5 ⇥ 10 m. It is worth to mention that the tracking error less than 10 if we do not consider 3 m. It trajectory 4 m if we do not consider error between desired the executed trajectory is given in Fig. 2(d). than 1.5 ⇥the 10limits, ishowever worth by toand mention that the trackingconstraints error is less than 10 the acceleration however adding the acceleration the trajectory is followed as much the acceleration limits, by adding the acceleration constraints the trajectory isisfollowed asasmuch the acceleration limits, however by adding the acceleration constraints the trajectory followed much Fig. 2(b) and Fig. 2(c) point out that the constraints on the joint velocity and acceleration are fully the acceleration limits, however by adding the acceleration constraints the trajectory is followed as much asaspossible while respecting the velocity and acceleration constraints. possible while respecting the velocity and acceleration constraints. as possible while respecting thethevelocity and acceleration constraints. respected, and Fig. 2(d) shows that error between the executed trajectory and the desired one is as possible while respecting the velocity and acceleration constraints. 2 Fig. 2(a) shows that thethejoint trajectories areare smooth, mathematically speaking they areare C 2less Fig. 2(a) shows that joint trajectories smooth, mathematically speaking they Cfunctions 3shows 4m Fig. 2(a) that the joint trajectories are smooth, mathematically speaking they are C 2functions functions 2 functions than 1.5 ⇥ 10 m. It is worth to mention that the tracking error is less than 10 if we do not consider Fig. 2(a) shows that the joint trajectories are smooth, mathematically speaking they are C as a aresult ofofconsidering constraints onon thethe acceleration. It is worth to to point outout thatthat thethe conventional result considering constraints acceleration. ItItis worth point conventional asacceleration a result of considering constraints on the acceleration. is worth to point out that the conventional theas limits, however by adding the acceleration constraints the trajectory is followed as much as a resultEq. of (7) considering constraints the acceleration. Ititerations, is worth to point out that the the conventional formulation has failed totofind aonsolution after few this is is because optimization Eq. (7) has failed solution after few iterations, this the optimization asformulation possible while respecting the velocity andaaacceleration constraints. formulation Eq. (7) has failed to find find a solution solutionafter after fewiterations, iterations, this isbecause because optimization formulation Eq. (7) has failed to find few this is because the the optimization problem became infeasible. 2 problem became infeasible. Fig. 2(a) shows that the joint trajectories are smooth, mathematically speaking they are C functions problem problembecame becameinfeasible. infeasible.

Advanced Robotics

as a result of considering constraints on the acceleration. It is worth to point out that the conventional formulation Eq. (7) has failed to find a solution after few iterations, this is because the optimization problem became infeasible.

22

0.4 0.40.4

21 1 1

0.2 0.4 0.2

Rad s-1 Rad s-1-1

Rad s

Rad s-1

01 1

1 0 0 1 0

1 1 2 2 33 1 Time(s) 2 3 Time(s) 1 2Time(s)3

0

(a) (a) Joint trajectories (a) Joint Joint trajectories trajectories (a) Time(s) Joint trajectories

0.4 0.2 0.4 0.4 00

44 4

0.4

4

0

Joint 3 Joint 4

0

1

·10 33 ·10 3 ·10 11

5

11

22 33 44 1Time(s)2 3 Time(s) Time(s) 2 3 4

4

(b) (b) Joint Jointvelocities velocities (b) Joint velocities Joint velocities

Time(s) (b)

3 ·10 (b) Joint velocities

1

1

Rad s-2

00

m

m m

0 0 0

0

0

m

Rad s

Joint 2

0.2 0 0.2 0.2

(a) Joint trajectories

5 5 5

Joint 3 Joint 3 Joint 4 Joint 4 Joint 4

Joint 1

00 0

0.2

10 0 0

-2

Joint 1 Joint Joint12 Joint 1 Joint Joint23 Joint 2

0.2

Rad

Rad Rad

2

Rad

June 6, 2016

Advanced Robotics

-2 Rad sRad s-2

une 6,9, 2016 June 6, 2016 June 2016

0

11 5

5 0

5 05 0 0

1

2 3 4 1 0 1 2 3 4 1 2 3 4 0 1 2 3 4 Time(s) Time(s) 2 3 3 4 4 0 01 12 Time(s)23 4 1 1 Time(s) 2 43 (c) Joint accelerations (d) End-effector tracking error: error in x is in dashed line and in y Time(s) (c) JointTime(s) accelerations (d)in End-effector trackingTime(s) error: error Time(s) in x is in dashed line and in y solid line 1

(c) accelerations Joint accelerations (c) Joint (c) accelerations

in solid line (d) End-effector tracking error: error in xline isinin line and y (d)2.End-effector error: error in x iserror: in dashed and (d) End-effector tracking error x dashed isininy dashed lineinand Figure Scenario 1 tracking line in solid Figure in 2. solid Scenario 1in yline in solid line

Figure 2.Figure Scenario 2. 1Scenario 1

Figure 2. Scenario 1

9

9

Advanced Robotics

ADR-SI-2015-W.Suleiman

Scenario 2: jerk limits

8.2

Scenario 2:2:jerk 8.2 scenario, Scenariowe jerklimits limits the same planar redundant manipulator of Scenario 1 with the following In 8.2 this considered 8.2 Scenario 2: jerk limits inequality constraints: Advanced Robotics ADR-SI-2015-W.Suleiman In scenario, we redundant manipulator of of Scenario 1 with the the following Inthis this scenario, weconsidered consideredthe thesame sameplanar planar redundant manipulator Scenario 1 with following InVelocity this scenario, considered the same planar redundant manipulator of Scenario 1 with the following inequality constraints: inequality constraints: (1) limitswe(22). inequality constraints: (2)(1) jerk constraints Velocity limits (22). (1) Velocity limits(24). (22). (1) Velocity limits (22). (2) jerk constraints (24). 8.2 Scenario 2: jerk limits (2) jerk constraints (24). For this scenario, we chose the following parameters: (2) jerk constraints (24).

For this we the following parameters: In this we considered the same planar manipulator of Scenario 1 with the following For+scenario, thisscenario, scenario, wechose chose the following parameters: T redundant ˙ ˙ • q = q = 0.4 ⇥ [1 1 1 1] For this scenario, we chose the following parameters: inequality constraints: ...+ ... T + = qq˙ − 0.4 ⇥ [1 1 1 1]T T • •q = = −q˙ ==500 • q˙q˙ + 0.4⇥ ×[1 [1 11 11 1] 1]

......+q+˙ + =limits ...q˙−(22). (1)• Velocity =500 0.4⇥⇥[1 [1 11 11 1] 1]TT T 3 q... = • T •q= q ...10 q...== = − 500 × [1 1 1 1] + (2) •jerk constraints (24). q = −3 • = 3 q = 500 ⇥ [1 1 1 1]T •• T 10 T = 10 3 • Tscenario, = 10 Fig. and Fig.we 3(c) point out that the constraints on the joint velocity and jerk are fully respected. For3(b) this chose the following parameters:

Fig. 3(b) and Fig. 3(c) point out that thetheconstraints onon the joint velocity and jerk areare fully respected. Fig. 3(b) and 3(c) point out that constraints the joint velocity jerk fully respected. On one hand, 3(d) shows that the has been slightly increased inand comparison with Scenario + = Ttracking Fig. 3(b)Fig. and=Fig. Fig. 3(c) point out that the constraints on the joint velocity and jerk are fully respected. ˙ ˙ •one q q 0.4 ⇥ [1 1 1 1] On hand, Fig. 3(d) shows that the tracking has been slightly increased in comparison with Scenario ...+mainly ...Fig. OnOnone hand, 3(d) shows that the tracking has been slightly increased in comparison with Scenario 1, this is a result of respecting the jerk constraints. On the other hand, Fig. 3(a) shows that T tracking has been slightly increased in comparison with Scenariojoint 3(d) shows q Fig. • qone =hand, =result 500 ⇥of [1respecting 1 that 1 the 1]the 1, this is mainly a jerk constraints. OnOn thethe other hand, shows that jointjoint 33(a) 1, this is mainly a result of respecting jerk constraints. other hand, Fig. 3(a) shows 3smoother trajectories are those of the Scenario 1, this is because they areFig. functions in that this 1, mainly a resultthan of respecting the jerk constraints. On the other hand, Fig. 3(a) shows that jointcase. • this T =is10 3 Cfunctions 3 trajectories are smoother than those of Scenario 1, this is because they are C in this case. smoother than those of Scenario 1,cannot thisisisbecause because theyareare functions in this case. It istrajectories important to the obtained resultas be compared with the conventional inverse trajectoriesare aremention smootherthat than those of Scenario 1, this they C 3Cfunctions in this case. ItItFig. isisimportant totomention that the obtained resultas cannot bevelocity compared with thefully conventional inverse 3(b) and Fig. 3(c) point out that the constraints on the joint and jerk are respected. important mention that the obtained resultas cannot be compared with the conventional inverse It is important to mention that thethose obtained resultassimply cannotcannot be compared the conventional inverse kinematics methods, this is because methods handlewith constraints on the jerk. kinematics is because those methods cannot handle constraints on Scenario thethe jerk. On one hand,methods, Fig. 3(d) this shows that the tracking beensimply slightly increased in comparison with kinematics methods, this because those methods simply cannot handle constraints jerk. kinematics methods, thisisis because those has methods simply cannot handle constraints onon the jerk.

0.2 0.2 0.2 Rad s-1 Radss-1-1 Rad

11 1

Rad Rad

Rad

1, this is mainly a result of respecting the jerk constraints. On the other hand, Fig. 3(a) shows that joint trajectories are smoother than those of Scenario 1, this is because they are C 3 functions in this case. It is important to mention that the obtained resultas cannot be compared with the conventional inverse 22 0.4 0.4 kinematics2 methods, this is because those methods simply cannot0.4 handle constraints on the jerk.

2

0.4

00 0

00 0

1

11 1

22 2 Time(s) Time(s)

33

3

Joint 4

0.4 0.4 0.4 00 0

0.2

4

Time(s)

4

0

2

(b) Joint velocities

2

(b) Joint velocities

3

·10

11

1 1

m m m

Rad s-3

1

00

0 0

0

11

00

(b) Joint (b)Joint Jointvelocities velocities4 2(b) 3velocities

22 ·10

(a) Joint trajectories

0 0

500

44 4

3 ·10 ·10 3Time(s) 3

500

500500 0 500 0

22 2 33 3 Time(s) Time(s)

Time(s)

Time(s)

500

11 1

0.4

(a) Joint trajectories

500500

Joint 3

0

44

Joint trajectories Joint trajectories3 (a) 1(a) (a) 2trajectories

0

Joint 2

0.2 0.2 0.2

Rad s-1

Rad

101 1

Joint 1

Joint 1 Joint 1 Joint 2 Joint 2 Joint 2 Joint 3 Joint 3 Joint 3 Joint 4 Joint 4 Joint 4

Joint 1

000

0.2

1

Rad s-3Rad s-3

June 6, 2016

ADR-SI-2015-W.Suleiman ADR-SI-2015-W.Suleiman

m

June 6, 2016

AdvancedRobotics Robotics Advanced

Rad s-3

June6, 9,2016 2016 June

1

11 11

22 Time(s) Time(s) 2 2

(c) Joint jerks (c) Joint jerks Time(s) Time(s)

(c) jerks (c)(c)Joint Joint jerks Joint jerks

33 3

3

44 4

0 1 22 0 0

4 20

2

0 1

11

12

22 Time(s) Time(s) 2 3

33

44 43

4

(d) End-effector tracking error: error in x is in dashed line and in y (d) End-effector tracking error: errorTime(s) in x is in dashed line and in y Time(s) in solid line in solid line (d) End-effector tracking error x is line in andy error: error inerror: x iserror: inerror dashed inin y dashed (d) End-effector tracking in line x in isand in dashed line and Figure(d)3. End-effector Scenario 2 tracking Figure 3. Scenario 2in y in solid line in solid line in solid line Figure 3.Figure Scenario 2 3. Scenario 2

Figure 3. Scenario 2

10

10 10

June 9, 2016

Advanced Robotics

8.3

ADR-SI-2015-W.Suleiman

Scenario 3: Computational Complexity

A simulated scenario of a Baxter research robot in which the robot should reach a randomized goal pose (position and orientation) from a rnadomized initial pose, both the initial and goal poses are in the reachable space of the robot (Fig. 4).

(a)

(b)

Figure 4. 6D Kinematic reachability space of Baxter’s left arm. The color density increases according to the number of possible configurations.

The reaching motion has been repeated 5000 times and only the joint velocity constraints have been considered. The computational time of the conventional method and the proposed one are reported in Table 1. The experiments were performed on Intel(R) Core(TM) i5-3470 CPU @ 3.20 GHz PC with 8 GB RAM. Table 1. Computational complexity

Method Conventional method Proposed method

Average comput. time 542µs 583µs

Max comput. time 621µs 638µs

Min comput. time 485µs 512µs

From Table 1, we can observe that the computational time of the proposed method and of the conventional one are quite similar. However, it is worth to mention that we have only considered the velocity constraints because the conventional method cannot handle the jerk or the acceleration constraints. 8.4

Scenario 4: Smooth and Collision-free Motions

A simulated scenario of a Baxter research robot and an obstacle that consists of a sphere attached to a thin cylindric rod is given in Fig. 5. The objective is to reach a goal pose from an initial pose while avoiding the collision with the obstacle. As mentioned in Section 2, the collision avoidance problem can be formulated as kinematics inequality constraints [16], an overview of the formulation can be found in Appendix A. In this scenario, the following constraints have been considered: (1) Collision avoidance: the Baxter robot is approximated by its collision geometry model from the Unified Robot Description Format (URDF) file, where the arms are approximated by cylinders and boxes. The collision avoidance is formulated as inequality constraints having the following form: Aq˙t ≤ B . (2) Joint limits of the robot joints, these values are given in the URDF file.

Advanced Robotics

ADR-SI-2015-W.Suleiman

(3) Velocity limits of the robot’s joints, these values are also given in the URDF file. (4)Robotics DesiredADR-SI-2015-W.Suleiman joint acceleration and jerk limits. Advanced The tries goalthese posevalues at each straight (3)end-effector Velocity limits of to thereach robot’sthe joints, areiteration also givenvia in athedirect URDF file. line, at the same time(4) theDesired collision avoidance inequality constraints repulse the arm to keep a minimum clearance to the joint acceleration and jerk limits. obstacles. (3)The Velocity limits oftries the robot’s joints, these pose valuesatare alsoiteration given invia the aURDF file. end-effector to reach the are goal each direct straight line, at are the shown same in Snapshots theacceleration simulated motion given in Fig. 5. The right arm’s joint trajectories (4) Desired of joint and jerk limits. time the collision avoidance inequality constraints repulse the arm to keep a minimum clearance to the Fig. 6(a) and the minimum distance to the obstacles is given in Fig. 6(b). The end-effector tries to reach the goal pose at each iteration via a direct straight line, at the same obstacles. time Snapshots the collision inequality constraints arm right to keep a minimum clearanceare to the ofavoidance the simulated motion are givenrepulse in Fig.the 5. The arm’s joint trajectories shown in obstacles. Fig. 6(a) and the minimum distance to the obstacles is given in Fig. 6(b). Snapshots of the simulated motion are given in Fig. 5. The right arm’s joint trajectories are shown in 1 4 2 to the obstacles is given in3 Fig. 6(b). Fig. 6(a) and the minimum distance

1 1

4

3

2

5

6 5

8

7

6

5

4

3

2

8

7

6

8

7

Figure 5. Reaching scenario while avoiding collision Figure 5. Reaching scenario while avoiding collision Figure 5. Reaching scenario while avoiding collision

2

1

2

right e0 right e0 0.2 right e1 0.2 right e1 right s0 right s0 right s1 right s1 right w0 right w0 right w1 0.15 0.15 right w1

1

0 0 1 1 0 0

m

June 6, 2016

ADR-SI-2015-W.Suleiman

m

June 6, 2016

Advanced Robotics

Rad Rad

June 9, 2016

0.1 0.1 1 1 2 2 3 3 4 Time Time (s) (s)

4 5

5

0

(a)(a)Baxter’s right armjoint jointjoint trajectories (a) Baxter’s arm trajectories Baxter’s rightright arm trajectories

0 1

12 23 34 Time (s) Time (s)

(b) distance obstacles (b) Minimum Minimum totoobstacles (b) Minimum distancedistance to obstacles

FigureFigure 6. Scenario 3 Figure Scenario 6.6. Scenario 33

12 12

45

5

June 9, 2016

Advanced Robotics

9.

ADR-SI-2015-W.Suleiman

Conclusion and Future Work

In this paper, a new method for inverse kinematics while considering jerk limits has been proposed. The method is simple to implement, yet efficient to handle acceleration and jerk constraints. As discussed in the paper, several attempts have been proposed in the literature to integrate the acceleration limits into the conventional inverse kinematics algorithms, however they are not numerically efficient and yield often to ill-conditioned optimization problems. Moreover, the proposed algorithm guarantees that the accelerations of the joints are smooth by defining a threshold on the jerk. Simulation results have been conducted and the efficiency of the algorithm has been pointed out. Future work will focus on the realtime implementation of the proposed algorithm and the validation on the real Baxter research robot. Acknowledgment This research is partially supported by a discovery grant (Prof. Wael Suleiman) from the Natural Sciences and Engineering Research Council of Canada (NSERC). References [1] Raghaven M, Roth B. Kinematic analysis of the 6r manipulator of general geometry. In: The Fifth International symposium on robotics research. 1990. p. 263–269. [2] Raghavan M, Roth B. Solving polynomial systems for the kinematic analysis and synthesis of mechanisms and robot manipulators. Journal of Mechanical Design. 1995 06;117(B):71–79. [3] Neppalli S, Csencsits MA, Jones BA, Walker ID. Closed-form inverse kinematics for continuum manipulators. Advanced Robotics. 2009;23(15):2077–2091. [4] Nakamura Y. Advanced Robotics: Redundancy and Optimization. Addison-Wesley. 1991. [5] Siciliano B, Sciavicco L, Villani L, Oriolo G. Robotics: Modelling, planning and control. Springer. 2009. [6] Whitney DE. The mathematics of coordinated control of prosthetic arms and manipulators. Journal of Dynamic Systems, Measurement, and Control. 1972 12;94(4):303–309. [7] Nakamura Y, Yamane K. Dynamics computation of structure-varying kinematic chains and its application to human figures. IEEE Transactions on Robotics and Automation. 2000 Apr;16(2):124–134. [8] Chan S, Lawrence P. General inverse kinematics with the error damped pseudoinverse. In: IEEE International Conference on Robotics and Automation (ICRA). 1988 Apr. p. 834–839 vol.2. [9] Flacco F, De Luca A. Optimal redundancy resolution with task scaling under hard bounds in the robot joint space. In: IEEE International Conference on Robotics and Automation (ICRA). 2013. p. 3969–3975. [10] Siciliano B. Kinematic control of redundant robot manipulators: A tutorial. Journal of Intelligent and Robotic Systems. 1990;3(3):201–212. [11] Kanehiro F, Lamiraux F, Kanoun O, Yoshida E, Laumond J-P. A Local Collision Avoidance Method for Non-strictly Convex Objects. In: Proceedings of Robotics: Science and Systems. Zurich, Switzerland. 2008 June. [12] Kanehiro F, Morisawa M, Suleiman W, Kaneko K, Yoshida E. Integrating geometric constraints into reactive leg motion generation. In: IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). 2010. p. 4069–4076. [13] Kanoun O. Real-time prioritized kinematic control under inequality constraints for redundant manipulators. In: Proceedings of Robotics: Science and Systems. Los Angeles, CA, USA. 2011 June. [14] Escande A, Mansard N, Wieber PB. Fast resolution of hierarchized inverse kinematics with inequality constraints. In: IEEE International Conference on Robotics and Automation (ICRA). 2010. p. 3733–3738. [15] Escande A, Mansard N, Wieber PB. Hierarchical quadratic programming: Fast online humanoid-robot motion generation. The International Journal of Robotics Research. 2014;33:1006–1028. [16] Faverjon B, Tournassoud P. A local based approach for path planning of manipulators with a high number of degrees of freedom. IEEE International Conference on Robotics and Automation (ICRA). 1987. p. 1152– 1159.

Advanced Robotics

ADR-SI-2015-W.Suleiman

Appendix A. Collision avoidance as kinematics constraint

Let us consider the two strictly convex objects O1 and O2 in Fig. A1, where O2 is supposed to be fixed obstacle and O1 is a part of the robot. Let d be the distance between the pair of closest points p1 and p2 belonging to O1 and O2 respectively. Since O1 and O1 are strictly convex objects, d is C 1 function.

d 1

n

p2 ds

O1 p

O2

di

June 9, 2016

Figure A1. Collision avoidance as kinematics inequality constraints

In order to incorporate the collision avoidance as an inequality constraint, the derivative of distance with respect to time, d˙, is bounded as follows:

−d˙ ≤ ζ

d − ds ; di − ds

di > ds

(A1)

where ζ is a positive coefficient that regulates the convergence speed, and di and ds are called respectively influence and security distances. As O2 is fixed, d˙ can be expressed as follows: d˙ = nT p˙1

(A2)

2) . where n is a unit vector defined as: n = (p1 −p d As O1 is a part of the robot, p˙1 can be expressed as a function of the robot generalized coordinates, q , its derivatives, q˙, and the jacobian matrix at the point p1 , J(q, p1 ), as follows:

p˙1 = J(q, p1 ) q˙

(A3)

By replacing A2 and A3 in A1, we obtain:

−nT J(q, p1 ) q˙ ≤ ζ

d − ds di − ds

(A4)

As it can be easily seen that (A4) is a linear inequality constraint with respect to the velocity vector q˙. Moreover, the above method has been extended to the case of non-strictly convex objects [11].