# Energy-Optimal Trajectory Planning for Planar UnderactuatedRR Robot Manipulators in the Absence of Gravity

Authors: John Gregory; Alberto Olivares; Ernesto Staffetti
Journal: Abstract and Applied Analysis
 (2013)

---

## Abstract

In this paper, we study the trajectory planning problem for planar underactuated robot manipulators with two revolute joints in the absence of gravity. This problem is studied as an optimal control problem in which, given the dynamic model of a planar horizontal robot manipulator with two revolute joints one of which is not actuated, the initial state, and some specifications about the final state of the system, we find the available control input and the resulting trajectory that minimize the energy consumption during the motion. Our method consists in a numerical resolution of a reformulation of the optimal control problem as an unconstrained calculus of variations problem in which the dynamic equations of the mechanical system are regarded as constraints and treated using special derivative multipliers. We solve the resulting calculus of variations problem using a numerical approach based on the Euler-Lagrange necessary condition in integral form in which time is discretized and admissible variations for each variable are approximated using a linear combination of piecewise continuous basis functions of time. The use of the Euler-Lagrange necessary condition in integral form avoids the need for numerical corner conditions and the necessity of patching together solutions between corners.


---

## Body

## 1. Introduction

The class of underactuated manipulators includes robots with rigid links and unactuated joints, robots with rigid links and elastic transmission elements, and robots with flexible links. Whereas in the first case, underactuation is a consequence of design, in the other cases, it is the result of an accurate dynamic modeling of the system, in which the control inputs only have effect on the rigid-body motion. In any case, the number of available control inputs is strictly less than the number of the degrees of freedom of the robot. However, the control problem of different underactuated manipulators may have different levels of difficulty. For example, the absence of gravity significantly increases the complexity of the control problem.In this paper we study the trajectory planning problem for planar horizontal underactuated robot manipulators with two revolute joints. The presence of two revolute joint in the mechanical system will be denoted byRR. We consider both possible models of planar horizontal underactuated RR robot manipulators, namely, the model in which only the shoulder joint is actuated, which will be denoted by RR¯, and the model in which only the elbow joint is actuated, which will be denoted by R¯R.Underactuated robots are mechanical systems with second-order nonholonomic constraints because the dynamic equation of the unactuated part of the mechanical system is a second-order differential constraint which, in general, is nonintegrable. It is not possible to integrate even partially this second-order differential constraint in the dynamic model of theRR¯ robot manipulator. However, in the presence of this second-order nonholonomic constraint, the system is controllable. On the contrary, in the dynamic model of the R¯R robot manipulator, this differential constraint is completely integrable. It can be converted into a holonomic constraint that makes the system not controllable. As a consequence, the trajectory planning problem has solutions only for particular initial and final states.Planning of dynamically feasible trajectories, their asymptotic tracking, and the regulation to a desired equilibrium configuration are the main control problems for this class of mechanical systems. However, a general control theory for this class of mechanical systems has not been developed yet, and only solutions for particular robot models have been obtained. A review of the most significant works on control of underactuated robots with passive joints can be found in [1].In this paper, the trajectory planning problem for planar horizontal underactuatedRR robot manipulators is studied as an energy-optimal control problem. Given the initial and final states, we find the available control inputs and the corresponding trajectory that satisfy the dynamic equation of the robot manipulator and steer the system between initial and final states minimizing the energy consumption during the motion. This problem is referred to as boundary value problem. We also consider initial value problems in which the final state is not completely specified. Our numerical method can also tackle final value problems in which part of the initial state is not specified.It is usually impossible to find analytical solutions to optimal control problems of robot manipulators, and, in general, numerical methods must be employed. To solve our optimal control problem, we apply a numerical method that falls into the class of indirect methods which are based on first-order necessary optimality conditions. More precisely, it is a variational approach in which the optimal control problem is transformed into an unconstrained calculus of variations problem by means of special derivative multipliers [2, Chapter VII].Our method substantially differs from the usual indirect approaches in which the Euler-Lagrange differential equation is solved using a suitable numerical method. To compute the extremals, we use the Euler-Lagrange necessary condition in integral form plus transversality conditions to take into account the components of state and control variables and multipliers that are not specified at the endpoints [3, Chapter 6]. In this way, we avoid the loss of information induced by the use of the differential form of the same condition that implies numerical corner conditions and the necessity of patching together solutions between corners.Moreover, in our method, the control inputs are derivatives of some components of the extended state vector which can be piecewise continuous functions, while the original state vector is supposed to be composed by piecewise smooth functions. Similarly, the multipliers which are the derivatives of other components of the extended state vector need only to be piecewise continuous. In our approach, time is discretized, and admissible variations for each variable are approximated by linear combinations of piecewise continuous basis functions of time. In this way, variations depend on the values of the coefficients at the mesh points. The conditions under which the objective functional is stationary with respect to all piecewise smooth variations that satisfy the boundary conditions are derived, and the set of nonlinear difference equations that must be satisfied by the coefficients is obtained. This set of equations is then solved using the Newton-Raphson method. This basic procedure can be modified to incorporate equality and inequality constraints by means of derivative multipliers and derivative excess variables.In [4], the necessary conditions for optimal control are derived using the ideas of Lagrangian reduction that is reduction under a symmetry group. The techniques presented in this work are designed for Lagrangian mechanical holonomic and nonholonomic systems with symmetry. The key idea is to link the method of Lagrange multipliers with Lagrangian reduction as an alternative to the Pontryagin Maximum Principle and Poisson reduction. [5, Chap. 7] is devoted to optimal control of nonholonomic mechanical systems. The relationship between variational nonholonomic control systems and the classical Lagrange problem of optimal control is presented. Then, kinematic and dynamic optimal control problems are discussed whereas related work on integrable systems is studied in the Internet supplement of this book. In [6] an affine connection formulation is used to study an optimal control problem for a class of nonholonomic, underactuated mechanical systems. The class of nonholonomic systems studied in this paper are wheeled vehicle. The nonholonomic affine connection together with Lagrange multiplier method in the calculus of variations is used to derive the optimal necessary conditions.The mechanical systems studied in this paper have similarities with the Pendubot and the Acrobot which are underactuated two-linkRR robot manipulators that move in a vertical plane and therefore are subjected to gravity force. In the Pendubot, only the shoulder joint is actuated, whereas in the Acrobot, only the elbow joint is actuated,. The control objective is usually in both cases to drive the manipulator away from the straight-down position and steer it at the straight-up position. In [7], a unified strategy for motion control of underactuated two-link manipulators with gravity, such as the Acrobot and the Pendubot, is presented. First, a control law is employed to increase the energy and control the posture of the actuated link in the swing-up region. Finally, an optimal control law is designed for the attractive region using a linear approximation model of the system around the straight-up position. In [8], a general control methodology for swinging up and stabilizing underactuated two-link robots is presented. It is based on Euler-Lagrange dynamics, passivity analysis, and dynamic programming theory. In [9], two different approaches for feedforward control design are presented. The first approach is based on a coordinate transformation into the nonlinear input-output normal form, whereas the second approach uses servo constraints and results in a set of differential algebraic equations.To the best knowledge of the authors, the optimal control of underactuatedRR robot manipulators without gravity and without breaks has not been addressed.This paper is organized as follows. In Section2, the dynamic models of the two planar horizontal underactuated RR robot manipulators are described, and in Section 3, their control properties are discussed. The optimal control problem for these dynamic systems is stated in Section 4. In Section 5, a reformulation of the optimal control problems as a calculus of variations problem is presented, and in Section 6 the proposed numerical method to solve the resulting calculus of variations problem is described. In Section 7, the results of the application of this numerical method to several optimal control problems for planar horizontal underactuated RR robot manipulators are reported. Finally, Section 8 contains the conclusions.


## 2. Dynamic Model of Underactuated Manipulators

The general dynamic model of a robot manipulator is described by the following second-order differential equation(1)B(θ)θ¨+C(θ,θ˙)θ˙+Fθ˙+e(θ)=u-G(θ)w,
where the first term of this equation, B(θ)θ¨, represents the inertial forces due to acceleration at the joints and the second term, C(θ,θ˙)θ˙, represents the Coriolis and centrifugal forces. The third term, Fθ˙, is a simplified model of the friction in which only the viscous friction is considered. The term, e(θ), represents the potential forces such as elasticity and gravity. Matrix G(θ) on the right-hand side maps the external forces/torques w to forces/torques at the joints. Finally u represents the forces/torques at the joints that are the control variables of the system.We suppose that the links are rigid as well as the transmission elements and that the robot moves in a horizontal plane in such a way the gravity does not affect the dynamics of the manipulator. Finally, we do not take into account the effects of the friction, and we suppose that no external forces are acting on the mechanical system. Under these hypotheses, the dynamic model of the robotic system reduces to(2)B(θ)θ¨+C(θ,θ˙)θ˙=u.A horizontal planarRR manipulator is composed of two homogeneous links and two revolute joints moving in a horizontal plane {x,y}, as shown in Figure 1, where li is the length of link i, ri is the distance between joint i and the mass center of link i, mi is the mass of link i, and Izi is the barycentric inertia with respect to a vertical axis z of link i, for i=1,2. In this case the two matrices B(θ) and C(θ,θ˙) have the form
(3)B(θ)=[α+2βcosθ2δ+βcosθ2δ+βcosθ2δ],C(θ,θ˙)=[-βsinθ2θ˙2-βsinθ2(θ˙1+θ˙2)βsinθ2θ˙10],
where θ=(θ1,θ2)T is the vector of configuration variables, being θ1 the angular position of link 1 with respect to the x axis of the reference frame {x,y} and θ2 the angular position of link 2 with respect to link 1 as illustrated in Figure 1. The vector θ˙=(θ˙1,θ˙2)T is the vector of angular velocities, and θ¨=(θ¨1,θ¨2)T is the vector of accelerations. The control inputs of the system are u=(u1,u2)T, where u1 is the torque applied by the actuator at joint 1 and u2 is the torque applied by the actuator at joint 2. The parameters α, β, and δ in (3) have the following expressions:
(4)α=Iz1+Iz2+m1r12+m2(l12+r22),β=m2l1r2,δ=Iz2+m2r22.Figure 1
An underactuated two-link robot manipulator that moves in a horizontal plane in which only one of the joints is actuated.A robot manipulator is said to be underactuated when the number of actuators is less than the degree of freedom of the mechanical system. The dynamic model (2) that does not consider the effects of gravity and friction can be rewritten for a RR robot manipulator underactuated by one control in the form [1]
(5)[Baa(θ)Bua(θ)Bua(θ)Buu(θ)](θ¨aθ¨u)+[Ca(θ,θ˙)Cu(θ,θ˙)]=(ua0),
in which the state variables θa and θu correspond to the actuated and unactuated joints and ua is the available control input. The last equation of (5) describes the dynamics of the unactuated part of the mechanical system and has the form
(6)Bua(θ)θ¨a+Buu(θ)θ¨u+Cu(θ,θ˙)=0,
which is a second-order differential constraint without input variables.Underactuated manipulators may be equipped with brakes at the passive joints. Hybrid optimal control strategies can be designed in this case [10, 11]. The presence of brakes will not be considered in this paper.


