Mechatronics 76 (2021) 102540

Contents lists available at ScienceDirect

Mechatronics
journal homepage: www.elsevier.com/locate/mechatronics

A novel 3D path following control framework for robots performing surface

finishing tasks
Yalun Wen, Prabhakar R. Pagilla ∗
Department of Mechanical Engineering, Texas A&M University, College Station, TX 77843, USA

ARTICLE INFO ABSTRACT

Keywords: We describe a novel 3D path following control framework for articulated robots in applications where
Motion control system constant speed travel along a path is desirable, such as robotic surface finishing tasks. Given the desired
Robot manipulators robot configuration sequence with a list of waypoints along a path, a trajectory optimization scheme based
Path following
on direct collocation is proposed to determine the Cartesian path and the maximum constant translation
Robotics technology
speed that are dynamically feasible. Employing the Hermite–Simpson collocation method, a Cartesian path
Mechatronics systems
is developed that not only preserves the characteristics of the original motion sequence but also satisfies the
physical requirements of the robot kinematics and dynamics. Since joint velocity control is quite common in
many industrial robots, we consider a 3D kinematic control in the robot tool frame with control inputs as
rate of change of orientation. The objective for the translation motion is to achieve constant speed along the
path tangent direction, and that of the orientation control is to orient the robot properly based on the path
provided by a converging path planner. We describe the optimization procedure employed with the direct
collocation method to obtain the desired Cartesian path, an arc-length based re-parametrization of the desired
path, and a path planner that provides a converging path to the desired path. To perform the surface finishing
operation, we further present the joint space control law that is converted from the synthesis of the proposed
path following and impedance force control in the tool frame. To verify and evaluate the performance of the
proposed framework, we have conducted extensive experiments with a six degrees-of-freedom (DOF) industrial
robot for several paths that can be employed for surface finishing of a variety of industrial parts.

1. Introduction the surface to facilitate uniform material removal in surface finishing

The robot end-effector motion for many industrial robotics applica-
tions, such as surface finishing, machining, welding, laser cutting, etc.,
can be cast as a path-following problem where the end-effector follows
a geometric path without any preassigned timing information. For
robotic surface finishing tasks, a decoupled motion planning scheme
is typically adopted where a high-level planner generates a list of way-
points based on the geometry of the workpiece without regard to the
robot dynamics [1]; subsequently, a controller is designed to achieve
point-to-point motion at a specified end-effector speed and a desired
normal force. However, driven by the need for high performance in
both motion and force control aspects, robot control schemes have to
effectively deal with the system dynamics and structural limitations
typical of a robotic manipulators such as joint space bounds of position,
velocity, acceleration and torque. In order to incorporate these and
other requirements and develop a conforming robot path with the tool
aligned normal to a 3D surface, we present a novel path following
control framework that reliably achieves constant travel speed along
operations.
To improve task efficiency, several optimal methods have been
considered for path construction, including dynamic programming [2]
and convex optimization [3]. Trajectory optimization is the process
of designing state and control trajectories that optimize some mea-
sure of performance while satisfying a set of constraints. Early work
in [4] formulated a cost function as a way to trade off time-optimality
against squared velocity and joint torques for motion smoothness.
Trajectory optimization for manipulators to travel along prescribed
geometric paths subject to kinodynamic constraints has been reported
in [5,6]. Various numerical methods have been proposed to solve
the optimization problem, which can generally be classified into two
methods, indirect and direct. Due to the complexity of the deriva-
tion of optimality conditions from Pontryagin minimum principle and
solving the subsequent boundary value problems, direct methods have
gained in popularity. In direct methods, the optimal control problem

∗ Corresponding author.
E-mail addresses: ywen27@tamu.edu (Y. Wen), ppagilla@tamu.edu (P.R. Pagilla).