## 3. Control Properties of UnderactuatedRR Robot Manipulators

In this section, the main control properties of planar underactuatedRR robot manipulators without the effects of the gravity will be described. For a more general description of the control properties of underactuated robot manipulators, see [1].Optimal control approaches to trajectory planning assume that there exists a control input that steers the system between two specify states. Thus, controllability is the most important aspect to check before studying optimal control of a dynamic system. If in the trajectory planning problem the duration of the motionT is not assigned, the existence of a finite-time solution for any state (θF,θ˙F) in a neighborhood of (θI,θ˙I) is equivalent for the robotic system to the property of local controllability at (θI,θ˙I). If local controllability holds at any state, then the system is controllable and the trajectory planning problem is solvable for any pair of initial and final states.For underactuatedRR robot manipulators, controllability is related to integrability of the second-order nonholonomic constraints. The second-order differential constraint (6) may either be partially integrable to a first-order differential equation or completely integrable to a holonomic equation. Necessary and sufficient integrability conditions are given in [12, 13]. If (6) is not partially integrable it is possible to steer the system between equilibrium points. This occurs for planar underactuated RR¯ robot manipulators without gravity which therefore are controllable. If (6) is completely integrable, to a holonomic constraint, the motion of the mechanical system is restricted to a 1-dimensional submanifold of the configuration space which depends on the initial configuration. This occurs for the dynamic equations of planar underactuated R¯R robot manipulators without the effects of the gravity [12]. For this robot model, the trajectory planning problem has solution only for particular initial and final states. Thus, when (6) is not partially or completely integrable, the mechanical system is controllable. However, several aspects of controllability can be studied which characterize this model of underactuated manipulator.A dynamical system is linearly controllable at an equilibrium point if the linear approximation of the system around this point is controllable. Planar underactuatedRR¯ robot manipulators in the absence of gravity are not linearly controllable. On the contrary both planar underactuated RR¯ and R¯R robot manipulators are linearly controllable in the presence of gravity.A mechanical system is said to be small-time locally controllable (STLC) at xI=(θI,θ˙I) if, for any neighborhood 𝒱x of xI and any time T>0, the set ℛ𝒱x,T(xI) of states that are reachable from xI within time T, along trajectories contained in 𝒱, includes a neighborhood of xI. Note that small-time local controllability is a stronger property than controllability [14]. Non-STLC but controllable system must in general perform finite maneuvers in order to perform arbitrarily small changes of configuration. It has been proven in [15, 16] that planar underactuated RR¯ robot manipulators in the absence of gravity are not STLC. Both planar underactuated RR¯ and R¯R robot manipulators in the presence of gravity are also not STLC.Second-order mechanical systems cannot beSTLC at states with nonzero velocity. Therefore, the weaker concept of small-time local configuration controllability has been introduced [17]. A system is said to be small-time local configuration controllable (STLCC) at a configuration θI if, for any neighborhood 𝒱θ of θI in the configuration space and any time T>0, the set ℛ𝒱θ,T(θI) of configurations that are reachable (with some final velocity θ˙) within T, starting from (θI,0) and along a path in configuration space contained in 𝒱θ, includes a neighborhood of θI. By definition, STLC systems are also STLCC. Sufficient conditions for STLCC are given in [17]. It has been proven in [15, 16] that planar underactuated RR¯ robot manipulators in the absence of gravity are not STLCC. Both planar underactuated RR¯ and R¯R robot manipulators in the presence of gravity are also not STLCC.A final question is to investigate if the trajectory planning problem forRR planar underactuated robot manipulators can be solved with algorithmic methods. A mechanical system is kinematically controllable (KC) if every configuration is reachable by means of a sequence of kinematic motions, that is, feasible paths in the configuration space which may be followed with any arbitrary timing law [15, 16, 18]. Note that KC mechanical systems are also STLCC and that kinematic controllability does not imply small-time local controllability. If a mechanism is KC, the trajectory planning problem may be solved with algorithmic methods. Planar underactuated RR¯ robot manipulators in the absence of gravity are not KC. Both planar underactuated RR¯ and R¯R robot manipulators in the presence of gravity are not KC [15].


## 4. The Optimal Control Problem

Given the dynamic equation of an underactuated planarRR robot manipulator, an initial state, (θI,θ˙I), and a final state, (θF,θ˙F), the optimal control problem consists in finding the available control input, u1(t) or u2(t), and the resulting trajectory with t∈[tI,tF] that steers the system between initial and final states satisfying the dynamic equation (5) and minimizing the objective functional
(7)J=∫tItF‍ui2dt,
where i=1,2 depending on which joint is actuated and tI and tF are the initial and final time values, respectively. This cost functional represents a measure of the energy consumed during the motion, since torque produced with an electromechanical actuator is approximately proportional to the current flow, and the rate of energy consumption is approximately equal to the square of this current.Ifθ˙I=θ˙F=0, the problem is called rest-to-rest trajectory planning problem. If the final or the initial states or part of them is not assigned, the problems are called initial value problem and final value problem, respectively. The final time tF may be fixed or not.This problem is a particular case of an optimal control problem which can be stated in a more general form as follows. Minimize the integral(8)J(x,u)=∫tItF‍f(t,x,u)dt
subject to
(9)x′(t)=g(t,x,u),(10)h(t,x,u)=0,(11)l(t,x,u)≤0,(12)p(t,x)=0,(13)q(t,x)≤0,(14)x(tI)=xtI,x(tF)=xtF,(15)u∈U,
where x(t)=(x1(t),x2(t),…,xn(t))T is an n-vector called the state vector, u(t)=(u1(t),u2(t),…,um(t))T is an m-vector called the control vector, the real-valued function J(x,u) is the objective functional, (9) is called the trajectory equation, and the conditions (14) are called the boundary conditions. The set U⊂ℝm is called the set of controls, with u(t)∈U for every t∈[tI,tF]. We assume that f, g, h, l, p, and q are sufficiently smooth for our purpose. This will imply solutions such that x(t) is piecewise smooth, whereas u(t) is piecewise continuous [19].


## 5. Variational Reformulation of the Optimal Control Problem

A variational approach has been used to solve the more general optimal control problem stated in the previous section.The classical calculus of variations problem is to minimize an integral of the form(16)I(y)=∫ab‍f(x,y,y′)dx,
such that
(17)y(a)=A,y(b)=B,
where the independent x variable is assumed to be in the interval [a,b] and the dependent variable y=y(x)=(y1(x),y2(x),…,yn(x))T is assumed to be an n-vector continuous on [a,b] with derivative y′=y′(x)=(y1′(x),y2′(x),…,yn′(x))T. It is also assumed that y is piecewise smooth, that is, there exists a finite set of points a1,a2,…,ak so that a≤a1<a2<⋯<ak≤b, y(x) is continuously differentiable on (al,al+1) and that the respective left- and right-handed limits of y′(x) exist. If y(x) is piecewise smooth, and satisfies the boundary conditions y(a)=A, y(b)=B, then y(x) is said to be an admissible arc. In words, this problem consists in finding, among all arcs connecting end points (a,A) and (b,B), the one minimizing the integral (16).The main optimality conditions are obtained by defining a variationz(x), a set of functions
(18)y(x,ϵ)=y(x)+ϵz(x)for|ϵ|<δ,
and a functional
(19)F(ϵ)=∫ab‍f(x,y(x,ϵ),y′(x,ϵ))dx,
where δ>0 is a fixed real number and the variation z(x) is a piecewise smooth function with z(a)=z(b)=0. Using a Taylor series expansion, it is easy to see that a necessary condition that 0 is a relative minimum to F is
(20)I′(y,z)=∫ab‍[fyz+fy′z′]dx=0,
where fy,fy′ denote the partial derivatives of f evaluated along (x,y(x),y′(x)) and the terms z and z′ are evaluated at x.Integrating (20) by parts for all admissible variations z(x), another necessary condition for y=y(x) to give a relative minimum of the variational problem (16)-(17) is obtained, which is the following second-order differential equation:
(21)ddxfy′=fy,
known as Euler-Lagrange condition. This equation must hold along (x,y(x),y′(x)) except at a finite number of points [3, Section 2.1].The extremals of (16)-(17) can be obtained by solving the Euler-Lagrange equation, but it only holds at points where the extremal y*(x) is smooth. At points where y*′(x) has jumps, called corners, the Weierstrass-Erdmann corner conditions must be fulfilled [3, Section 2.3]. Since the location of the corners, their number, and the amplitudes of the jumps in y′*(x) are not known in advance, it is difficult to obtain a numerical method for a general problem using the Euler-Lagrange equation (21). One of the key aspects of our method is that the integral form of this condition
(22)fy′(x,y(x),y′(x))=∫ax‍fy(x,y(x),y′(x))dx+c
holds for all x∈[a,b] and some c, and therefore the Weierstrass-Erdmann corner conditions are not needed. Thus, an alternative way of computing the extremals can be based on this necessary condition in integral form.Note that necessary condition requires that boundary values fulfill Euler-Lagrange equation. Thus, if some of the four valuesa,  y(a),  b and y(b) are not explicitly given, alternate boundary conditions have to be provided. This is what transversality conditions do. Assume that a, y(a) and b are given but y(b) is free. In this case, the additional necessary transversality condition
(23)fy′(b,y*(b),y′*(b))=0
must hold.The variational approach does not consider constraints. However, the optimal control problem has, at least, a first-order differential constraint (9) representing the dynamic equation of the system. Moreover, since the dynamic equation of a planar RR robot manipulator is a second-order differential equation, additional differential constraints will arise while rewriting it as a first-order differential equation. Therefore, the optimal control problem must be reformulated as an unconstrained calculus of variations problem in order to deal with differential and algebraic constraints as described in the following section.Following [3, Chapter 5], we reformulate as an unconstrained calculus of variations problem the optimal control problem consisting in minimizing (8) subject to (9), (10), (11), (14), and (15). Notice that we omitted constraints (12) and (13) which need a special treatment.For convenience, we change the independent variable fromt to x and the dependent variable from x to y to be consistent with the notation of calculus of variations. Our reformulation is based on special derivative multipliers and a change of variables in which y1(x)=y(x) is the renamed state vector,y2′(x)=u(x) is the renamed state vector,y3′(x) is the multiplier associated with (9),y4′(x)is the multiplier associated with constraint (10),y5′(x) is the multiplier associated with constraint (11),y6′(x) is the excess variable of constraint (11).Since y2(x),…,y6(x) are not unique without an extra condition, we initialize these variables by defining yi(xI)=0,i=1,…,6. Thus, our problem becomes
(24)minI(Y)=∫xIxF‍F(x,Y,Y′)dx,
where
(25)Y=(y1,y2,y3,y4,y5,y6)T,F=f(x,y1,y2′)+y3′T(y1′-g(x,y1,y2′))+y4′Th(x,y1,y2′)+y5′T(l(x,y1,y2′)+y6′2).
Since the values of yi(xF), i=2,…,6 are unknown, transversality conditions are needed, having the form
(26)FY¯′(xF,Y(xF),Y′(xF))=0,
with Y=(y1,Y¯) and Y¯=(y2,y3,y4,y5,y6)T.