https://doi.org/10.1016/j.mechatronics.2021.102540
Received 7 December 2020; Accepted 7 March 2021
Available online 26 April 2021
0957-4158/© 2021 Published by Elsevier Ltd.
Y. Wen and P.R. Pagilla
is transformed to a general high dimensional NLP problem through
discretization.
The well-known direct methods are multiple shooting [7] and collo-
cation [8]. Compared to multiple shooting, collocation method is well
suited for a larger number of optimization parameters, stiff systems,
and can achieve higher levels of accuracy for tasks with more complex
control trajectory [9] such as in the case of articulated robots. The
direct collocation method has proven particularly effective for robot
manipulators where the state trajectories, either in joint space or task
space, are represented by smooth and parameterized splines [10]. The
joint space implementation of such a strategy is simpler and numeri-
cally efficient because the joint constraints can be introduced without
using the highly nonlinear inverse kinematics. In particular, cubic
splines are ubiquitous in robotic applications, as they provide a simple
means for generating twice differentiable trajectories for robot motion.
For the construction of Cartesian finishing paths, splines have been
extensively used to model complex free-form surfaces, such as complex
dies, molds and automotive parts [11]. Spline-interpolated paths are
known to result in better surface quality, reduced vibration [12], and
longer tool life [13].
Given a geometric description of the desired path, the path follow-
ing problem is concerned with the design of a control law without a
time parametrization [14]. The fundamental difference between path
following and trajectory tracking is that the reference states are found
based on the current system states in path following [15], whereas
an object is forced to catch up with the constantly evolving reference
on the trajectory based on time and this may result in excessive use
of actuators [16]. The controller development of many path follow-
ing problems relies on a path parameter, such as arc-length (natural
parametrization), in order to establish a time-invariant description of
the path. For example, many path following problems are studied using
the Frenet–Serret Frames [17] where the arc-length is used to represent
the total distance the robot has traveled along the curve. Depending on
the curve parameter, different control strategies can be utilized [18].
In [17], the target point is designed to be the closest point on the path,
relative to the current position of the object. For controller design,
a partial feedback linearization method has been reported in [19]
and an MPC formulation can be found in [20]. Research on time-
optimal path following with robot manipulators was presented in [21],
and a convex reformulation of this problem was suggested in [22].
Methods for online time-optimal path tracking were considered in [23].
Path following can result in smoother convergence and higher accu-
racy than trajectory tracking [17]. Thus, the path following method
has gained increased attention in recent developments in intelligent
robotics, UAVs, and autonomous vehicles [24]. However, it should be
noted that many path following problems have been considered for
pure motion control problems, and the path following error is usually
formulated in a direction normal to the desired path [17]. Although this
allows for rapid convergence to the path, the end-effector could also
easily overshoot [25]. Since the end-effector position is constrained by
the workpiece geometry in the case of many robotic tasks that involve
contact, overshooting can cause transition problems [26]. As discussed
in [27], the transition from free motion to constrained motion leads
to impact forces and can result in discontinuities in system equations
which can cause significant stability problems due to non-zero velocity
of impact in the surface normal direction.
In this paper, we have provided a comprehensive framework for
path following for constrained robot tasks where a set of waypoints on
the constraint surface is given. In this regard, the main contributions of
this work can be summarized by the following: (1) A multistage time
optimal trajectory optimization scheme is developed to calculate the
maximum achievable constant speed along a given list of waypoints
while satisfying robot dynamics and actuator constraints. (2) An arc-
length based Cartesian path is developed for motion along the Cartesian
path with constant speed. (3) A novel convergent path planner for path
following control is designed to avoid non-zero velocity of impact in
2
Mechatronics 76 (2021) 102540
Fig. 1. Path on a Workpiece Surface.
the surface normal direction. (4) A velocity control law is synthesized
based on a 3D kinematic model to traverse the path with constant
speed while simultaneously achieving regulation of contact force in
the surface normal direction. (5) Simulations and experimental results
with two different robots (UR5 and UR10) are presented to verify the
optimized path generation procedure as well as the overall framework
via surface polishing experiments.
The rest of the paper is organized as follows. Section 2 provides
the problem description. In Section 3, we present the development
of a multi-stage trajectory optimization scheme. The construction of
a constant speed arc-length based Cartesian path curve is described
in Section 4. Section 5 describes the converging path planner, the
kinematic model used to command the end-effector motion and a
control law to achieve desired motion along the converging path with
the constant speed. Section 6 provides the impedance force control and
the synthesized joint space control command for simultaneous motion
and force control. Simulation and experimental results for the entire
proposed scheme with two six DOF industrial robots are presented and
discussed in Section 7. Concluding remarks are provided in Section 8.
Additional contributions in the journal paper over the IFAC con-
ference paper and other preliminary work: Our preliminary work
on the proposed path following framework for planar 2D curves was
presented in [25] and for 3D curves in [28]. Additional original con-
tributions over the IFAC 2020 conference paper [28] include: (1)
Development of the multistage optimization scheme to generate a
dynamically feasible Cartesian path that is compatible with robot dy-
namics and constraints and the maximum achievable constant speed
along the path (Section 3). (2) Synthesis of the simultaneous motion
and force control law for surface finishing tasks (Section 6). (3) Exper-
imental results with two robots verifying the optimization procedure
and overall framework for robotic polishing operation (Section 7). In
addition, we have rewritten and edited many parts of the conference
paper content in the journal paper to be consistent with the new
contributions and to streamline the journal paper content. We have also
added new figures to better explain the concepts and developments.
2. Problem description
In a typical robotic surface finishing application, the main task for
the motion controller is to follow a list of waypoints based on the
workpiece surface geometry. For example, Fig. 1 shows the profile of
an aircraft blade with sample waypoints that should be followed for
sanding of the blade surface; one sample finishing path that passes
through the waypoints is shown by the green curve. Since uniform
material removal is desirable, the surface finishing tool mounted on
the robot is required to travel on the surface with a constant speed
whose direction is along the tangent to the path; and the orientation
of the tool axis of rotation to be normal to the tangent plane along the
path. To improve task efficiency, it is desirable for the robot to travel
with maximum speed that is allowable by the joint constraints and the
robot dynamics; in some cases one may choose the speed to be less
than the maximum speed, but one can make such a choice only when
the maximum speed is known. Since engineered curved surfaces of
many products, such as turbine blades, aircraft and automobile bodies,
Y. Wen and P.R. Pagilla
Fig. 2. Overall Structure of the Proposed Control Framework.
need finishing, and these tasks are automated by employing articulated
robots, there is a significant need for the development of an efficient
3D path following scheme.
Given the discrete representation of the workpiece surface, one can
use a high-level planning algorithm to generate a series of kinematically
feasible waypoints in joint space that the robot should pass through
to complete the task. We assume the joint space motion sequence is
dense enough to be representative of the curvature of the workpiece
surface. The goal is to perform robotic surface finishing along a path
passing through the given waypoints with constant speed. If the robot
experiences perturbations and veers from the desired path, then we de-
sire a subsequent path that will converge to the desired path smoothly
without impacting the surface. To achieve this goal, we formulate
the following three subproblems which form the focus of this paper:
(1) the construction of a dynamically feasible Cartesian path and the
maximum achievable speed based on the given discrete joint space
waypoints and robot dynamics and constraints; (2) an arc-length based
parametrization of the Cartesian path for the maximum speed; and (3)
a robot control law that can execute the dynamically feasible Cartesian
path. The proposed framework for the three subproblems and how they
are related is illustrated in Fig. 2.
3. Trajectory optimization
We consider the task of generating a smooth Cartesian path for the
robot end-effector passing through the given waypoints and moving
with the maximum possible speed. We employ a cubic spline interpo-
lation with the Hermite–Simpson collocation method. In the following,
we present the continuous time and transcribed discrete formulation
of the proposed trajectory optimization procedure with equality and
inequality constraints.
We consider the following state space representation of the robot
equations of motion:
[ ] [ ] 𝑡𝑝𝑓 = 𝑡𝑝+1 . An illustration of this notation is provided along the time-axis
𝑿̇ 1 𝑿2
= ∶= 𝐅(𝑡). (1)
𝑿̇ 2 𝐇(𝑿1 )−1 (𝐮 − 𝐂(𝑿1 , 𝑿2 ) − 𝐠(𝑿1 ))
where 𝑿1 ∶= 𝐪(𝑡) and 𝑿2 ∶= 𝐪(𝑡) ̇ with 𝐪(𝑡) and 𝐪(𝑡)
̇ as the joint position
and velocity vectors, 𝐮(𝑡) is the vector of joint torques, 𝑛𝑥 is the number
of robot joints, 𝐇(⋅) is the joint space inertia matrix, 𝐂(⋅) is the matrix of
Coriolis and centrifugal terms, and 𝐠(⋅) is the joint space gravity vector,
𝑡 denotes time. We also define the robot joint space state vector as:
𝑿(𝑡) = [𝑿1𝑇 , 𝑿2𝑇 ]𝑇 .
We consider the following formulation for generating a time optimal
trajectory that conforms to all the given constraints:
minimize 𝑇𝑓
𝑿(𝑡),𝐮(𝑡)
̇ where 𝑘 = 0, … , 𝑛𝑝 .
subject to 𝑿(𝑡) = 𝐅(𝑡)
𝑔 (𝑿(𝑡), 𝐮(𝑡)) = 𝒈𝑙 (2)
ℎ (𝑿(𝑡)) = 0
𝜂 𝑙 ≤ 𝜂 (𝑿(𝑡), 𝐮(𝑡)) ≤ 𝜂 𝑢 .
where 𝑇𝑓 is the total time taken to execute the trajectory, 𝑔(⋅) denotes
the boundary constraints on the initial and terminal values of the state
3
Mechatronics 76 (2021) 102540
and input, ℎ(⋅) denotes the path constraints which includes the kine-
matic constraints associated with constant speed travel while passing
through all the waypoints, and 𝜂(⋅) denotes the system inequality con-
straints associated with the robot with lower and upper bounds given
by 𝜂 𝑙 and 𝜂 𝑢 , respectively. This is a complex problem for a multiple
DOF articulated robot which depends on the robot configuration and
its constrained motion. To numerically solve this problem, we can
transcribe the continuous-time problem using the direct collocation
method into a finite dimensional nonlinear program (NLP) [29].
3.1. Direct collocation
In the Hermite–Simpson collocation method, the control inputs and
states are approximated by piecewise continuous linear functions and
cubic polynomials, respectively. Since the boundary conditions, path
constraints and system inequality constraints are functions of the states
and control, they can be converted to a set of equality and inequality
as functions of the linear and cubic polynomials coefficients. In the
following we utilize the transcription process described in [8] and
modify it to include all the given waypoints and introduce additional
constraints associated with our problem.
We first present the transcription process to transcribe the continu-
ous formulation provided in (2) into a discrete NLP which can be solved
using a large-scale NLP solver, i.e., the continuous state and control are
discretized at appropriately chosen time grids in the time interval of
interest. Suppose (𝑁 + 1) ∈ Z+ desired waypoints in joint space are
given as (𝐪𝑑0 , … , 𝐪𝑑𝑁 ), where 𝐪𝑑𝑖 ∈ R𝑛𝑥 . Let 0 = 𝑡00 < ⋯ < 𝑡𝑝0 < ⋯ <
𝑡𝑁
0
= 𝑇𝑓 define the time intervals and 𝑝 ∈ [0, … , 𝑁] denote the stage
number. The time interval of stage 𝑝 is partitioned into 𝑛𝑝 uniform
time grids, where 𝑛𝑝 is a design parameter. For convenience, we denote
𝛥𝑡𝑝 = (𝑡𝑝+1
0
− 𝑡𝑝0 )∕𝑛𝑝 as the time interval between two consecutive time
grids. Thus, 𝑡𝑝𝑘 = 𝑡𝑝0 + 𝑘𝛥𝑡𝑝 , 𝑘 ∈ [0, 1, … , 𝑛𝑝 ], 𝑡𝑝𝑘+0.5 = 12 (𝑡𝑝𝑘 + 𝑡𝑝𝑘+1 ) and
0
in Fig. 3; note that the collocation point (𝑡𝑝𝑘+0.5 ) is not shown.
3.2. Equality constraints
3.2.1. System dynamics
The system dynamics is integrated over the time grids via the
Simpson quadrature that converts the differential equations into a set
of equality constraints along the time grids. For every two consecutive
time grids of stage 𝑝, the state is approximated as follows:
1 𝑝
𝑿(𝑡𝑝𝑘+1 ) − 𝑿(𝑡𝑝𝑘 ) ≈ 𝛥𝑡 (𝐅(𝑡𝑝𝑘 ) + 4𝐅(𝑡𝑝𝑘+0.5 ) + 𝐅(𝑡𝑝𝑘+1 )). (3)
6
3.2.2. Boundary constraints
Assuming that the robot manipulator is at rest at the initial and final
time, the corresponding boundary conditions for the state and control
are:
Joint position: 𝐪(0) = 𝐪𝑑0 , 𝐪(𝑇𝑓 ) = 𝐪𝑑𝑁 ; (4a)
Y. Wen and P.R. Pagilla Mechatronics 76 (2021) 102540