### 5.1. Holonomic Equality Constraints

In this section, we show how to deal with the equality constraint (12). For the sake of clarity, we consider the problem
(27)min∫ab‍ψ(x,y,y′)
subject to
(28)φ(x,y)=0,boundaryconditions,
which is related to the problem
(29)min∫ab‍ψ(x,y,y′)
subject to
(30)ϕ(x,y,y′)=0,boundaryconditions.
In the above lines y(x) is an n-vector, and ψ,φ,ϕ are assumed to be differentiable in their arguments or with the needed smoothness. We also assume that ψy′y′>0. The boundary conditions of the problems are any combination of fixed boundary conditions for the components of y with the possibility of leaving some of them unspecified.If we reformulate problem (27)-(28) using the technique described in Section 5, we get the following Hamiltonian Ψ(x,Y,Y′)=ψ(x,y1,y1′)+y2′φ(x,y1), with y1(x)=y(x) where y2′ is the multiplier. We have in this case
(31)ΨY′Y′=[ψy1′y1′000]
which is singular. The singularity of ΨY′Y′ is a difficulty we must avoid. Furthermore, even when it is not difficult to change from the φ constraint to the ϕ constraint by increasing the dimension of the independent variables, it is not easy to deal with the new associated boundary conditions. This is the reason that problem (27)-(28) is so difficult to solve.It has been shown in [20] that problem (27)-(28) can be reformulated as an equivalent problem of the form (29)-(30). In particular, y(x) is a solution to (27)-(28) if and only if y(x) is a solution to
(32)min∫ab‍ψ(x,y,y′)
subject to
(33)φx+φyy′=0,φ(a,y(a))=0,boundaryconditions.Ify(x) is a solution to (27)-(28), then φ(a,y(a))=0 and (d/dx)φ(x,y(x))=φx+φyy′=0. On the other hand, if y(x) is a solution to (32)-(33), then for x∈[a,b](34)φ(x,y(x))=∫ax‍(φx(z,y(z))+φy(z,y(z))y′(z))dz+φ(a,y(a))=0+0=0.


## 5.1. Holonomic Equality Constraints

In this section, we show how to deal with the equality constraint (12). For the sake of clarity, we consider the problem
(27)min∫ab‍ψ(x,y,y′)
subject to
(28)φ(x,y)=0,boundaryconditions,
which is related to the problem
(29)min∫ab‍ψ(x,y,y′)
subject to
(30)ϕ(x,y,y′)=0,boundaryconditions.
In the above lines y(x) is an n-vector, and ψ,φ,ϕ are assumed to be differentiable in their arguments or with the needed smoothness. We also assume that ψy′y′>0. The boundary conditions of the problems are any combination of fixed boundary conditions for the components of y with the possibility of leaving some of them unspecified.If we reformulate problem (27)-(28) using the technique described in Section 5, we get the following Hamiltonian Ψ(x,Y,Y′)=ψ(x,y1,y1′)+y2′φ(x,y1), with y1(x)=y(x) where y2′ is the multiplier. We have in this case
(31)ΨY′Y′=[ψy1′y1′000]
which is singular. The singularity of ΨY′Y′ is a difficulty we must avoid. Furthermore, even when it is not difficult to change from the φ constraint to the ϕ constraint by increasing the dimension of the independent variables, it is not easy to deal with the new associated boundary conditions. This is the reason that problem (27)-(28) is so difficult to solve.It has been shown in [20] that problem (27)-(28) can be reformulated as an equivalent problem of the form (29)-(30). In particular, y(x) is a solution to (27)-(28) if and only if y(x) is a solution to
(32)min∫ab‍ψ(x,y,y′)
subject to
(33)φx+φyy′=0,φ(a,y(a))=0,boundaryconditions.Ify(x) is a solution to (27)-(28), then φ(a,y(a))=0 and (d/dx)φ(x,y(x))=φx+φyy′=0. On the other hand, if y(x) is a solution to (32)-(33), then for x∈[a,b](34)φ(x,y(x))=∫ax‍(φx(z,y(z))+φy(z,y(z))y′(z))dz+φ(a,y(a))=0+0=0.


## 6. Numerical Method