joint states and control are continuous, to ensure continuity between

stages, the following continuity conditions are enforced:

Position continuity: 𝐪(𝑡𝑝𝑓 ) = 𝐪(𝑡𝑝+1

0
); (9a)

Velocity continuity: ̇ 𝑝𝑓 )
𝐪(𝑡 = ̇ 𝑝+1
𝐪(𝑡 0
); (9b)

Acceleration continuity: ̈ 𝑝𝑓 )
𝐪(𝑡 = 𝑝+1
̈ 0 );
𝐪(𝑡 (9c)

Input continuity: 𝐮(𝑡𝑝𝑓 ) = 𝐮(𝑡𝑝+1

0
). (9d)

Since we have 𝐪(𝑡𝑝𝑓 ) = 𝐪(𝑡𝑝+1

0
̇ 𝑝𝑓 ) = 𝐪(𝑡
), 𝐪(𝑡 ̇ 𝑝+1
0
) and the kinematic
constraint from Eq. (5), we should have 𝑣𝑝𝑑 = 𝑣𝑝+1
𝑑
for all 𝑝. Since the
speed for all the stages is the same, from now on we will denote 𝑣𝑝𝑑
by 𝑣𝑑 .

Fig. 3. Illustration of the state trajectory for joint 1 showing stages and time grids for
3.3. Inequality constraints
the proposed multi-stage TO. The dots refer to the discretized states and the blue curve
is the cubic spline interpolation. In addition, the green dots refer to the given joint
The joint inequality constraints encode physical limitations of the
space waypoints. The states at 𝑡00 and 𝑡𝑁−1
𝑓
are given by the boundary conditions. The
state at the initial time of a given stage is set to be equal to the joint waypoint. The robot actuators into the problem. These joint constraints include:
user has the flexibility to determine the number of grids for a given stage based on the
surface geometry: for a stage where rapid state changes are expected, more grid points Position bounds: 𝐪 ≤ 𝐪(𝑡𝑝𝑘 ) ≤ 𝐪; (10a)
can be assigned. For example, the illustration shows uniformly distributed 5 time grids
for stage 𝑝 with 𝑛𝑝 = 4. The last state of each stage has to coincide with the initial Velocity bounds: 𝐪̇ ≤ ̇ 𝑝𝑘 )
𝐪(𝑡 ≤ 𝐪;
̇ (10b)
state (waypoint) of the next stage; this is enforced by the continuity conditions. Note
that 𝑡𝑁−1 = 𝑇𝑓 which will be obtained by solving the trajectory optimization problem.
Acceleration bounds: 𝐪̈ ≤ ̈ 𝑝𝑘 )
𝐪(𝑡 ≤ 𝐪;
̈ (10c)
𝑓

Input bounds: 𝐮≤ 𝐮(𝑡𝑝𝑘 ) ≤ 𝐮. (10d)

where the underline and overline on variables indicate the lower and
Joint velocity: ̇
𝐪(0) = 𝟎, ̇ 𝑓 ) = 𝟎;
𝐪(𝑇 (4b)
upper bounds, respectively. Additional external workcell or obstacle
Joint input: 𝐮(0) = 𝟎, 𝐮(𝑇𝑓 ) = 𝟎. (4c) constraints can also be included into this formulation.
where 𝟎 indicates the zero vector.
3.4. NLP and joint space trajectory
3.2.3. Kinematic constraints
Since we wish to have the end-effector or tool travel at a constant
Based on the transcription process and the constraints described
speed, we have the following kinematic constraint:
above, we can now formulate the optimization problem as a NLP in
‖𝐉𝑣 (𝐪(𝑡𝑝𝑘 ))𝐪(𝑡
̇ 𝑝𝑘 )‖ = 𝑣𝑝𝑑 . (5) the following manner. Denote the vector of decision variables as 𝐲 =
{𝑿(𝑡𝑝𝑘 ), 𝑿(𝑡𝑝𝑘+0.5 ), 𝐮(𝑡𝑝𝑘 ), 𝛥𝑡𝑝 } for 𝑝 ∈ [0, 𝑁], 𝑘 ∈ [0, … , 𝑛𝑝 ]. The resulting
where 𝐉𝑣 (⋅) is the partition of robot Jacobian matrix corresponding to
NLP after the transcription can be formulated as:
the end-effector velocity and 𝑣𝑝𝑑 is the magnitude of the velocity vector
𝑛
for stage 𝑝, which is to be determined. Since we are seeking a constant 𝑁 ∑
∑ 𝑝

magnitude velocity of travel along the Cartesian path, this constraint minimize (𝛥𝑡𝑝 )
𝐲
𝑝=0 𝑘=1
coupled with time optimal formulation will ensure maximum speed of (11)
subject to 𝜁 (𝐲) = 𝟎,
travel. Since we have set the length of two time grids as the decision
𝐛𝑚𝑖𝑛 ≤ 𝐛(𝐲) ≤ 𝐛𝑚𝑎𝑥 ,
variables, 𝑣𝑑 is a dependent variable. For a given stage number 𝑝, the
𝐲𝑚𝑖𝑛 ≤ 𝐲 ≤ 𝐲𝑚𝑎𝑥 .
total distance the robot has to travel within a time interval can be
approximated by This NLP minimizes the overall trajectory execution time subject to the
𝑛𝑝 −1 set of boundary functions 𝐛(⋅) and bounds on the decision variables as
∑
𝑙𝑝 = ‖𝐟(𝐪(𝑡𝑝𝑘+1 )) − 𝐟(𝐪(𝑡𝑝𝑘 ))‖. (6) indicated by 𝐲𝑚𝑖𝑛 and 𝐲𝑚𝑎𝑥 . The equations of motion are transcribed to
𝑘=0 be included in the set of equality constraints 𝜁 (⋅). The proposed NLP
where 𝐟(⋅) is the forward position kinematics function of the robot provides a solution for the time grids as well as the values of the state
which provides the current end-effector position. Thus, the equality and control trajectories at each time grid. Specifically, a total number
∑
constraint between 𝑙𝑝 and 𝑣𝑑 is given by of 𝑛𝑑 = 𝑁 𝑝=0 (2𝑛𝑝 + 1) − (𝑁 − 1) joint position can be solved. Once the
decision variable 𝛥𝑡𝑝 is obtained, we can utilize (7) to obtain 𝑣𝑑 .
𝑙𝑝 = 𝑣𝑝𝑑 𝛥𝑡𝑝 𝑛𝑝 . (7) Because the state is represented with cubic polynomials in the
Hermite–Simpson collocation, the direct collocation solves for the de-
3.2.4. Waypoint constraints cision variables as well as the coefficients of the cubic polynomials
For the optimal path to pass through the given waypoints, we
simultaneously. For a given stage 𝑝 and 𝑡 ∈ [𝑡𝑝0 , 𝑡𝑝𝑓 ), the joint trajectory
impose the following constraints on the states at the beginning of each
is described by the following expression [8]:
stage:
𝛿𝑡
′ 𝑿(𝑡) = 𝑿(𝑡𝑝0 ) + 𝐅(𝑡𝑝0 )( )
𝐪(𝑡𝑝0 ) = 𝐪𝑑𝑝′ , 𝑝′ ∈ [1, 𝑁 − 1]. (8) ℎ𝑝
3 1 𝛿𝑡
+ (− 𝐅(𝑡𝑝0 ) + 2𝐅(𝑡𝑝0.5 ) − 𝐅(𝑡𝑝𝑓 ))( 𝑝 )2
3.2.5. Continuity constraints 2 2 ℎ
2 4 1 2 𝛿𝑡
We have so far established constraints associated with system dy- + ( 𝐅(𝑡𝑝0 ) − 𝐅(𝑡𝑝0 + ) + 𝐅(𝑡𝑝𝑓 ))( 𝑝 )3 (12)
namics and kinematics for a given stage 𝑝. The necessity of the conti- 3 3 2 3 ℎ
nuity condition is illustrated and explained in Fig. 3. Since the robot where ℎ𝑝 = 𝑡𝑝𝑓 − 𝑡𝑝0 and 𝛿𝑡 = 𝑡 − 𝑡𝑝0 .

4
Y. Wen and P.R. Pagilla Mechatronics 76 (2021) 102540

3.5. Cartesian path interpolation

To obtain the discrete Cartesian coordinates for Cartesian path

interpolation, we can employ forward kinematics and this can be done
in two ways: (1) Use discrete joint positions obtained at the defined
grid points in the NLP problem or (2) use the continuous joint space
trajectory obtained from (12) and discretize the resulting Cartesian
trajectory using user-defined grid spacing. The second option is more
advantageous as it allows us to specify a finer grid spacing. The (𝑁 ′ +1)
discrete points chosen for the second option could be much larger than
Fig. 4. Equally Segmented Curve by Arc-Length.
the number of grid points utilized in the NLP, i.e., 𝑁 ′ + 1 > 𝑛𝑑 . As a
result, the Cartesian path not only preserves the property of the original
motion sequence since the path passes through joint space waypoints
In the following, 𝑠̄ will be referred to as the scaled arc-length. Denote
but it is also consistent with the robot dynamics and constraints. By
the line integrand defined for the polynomials at index 𝑗 with respect
performing the cubic spline interpolation on the resulting discrete
to the Cartesian path parameter 𝜙 by:
Cartesian waypoints, we can determine the Cartesian desired path:
√
𝜎 (𝜙) = (𝑥(𝜙),
̃ 𝑦(𝜙),
̃ 𝑧(𝜙)).
̄ (13) 𝜑(𝜙, 𝑗) = (𝑥̃ 𝑗 (𝜙)′ )2 + (𝑦̃𝑗 (𝜙)′ )2 + (𝑧̃ 𝑗 (𝜙)′ )2 (17)

where the path parameter 𝜙 is monotonically increasing and one can where the prime on 𝑥̃ 𝑗 (𝜙)′ denotes derivative of 𝑥̃ 𝑗 with respect to 𝜙.
specify (𝑁 ′ + 1) discrete break points (𝜙0 , 𝜙1 , … , 𝜙𝑁 ′ ) for interpolation. The arc-length can be calculated from:
For a 𝜙 in the interval (𝜙𝑗 ≤ 𝜙 < 𝜙𝑗+1 ), one can consider the following 𝜙

cubic spline interpolation: 𝑠(𝜙) =𝑆(𝑗 − 1) + 𝜑(𝜙, 𝑗)𝑑𝜙,

∫𝜙𝑗
(18)
∑
3 ∑
3
∑
𝑗−1 𝜙𝑙+1
𝑥̃ 𝑗 (𝜙) = 𝑎𝑗𝑖 (𝜙 − 𝜙𝑗 )𝑖 , 𝑦̃𝑗 (𝜙) = 𝑏𝑗𝑖 (𝜙 − 𝜙𝑗 )𝑖 , 𝑆(𝑗 − 1) = 𝜑(𝜙, 𝑙)𝑑𝜙.
𝑖=0 𝑖=0
∫𝜙𝑙
𝑙=0
∑3
where 𝑆(𝑗 − 1) is the arc-length for the path starting from 𝜙0 to 𝜙𝑗 . We
𝑧̃ 𝑗 (𝜙) = 𝑐𝑗𝑖 (𝜙 − 𝜙𝑗 )𝑖 . (14)
𝑖=0
uniformly divide the arc-length into 𝑚′ segments and denote the list
of arc-length parameterized points as (𝑠0 , … , 𝑠𝑚′ ) (see Fig. 4.) Denote
where 𝑎𝑗𝑖 , 𝑏𝑗𝑖 , 𝑐𝑗𝑖 , 𝑖 = 0, … , 3, 𝑗 = 0, … , 𝑁 ′ are the cubic spline 𝑠̄𝑚 = 𝑠𝑚 ∕𝑣𝑑 , 𝑚 = 0, … , 𝑚′ and (𝑥̄ 𝑚 , 𝑦̄𝑚 , 𝑧̄ 𝑚 ) as the Cartesian coordinates
parameters. associated with 𝑠̄𝑚 , we reinterpolate the spline curve by interpolating
Note that the above 3D Cartesian path construction does not in [(𝑠̄0 , 𝑥̄ 0 ), … , (𝑠̄𝑚′ , 𝑥̄ 𝑚′ )], [(𝑠̄0 , 𝑦̄0 ), … , (𝑠̄𝑚′ , 𝑦̄𝑚′ )] and [(𝑠̄0 , 𝑧̄ 0 ), … , (𝑠̄𝑚′ , 𝑧̄ 𝑚′ )]
general represent a constant speed path with the path variable 𝜙. In with cubic spline, and then we have the desired path 𝐏( ̄ 𝑠).
̄ For a 𝑠̄ in
the following section, we will re-parameterize the curve with an arc- the interval (𝑠̄𝑚 ≤ 𝑠̄ < 𝑠̄𝑚+1 ), the Cartesian components of the desired
length based parametrization to add that information to the Cartesian path are given by:
path. We will also describe a particular method for re-parametrization
method with the calculation of the arc-length that is suitable for our ∑
3 ∑
3 ∑
3
𝑥̄ 𝑚 (𝑠)
̄ = 𝑎̄𝑚𝑖 𝑠̄𝑖𝑒 , 𝑦̄𝑚 (𝑠)
̄ = 𝑏̄ 𝑚𝑖 𝑠̄𝑖𝑒 , 𝑧̄ 𝑚 (𝑠)
̄ = 𝑐̄𝑚𝑖 𝑠̄𝑖𝑒 . (19)
subsequent convergent path planning. 𝑖=0 𝑖=0 𝑖=0

where 𝑠̄𝑒 = 𝑠̄ − 𝑠̄𝑚 , 𝑎̄𝑚𝑖 , 𝑏̄ 𝑚𝑖 and 𝑐̄𝑚𝑖 are the cubic spline parameters
4. Arc-length based 𝟑D path construction
for the re-interpolated cubic spline segment with the scaled arc-length
parameter. The construction of the desired Cartesian path for the robot
Let the Cartesian path be reparameterized based on arc-length is summarized in Algorithm 1.
which is denoted by 𝑠. Denote 𝐏 ∶ 𝐽 → R3 as the arc-length pa-
rameterized curve, where 𝐽 is an open interval and ‖𝑑𝐏(𝑠)∕𝑑𝑠‖2 = 1,
i.e., the curve is of unit speed. Since we would like the end-effector to Algorithm 1 Proposed spline interpolation method
travel along the parameterized curve with constant speed of magnitude Input: Given list of 𝑁 ′ indexed Cartesian coordinates expressed in the
𝑣𝑑 > 0, our goal is to obtain a re-parametrization of the Cartesian robot base frame.
path, as in Eq. (13), with a new path parameter 𝑠̄ such that the Output: Piecewise cubic spline parameterized by the scaled arc-length,
following condition is met: ‖𝑑 𝐏( ̄ 𝑠)∕𝑑
̄ 𝑠‖̄ 2 = 𝑣𝑑 , where 𝐏̄ ∶ 𝐼 → R3 is ̄ 𝑠)
P( ̄ = (𝑥(
̄ 𝑠),
̄ 𝑦(
̄ 𝑠),
̄ 𝑧( ̄ with constant speed 𝑣𝑑 .
̄ 𝑠)),
the reparameterized arc-length based curve. Assuming the interpolated 1: Interpolate the list of indexed control points with cubic spline as
curve to be regular, there exists a differentiable function ℎ ∶ 𝐽 → 𝐼 𝜎 (𝜙) = (𝑥(𝜙),
̃ 𝑦(𝜙),
̃ 𝑧(𝜙)).
̄
̄
such that 𝐏 = 𝐏(ℎ) and 𝑠̄ = ℎ(𝑠); to ensure that the reparametrization 2: Equally partition the Cartesian path 𝜎 by its arc-length into 𝑚′ seg-
is orientation preserving, we have 𝑑ℎ(𝑠)∕𝑑𝑠 > 0. Since the arc-length ments, and we have 𝑚′ + 1 knot points whose Cartesian coordinates
interpolated curve 𝐏(𝑠) is of unit-speed, we have the following relation: corresponding to 𝑠𝑚 are (𝑥̄ 𝑚 , 𝑦̄𝑚 , 𝑧̄ 𝑚 ), 𝑚 = 0 ∶ 𝑚′ .
3: Re-interpolate the 𝑚′ + 1 knot points (𝑥̄ 𝑚 , 𝑦̄𝑚 , 𝑧̄ 𝑚 ) with the scaled
‖ 𝑑𝐏(𝑠) ‖ ‖ 𝑑ℎ(𝑠) 𝑑 𝐏(ℎ(𝑠))
̄ ‖
‖ ‖
1=‖ ‖
‖ 𝑑𝑠 ‖ = ‖ ‖ arc-length 𝑠̄𝑚 = 𝑠𝑚 ∕𝑣𝑑 with cubic spline.
‖ ‖2 ‖ ‖ 𝑑𝑠 𝑑ℎ(𝑠) ‖
‖2
‖ ̄ ‖
𝑑ℎ(𝑠) ‖ 𝑑 𝐏(ℎ(𝑠)) ‖ 𝑑ℎ(𝑠)
= ‖ ‖ = 𝑣 . (15) 5. Motion control strategy for path following
𝑑𝑠 ‖‖ 𝑑ℎ(𝑠) ‖2
‖ 𝑑𝑠 𝑑
Since 𝑣𝑑 ≠ 0, the re-parametrization function ℎ is bijective and con- Our strategy relies on modeling the end-effector velocity via a kine-
tinuous. We would like the re-parametrization to have zero initial matic model and utilizing this model for path following while achieving
condition, thus the constant speed parametrization is related to the constant speed. Since most of the industrial robots are controlled in
arc-length parametrization by: the joint position or velocity mode, the control law is developed in
1 the task space to describe the desired behavior of end-effector velocity.
𝑠̄ = ℎ(𝑠) = 𝑠. (16) We separate the controller design into the tangential and transversal
𝑣𝑑

5
Y. Wen and P.R. Pagilla Mechatronics 76 (2021) 102540

point 𝐩𝑑 ∈ R3 refers to the unique Cartesian point on the desired path

from Eq. (20), which is illustrated in Fig. 5(a). Since the path parameter
and the local geometric property of the desired path can be calculated
from 𝐩𝑑 , the path planner provides the low level control with a path
such that the manipulator has to follow, i.e., the robot moves in an
attractive field where the position 𝐩𝑑 can be reached smoothly from
the current end-effector position. In order for the robot to move along
the converging path, the orientation of the end-effector velocity should
be aligned with the gradients of the converging path with respect to
the path variable. Since we would like the end-effector velocity to be
aligned with the desired path tangent when the robot returns from free-
space to constrained space, there is a need for taking corrective actions
by the path planner when deviations from the nominal path occur. In
the following, we present the converging path planner and elaborate
its functionality.
Given the current end-effector position, we can find the reference
value 𝐩𝑑 and scaled arc-length 𝑠̄𝑐 on the desired nominal path whose
coordinates belong to the following cubic spline segment of index 𝑚:
∑
3 ∑
3 ∑
3
𝑥𝑑 = 𝑎̄𝑚𝑖 𝑠̄𝑖𝑐𝑒 , 𝑦𝑑 = 𝑏̄ 𝑚𝑖 𝑠̄𝑖𝑐𝑒 , 𝑧𝑑 = 𝑐̄𝑚𝑖 𝑠̄𝑖𝑐𝑒 . (20)
𝑖=0 𝑖=0 𝑖=0

⃖⃖⃖⃖⃖⃖⃗ as:
where 𝑠̄𝑐𝑒 = 𝑠̄𝑐 − 𝑠̄𝑚 . We define the vector 𝐷𝐶
⃖⃖⃖⃖⃖⃖⃗ = 𝐩𝑐 − 𝐩𝑑 = [𝑥𝑑𝑐 , 𝑦𝑑𝑐 , 𝑧𝑑𝑐 ]𝑇 .
𝐷𝐶 (21)

Based on this, the 3D converging path parameterized by 𝑠̄ is 𝐐(𝑠) ̄ =

(𝑥(𝑠),
̄ 𝑦(𝑠), ̄ (See Fig. 5(b)). Denote 𝑠̄𝑑𝑒 = 𝑠̄ − 𝑠̄𝑐 , the converging path
̄ 𝑧(𝑠))
from 𝐶 to 𝐷, as

̄ = 𝐩𝑑 + [𝑥𝑑𝑐 , 𝑦𝑑𝑐 , 𝑧𝑑𝑐 ]𝑇 exp (𝛽𝑠 𝑠̄𝑑𝑒 ).

𝐐(𝑠) (22)

where 𝛽𝑠 < 0 is a design constant whose magnitude governs the

convergence behavior of the end-effector as shown in Fig. 5(b): faster
convergence to the desired path is achieved for 𝛽𝑠 with a larger magni-
tude. For the local geometric property, we are interested in finding the
tangent of the converging path at 𝐩𝑐 since the orientation of the end-
effector linear velocity has to be aligned with that of the converging
path tangent if the end-effector is to reach the desired nominal path
along the converging path. Denote the components of the tangent
Fig. 5. Numerical projection and converging path illustration.
vector as ⃗𝑡 = [𝑡𝑥 , 𝑡𝑦 , 𝑡𝑧 ]𝑇 , which are given by:

⎡𝑡𝑥 ⎤ ⎡∑3 𝑖𝑎̄ 𝑠̄𝑖−1 ⎤ ⎡𝑥𝑑𝑐 ⎤

⎢𝑡 ⎥ = ⎢∑𝑖=1
3
𝑗𝑖 𝑒
⎥ ⎢𝑦 ⎥ 𝛽𝑠 exp (𝛽𝑠 ).
directions, where the objective of the tangential direction is to achieve ⎢ 𝑖=1 𝑖𝑏̄ 𝑗𝑖 𝑠̄𝑒 ⎥ +
𝑖−1 (23)
motion with minimum time at a given constant translation speed 𝑣𝑑 ⎢ 𝑦⎥ ∑
⎢ 3 𝑖𝑐̄ 𝑠̄𝑖−1 ⎥ ⎢ 𝑑𝑐 ⎥
⎣𝑡𝑧 ⎦ ⎣ 𝑖=1 𝑗𝑖 𝑒 ⎦ ⎣𝑧𝑑𝑐 ⎦
while the transversal control is to coordinate the other DOF of the ⏟⏞⏞⏞⏞⏞⏞⏞⏟⏞⏞⏞⏞⏞⏞⏞⏟ ⏟⏞⏞⏞⏞⏞⏞⏞⏞⏟⏞⏞⏞⏞⏞⏞⏞⏞⏟
robotic end-effector frame in a desired orientation according to the desired path tangent path planner compensation

path provided by the converging path planner such that the robot can
follow the desired nominal path precisely. With the proposed multi-
stage optimization scheme that considers the input constraints, we can
assume that there is a lower level control that provides the necessary
compensation for torque control.
5.1. Converging path planner
Due to the registration uncertainty associated with the workpiece
and process disturbances, the end-effector may veer off the desired
path. Further, in some applications, the end-effector may need to lift off
the surface and make contact again for surfaces with complex contours
that have disconnected finishing segments. During this process, if the
robot impacts the workpiece with a non-zero velocity in the surface nor-
mal direction, there is possibility of bouncing behavior since the part to
be machined usually has high stiffness. To deal with this problem, we
propose a converging path that the end-effector must follow from its
current position to reach the reference value on the desired path. For
a given robot configuration 𝐪 ∈ R𝑛 , the robot end-effector Cartesian
position 𝐩𝑐 ∈ R3 can be uniquely defined in the base frame. Since we
can calculate the total arc-length the robot has traveled, the reference
̄ 𝑠), ⃖⃖⃖⃖⃖⃖⃗ = 𝟎, and there is no compensation.
If 𝐩𝑐 ∈ 𝐏( ̄ we have 𝐩𝑐 = 𝐩𝑑 and 𝐷𝐶
̄ 𝑠),
If 𝐩𝑐 ∉ 𝐏( ̄ the orientation compensation provided by the path planner
depends on the magnitude of 𝛽𝑠 and ‖⃖⃖⃖⃖⃖⃖ ⃗
𝐷𝐶‖.
In summary, the robot end-effector follows one of the converging
paths to reach its reference value in a manner illustrated in Fig. 5(b):
the dashed line represents the translated spline via vector 𝐷𝐶 ⃖⃖⃖⃖⃖⃖⃗ and the
blue line represents the desired path. A 2D illustration of the converging
path as an attractive field is provided in Fig. 5(c). For drawing this
illustration, we randomly generated a list of points around the desired
path as current robot positions and calculated the corresponding con-
verging paths. The tangent vectors around the converging paths, as
shown by the arrows, form an attractive field around the desired path.
When 𝐩𝑐 ∉ 𝐏( ̄ 𝑠),
̄ the converging path connects the current end-effector
position to the desired path in a way that the end-effector would return
to the desired path with tool aligned with the path. The converging
path could be different from the desired nominal path. The desired
nominal path is interpolated from the discrete waypoints, and is used
by the numerical projection to locate a unique reference for a given
end-effector position. When the current robot pose and reference value
is known, the converging path planner provides the desired converging
path for the path control such that the robot can converge and stay

6
Y. Wen and P.R. Pagilla Mechatronics 76 (2021) 102540
√
where 𝑑𝑥𝑦 = 𝑡2𝑥 + 𝑡2𝑦 . Since we have constrained the translation
speed of end-effector to be a constant, the control for the end-effector
orientation is used to regulate the orientation error between the end-
effector frame and the desired orientation derived from the converging
path. Define these orientation errors as 𝑒1 (𝑡) = 𝜃 − 𝜃𝑑 (𝑡) and 𝑒2 (𝑡) =
𝜓(𝑡) − 𝜓𝑑 (𝑡). When the end-effector is traveling along the path with
constant translational speed, we have 𝑑𝑑𝑡𝑠̄ = 𝑣𝑑 . The error governing
equations after simplification are given by:
( )
𝑣 𝜕𝑡𝑦 𝜕𝑡
𝑒̇ 1 = 𝑢1 − 𝑑 𝑡𝑥 − 𝑥 𝑡𝑦
𝑑𝑥𝑦 𝜕𝑠 𝜕𝑠
=∶ 𝑢1 − 𝑒1 (𝑠𝑒 ), (27)
( )
𝑣𝑑 𝜕𝑡 𝜕𝑡𝑦 𝜕𝑡
𝑒̇ 2 = 𝑢2 − ( 𝑥 𝑡𝑥 + 𝑡 )𝑡 − 𝑧 𝑑 2
2
𝑑𝑥𝑦 𝑑𝑥𝑦𝑧 𝜕𝑠 𝜕𝑠 𝑦 𝑧 𝜕𝑠 𝑥𝑦
Fig. 6. Illustration of the 3D kinematics model. The end-effector velocity is aligned
with the path tangent. =∶ 𝑢2 − 𝑒2 (𝑠𝑒 ). (28)
√
where 𝑑𝑥𝑦𝑧 = 𝑡2𝑥 + 𝑡2𝑦 + 𝑡2𝑧 . One can choose an orientation control law
based on feedback linearization and a PID controller in the following
onto the desired nominal path with its tangent aligned with that of the
desired path to avoid impact with the surface; When the robot is on the form to impose a linear error dynamics and guarantee the end-effector
desired path, the current robot position coincides with the reference to follow the converging path:
Cartesian point. [ ] [ 𝑡
]
𝑢1 𝑒1 (𝑠𝑒 ) − 𝑘𝑝1 𝑒1 − 𝑘𝑑1 𝑒̇ 1 − 𝑘𝑖1 ∫0 𝑒1 𝑑𝑡
To constrain the end-effector position onto the desired path, the = 𝑡 . (29)
𝑢2 𝑒2 (𝑠𝑒 ) − 𝑘𝑝2 𝑒2 − 𝑘𝑑2 𝑒̇ 2 − 𝑘𝑖2 ∫0 𝑒2 𝑑𝑡
velocity vector must travel in the direction of the tangent of the
converging path. Therefore, the objective of the control action must where the 𝑘𝑝𝑗 , 𝑘𝑑𝑗 , 𝑘𝑖𝑗 , 𝑗 = [1, 2] are the corresponding controller gains.
be to align the velocity vector of the end-effector orientation with that
of the tangent of the converging path. 6. Simultaneous motion and force control

5.2. Kinematic model
To determine the end-effector velocity control input, we consider
the following kinematic model for the end-effector motion to impose a
constant velocity 𝑣𝑑 along the path:
𝑥̇ 𝑏 = 𝑣𝑑 sin 𝜓 cos 𝜃, 𝑦̇ 𝑏 = 𝑣𝑑 sin 𝜓 sin 𝜃,
𝑧̇ 𝑏 = 𝑣𝑑 cos 𝜓, 𝜃̇ = 𝑢1 , 𝜓̇ = 𝑢2 .
where 𝑥̇ 𝑏 , 𝑦̇ 𝑏 , 𝑧̇ 𝑏 are the components of the end-effector velocity ex-
pressed in the base frame, 𝑢1 and 𝑢2 are the pseudo-control inputs
for this kinematic model. The proposed kinematic model describes a
space velocity vector of constant speed 𝑣𝑑 expressed in the base frame:
when the translational speed of the end-effector is constrained to be
a constant, a set of spherical coordinates can fully describe a given 3D
Cartesian velocity vector. The parameters 𝜃 and 𝜓 continuously modify
the orientation of the robot end-effector frame. Denote the component
of the current robot end-effector velocity vector 𝑣⃗𝑑 in the base frame
as 𝑣⃗𝑑 = [𝑣𝑑𝑥 , 𝑣𝑑𝑦 , 𝑣𝑑𝑧 ]𝑇 . The orientation 𝜃 and 𝜓 can be calculated
according to:
√ effector velocity as described in (24) and we have chosen the following.
𝜃 = atan2(𝑣𝑑𝑦 , 𝑣𝑑𝑥 ), 𝜓 = atan2( 𝑣2𝑑𝑥 + 𝑣2𝑑𝑦 , 𝑣𝑑𝑧 ). (25)
The variables associated with this 3D kinematic model for the end-
effector frame are defined through the base and end-effector frames as
shown in Fig. 6. Without loss of generality, we assumed the direction of
the velocity vector 𝑣⃗𝑑 to be aligned with the 𝑌 -axis of the end-effector
frame. As shown in Fig. 6, the objective of the control laws for 𝑢1 and
𝑢2 is to align the velocity orientation, shown by the red arrow, with
that of the converging path tangent, shown by the green arrow.
5.3. Robot end-effector velocity control strategy
The objective of the motion control in the end-effector frame is
to orient the velocity vector based on the path tangent provided by
the converging path planner. To transform the local tangent ⃗𝑡 on the
converging path into the orientation variables defined in (24), we have:
𝜃𝑑 = atan2(𝑡𝑦 , 𝑡𝑥 ), 𝜓𝑑 = atan2(𝑑𝑥𝑦 , 𝑡𝑧 ).
Since the motion and contact force can only be regulated in mutu-
ally independent directions, force is controlled along the surface normal
direction and motion is controlled along the other constrained task
directions according to the workpiece geometry. Instead of using the
fixed base frame to decompose motion and force control directions, we
utilize the intuitive end-effector frame 𝑍-axis as the direction normal to
the surface which is utilized for force control. For the motion control,
(24)
since the kinematic model in (24) describes a velocity in the base frame,
the rotation axis-angle formula is employed to transform it to the end-
effector frame. To find the desired rotation velocity, the desired base
frame rotation velocity 𝜔 𝑏 can be calculated by:
𝜔 𝑏 = [−𝑢2 sin 𝜃, 𝑢2 cos 𝜃, 𝑢1 ]𝑇 . (30)
The unit rotation axis 𝑤̂ ∈ R3 and the rate of rotation about that axis
𝑤𝜃 ∈ R are given by
𝜔𝑏 ‖,
𝑤̂ = 𝜔 𝑏 ∕‖𝜔 𝑤𝜃 = ‖𝜔
𝜔𝑏 ‖. (31)
There are several ways to change the orientation of the robot end-
We fix the velocity orientation in the end-effector frame, and without
loss of generality, we can chose the end-effector 𝑌 -axis as the motion
controlled direction. To change the orientation of this velocity vector,
we change the orientation of the robot end-effector frame. Thus, the
joint velocity control command to be sent to the robot controller can
be calculated from:
𝐪̇ 𝑑 = 𝐉−1 𝜔 𝑇
𝑡 [𝜔 𝑒𝑒 , [0, 𝑣𝑑 , 𝑣𝑓 ] ]. (32)
where 𝐪̇ 𝑑 is the desired joint velocity vector, 𝐉𝑡 is the Jacobian matrix
at the robot end-effector or tool tip (see [30]), 𝜔 𝑒𝑒 ∈ R3 is the desired
rotation velocity expressed in the robot end-effector frame, i.e., 𝜔 𝑒𝑒 =
𝐑𝑒𝑒𝑤̂ 𝑤𝜃 , where 𝐑𝑒𝑒 ∈ 𝑆𝑂(3) is the rotation matrix from the base frame
to the end-effector frame, and 𝑣𝑓 is the impedance term that is utilized
for force control in the surface normal direction. Denote 𝐟𝑑 as the
desired force and 𝐟𝑐 as the measured force. A generalized impedance
model is employed to convert the contact force error 𝐟𝑒 = 𝐟𝑐 − 𝐟𝑑 to its
(26) corresponding position error which is then regulated via end-effector

7
Y. Wen and P.R. Pagilla Mechatronics 76 (2021) 102540

velocity. The proposed impedance model imposes a dynamics in the

force controlled direction of the robot end-effector frame according to:

𝐯𝑓 = −𝐤𝑓 𝑝 𝐟𝑒 − 𝐤𝑓 𝑑 𝐟𝑖 , 𝐟̇ 𝑖 = 𝐟𝑒 . (33)

where 𝐯𝑓 is the end-effector velocity in the force controlled direction

and the parameters 𝐤𝑓 𝑝 and 𝐤𝑓 𝑑 are the controller gains.
The joint space control law in Eq. (32) is designed specifically for a
surface finishing operation. For a general task one can choose:

𝐪̇ 𝑑 = 𝐉−1 𝛴
𝑡 (𝛴 𝑚 𝐕𝑚 + (𝐈 − 𝛴 𝑚 )𝐕𝑓 ). (34)

where 𝐕𝑚 , 𝐕𝑓 ∈ R6 are the desired task space velocity for motion

and generalized force, respectively, I is the identity matrix and 𝛴 𝑚
is the selection matrix as defined in [31]. In our application, 𝛴 𝑚 is
a six-by-six diagonal matrix whose diagonal terms are (1, 1, 1, 1, 1, 0),
𝐕𝑚 = [𝜔𝜔𝑒𝑒 , [0, 𝑣𝑑 , 0]𝑇 ] and 𝐕𝑓 = [0, 0, 0, 0, 0, 𝑣𝑓 ]𝑇 . The above joint space
control law assumes an industrial manipulator with six DOF and motion
in 3D.

7. Experiments

In this section, we present and discuss results from path following

and surface finishing experiments to evaluate the proposed strategy.
The proposed nonlinear optimization program is solved by using an
established C++ interior point solver (IPOPT) [32] and HSL-MA27 as
internal linear solver [33]. The robot dynamic model is interpolated
with the available Unified Robot Description Format (URDF) files and
the kinematics and dynamics are solved via Rigid-Body Dynamics
Library (RBDL) [34]. The sparsity of the formulated NLP problem
from direct collocation and the equality constraints from the continu-
ity conditions are utilized to obtain the derivative functions. In our
implementation, the matrix sparsity structure is exploited for the com-
putation of the Jacobian and Hessian matrices as detailed in [35] and a
forward finite difference approximation with the step size determined
as given in [36].
We choose 𝑛𝑝 = 10 for all 𝑝 ∈ [0, 𝑁]; larger values of 𝑛𝑝 provide a
more refined solution and possibly smaller time for executing the path,
however, this will increase the computation time. The initial value for
𝛥𝑡𝑝 is chosen as 0.01, and the time step is constrained to a range given
by 𝛥𝑡𝑝 ∈ [1𝑒−4 , 0.2]. Based on this initial setup of time grids and the
list of desired joint position, a cubic spline is obtained from which the
joint position for a given time grid 𝑡𝑝𝑘 is interpolated. By taking the
derivatives of this joint space cubic spline, the initial values for joint
Fig. 7. Joint Space Trajectory: Simulations and Experiments to Verify the Optimization
velocity and acceleration at a given time grid are calculated. Based on Procedure.
the interpolated joint states, the initial value for the joint torque inputs
are calculated from the robot inverse dynamics.
7.2. Surface finishing operation results
7.1. Joint space trajectory verification results
To evaluate the proposed framework, we present results of a surface
finishing experiment with a UR10 robot with the setup as shown in
We have conducted model simulations and experiments to evaluate
Fig. 8; the object to be surface finished with the finishing tool has a
the joint space trajectory that is obtained from the proposed trajectory
curved surface as shown in Fig. 11(a); the geometry of this surface
optimization method. We consider seven joint space waypoints for each mimics an aircraft blade cross section. We consider 44 points on the sur-
joint. The waypoints, grid points, and interpolated joint space trajectory face; shown by black circles in Fig. 9(a). For these waypoints, we have
obtained from TO are provided in Fig. 7(a); note that the optimal joint the robot joint space waypoints which form the input to the trajectory
space trajectory that is obtained from the TO includes all the given way- optimization problem. To account for modeling uncertainty and path
points. To verify whether the TO has appropriately considered all the disturbances, 70% of the total available torque capacity of the robot is
constraints, we have conducted real-time experiments on a UR5 robot in utilized during the optimization to generate the joint space trajectory.
ROS by sending joint position commands that are obtained from the TO The calculated maximum translation speed is 0.15 m∕s. We extracted
generated interpolated joint space trajectory. The experimental results 880 coordinates for each joint from the generated joint space trajec-
are provided in Fig. 7(b). It can be seen that the planned joint space tory. After performing forward kinematics, the continuous Cartesian
motion by the TO is experimentally feasible with the total motion time path can be interpolated from the corresponding Cartesian coordinates
is about 3 seconds, and closely follows the interpolated trajectory from (following the procedure described in Section 3 and Section 4). The
the TO. interpolated Cartesian path is given by the solid red curve in Fig. 9.

8
Y. Wen and P.R. Pagilla Mechatronics 76 (2021) 102540

Fig. 8. Robot Setup for Surface Finishing Experiments.

As highlighted by the enlarged green area in plot (b), at the location

where the robot is about to turn or change direction, the Cartesian
path connecting two waypoints is smooth and avoids sharp turns.
Compared to the path segment generated from linear interpolation of
two consecutive waypoints in Cartesian space, the proposed framework
provides a trajectory which connects the two waypoints in a dynamic
and smooth manner; further, the generated Cartesian path preserves
the characteristics of the original joint space motion sequence but also
satisfies physical consistency that is not included in the original data. Fig. 9. Cartesian Path from Trajectory Optimization.
The objective of surface finishing experiment was to move the
robot with constant speed along the Cartesian path while maintaining
prescribed normal force to finish the surface. The 3D path following planner that governs the transversal control behavior to avoid non-
result for one of the experiments is shown in Fig. 10(a). Fig. 10(b) zero velocity of impact in the surface normal direction and the path
shows the evolution of the regulated normal force signal which was following control method achieves constant speed along the tangent
filtered with a second-order low pass filter (70 Hz cutoff frequency); direction. To maintain a safe contact and regulate normal contact force,
the noise in the force signal is primarily due to tool rotation. The we have introduced the task space impedance control and synthesized
experimental result shows that the tool follows the prescribed path and the motion and force control in joint space as commands to send to the
the average normal force is regulated around the reference value of 10 robot manipulator. The feasibility and effectiveness of each component
N. Fig. 11 provides the picture of the workpiece surface before and after of the proposed framework have been verified via simulations and
finishing. extensive experiments, and we have demonstrated that we can obtain
consistent surface finish quality with the automated robotic surface
8. Conclusions finishing system with the proposed framework.

In this paper, we have proposed a novel control framework for
robots to perform surface finishing operations when there is only
discrete data from scanners/sensors available to characterize the path
on the surface. To determine the maximum achievable constant speed
and generate a path that is consistent with robot dynamics and joint
constraints, we have posed the problem as a nonlinear optimal control
program and solved it with an efficient numerical method based on
direct collocation. By interpolating the discrete Cartesian waypoints
extracted from the joint position trajectory, we have formulated an
arc-length based Cartesian path consistent with the desired constant
speed of travel along the path. To perform robotic surface finishing
operations, we have introduced a simultaneous motion and force con-
trol method. For the motion control, we present a converging path
Declaration of competing interest
No author associated with this paper has disclosed any potential or
pertinent conflicts which may be perceived to have impending conflict
with this work. For full disclosure statements refer to https://doi.org/
10.1016/j.mechatronics.2021.102540.
Acknowledgments
The authors would like to thank Jie hu and Daniel Jaeger for their
help in setting up the robots and force torque sensor under ROS. The
first author would like to thank Yinai Fan from UIUC for his generous
help and suggestions on the development of the trajectory optimization
section.

9
Y. Wen and P.R. Pagilla
Fig. 10. Surface Finishing Experimental Result.
Fig. 11. Workpiece Before and After Surface Finishing.
References
[1] Flacco Fabrizio, De Luca Alessandro, Khatib Oussama. Motion control of redun-
dant robots under joint constraints: Saturation in the null space. In: 2012 IEEE
international conference on robotics and automation. IEEE; 2012, p. 285–92.
[2] Shin Kang, McKay Neil. A dynamic programming approach to trajectory planning
of robotic manipulators. IEEE Trans Automat Control 1986;31(6):491–500.
[3] Debrouwere Frederik, Van Loock Wannes, Pipeleers Goele, Dinh Quoc Tran,
Diehl Moritz, De Schutter Joris, et al. Time-optimal path following for robots
with convex–concave constraints using sequential convex programming. IEEE
Trans Robot 2013;29(6):1485–95.
[4] Pfeiffer Friedrich, Johanni Rainer. A concept for manipulator trajectory planning.
IEEE J Robot Autom 1987;3(2):115–23.
[5] Shin Kang, McKay Neil. Minimum-time control of robotic manipulators with
geometric path constraints. IEEE Trans Automat Control 1985;30(6):531–41.
[6] Chettibi Taha. Synthesis of dynamic motions for robotic manipulators with
geometric path constraints. Mechatronics 2006;16(9):547–63.
[7] Bock Hans Georg, Plitt Karl-Josef. A multiple shooting algorithm for direct
solution of optimal control problems. IFAC Proc Vol 1984;17(2):1603–8.
[8] Kelly Matthew. An introduction to trajectory optimization: How to do your own
direct collocation. SIAM Rev 2017;59(4):849–904.
10
Mechatronics 76 (2021) 102540
[9] Sager Sebastian. Numerical methods for mixed-integer optimal control problems.
Der Andere Verlag Tönning; 2005.
[10] Kolter J Zico, Ng Andrew Y. Task-space trajectories via cubic spline optimization.
In: 2009 IEEE international conference on robotics and automation. IEEE; 2009,
p. 1675–82.
[11] Siu Anthony. Real time trajectory generation and interpolation [Ph.D. thesis],
University of British Columbia; 2011.
[12] Duan Molong, Yoon Deokkyun, Okwudire Chinedum E. A limited-preview filtered
B-spline approach to tracking control–with application to vibration-induced error
compensation of a 3D printer. Mechatronics 2018;56:287–96.
[13] Cohen Elaine, Riesenfeld Richard F, Elber Gershon. Geometric modeling with
splines: an introduction. CRC Press; 2001.
[14] Aguiar A Pedro, Hespanha Joao P, Kokotovic Petar V. Path-following for non-
minimum phase systems removes performance limitations. IEEE Trans Automat
Control 2005;50(2):234–9.
[15] Olofsson Björn, Nielsen Lars. Path-tracking velocity control for robot
manipulators with actuator constraints. Mechatronics 2017;45:82–99.
[16] Xiang Xianbo, Lapierre Lionel, Liu Chao, Jouvencel Bruno. Path tracking:
Combined path following and trajectory tracking for autonomous underwater
vehicles. In: 2011 IEEE/RSJ international conference on intelligent robots and
systems. IEEE; 2011, p. 3558–63.
[17] Lapierre Lionel, Soetanto D, Pascoal Antonio. Nonsingular path following control
of a unicycle in the presence of parametric modelling uncertainties. Int J Robust
Nonlinear Control IFAC-Aff J 2006;16(10):485–503.
[18] Lapierre Lionel, Jouvencel Bruno. Robust nonlinear path-following control of an
AUV. IEEE J Ocean Eng 2008;33(2):89–102.
[19] Gill Rajan J, Kulić Dana, Nielsen Christopher. Spline path following for redundant
mechanical systems. IEEE Trans Robot 2015;31(6):1378–92.
[20] Kanjanawanishkul Kiattisin, Hofmeister Marius, Zell Andreas. Path follow-
ing with an optimal forward velocity for a mobile robot. IFAC Proc Vol
2010;43(16):19–24.
[21] Shiller Z. Time-energy optimal control of articulated systems with geometric path
constraints. In: Proceedings of the 1994 IEEE international conference on robotics
and automation. IEEE; 1994, p. 2680–5.
[22] Verscheure Diederik, Demeulenaere Bram, Swevers Jan, De Schutter Joris,
Diehl Moritz. Time-optimal path tracking for robots: A convex optimization
approach. IEEE Trans Automat Control 2009;54(10):2318–27.
[23] Verscheure Diederik, Diehl Moritz, De Schutter Joris, Swevers Jan. Recursive log-
barrier method for on-line time-optimal robot path tracking. In: 2009 American
control conference. IEEE; 2009, p. 4134–40.
[24] Guo Hongyan, Liu Jun, Cao Dongpu, Chen Hong, Yu Ru, Lv Chen. Dual-envelop-
oriented moving horizon path tracking control for fully automated vehicles.
Mechatronics 2018;50:422–33.
[25] Wen Yalun, Pagilla Prabhakar. A novel path following scheme for robot
end-effectors. In: 2020 American control conference (ACC). IEEE; 2020, p.
2452–7.
[26] Pagilla Prabhakar R, Yu Biao. A stable transition controller for constrained
robots. IEEE/ASME Trans Mechatron 2001;6(1):65–74.
[27] Pagilla Prabhakar R, Yu Biao. Robotic surface finishing processes: modeling,
control, and experiments. J Dyn Syst Meas Control 2001;123(1):93–102.
[28] Wen Yalun, Pagilla Prabhakar R. A 3D path following control scheme for robot
manipulators. IFAC Proc Vol 2020.
[29] Betts John T. Survey of numerical methods for trajectory optimization. J Guid
Control Dyn 1998;21(2):193–207.
[30] Lynch Kevin M, Park Frank C. Modern robotics. Cambridge University Press;
2017.
[31] Khatib Oussama. A unified approach for motion and force control of
robot manipulators: The operational space formulation. IEEE J Robot Autom
1987;3(1):43–53.
[32] Wächter Andreas, Biegler Lorenz T. On the implementation of an interior-point
filter line-search algorithm for large-scale nonlinear programming. Math Program
2006;106(1):25–57.
[33] HSL. Collection of fortran codes for large-scale scientific computation. 2007, See
http://www.hsl.rl.ac.uk.
[34] Felis Martin L. RBDL: an efficient rigid-body dynamics library using recursive
algorithms. Auton Robots 2017;41(2):495–511.
[35] Patterson Michael A, Rao Anil V. GPOPS-II: A MATLAB software for solving
multiple-phase optimal control problems using hp-adaptive Gaussian quadrature
collocation methods and sparse nonlinear programming. ACM Trans Math Softw
2014;41(1):1–37.
[36] Gill Philip E, Murray Walter, Wright Margaret H. Practical optimization. SIAM;
2019.
Y. Wen and P.R. Pagilla
Yalun Wen is currently pursuing the Ph.D. degree in
mechanical engineering in J. Mike Walker’66 Department
of Mechanical Engineering, Texas A&M University, College
Station, TX, USA. His current research interests include
robotics, manipulator trajectory optimization, motion and
force control, and mechatronics.
11
Mechatronics 76 (2021) 102540
Prabhakar R. Pagilla received the B.Eng. degree in me-
chanical engineering from Osmania University, Hyderabad,
India, in 1990, and the M.S. and Ph.D. degrees in me-
chanical engineering from the University of California at
Berkeley, Berkeley, CA, USA, in 1994 and 1996, respec-
tively. He is currently the James J. Cain’51 Professor II
and Associate Department Head in J. Mike Walker’66 De-
partment of Mechanical Engineering, Texas A&M University,
College Station, TX, USA. He is currently interested in the
broad area of modeling and control of dynamic systems
with applications in roll-to-roll manufacturing, large-scale
systems, robotics, and mechatronics. He was a recipient of
the National Science Foundation CAREER Award in 2000.
He was an Associate Editor of the ASME Journal of Dynamic
Systems, Measurement and Control and a Technical Editor
of the IEEE/ASME Transactions on Mechatronics. He is a
fellow of the American Society of Mechanical Engineers.