The numerical method used is based on the discretization of the unconstrained variational calculus problem stated in the previous section. In particular, the main underlying idea is obtaining a discretized solutionyh(x) solving (20) for all piecewise linear spline function variations z(x) instead of dealing with the Euler-Lagrange equation (21). Thus, this method uses no numerical corner conditions and avoids patching solutions to (21) between corners.LetN be a large positive integer, h=(b-a)/N, and let π=(a=a0<a1<⋯<aN=b) be a partition of the interval [a,b], where ak=a+kh for k=0,1,…,N. Define the one-dimensional spline hat functions
(35)wk(x)={x-ak-1hifak-1<x<ak,ak+1-xhifak<x<ak+1,0otherwise,
where k=1,2,…,N-1. Define also the m-dimensional, piecewise linear component functions
(36)yh(x)=∑k=0N‍Wk(x)Ck,zh(x)=∑k=0N‍Wk(x)Dk,
where Wk(x)=wk(x)Im×m, yh(x) is the sought numerical solution, and zh(x) is a numerical variation. In particular, the constant vectors Ck are to be determined by the algorithm developed by us, and the constant vectors Dk are arbitrary. Thus, the discretized form of (20) is obtained in each subinterval [ak-1,ak+1]. For the sake of clarity of exposition, we assume that m=1. Note that I′(y,z) in (20) is linear in z so that a three-term relationship may be obtained at x=ak by choosing z(x)=wk(x) for k=1,2,…,N-1. Thus,
(37)0=I′(y,wk)=∫ak-1ak+1‍[fy(x,y,y′)wk+fy′(x,y,y′)wk′]dx=∫ak-1ak‍[fy(x,yh(x),yh′(x))x-ak-1h]dx+∫ak-1ak‍[fy′(x,yh(x),yh′(x))1h]dx+∫akak+1‍[fy(x,yh(x),yh′(x))ak+1-xh]dx+∫akak+1‍[fy′(x,yh(x),yh′(x))(-1h)]dx=fy(ak-1*,yk+yk-12,yk-yk-1h)h221h+fy′(ak-1*,yk+yk-12,yk-yk-1h)1hh+fy(ak*,yk+yk+12,yk+1-ykh)h221h+fy′(ak*,yk+yk+12,yk+1-ykh)(-1h)h
or
(38)0=fy′(ak-1*,yk+yk-12,yk-yk-1h)+h2fy(ak-1*,yk+yk-12,yk-yk-1h)-fy′(ak*,yk+yk+12,yk+1-ykh)+h2fy(ak*,yk+yk+12,yk+1-ykh).
In these equations ak*=(ak+ak+1)/2 and yk=yh(ak) is the computed value of yh(x) at ak. In the general case, when m>1, the same result is obtained but fy′ and fy are column m-vectors of functions with ith component fyi′ and fyi, respectively. Similarly, (yk+yk-1)/2 is the m-vector which is the average of the m-vectors yh(ak) and yh(ak-1).By the same arguments that led to (38),
(39)fy′(aN-1*,yN+yN-12,yN-yN-1h)+h2fy(ak-1*,yN+yN-12,yN-yN-1h)=0,
which is the numerical equivalent of the transversality condition (23). For further details, see [3, Chapter 6].It has been shown in [21] that with this method the global error has a priori global reduction ratio of O(h2). In practice, if the step size h is halved, the error decreases by 4.


## 7. Implementation and Results

Several numerical experiments have been carried out for bothRR¯ and R¯R planar horizontal underactuated robot manipulators.


### 7.1. Planar Horizontal UnderactuatedRR¯ Robot Manipulator

In this section, the optimal control problem of a planar horizontal underactuatedRR¯ is studied. In this robot model the second joint is not actuated; thus, u=(u1,0)T. In this case, it is neither possible to integrate partially nor completely the nonholonomic constraint because the manipulator inertia matrix contains terms in θ2 (see [12]). Hence, the system is controllable. The numerical results of the application of our method for optimal control to a boundary value problem and to an initial value problem for this system will be described.For a planar horizontal underactuatedRR¯, (2) can be split into
(40)(α+2βcosθ2)θ¨1+(δ+βcosθ2)θ¨2-βsinθ2(2θ˙1θ˙2+θ˙22)=u1,(δ+βcosθ2)θ¨1+δθ¨2+βsinθ2θ˙12=0.
To express optimal control problems that involve this second-order differential constraints in the form of a basic optimal control problem, we have first to convert it into first-order differential constraints introducing the following change of variables:
(41)x1=θ1,x3=θ˙1x2=θ2,x4=θ˙2,
with the following additional relations
(42)x1′=x3,x2′=x4.
Thus, the second-order differential equations (40) are converted into the first-order differential equations
(43)(α+2βcosx2)x3′+(δ+βcosx2)x4′-βsinx2(2x3x4+x42)=u1,(44)(δ+βcosx2)x3′+δx4′+βsinx2x32=0.
Relations (42), (43), and (44) are now the differential constraints of the optimal control problem, and the objective functional to minimize is
(45)J=∫tItF‍u12dt.Then, we introduce the following new variables:(46)X=[X1⋯X9],
such that
(47)Xi=xi,i=1,…,4,(48)X5′=u1,X5(tI)=0,
where X6′ with X6(tI)=0 is the multiplier associated with differential constraint (43), X7′ with X7(tI)=0 is the multiplier associated with the differential constraint (44), and X8′ with X8(tI)=0 and X9′ with X9(tI)=0 are the multipliers associated with the additional equality constraints (42).Thus, the unconstrained reformulation (24) of the problem is in this case
(49)minI(X)=∫tItF‍G(t,X,X′)dt,
where
(50)G=X5′2+X6′(-βsin(X2)X3X4-βsin(X2)X4(X3+X4)+(α+2βcos(X2))X3′+(δ+βcos(X2))X4′-X5′)+X7′(βsin(X2)X32+(δ+βcos(X2))X3′+δX4′)+X8′(X1′-X3)+X9′(X2′-X4)
with initial conditions
(51)Xi(tI)=νi,i=1,…,4withνiassignedconstantvalues,Xi(tI)=0,i=5,…,9,
for both the boundary value problem and the initial value problem. The final conditions for the boundary value problem have the form
(52)Xi(tF)=μi,i=1,…,4withμiassignedconstantvalues,
whereas in the initial value problem, Xi(tF) will be free for some i.The initial values of control variables and multipliers have been set to zero, whereas their final values have not been assigned in both optimal control problems. Therefore, transversality conditions are needed in both cases for the variablesXi(tF), i=5,…,10, and they will be of the form
(53)GX¯′(tF,X(tF),X′(tF))=0,
with X=(X1,X2,X3,X4,X¯). Moreover, in the initial value problem, additional transversality conditions will be needed for each Xi(tF) that is let free. We have used for both problems the following settings Iz1=Iz2=1 [kg m2], l1=l2=1 [m], m1=m2=1 [kg], whereas the initial and final times have been tI=0 and tF=1[s], respectively.


#### 7.1.1. Problem 1: Boundary Value Problem

The following initial and final conditions have been imposed on the state variables as follows:(54)θ1(tI)=0[rad],θ1(tF)=π2[rad],θ2(tI)=-π2[rad],θ2(tF)=-π2[rad],θ˙1(tI)=0[rad/s],θ˙1(tF)=0[rad/s],θ˙2(tI)=0[rad/s],θ˙2(tF)=0[rad/s].
The initial values of control variable and of the multipliers have been set to zero, whereas their final values are left free. Figure 2 shows the sequence of configurations of the robot at times t=k/32, k=0,1,…,32. Since the configurations of the sequence overlap, it has been split into smaller sequences for a better visualization of the manipulator motion. Figure 3 depicts the corresponding control and state variables of the optimal solution of this boundary value problem obtained with a discretization of the time interval [tI,tF] into 64 subintervals. The value of the objective functional for this solution is 34518.5 [J].Sequence of configurations of the robot manipulator at timesk(1/32) with k=0,1,…,32 corresponding to the optimal solution of problem 1, a boundary value problem for the planar RR¯ robot manipulator with boundary conditions θ1(tI)=0[rad], θ2(tI)=-π/2 [rad], θ˙1(tI)=0[rad/s], θ˙2(tI)=0[rad/s], θ1(tF)=π/2[rad], θ2(tF)=-π/2[rad], θ˙1(tF)=0[rad/s], and θ˙2(tF)=0[rad/s], obtained with a discretization of [tI,tF] into 64 subintervals. The initial and final times are tI=0 and tF=1[s], respectively. The corresponding control and state variables are represented in Figure 3.
(a)
t
=
k
/
32, k=0,1,…,11
(b)
t
=
k
/
32, k=12,…,25
(c)
t
=
k
/
32, k=26,…,32Control and state variables of the optimal solution of problem 1, a boundary value problem for the planarRR¯ robot manipulator with boundary conditions θ1(tI)=0[rad], θ2(tI)=-π/2[rad], θ˙1(tI)=0[rad/s], θ˙2(tI)=0[rad/s], θ1(tF)=π/2[rad], θ2(tF)=-π/2[rad], θ˙1(tF)=free, and θ˙2(tF)=0[rad/s]. The initial and final times are tI=0 and tF=1[s], respectively. The corresponding sequence of configurations of the robot manipulator at times k(1/32) with k=0,1,…,32 is represented in Figure 2.
(a)
θ1
(b)
θ
2
(c)
θ
˙
1
(d)
θ
˙
2
(e)
u
1


#### 7.1.2. Problem 2: Initial Value Problem

An initial value problem has also been solved with the following initial and final conditions(55)θ1(tI)=0[rad],θ1(tF)=π2[rad],θ2(tI)=-π2[rad],θ2(tF)=-π2[rad],θ˙1(tI)=0[rad/s],θ˙1(tF)=free,θ˙2(tI)=0[rad/s],θ˙2(tF)=0[rad/s].
The initial values of control variable and of the multipliers have been set to zero, whereas their final values are left free. The only difference between these conditions and those of the boundary value problem described in Section 7.1.1 is that now θ˙1(tF)=free.Figures4 and 5 depict the sequence of configurations the RR¯ robot manipulator, and the corresponding control and state variables of the optimal solution of this initial value problem, respectively, obtained with a discretization of the time interval [tI,tF] into 64 subintervals. The value of the objective functional for this solution is 5647.2 [J]. This value is lower than the value of the objective functional of the solution of the boundary value problem described in Section 7.1.1 because now is θ˙1(tF)=free, and the control system does not have to spend energy to stop it.Sequence of configurations of the robot manipulator at timesk(1/32) with k=0,1,…,32 corresponding to the optimal solution of problem 2, an initial value problem for an RR¯ robot manipulator with boundary conditions θ1(tI)=0[rad], θ2(tI)=-π/2[rad], θ˙1(tI)=0[rad/s], θ˙2(tI)=0[rad/s], θ1(tF)=π/2[rad], θ2(tF)=-π/2[rad], θ˙1(tF)=free, and θ˙2(tF)=0[rad/s], obtained with a discretization of [tI,tF] into 64 intervals. The initial and final times are tI=0 and tF=1[s], respectively. The corresponding control and state variables are represented in Figure 5.
(a)
t
=
k
/
32, k=0,1,…,16
(b)
t
=
k
/
32, k=17,…,32Control and state variables of the optimal solution of problem 2, an initial value problem for aRR¯ robot manipulator with boundary conditions θ1(tI)=0[rad], θ2(tI)=-π/2[rad], θ˙1(tI)=0[rad/s], θ˙2(tI)=0[rad/s], θ1(tF)=π/2[rad], θ2(tF)=-π/2[rad], θ˙1(tF)=free, and θ˙2(tF)=0[rad/s]. The initial and final times are tI=0 and tF=1[s], respectively. The corresponding sequence of configurations of the robot manipulator at times k(1/32) with k=0,1,…,32 is represented in Figure 4.
(a)
θ1
(b)
θ
2
(c)
θ
˙
1
(d)
θ
˙
2
(e)
u
1


### 7.2. Planar Horizontal UnderactuatedR¯R Robot Manipulator

In this section, the optimal control problem of a planar horizontal underactuatedR¯R robot manipulator is studied. In this robot model, the first joint is not actuated; thus, u=(0,u2)T and (2) can be split into
(56)(α+2βcosθ2)θ¨1+(δ+βcosθ2)θ¨2-βsinθ2(2θ˙1θ˙2+θ˙22)=0,(57)(δ+βcosθ2)θ¨1+δθ¨2+βsinθ2θ˙12=u2.As explained in [12], since gravity terms are all zero and θ1 does not intervene in the system inertia matrix, (56) can be partially integrated to
(58)(α+2βcosθ2)θ˙1+(δ+βcosθ2)θ˙2+c1=0.
Actually, constraint (56) is completely integrable giving rise to an holonomic constraint. The resulting holonomic constraint takes different forms depending on the value of c1 which depends on the initial conditions. Therefore, two cases have been considered: (i)
when the initial velocitiesθ˙1(tI) and θ˙2(tI) are both zero,(ii)
when the initial velocityθ˙1(tI) is nonzero.


#### 7.2.1. Problem 3: Initial Value Problem with Zero Initial Velocities

An initial value problem has been solved with the following initial and final conditions:(59)θ1(tI)=0[rad],θ1(tF)=free,θ2(tI)=0[rad],θ2(tF)=π[rad],θ˙1(tI)=0[rad/s],θ˙1(tF)=0[rad/s],θ˙2(tI)=0[rad/s],θ˙2(tF)=0[rad/s].
The initial values of the control variable and of the multipliers have been set to zero, whereas their final value is left free.Since there is a holonomic constraint that relates the values of the anglesθ1 and θ2, without integrating (58), we are not able to find the value of θ1(tF) consistent with θ1(tI). Therefore, no final conditions have been imposed on θ1.From the initial conditions of the problem, we obtainc1=0. Equation (58) with c1=0 corresponds to the homogeneous differential constraint
(60)dζ=(α+2βcosθ2)dθ1+(δ+βcosθ2)dθ2=0.
The differential dζ is not exact. However, it becomes an exact differential if multiplied by the factor 1/(α+2βcosθ2). This operation does not alter the differential equation (60). In this case, there does exist a function ζ whose differential coincides with the expression dζ/(α+2βcosθ2). Due to the existence of this function, the integral of dζ between two points depends only on these points and not on the integration path. Equation (60) rewritten in this form
(61)dθ1=-(δ+βcosθ2)α+2βcosθ2dθ2
can be integrated by separating variables. The corresponding holonomic constraint has the following expression:
(62)θ1=(α-2δ)tanh-1[2β-α4β2-α2tan(θ22)]×(4β2-α2)-1-θ22+c2.To express this optimal control problem in the form of a basic optimal control problem, we first have to convert (57) into a first-order differential model introducing the following change of variables:
(63)x1=θ1,x3=θ˙1x2=θ2,x4=θ˙2,
with the following additional relations:
(64)x1′=x3,x2′=x4.
Thus, the optimal control problem is to minimize
(65)∫01‍u22dt
subject to the constraints
(66)x1=(α-2δ)tanh-1[2β-α4β2-α2tan(x22)]×(4β2-α2)-1-x22+c2,(67)(δ+βcosθ2)x3′+δx4′+βsinx2x32=u2,
and the additional constraints (64). To reformulate this optimal control problem as an unconstrained calculus of variations problem, let X be
(68)X=[X1⋯X9],
such that
(69)Xi=xi,i=1,…,4,X5′=u1,X5(tI)=0,
where X6′ with X6(tI)=0 is the multiplier associated with the holonomic constraint (66), X7′ with X7(tI)=0 is the multipliers associated with the differential constraint (67), and X8′ with X8(tI)=0 and X9′ with X9(tI)=0 are the multipliers associated with the additional equality constraints (64).Thus, the holonomic constraint of the problem can be rewritten as follows:(70)φ(t,X)=X1-((α-2δ)(4β2-α2)[2β-α4β2-α2tan(X22)]×tanh-1[2β-α4β2-α2tan(X22)])×(4β2-α2)-1+X22=0.
Now, the technique described in Section 5.1 to deal with holonomic constraints can be applied to φ(t,X), and this holonomic constraint is replaced by
(71)φt+φXX′=0,φ(0,X(0))=0.
From the initial conditions of the problem, the latter equation reduces to the equality 0=0, whereas the former takes the following form:
(72)(α+2βcos(X2))X3+(δ+βcos(X2))X4=0.
The corresponding Hamiltonian is
(73)G1=X5′2+X6′((α+2βcos(X2))X3+(δ+βcos(X2))X4)+X7′(βsin(X2)X32+(δ+βcos(X2))X3′+δX4′-X5′)+X8′(X1′-X3)+X9′(X2′-X4).
It is not difficult to check that matrix G1X′X′ is singular. This is due to the fact that to handle our optimal control problem which involves second-order differential constraints we converted them into first-order differential constraints. Therefore, we apply again the technique of Section 5.1 obtaining the identity 0=0 and the following constraint:
(74)-βsin(X2)X3X4-βsin(X2)X4(X3+X4)+(α+2βcos(X2))X3′+(δ+βcos(X2))X4′=0.
The corresponding Hamiltonian is
(75)G2=X5′2+X6′×(-βsin(X2)X3X4-βsin(X2)X4(X3+X4)+(α+2βcos(X2))X3′+(δ+βcos(X2))X4′)+X7′(βsin(X2)X32+(δ+βcos(X2))X3′+δX4′-X5′)+X8′(X1′-X3)+X9′(X2′-X4).
It is not difficult to check that matrix G2X′X′ in this case is not singular since its determinant is
(76)det(G2X′X′)=12(β2+2δ(-α+δ)+β2cos(2X2))2.
Substituting the values of α, β, and δ, this expression becomes det(G2X′X′)=1/128(43-2cos(2X2))2 which is always positive for any real value X2. Figure 6 shows the sequence of configurations of the robot at times k/32 with k=0,1,…,32 and Figure 7 depicts control and state variables of the optimal solution obtained with a discretization of the interval [tI,tF] into 64 subintervals.Figure 6
Sequence of configurations of the robot manipulator at timesk/32 with k=0,1,…,32 corresponding to the optimal solution of problem 3, an initial value problem for an underactuated R¯R planar robot manipulator with boundary conditions θ1(tI)=0[rad], θ2(tI)=0[rad], θ˙1(tI)=0[rad/s], θ˙2(tI)=0[rad/s], θ1(tF)=free, θ2(tF)=π[rad], θ˙1(tF)=0[rad/s], and θ˙2(tF)=0[rad/s], obtained with a discretization of [tI,tF] into 64 subintervals. The initial and final times are tI=0 and tF=1[s], respectively. The corresponding control and state variables are represented in Figure 7.Control and state variables of the optimal solution of problem 3, an initial value problem for an underactuatedR¯R planar robot manipulator with boundary conditions θ1(tI)=0[rad], θ2(tI)=0[rad], θ˙1(tI)=0[rad/s], θ˙2(tI)=0[rad/s], u2(tI)=0 Nm, θ1(tF)=free, θ2(tF)=π[rad], θ˙1(tF)=0[rad/s], and θ˙2(tF)=0[rad/s]. The initial and final times are tI=0 and tF=1[s], respectively. The corresponding sequence of configurations of the robot manipulator at times k(1/32) with k=0,1,…,32 is represented in Figure 6.
(a)
θ
1
(b)
θ
2
(c)
θ
˙
1
(d)
θ
˙
2
(e)
u
2In particular, we getθ1(tF)=-1.10248 [rad]. To check the consistency of this result with the holonomic constraint (62), since θ˙1(tI)=θ˙2(tI)=0 [rad/s], we get from (58) that c1=0 and using the initial condition θ1(tI)=θ2(tI)=0 [rad], we get from (62) that c2=0. Having established the value of the constant c2, we obtain from the same equation, for θ2(tF)=π [rad], that θ1(tF)=-1.10248 [rad] which coincides with the value of θ1(tF) obtained numerically.


#### 7.2.2. Problem 4: Initial Value Problem with Nonzero Initial Velocityθ˙1

Another initial value problem has been solved with the following conditions:(77)θ1(tI)=0[rad],θ1(tF)=free,θ2(tI)=0[rad],θ2(tF)=π[rad],θ˙1(tI)=5[rad/s],θ˙1(tF)=free,θ˙2(tI)=0[rad/s],θ˙2(tF)=0[rad/s].
The initial values of the multipliers have been set to zero whereas their final value is left free. Notice that no final conditions have been imposed on θ1 and θ˙1. The same considerations done in previous section hold in this case, as well. The technique described in Section 5.1 must be applied twice leading to the differential constraint (74) and to the Hamiltonian (75). Figure 8 shows the sequence of configurations of the robot at times k/32 with k=0,1,…,32, and Figure 9 depicts the control and state variables of the optimal solution obtained with a discretization of the interval [tI,tF] into 64 subintervals. In particular, we get that θ1(tF)=6.17172 [rad] and θ˙1(tF)=9.00163 [rad/s]. To check the consistency of the obtained value of θ1(tF) with the holonomic constraint, consider (58). We can calculate the constant c1 using the initial conditions of the problem obtaining
(78)c1=-θ˙1(tI)(α+2βcosθ2(tI))-θ˙2(tI)(δ+βcosθ2(tI))=-22.5.
Since c1≠0, (58) corresponds in this case to the differential constraint
(79)dη=(α+2βcosθ2)dθ1+(δ+βcosθ2)dθ2+c1dt=0.
Equation (79) can be rewritten in this form
(80)dθ1=-(δ+βcosθ2(t))α+2βcosθ2(t)dθ2-c1α+2βcosθ2(t)dt.
To check the obtained value of θ1(tF), dθ1 is numerically integrated between θ1(tI) and θ1(tF) using the interpolated numerical optimal solution obtained for θ2(t). We get that θ1(tF)=6.18705. This value is close to 6.17172.Figure 8
Sequence of configurations of the robot manipulator at timesk/32 with k=0,1,…,32 corresponding to the optimal solution of problem 4, an initial value problem for an underactuated R¯R planar robot manipulator with boundary conditions θ1(tI)=0[rad], θ2(tI)=0[rad], θ˙1(tI)=5[rad/s], θ˙2(tI)=0[rad/s], θ1(tF)=free, θ2(tF)=π[rad], θ˙1(tF)=free,  and θ˙2(tF)=0[rad/s], obtained with a discretization of [tI,tF] into 64 intervals. The initial and final times are tI=0 and tF=1[s], respectively. The corresponding control and state variables are represented in Figure 9.Control and state variables of the optimal solution of problem 4, an initial value problem for an underactuatedR¯R planar robot manipulator with boundary conditions θ1(tI)=0[rad], θ2(tI)=0[rad], θ˙1(tI)=5[rad/s], θ˙2(tI)=0[rad/s], θ1(tF)=free, θ2(tF)=π[rad], θ˙1(tF)=free, and θ˙2(tF)=0[rad/s]. The initial and final times are tI=0 and tF=1[s], respectively. The corresponding sequence of configurations of the robot manipulator at times k(1/32) with k=0,1,…,32 is represented in Figure 8.
(a)
θ
1
(b)
θ
2
(c)
θ
˙
1
(d)
θ
˙
2
(e)
u
2To check the consistency of the obtained value ofθ˙1(tF) with the constraint (58), using the computed value c1=-22.5 and the final conditions θ˙2(tF)=0, θ2(tF)=π of the problem, we obtain
(81)θ˙1(tF)=-(δ+βcosθ2(tF))α+2βcosθ2(tF)θ˙2(tF)-c1α+2βcosθ2(tF)=9.
This value is very close to the value of θ˙1(tF) obtained numerically.


### 7.3. Computational Issues

If the optimal control problem hasm variables and the time interval [tI,tF] has been discretized into N subintervals, the resulting set of difference equations (38) has m×(N-1) equations and m×(N-1) variables plus the equations and variables due to transversality conditions. Feasible solutions have been used as initial guesses of the algorithm.The solution of the nonlinear system of difference equations (38) has been obtained using a damped Newton algorithm within a line search methodology implemented in Mathematica 7 under Mac OS X operating system (see [22, 23] for more details).


## 7.1. Planar Horizontal UnderactuatedRR¯ Robot Manipulator

In this section, the optimal control problem of a planar horizontal underactuatedRR¯ is studied. In this robot model the second joint is not actuated; thus, u=(u1,0)T. In this case, it is neither possible to integrate partially nor completely the nonholonomic constraint because the manipulator inertia matrix contains terms in θ2 (see [12]). Hence, the system is controllable. The numerical results of the application of our method for optimal control to a boundary value problem and to an initial value problem for this system will be described.For a planar horizontal underactuatedRR¯, (2) can be split into
(40)(α+2βcosθ2)θ¨1+(δ+βcosθ2)θ¨2-βsinθ2(2θ˙1θ˙2+θ˙22)=u1,(δ+βcosθ2)θ¨1+δθ¨2+βsinθ2θ˙12=0.
To express optimal control problems that involve this second-order differential constraints in the form of a basic optimal control problem, we have first to convert it into first-order differential constraints introducing the following change of variables:
(41)x1=θ1,x3=θ˙1x2=θ2,x4=θ˙2,
with the following additional relations
(42)x1′=x3,x2′=x4.
Thus, the second-order differential equations (40) are converted into the first-order differential equations
(43)(α+2βcosx2)x3′+(δ+βcosx2)x4′-βsinx2(2x3x4+x42)=u1,(44)(δ+βcosx2)x3′+δx4′+βsinx2x32=0.
Relations (42), (43), and (44) are now the differential constraints of the optimal control problem, and the objective functional to minimize is
(45)J=∫tItF‍u12dt.Then, we introduce the following new variables:(46)X=[X1⋯X9],
such that
(47)Xi=xi,i=1,…,4,(48)X5′=u1,X5(tI)=0,
where X6′ with X6(tI)=0 is the multiplier associated with differential constraint (43), X7′ with X7(tI)=0 is the multiplier associated with the differential constraint (44), and X8′ with X8(tI)=0 and X9′ with X9(tI)=0 are the multipliers associated with the additional equality constraints (42).Thus, the unconstrained reformulation (24) of the problem is in this case
(49)minI(X)=∫tItF‍G(t,X,X′)dt,
where
(50)G=X5′2+X6′(-βsin(X2)X3X4-βsin(X2)X4(X3+X4)+(α+2βcos(X2))X3′+(δ+βcos(X2))X4′-X5′)+X7′(βsin(X2)X32+(δ+βcos(X2))X3′+δX4′)+X8′(X1′-X3)+X9′(X2′-X4)
with initial conditions
(51)Xi(tI)=νi,i=1,…,4withνiassignedconstantvalues,Xi(tI)=0,i=5,…,9,
for both the boundary value problem and the initial value problem. The final conditions for the boundary value problem have the form
(52)Xi(tF)=μi,i=1,…,4withμiassignedconstantvalues,
whereas in the initial value problem, Xi(tF) will be free for some i.The initial values of control variables and multipliers have been set to zero, whereas their final values have not been assigned in both optimal control problems. Therefore, transversality conditions are needed in both cases for the variablesXi(tF), i=5,…,10, and they will be of the form
(53)GX¯′(tF,X(tF),X′(tF))=0,
with X=(X1,X2,X3,X4,X¯). Moreover, in the initial value problem, additional transversality conditions will be needed for each Xi(tF) that is let free. We have used for both problems the following settings Iz1=Iz2=1 [kg m2], l1=l2=1 [m], m1=m2=1 [kg], whereas the initial and final times have been tI=0 and tF=1[s], respectively.


### 7.1.1. Problem 1: Boundary Value Problem

The following initial and final conditions have been imposed on the state variables as follows:(54)θ1(tI)=0[rad],θ1(tF)=π2[rad],θ2(tI)=-π2[rad],θ2(tF)=-π2[rad],θ˙1(tI)=0[rad/s],θ˙1(tF)=0[rad/s],θ˙2(tI)=0[rad/s],θ˙2(tF)=0[rad/s].
The initial values of control variable and of the multipliers have been set to zero, whereas their final values are left free. Figure 2 shows the sequence of configurations of the robot at times t=k/32, k=0,1,…,32. Since the configurations of the sequence overlap, it has been split into smaller sequences for a better visualization of the manipulator motion. Figure 3 depicts the corresponding control and state variables of the optimal solution of this boundary value problem obtained with a discretization of the time interval [tI,tF] into 64 subintervals. The value of the objective functional for this solution is 34518.5 [J].Sequence of configurations of the robot manipulator at timesk(1/32) with k=0,1,…,32 corresponding to the optimal solution of problem 1, a boundary value problem for the planar RR¯ robot manipulator with boundary conditions θ1(tI)=0[rad], θ2(tI)=-π/2 [rad], θ˙1(tI)=0[rad/s], θ˙2(tI)=0[rad/s], θ1(tF)=π/2[rad], θ2(tF)=-π/2[rad], θ˙1(tF)=0[rad/s], and θ˙2(tF)=0[rad/s], obtained with a discretization of [tI,tF] into 64 subintervals. The initial and final times are tI=0 and tF=1[s], respectively. The corresponding control and state variables are represented in Figure 3.
(a)
t
=
k
/
32, k=0,1,…,11
(b)
t
=
k
/
32, k=12,…,25
(c)
t
=
k
/
32, k=26,…,32Control and state variables of the optimal solution of problem 1, a boundary value problem for the planarRR¯ robot manipulator with boundary conditions θ1(tI)=0[rad], θ2(tI)=-π/2[rad], θ˙1(tI)=0[rad/s], θ˙2(tI)=0[rad/s], θ1(tF)=π/2[rad], θ2(tF)=-π/2[rad], θ˙1(tF)=free, and θ˙2(tF)=0[rad/s]. The initial and final times are tI=0 and tF=1[s], respectively. The corresponding sequence of configurations of the robot manipulator at times k(1/32) with k=0,1,…,32 is represented in Figure 2.
(a)
θ1
(b)
θ
2
(c)
θ
˙
1
(d)
θ
˙
2
(e)
u
1


### 7.1.2. Problem 2: Initial Value Problem

An initial value problem has also been solved with the following initial and final conditions(55)θ1(tI)=0[rad],θ1(tF)=π2[rad],θ2(tI)=-π2[rad],θ2(tF)=-π2[rad],θ˙1(tI)=0[rad/s],θ˙1(tF)=free,θ˙2(tI)=0[rad/s],θ˙2(tF)=0[rad/s].
The initial values of control variable and of the multipliers have been set to zero, whereas their final values are left free. The only difference between these conditions and those of the boundary value problem described in Section 7.1.1 is that now θ˙1(tF)=free.Figures4 and 5 depict the sequence of configurations the RR¯ robot manipulator, and the corresponding control and state variables of the optimal solution of this initial value problem, respectively, obtained with a discretization of the time interval [tI,tF] into 64 subintervals. The value of the objective functional for this solution is 5647.2 [J]. This value is lower than the value of the objective functional of the solution of the boundary value problem described in Section 7.1.1 because now is θ˙1(tF)=free, and the control system does not have to spend energy to stop it.Sequence of configurations of the robot manipulator at timesk(1/32) with k=0,1,…,32 corresponding to the optimal solution of problem 2, an initial value problem for an RR¯ robot manipulator with boundary conditions θ1(tI)=0[rad], θ2(tI)=-π/2[rad], θ˙1(tI)=0[rad/s], θ˙2(tI)=0[rad/s], θ1(tF)=π/2[rad], θ2(tF)=-π/2[rad], θ˙1(tF)=free, and θ˙2(tF)=0[rad/s], obtained with a discretization of [tI,tF] into 64 intervals. The initial and final times are tI=0 and tF=1[s], respectively. The corresponding control and state variables are represented in Figure 5.
(a)
t
=
k
/
32, k=0,1,…,16
(b)
t
=
k
/
32, k=17,…,32Control and state variables of the optimal solution of problem 2, an initial value problem for aRR¯ robot manipulator with boundary conditions θ1(tI)=0[rad], θ2(tI)=-π/2[rad], θ˙1(tI)=0[rad/s], θ˙2(tI)=0[rad/s], θ1(tF)=π/2[rad], θ2(tF)=-π/2[rad], θ˙1(tF)=free, and θ˙2(tF)=0[rad/s]. The initial and final times are tI=0 and tF=1[s], respectively. The corresponding sequence of configurations of the robot manipulator at times k(1/32) with k=0,1,…,32 is represented in Figure 4.
(a)
θ1
(b)
θ
2
(c)
θ
˙
1
(d)
θ
˙
2
(e)
u
1


## 7.1.1. Problem 1: Boundary Value Problem

The following initial and final conditions have been imposed on the state variables as follows:(54)θ1(tI)=0[rad],θ1(tF)=π2[rad],θ2(tI)=-π2[rad],θ2(tF)=-π2[rad],θ˙1(tI)=0[rad/s],θ˙1(tF)=0[rad/s],θ˙2(tI)=0[rad/s],θ˙2(tF)=0[rad/s].
The initial values of control variable and of the multipliers have been set to zero, whereas their final values are left free. Figure 2 shows the sequence of configurations of the robot at times t=k/32, k=0,1,…,32. Since the configurations of the sequence overlap, it has been split into smaller sequences for a better visualization of the manipulator motion. Figure 3 depicts the corresponding control and state variables of the optimal solution of this boundary value problem obtained with a discretization of the time interval [tI,tF] into 64 subintervals. The value of the objective functional for this solution is 34518.5 [J].Sequence of configurations of the robot manipulator at timesk(1/32) with k=0,1,…,32 corresponding to the optimal solution of problem 1, a boundary value problem for the planar RR¯ robot manipulator with boundary conditions θ1(tI)=0[rad], θ2(tI)=-π/2 [rad], θ˙1(tI)=0[rad/s], θ˙2(tI)=0[rad/s], θ1(tF)=π/2[rad], θ2(tF)=-π/2[rad], θ˙1(tF)=0[rad/s], and θ˙2(tF)=0[rad/s], obtained with a discretization of [tI,tF] into 64 subintervals. The initial and final times are tI=0 and tF=1[s], respectively. The corresponding control and state variables are represented in Figure 3.
(a)
t
=
k
/
32, k=0,1,…,11
(b)
t
=
k
/
32, k=12,…,25
(c)
t
=
k
/
32, k=26,…,32Control and state variables of the optimal solution of problem 1, a boundary value problem for the planarRR¯ robot manipulator with boundary conditions θ1(tI)=0[rad], θ2(tI)=-π/2[rad], θ˙1(tI)=0[rad/s], θ˙2(tI)=0[rad/s], θ1(tF)=π/2[rad], θ2(tF)=-π/2[rad], θ˙1(tF)=free, and θ˙2(tF)=0[rad/s]. The initial and final times are tI=0 and tF=1[s], respectively. The corresponding sequence of configurations of the robot manipulator at times k(1/32) with k=0,1,…,32 is represented in Figure 2.
(a)
θ1
(b)
θ
2
(c)
θ
˙
1
(d)
θ
˙
2
(e)
u
1


## 7.1.2. Problem 2: Initial Value Problem

An initial value problem has also been solved with the following initial and final conditions(55)θ1(tI)=0[rad],θ1(tF)=π2[rad],θ2(tI)=-π2[rad],θ2(tF)=-π2[rad],θ˙1(tI)=0[rad/s],θ˙1(tF)=free,θ˙2(tI)=0[rad/s],θ˙2(tF)=0[rad/s].
The initial values of control variable and of the multipliers have been set to zero, whereas their final values are left free. The only difference between these conditions and those of the boundary value problem described in Section 7.1.1 is that now θ˙1(tF)=free.Figures4 and 5 depict the sequence of configurations the RR¯ robot manipulator, and the corresponding control and state variables of the optimal solution of this initial value problem, respectively, obtained with a discretization of the time interval [tI,tF] into 64 subintervals. The value of the objective functional for this solution is 5647.2 [J]. This value is lower than the value of the objective functional of the solution of the boundary value problem described in Section 7.1.1 because now is θ˙1(tF)=free, and the control system does not have to spend energy to stop it.Sequence of configurations of the robot manipulator at timesk(1/32) with k=0,1,…,32 corresponding to the optimal solution of problem 2, an initial value problem for an RR¯ robot manipulator with boundary conditions θ1(tI)=0[rad], θ2(tI)=-π/2[rad], θ˙1(tI)=0[rad/s], θ˙2(tI)=0[rad/s], θ1(tF)=π/2[rad], θ2(tF)=-π/2[rad], θ˙1(tF)=free, and θ˙2(tF)=0[rad/s], obtained with a discretization of [tI,tF] into 64 intervals. The initial and final times are tI=0 and tF=1[s], respectively. The corresponding control and state variables are represented in Figure 5.
(a)
t
=
k
/
32, k=0,1,…,16
(b)
t
=
k
/
32, k=17,…,32Control and state variables of the optimal solution of problem 2, an initial value problem for aRR¯ robot manipulator with boundary conditions θ1(tI)=0[rad], θ2(tI)=-π/2[rad], θ˙1(tI)=0[rad/s], θ˙2(tI)=0[rad/s], θ1(tF)=π/2[rad], θ2(tF)=-π/2[rad], θ˙1(tF)=free, and θ˙2(tF)=0[rad/s]. The initial and final times are tI=0 and tF=1[s], respectively. The corresponding sequence of configurations of the robot manipulator at times k(1/32) with k=0,1,…,32 is represented in Figure 4.
(a)
θ1
(b)
θ
2
(c)
θ
˙
1
(d)
θ
˙
2
(e)
u
1


## 7.2. Planar Horizontal UnderactuatedR¯R Robot Manipulator

In this section, the optimal control problem of a planar horizontal underactuatedR¯R robot manipulator is studied. In this robot model, the first joint is not actuated; thus, u=(0,u2)T and (2) can be split into
(56)(α+2βcosθ2)θ¨1+(δ+βcosθ2)θ¨2-βsinθ2(2θ˙1θ˙2+θ˙22)=0,(57)(δ+βcosθ2)θ¨1+δθ¨2+βsinθ2θ˙12=u2.As explained in [12], since gravity terms are all zero and θ1 does not intervene in the system inertia matrix, (56) can be partially integrated to
(58)(α+2βcosθ2)θ˙1+(δ+βcosθ2)θ˙2+c1=0.
Actually, constraint (56) is completely integrable giving rise to an holonomic constraint. The resulting holonomic constraint takes different forms depending on the value of c1 which depends on the initial conditions. Therefore, two cases have been considered: (i)
when the initial velocitiesθ˙1(tI) and θ˙2(tI) are both zero,(ii)
when the initial velocityθ˙1(tI) is nonzero.


### 7.2.1. Problem 3: Initial Value Problem with Zero Initial Velocities

An initial value problem has been solved with the following initial and final conditions:(59)θ1(tI)=0[rad],θ1(tF)=free,θ2(tI)=0[rad],θ2(tF)=π[rad],θ˙1(tI)=0[rad/s],θ˙1(tF)=0[rad/s],θ˙2(tI)=0[rad/s],θ˙2(tF)=0[rad/s].
The initial values of the control variable and of the multipliers have been set to zero, whereas their final value is left free.Since there is a holonomic constraint that relates the values of the anglesθ1 and θ2, without integrating (58), we are not able to find the value of θ1(tF) consistent with θ1(tI). Therefore, no final conditions have been imposed on θ1.From the initial conditions of the problem, we obtainc1=0. Equation (58) with c1=0 corresponds to the homogeneous differential constraint
(60)dζ=(α+2βcosθ2)dθ1+(δ+βcosθ2)dθ2=0.
The differential dζ is not exact. However, it becomes an exact differential if multiplied by the factor 1/(α+2βcosθ2). This operation does not alter the differential equation (60). In this case, there does exist a function ζ whose differential coincides with the expression dζ/(α+2βcosθ2). Due to the existence of this function, the integral of dζ between two points depends only on these points and not on the integration path. Equation (60) rewritten in this form
(61)dθ1=-(δ+βcosθ2)α+2βcosθ2dθ2
can be integrated by separating variables. The corresponding holonomic constraint has the following expression:
(62)θ1=(α-2δ)tanh-1[2β-α4β2-α2tan(θ22)]×(4β2-α2)-1-θ22+c2.To express this optimal control problem in the form of a basic optimal control problem, we first have to convert (57) into a first-order differential model introducing the following change of variables:
(63)x1=θ1,x3=θ˙1x2=θ2,x4=θ˙2,
with the following additional relations:
(64)x1′=x3,x2′=x4.
Thus, the optimal control problem is to minimize
(65)∫01‍u22dt
subject to the constraints
(66)x1=(α-2δ)tanh-1[2β-α4β2-α2tan(x22)]×(4β2-α2)-1-x22+c2,(67)(δ+βcosθ2)x3′+δx4′+βsinx2x32=u2,
and the additional constraints (64). To reformulate this optimal control problem as an unconstrained calculus of variations problem, let X be
(68)X=[X1⋯X9],
such that
(69)Xi=xi,i=1,…,4,X5′=u1,X5(tI)=0,
where X6′ with X6(tI)=0 is the multiplier associated with the holonomic constraint (66), X7′ with X7(tI)=0 is the multipliers associated with the differential constraint (67), and X8′ with X8(tI)=0 and X9′ with X9(tI)=0 are the multipliers associated with the additional equality constraints (64).Thus, the holonomic constraint of the problem can be rewritten as follows:(70)φ(t,X)=X1-((α-2δ)(4β2-α2)[2β-α4β2-α2tan(X22)]×tanh-1[2β-α4β2-α2tan(X22)])×(4β2-α2)-1+X22=0.
Now, the technique described in Section 5.1 to deal with holonomic constraints can be applied to φ(t,X), and this holonomic constraint is replaced by
(71)φt+φXX′=0,φ(0,X(0))=0.
From the initial conditions of the problem, the latter equation reduces to the equality 0=0, whereas the former takes the following form:
(72)(α+2βcos(X2))X3+(δ+βcos(X2))X4=0.
The corresponding Hamiltonian is
(73)G1=X5′2+X6′((α+2βcos(X2))X3+(δ+βcos(X2))X4)+X7′(βsin(X2)X32+(δ+βcos(X2))X3′+δX4′-X5′)+X8′(X1′-X3)+X9′(X2′-X4).
It is not difficult to check that matrix G1X′X′ is singular. This is due to the fact that to handle our optimal control problem which involves second-order differential constraints we converted them into first-order differential constraints. Therefore, we apply again the technique of Section 5.1 obtaining the identity 0=0 and the following constraint:
(74)-βsin(X2)X3X4-βsin(X2)X4(X3+X4)+(α+2βcos(X2))X3′+(δ+βcos(X2))X4′=0.
The corresponding Hamiltonian is
(75)G2=X5′2+X6′×(-βsin(X2)X3X4-βsin(X2)X4(X3+X4)+(α+2βcos(X2))X3′+(δ+βcos(X2))X4′)+X7′(βsin(X2)X32+(δ+βcos(X2))X3′+δX4′-X5′)+X8′(X1′-X3)+X9′(X2′-X4).
It is not difficult to check that matrix G2X′X′ in this case is not singular since its determinant is
(76)det(G2X′X′)=12(β2+2δ(-α+δ)+β2cos(2X2))2.
Substituting the values of α, β, and δ, this expression becomes det(G2X′X′)=1/128(43-2cos(2X2))2 which is always positive for any real value X2. Figure 6 shows the sequence of configurations of the robot at times k/32 with k=0,1,…,32 and Figure 7 depicts control and state variables of the optimal solution obtained with a discretization of the interval [tI,tF] into 64 subintervals.Figure 6
Sequence of configurations of the robot manipulator at timesk/32 with k=0,1,…,32 corresponding to the optimal solution of problem 3, an initial value problem for an underactuated R¯R planar robot manipulator with boundary conditions θ1(tI)=0[rad], θ2(tI)=0[rad], θ˙1(tI)=0[rad/s], θ˙2(tI)=0[rad/s], θ1(tF)=free, θ2(tF)=π[rad], θ˙1(tF)=0[rad/s], and θ˙2(tF)=0[rad/s], obtained with a discretization of [tI,tF] into 64 subintervals. The initial and final times are tI=0 and tF=1[s], respectively. The corresponding control and state variables are represented in Figure 7.Control and state variables of the optimal solution of problem 3, an initial value problem for an underactuatedR¯R planar robot manipulator with boundary conditions θ1(tI)=0[rad], θ2(tI)=0[rad], θ˙1(tI)=0[rad/s], θ˙2(tI)=0[rad/s], u2(tI)=0 Nm, θ1(tF)=free, θ2(tF)=π[rad], θ˙1(tF)=0[rad/s], and θ˙2(tF)=0[rad/s]. The initial and final times are tI=0 and tF=1[s], respectively. The corresponding sequence of configurations of the robot manipulator at times k(1/32) with k=0,1,…,32 is represented in Figure 6.
(a)
θ
1
(b)
θ
2
(c)
θ
˙
1
(d)
θ
˙
2
(e)
u
2In particular, we getθ1(tF)=-1.10248 [rad]. To check the consistency of this result with the holonomic constraint (62), since θ˙1(tI)=θ˙2(tI)=0 [rad/s], we get from (58) that c1=0 and using the initial condition θ1(tI)=θ2(tI)=0 [rad], we get from (62) that c2=0. Having established the value of the constant c2, we obtain from the same equation, for θ2(tF)=π [rad], that θ1(tF)=-1.10248 [rad] which coincides with the value of θ1(tF) obtained numerically.


### 7.2.2. Problem 4: Initial Value Problem with Nonzero Initial Velocityθ˙1

Another initial value problem has been solved with the following conditions:(77)θ1(tI)=0[rad],θ1(tF)=free,θ2(tI)=0[rad],θ2(tF)=π[rad],θ˙1(tI)=5[rad/s],θ˙1(tF)=free,θ˙2(tI)=0[rad/s],θ˙2(tF)=0[rad/s].
The initial values of the multipliers have been set to zero whereas their final value is left free. Notice that no final conditions have been imposed on θ1 and θ˙1. The same considerations done in previous section hold in this case, as well. The technique described in Section 5.1 must be applied twice leading to the differential constraint (74) and to the Hamiltonian (75). Figure 8 shows the sequence of configurations of the robot at times k/32 with k=0,1,…,32, and Figure 9 depicts the control and state variables of the optimal solution obtained with a discretization of the interval [tI,tF] into 64 subintervals. In particular, we get that θ1(tF)=6.17172 [rad] and θ˙1(tF)=9.00163 [rad/s]. To check the consistency of the obtained value of θ1(tF) with the holonomic constraint, consider (58). We can calculate the constant c1 using the initial conditions of the problem obtaining
(78)c1=-θ˙1(tI)(α+2βcosθ2(tI))-θ˙2(tI)(δ+βcosθ2(tI))=-22.5.
Since c1≠0, (58) corresponds in this case to the differential constraint
(79)dη=(α+2βcosθ2)dθ1+(δ+βcosθ2)dθ2+c1dt=0.
Equation (79) can be rewritten in this form
(80)dθ1=-(δ+βcosθ2(t))α+2βcosθ2(t)dθ2-c1α+2βcosθ2(t)dt.
To check the obtained value of θ1(tF), dθ1 is numerically integrated between θ1(tI) and θ1(tF) using the interpolated numerical optimal solution obtained for θ2(t). We get that θ1(tF)=6.18705. This value is close to 6.17172.Figure 8
Sequence of configurations of the robot manipulator at timesk/32 with k=0,1,…,32 corresponding to the optimal solution of problem 4, an initial value problem for an underactuated R¯R planar robot manipulator with boundary conditions θ1(tI)=0[rad], θ2(tI)=0[rad], θ˙1(tI)=5[rad/s], θ˙2(tI)=0[rad/s], θ1(tF)=free, θ2(tF)=π[rad], θ˙1(tF)=free,  and θ˙2(tF)=0[rad/s], obtained with a discretization of [tI,tF] into 64 intervals. The initial and final times are tI=0 and tF=1[s], respectively. The corresponding control and state variables are represented in Figure 9.Control and state variables of the optimal solution of problem 4, an initial value problem for an underactuatedR¯R planar robot manipulator with boundary conditions θ1(tI)=0[rad], θ2(tI)=0[rad], θ˙1(tI)=5[rad/s], θ˙2(tI)=0[rad/s], θ1(tF)=free, θ2(tF)=π[rad], θ˙1(tF)=free, and θ˙2(tF)=0[rad/s]. The initial and final times are tI=0 and tF=1[s], respectively. The corresponding sequence of configurations of the robot manipulator at times k(1/32) with k=0,1,…,32 is represented in Figure 8.
(a)
θ
1
(b)
θ
2
(c)
θ
˙
1
(d)
θ
˙
2
(e)
u
2To check the consistency of the obtained value ofθ˙1(tF) with the constraint (58), using the computed value c1=-22.5 and the final conditions θ˙2(tF)=0, θ2(tF)=π of the problem, we obtain
(81)θ˙1(tF)=-(δ+βcosθ2(tF))α+2βcosθ2(tF)θ˙2(tF)-c1α+2βcosθ2(tF)=9.
This value is very close to the value of θ˙1(tF) obtained numerically.


## 7.2.1. Problem 3: Initial Value Problem with Zero Initial Velocities

An initial value problem has been solved with the following initial and final conditions:(59)θ1(tI)=0[rad],θ1(tF)=free,θ2(tI)=0[rad],θ2(tF)=π[rad],θ˙1(tI)=0[rad/s],θ˙1(tF)=0[rad/s],θ˙2(tI)=0[rad/s],θ˙2(tF)=0[rad/s].
The initial values of the control variable and of the multipliers have been set to zero, whereas their final value is left free.Since there is a holonomic constraint that relates the values of the anglesθ1 and θ2, without integrating (58), we are not able to find the value of θ1(tF) consistent with θ1(tI). Therefore, no final conditions have been imposed on θ1.From the initial conditions of the problem, we obtainc1=0. Equation (58) with c1=0 corresponds to the homogeneous differential constraint
(60)dζ=(α+2βcosθ2)dθ1+(δ+βcosθ2)dθ2=0.
The differential dζ is not exact. However, it becomes an exact differential if multiplied by the factor 1/(α+2βcosθ2). This operation does not alter the differential equation (60). In this case, there does exist a function ζ whose differential coincides with the expression dζ/(α+2βcosθ2). Due to the existence of this function, the integral of dζ between two points depends only on these points and not on the integration path. Equation (60) rewritten in this form
(61)dθ1=-(δ+βcosθ2)α+2βcosθ2dθ2
can be integrated by separating variables. The corresponding holonomic constraint has the following expression:
(62)θ1=(α-2δ)tanh-1[2β-α4β2-α2tan(θ22)]×(4β2-α2)-1-θ22+c2.To express this optimal control problem in the form of a basic optimal control problem, we first have to convert (57) into a first-order differential model introducing the following change of variables:
(63)x1=θ1,x3=θ˙1x2=θ2,x4=θ˙2,
with the following additional relations:
(64)x1′=x3,x2′=x4.
Thus, the optimal control problem is to minimize
(65)∫01‍u22dt
subject to the constraints
(66)x1=(α-2δ)tanh-1[2β-α4β2-α2tan(x22)]×(4β2-α2)-1-x22+c2,(67)(δ+βcosθ2)x3′+δx4′+βsinx2x32=u2,
and the additional constraints (64). To reformulate this optimal control problem as an unconstrained calculus of variations problem, let X be
(68)X=[X1⋯X9],
such that
(69)Xi=xi,i=1,…,4,X5′=u1,X5(tI)=0,
where X6′ with X6(tI)=0 is the multiplier associated with the holonomic constraint (66), X7′ with X7(tI)=0 is the multipliers associated with the differential constraint (67), and X8′ with X8(tI)=0 and X9′ with X9(tI)=0 are the multipliers associated with the additional equality constraints (64).Thus, the holonomic constraint of the problem can be rewritten as follows:(70)φ(t,X)=X1-((α-2δ)(4β2-α2)[2β-α4β2-α2tan(X22)]×tanh-1[2β-α4β2-α2tan(X22)])×(4β2-α2)-1+X22=0.
Now, the technique described in Section 5.1 to deal with holonomic constraints can be applied to φ(t,X), and this holonomic constraint is replaced by
(71)φt+φXX′=0,φ(0,X(0))=0.
From the initial conditions of the problem, the latter equation reduces to the equality 0=0, whereas the former takes the following form:
(72)(α+2βcos(X2))X3+(δ+βcos(X2))X4=0.
The corresponding Hamiltonian is
(73)G1=X5′2+X6′((α+2βcos(X2))X3+(δ+βcos(X2))X4)+X7′(βsin(X2)X32+(δ+βcos(X2))X3′+δX4′-X5′)+X8′(X1′-X3)+X9′(X2′-X4).
It is not difficult to check that matrix G1X′X′ is singular. This is due to the fact that to handle our optimal control problem which involves second-order differential constraints we converted them into first-order differential constraints. Therefore, we apply again the technique of Section 5.1 obtaining the identity 0=0 and the following constraint:
(74)-βsin(X2)X3X4-βsin(X2)X4(X3+X4)+(α+2βcos(X2))X3′+(δ+βcos(X2))X4′=0.
The corresponding Hamiltonian is
(75)G2=X5′2+X6′×(-βsin(X2)X3X4-βsin(X2)X4(X3+X4)+(α+2βcos(X2))X3′+(δ+βcos(X2))X4′)+X7′(βsin(X2)X32+(δ+βcos(X2))X3′+δX4′-X5′)+X8′(X1′-X3)+X9′(X2′-X4).
It is not difficult to check that matrix G2X′X′ in this case is not singular since its determinant is
(76)det(G2X′X′)=12(β2+2δ(-α+δ)+β2cos(2X2))2.
Substituting the values of α, β, and δ, this expression becomes det(G2X′X′)=1/128(43-2cos(2X2))2 which is always positive for any real value X2. Figure 6 shows the sequence of configurations of the robot at times k/32 with k=0,1,…,32 and Figure 7 depicts control and state variables of the optimal solution obtained with a discretization of the interval [tI,tF] into 64 subintervals.Figure 6
Sequence of configurations of the robot manipulator at timesk/32 with k=0,1,…,32 corresponding to the optimal solution of problem 3, an initial value problem for an underactuated R¯R planar robot manipulator with boundary conditions θ1(tI)=0[rad], θ2(tI)=0[rad], θ˙1(tI)=0[rad/s], θ˙2(tI)=0[rad/s], θ1(tF)=free, θ2(tF)=π[rad], θ˙1(tF)=0[rad/s], and θ˙2(tF)=0[rad/s], obtained with a discretization of [tI,tF] into 64 subintervals. The initial and final times are tI=0 and tF=1[s], respectively. The corresponding control and state variables are represented in Figure 7.Control and state variables of the optimal solution of problem 3, an initial value problem for an underactuatedR¯R planar robot manipulator with boundary conditions θ1(tI)=0[rad], θ2(tI)=0[rad], θ˙1(tI)=0[rad/s], θ˙2(tI)=0[rad/s], u2(tI)=0 Nm, θ1(tF)=free, θ2(tF)=π[rad], θ˙1(tF)=0[rad/s], and θ˙2(tF)=0[rad/s]. The initial and final times are tI=0 and tF=1[s], respectively. The corresponding sequence of configurations of the robot manipulator at times k(1/32) with k=0,1,…,32 is represented in Figure 6.
(a)
θ
1
(b)
θ
2
(c)
θ
˙
1
(d)
θ
˙
2
(e)
u
2In particular, we getθ1(tF)=-1.10248 [rad]. To check the consistency of this result with the holonomic constraint (62), since θ˙1(tI)=θ˙2(tI)=0 [rad/s], we get from (58) that c1=0 and using the initial condition θ1(tI)=θ2(tI)=0 [rad], we get from (62) that c2=0. Having established the value of the constant c2, we obtain from the same equation, for θ2(tF)=π [rad], that θ1(tF)=-1.10248 [rad] which coincides with the value of θ1(tF) obtained numerically.


## 7.2.2. Problem 4: Initial Value Problem with Nonzero Initial Velocityθ˙1

Another initial value problem has been solved with the following conditions:(77)θ1(tI)=0[rad],θ1(tF)=free,θ2(tI)=0[rad],θ2(tF)=π[rad],θ˙1(tI)=5[rad/s],θ˙1(tF)=free,θ˙2(tI)=0[rad/s],θ˙2(tF)=0[rad/s].
The initial values of the multipliers have been set to zero whereas their final value is left free. Notice that no final conditions have been imposed on θ1 and θ˙1. The same considerations done in previous section hold in this case, as well. The technique described in Section 5.1 must be applied twice leading to the differential constraint (74) and to the Hamiltonian (75). Figure 8 shows the sequence of configurations of the robot at times k/32 with k=0,1,…,32, and Figure 9 depicts the control and state variables of the optimal solution obtained with a discretization of the interval [tI,tF] into 64 subintervals. In particular, we get that θ1(tF)=6.17172 [rad] and θ˙1(tF)=9.00163 [rad/s]. To check the consistency of the obtained value of θ1(tF) with the holonomic constraint, consider (58). We can calculate the constant c1 using the initial conditions of the problem obtaining
(78)c1=-θ˙1(tI)(α+2βcosθ2(tI))-θ˙2(tI)(δ+βcosθ2(tI))=-22.5.
Since c1≠0, (58) corresponds in this case to the differential constraint
(79)dη=(α+2βcosθ2)dθ1+(δ+βcosθ2)dθ2+c1dt=0.
Equation (79) can be rewritten in this form
(80)dθ1=-(δ+βcosθ2(t))α+2βcosθ2(t)dθ2-c1α+2βcosθ2(t)dt.
To check the obtained value of θ1(tF), dθ1 is numerically integrated between θ1(tI) and θ1(tF) using the interpolated numerical optimal solution obtained for θ2(t). We get that θ1(tF)=6.18705. This value is close to 6.17172.Figure 8
Sequence of configurations of the robot manipulator at timesk/32 with k=0,1,…,32 corresponding to the optimal solution of problem 4, an initial value problem for an underactuated R¯R planar robot manipulator with boundary conditions θ1(tI)=0[rad], θ2(tI)=0[rad], θ˙1(tI)=5[rad/s], θ˙2(tI)=0[rad/s], θ1(tF)=free, θ2(tF)=π[rad], θ˙1(tF)=free,  and θ˙2(tF)=0[rad/s], obtained with a discretization of [tI,tF] into 64 intervals. The initial and final times are tI=0 and tF=1[s], respectively. The corresponding control and state variables are represented in Figure 9.Control and state variables of the optimal solution of problem 4, an initial value problem for an underactuatedR¯R planar robot manipulator with boundary conditions θ1(tI)=0[rad], θ2(tI)=0[rad], θ˙1(tI)=5[rad/s], θ˙2(tI)=0[rad/s], θ1(tF)=free, θ2(tF)=π[rad], θ˙1(tF)=free, and θ˙2(tF)=0[rad/s]. The initial and final times are tI=0 and tF=1[s], respectively. The corresponding sequence of configurations of the robot manipulator at times k(1/32) with k=0,1,…,32 is represented in Figure 8.
(a)
θ
1
(b)
θ
2
(c)
θ
˙
1
(d)
θ
˙
2
(e)
u
2To check the consistency of the obtained value ofθ˙1(tF) with the constraint (58), using the computed value c1=-22.5 and the final conditions θ˙2(tF)=0, θ2(tF)=π of the problem, we obtain
(81)θ˙1(tF)=-(δ+βcosθ2(tF))α+2βcosθ2(tF)θ˙2(tF)-c1α+2βcosθ2(tF)=9.
This value is very close to the value of θ˙1(tF) obtained numerically.


## 7.3. Computational Issues

If the optimal control problem hasm variables and the time interval [tI,tF] has been discretized into N subintervals, the resulting set of difference equations (38) has m×(N-1) equations and m×(N-1) variables plus the equations and variables due to transversality conditions. Feasible solutions have been used as initial guesses of the algorithm.The solution of the nonlinear system of difference equations (38) has been obtained using a damped Newton algorithm within a line search methodology implemented in Mathematica 7 under Mac OS X operating system (see [22, 23] for more details).


## 8. Conclusion

In this paper the trajectory planning problem for planar underactuated robot manipulators with two revolute joints without gravity has been studied. This problem is solved as an optimal control problem based on a numerical resolution of an unconstrained variational calculus reformulation of the optimal control problem in which the dynamic equation of the mechanical system is regarded as a constraint. It has been shown that this reformulation method based on special derivative multipliers is able to tackle both integrable and nonintegrable differential constraints of the dynamic models of underactuated planar horizontal robot manipulators with two revolute joints. This method can be seamlessly applied in the presence of additional constraints on the mechanical system.



---

*Source: 476094-2013-05-27.xml*