Accepted Version: 2022 IEEE International Conference on Intelligent Robots and Systems (IROS),

Oct.23-27, 2022, Kyoto, Japan. ©2012 IEEE. Personal use of this material is permitted.
Permission from IEEE must be obtained for all other uses.

Constraint-based Task Specification and Trajectory Optimization for

Sequential Manipulation
Mun Seng Phoon, Philipp S. Schmitt1 , Georg v. Wichert1

Abstract— To economically deploy robotic manipulators the

programming and execution of robot motions must be swift.
arXiv:2208.09219v1 [cs.RO] 19 Aug 2022

To this end, we propose a novel, constraint-based method to

intuitively specify sequential manipulation tasks and to compute
time-optimal robot motions for such a task specification. Our
approach follows the ideas of constraint-based task specification
by aiming for a minimal and object-centric task description that
is largely independent of the underlying robot kinematics. We
transform this task description into a non-linear optimization
problem. By solving this problem we obtain a (locally) time-
optimal robot motion, not just for a single motion, but for an
entire manipulation sequence. We demonstrate the capabilities
of our approach in a series of experiments involving five distinct
robot models, including a highly redundant mobile manipulator.

I. INTRODUCTION Fig. 1. Sequential manipulation task: A robot arm mounted on a mobile

base performs a wall drilling task. The robot must move a tool along a
This paper addresses the problem of specifying and com- series of Cartesian straight line segments. In order to obtain smooth and
time-optimal motions, the robot should use redundant degrees of freedom,
puting motions for robotic manipulators. The focus of our e. g., re-position the mobile base while the arm is working with the tool.
work lies on a high-level, robot-independent specification
that should result in a time-optimal motion of the manipula- sequence of Cartesian paths and their geometric shape as
tor. Intuitive programming of complex yet fast robot motions well as the requirement that the mobile base should not
would allow for a wider range of industrial applications collide with the wall. Furthermore, the specification must
where robots assist and complement human workers. be compatible with the optimization of sequential motions
To illustrate the key challenges we address in this work, described in the previous paragraph.
Fig. 1 shows an exemplary manipulation task. There, a One promising approach to a minimal yet flexible task
mobile manipulator with an arm mounted on a mobile base specification can be found in the work on constraint-based
moves a tool along with a series of Cartesian straight-line task specification and control [1]. Here, tasks are described as
paths. sets of constraints that can be flexibly composed and from
Time-Optimality: In order to be productive, the manip- which motion controllers can be derived automatically. A
ulator should execute its task as quickly as possible. To systematic treatment of the interdependencies in sequential
do so it must use the degrees of freedom left by the task manipulation can be found in the literature on manipu-
at hand. In our exemplary task, the motion between the lation planning [2]. Recent work developed methods to
Cartesian segments is not specified and no stops are required combine constraint-based models of tasks with manipulation
in between them. Thus, the robot can use such degrees of planning [3] or methods to perform optimal manipulation
freedom in the task to move the tool in and out of Cartesian planning [4].
segments with a positive velocity similar to conventional The contribution of this paper is an integrated approach
blending that can be specified on industrial manipulators. to specify sequential manipulation tasks in a constraint-
Also, the robot is not required to keep the mobile base at based fashion and compute time-optimal motions for this
rest while working with the tool. This freedom can be used specification. The key idea is a formulation of the task
to continuously move the mobile base in the direction of constraints that allows for a numerical optimization for the
work so that the next target for the tool is already in range robot motion across an entire manipulation sequence.
when the previous one has been processed. We evaluate our approach on a series of distinct tasks and
What we aim to illustrate here is: In order to obtain time- robot models. These experiments show that our approach
optimal motions, the robot must reason about a complete produces substantially faster robot motions than a baseline
sequence of interdependent motions simultaneously. that does not optimize across the sequence of motions.
Task Specification: Ideally, the specification of a motion
II. RELATED WORK
task should be minimal in that it only includes the quantities
relevant to the user. In the example, this would include the In this paper, we study the specification of robot motions
for manipulation tasks with dependencies between multiple,
1 Siemens Technology, Munich, Germany sequential motions. In practice, manipulation requires a vast
variety of different robot motions and we thus need a formal- trajectory simultaneously, whereas LGP fixes transition con-
ism to capture this variety. Constraint-based programming as figurations between phases during a high-level optimization
presented by Ambler and Popplestone [5] defines actions or pass. In contrast to the remaining literature on manipulation
tasks through physical relations between objects and goals planning, our approach deliberately restricts the model to a
which are then converted into mathematical constraints. This linear sequence of motion phases, i. e., there is no decision to
approach forms the basis for constraint-based task specifi- be made between alternative (high-level) courses of action.
cation and control [1] used in the iTaSC framework [6].
Constraints between quantities of interest, e. g., Cartesian III. PROBLEM STATEMENT AND NOTATION
positions, are used to derive controllers automatically. This We denote the configuration of a robot at a given time t
approach is highly flexible and several extensions have as q(t) ∈ Rn . In the example of Fig. 1 this vector in-
been proposed, such as the usage of graphs of symbolic cludes three values for the mobile base and six for the
expressions in the eTaSL framework [7] or non-instantaneous robot arm (n = 9). The velocity of our system is denoted
optimization with a short time-horizon [8]. We follow the as q̇(t) ∈ Rn .
basic constraint-based approach to task specification and A complete state of a system includes positions and
the use of expression graphs as in the eTaSL framework. velocities and is denoted as x = [q > , q̇ > ]> ∈ R2n . This state
However, the approaches so far are not suitable to address evolves over time (t) according to the differential equation:
multiple, interdependent motions and our work extends both
ẋ(t) = f (x(t), u(t)),
the task specification and motion generation aspects relative
to the prior work. where u denotes the control input. Two natural candidates
The problem of planning sequences of interdependent for the control input u are joint torques or joint accelerations.
motions of robots and manipulated objects is addressed in For simplicity, we will assume joint accelerations as control
the literature on manipulation planning [2]. As robot motion input u = q̈ ∈ Rn for the remainder of this paper.
involves high-dimensional configuration spaces, sampling- At all times, the state variables and control inputs are
based approaches to manipulation planning have shown good subject to box constraints:
empirical performance [9]–[11] and have also been extended
to asymptotically optimal planners [4, 12]. xl ≤ x(t) ≤ xu and
The MoveIt Task Constructor (MTC) by Gorner et al. [13] −umax ≤ u(t) ≤ umax .
provides a high-level user interface to specify sequences
of motions and plan a continuous motion using sampling- In the example of Fig. 1, the individual joints of the arm
based planners. Similar to our approach, tasks are described have upper and lower limits for positions, velocities, and
as a sequence of phases that define requirements on the accelerations.
motion during the phase and at the beginning or end of This paper utilizes a multi-phase trajectory optimization
phases. However, as phases are solved by individual planning approach to solve the optimal control problem (OCP) for
runs, no time-optimality is achieved across the entire motion sequential motion planning. This approach breaks the OCP
sequence (e.g., stops are inserted between phases). This is into m motion phases, as shown in Fig. 2 and optimizes the
addressed in our approach by treating the sequential planning entire sequence of motions instead of each individual motion
task as a single numerical optimization problem. separately.
A major limitation of sampling-based planners is the re- In sequential motion planning, we consider a predeter-
liance on domain-dependent sampling mechanisms and local mined sequence of motions that succeed each other. Each
planners. This makes modeling different planning problems motion phase has terminal constraints and optionally path
difficult, both for modeling the task and the robot model constraints depending on the task at hand. Terminal con-
(e. g., closed-kinematic chains). A promising solution to this straints specify the goal at the end of a motion phase. E. g.,
problem is to incorporate symbolic constraints in the motion in order to stop the end-effector at a target position, its
planning process [14]. An overview of different methods for pose would be constrained to the target and its velocity
constrained and sampling-based motion planning is provided be constrained to zero within the terminal constraint. To
by Kingston et al. [15] define an intermediate point of a motion, a terminal position
Two approaches where numerical constraints are used constraint can be specified without limiting the velocity. We
for modeling manipulation planning can be found in the usually specify the end-effector pose in Cartesian space or a
constraint graph presented by Mirabel and Lamiraux [16] and robot configuration in joint-space as terminal constraints.
Logic Geometric Programming (LGP) by Toussaint [17]. The Path constraints must hold at all times within a motion
constraint graph models numerical constraints on motions as phase. Examples include moving in a straight-line motion,
constraints of state, whereas LGP models them as constraints keeping the end-effector in the vertical orientation, or keep-
of high-level actions. ing the mobile base in a predefined area to avoid collisions.
Our work shares the focus on optimization criteria as part Some path constraints can be part of multiple motion phases
of the task specification with LGP but specifies constraints as (e. g., collision avoidance).
a result of states similar to the model of the constraint graph. Terminal and path constraints may be either equality
It differs from LGP in that we optimize the entire motion or inequality constraints. Examples for equality constraints
Fig. 2. Motion phases i = 1, . . . , m with terminal and path constraints in a multi-phase trajectory optimization problem. Ti is the accumulative trajectory
time and Tphase i is the time of each motion phase.

are typically task-related, such as a desired end-effector end of each phase, the respective terminal constraints must
pose. Examples of inequality constraints are typically robot- be fulfilled.
related, such as collision avoidance. In our notation we
IV. TRANSCRIPTION TO A DISCRETE-TIME
only refer to inequality terminal constraints of phase i as
PROBLEM
g terminal,i (x) ≤ 0 and path constraints g path,i (x) ≤ 0 as
equalities can be modeled using two inequalities. To solve our multi-phase optimal control problem (1) we
An informal problem formulation would now be to move discretize the time in each motion phase into N samples
the robot, starting in initial state xinit at time T0 , through using the multiple shooting method [18] which results in a
the sequence of motion phases as quickly as possible with- total of mN time steps. To simplify the mapping of time
out violating the constraints of the current motion phase. steps and motion phases let p(k) = bk/N c + 1 denote the
Formally, we model this as the following optimal control phase of discretization step k. This results in the following
problem (OCP): optimization problem with i = 1, . . . , m:

mN −1
Z Tm
X Tp(k) − Tp(k)−1
minimize Tm + l(xk , uk )
minimize Tm + l(x(t), u(t)) dt x ,x ,...,x
0 1 mN , N
x(t),u(t), u0 ,u1 ,...,umN −1 , k=0
T0
T1 ,T2 ,...,Tm T1 ,T2 ,...,Tm

subject to x(T0 ) = xinit , subject to x0 = xinit ,

ẋ(t) = f (x(t), u(t)), for k ∈ {0 . . . mN − 1},
for i ∈ {1 . . . m}, (1) Tp(k) − Tp(k)−1
xk+1 = f d (xk , uk , ),
Ti−1 ≤ Ti , N
for i ∈ {1 . . . m},
xl ≤ x(t) ≤ xu ,
Ti−1 + ∆T,min ≤ Ti ,
− umax ≤ u(t) ≤ umax ,
xl ≤ xk ≤ xu ,
g path,i (x(t)) ≤ 0 for t ∈ [Ti−1 , Ti ],
− umax ≤ uk ≤ umax ,
g terminal,i (x(Ti )) ≤ 0
g path,p(k) (xk ) ≤ 0,
The optimization variables are the trajectories of g terminal,i (xiN −1 ) ≤ 0
states x(t) and controls u(t) as well as the end times Ti (2)
of the m motion phases. The final trajectory time Tm is where f d (·) is the approximation of the dynamics function
the main component of the cost function as we aim for using a fixed-step Runge-Kutta (RK4) numerical integrator.
time-optimality. This alone may not determine a unique This optimization problem is the straight-forward discretiza-
solution, so we add a regularization term l(·) that penalizes tion of (1) with the minor addition of a minimum phase
component-wise squared velocities and accelerations. duration ∆T,min . This minimum duration is added to prevent
The first three constraints encode the physical plausibility numerical instabilities that may occur due to products that
T −T
of the motion: it starts at the initial state xinit , follows the involve the step duration p(k) N p(k)−1 .
system dynamics, and the switches between motion phases
V. IMPLEMENTATION AND EXPERIMENTS
have a chronological order.
The last four constraints are task requirements: At no time A. Implementation Details
will the system violate its state and control bounds or the Our implementation operates in four stages: two for mod-
path constraints of the currently active motion phase. At the eling and two for numerical solving.
 (a) Pick-and-place (b) Pick-and-place with mobile manipulator (c) Wall drilling with mobile manipulator
Fig. 3. Robot models and tasks for the experiments: The green spheres indicate via-points (and thus terminal constraints) that the end-effector must pass
through. The green line shows (parts of) the solution computed by our approach. Note, that the robot is required to return to its initial configuration (the
orange sphere).
TABLE I
T RAJECTORY DURATION AND COMPUTATION TIME FOR DIFFERENT SEQUENTIAL MOTION TASKS

Sequential motion task Trajectory duration Computation time

(Number of discretization steps per phase N = 20) Baseline Our approach Baseline Our approach
Pick-and-place [7 motion phases] 11.34 s 9.44 s 6.63 s 31.22 s
Pick-and-place with mobile manipulator [7 motion phases] 7.34 s 7.48 s 2.14 s 20.94 s
Wall drilling with mobile manipulator [15 motion phases] 9.57 s 7.57 s 4.35 s 93.04 s

In the first stage, a robot is modeled as a graph of sym- The first one would be to compare this approach to the
bolic mathematical expressions as pioneered by the eTaSL interpolators of typical industrial robots when manual blend-
framework [7]. To this end, a robot modeled within the ing between motions is specified by the user. However,
Universal Robot Description Format (URDF) is parsed into a this comes with a large number of arbitrary choices for
tree of symbolic expressions representing the forward kine- parameters such as blending distances or velocity profiles
matics formula. These expressions are implemented using for Cartesian motions and the baseline would be limited
the CasADi framework [19]. to typical robot arms (i. e., no mobile manipulators). The
In the second stage, constraints are specified for different second candidate would be to model a motion task as a Logic
phases of a motion task. To this end, we model a set of re- Geometric Program that has a purely linear sequence of
curring motion constraints as described in Section III. These symbolic states and solve it via the LGP solver as presented
constraints link different symbolic expressions within the by Toussaint [17]. We approximate this second baseline, by
previously constructed expression graph, e. g., to constrain sequentially solving the individual motion phases one-by-one
the forward kinematics of the end-effector for a positioning without considering the remaining phases.
task. We defined three tasks for experimental evaluation, as
In stage three, the expression graph and the constraints shown in Fig. 3. These tasks vary in complexity both with
are transcribed into constraints and cost-functions for a time- respect to the robot model and the number of phases. Each
discrete, non-linear optimization problem as described in the task contains at least several Cartesian motions and all tasks
previous section. Phase four computes a solution to this require the robot to return to its initial configuration.
problem, and thus a motion trajectory, using the IPOPT
solver [20]. C. Results
IPOPT requires an initial guess which, ideally, should We perform the experiments on different simulated robot
already fulfill all constraints of the OCP. For this initial guess, systems and evaluate them based on a series of perfor-
we set all positions of the trajectory to the initial position mance metrics. All experiments were performed on an In-
and all velocities and accelerations to zero. This choice of tel i7 10510U 2.8 GHz processor.
initial guess fulfills the constraints of the initial state, system Fig. 4 shows that our approach has a faster motion
dynamics, as well as state and control bounds but will violate with around 15% improvement than the baseline on the
the task constraints. trajectory duration of the pick-and-place task. The base-
line approach returns a trajectory where the motion stops
B. Experimental Setup between the phases. In our approach, no post-processing
We conducted a set of experiments to validate whether our of the generated trajectory, i.e., blending or smoothness
approach allows us to model typical robot motion profiles algorithms, is required as these are already integrated as
and whether it improves the execution speed compared to a objectives in the OCP. The results avoid non-smoothness
baseline. in typical sampling-based path planning algorithms and
There are two natural candidates for such a baseline: the sub-optimality caused by path-velocity decomposition
 Baseline Number of discretization steps per phase N = 5
1.0 1.0

0.5 0.5

0.0 0.0
Norm. Joint Velocity

Norm. Joint Velocity

−0.5 −0.5

−1.0 −1.0
Our Approach Number of discretization steps per phase N = 30
1.0 1.0

0.5 0.5

0.0 0.0

−0.5 −0.5

−1.0 −1.0
0 2 4 6 8 10 0 2 4 6 8 10 12
Trajectory Duration (s) Trajectory Duration (s)

Joint 1 Joint 2 Joint 3 Joint 4 Joint 5 Joint 6 Joint 1 Joint 2 Joint 3 Joint 4 Joint 5 Joint 6

Fig. 4. Normalized joint velocities: This graph shows the joint velocities Fig. 5. Effect of the number of discretization steps: This graph depicts the
of the baseline and our approach for a 6-axis robot pick-and-place task. All joint velocities for a 6DoF robot pick-and-place task with a small (5) and
values are normalized to the maximal velocity of the respective joint. The large (30) number of discretization steps N . As can be seen, a larger N
task comprises seven motion phases for which the boundaries are marked by results both in a substantially shorter trajectory and smother velocity profiles.
vertical dashed lines. As can be seen, our approach shows a substantially
reduced execution time. One reason for this is that our approach enters
intermediate phase-boundaries with positive velocity whereas the baseline
100
stops between all phases.
11.5

approaches. 80
The baseline approach exerts bang-bang controls, causing 11.0
Trajectory duration (s)

Optimization time (s)

a motion profile with either maximal or zero acceleration
60
for the joint that is the current bottleneck. Our approach has 10.5
similar behaviour, but it spreads some of the joint motions,
which are not the bottlenecks of the optimizer, across a 40
10.0
longer horizon. This results in a motion that does not stop at
the transition between phases and thus a faster and smoother 20
overall motion. 9.5
Table I shows the duration of the computed trajectories as
5 10 15 20 25 30
well as the computation time of both the baseline and our Number of discretization steps per phase N
approach. Naturally, our approach shows substantially larger
computation times due to the simultaneous optimization of
all phases. In the first and third benchmark, our approach Fig. 6. Evaluation of different number of discretization steps per phase N
for a 6DoF robot pick-and-place task in terms of trajectory execution and
results in approx. 17% and 20% reduction in trajectory du- optimization time
ration. Interestingly, in the second benchmark our approach
produces a slightly longer trajectory than the baseline. The between lower cost and faster computation time.
reason for this is, that this benchmark is dominated by long We can reuse the task definitions for different robot setups
“cruise-phases” where one joint is the bottleneck at constant as the task specification and the robot model are separated.
maximal velocity. In this case there is little room for the Examples are shown in Fig. 7, where the same task is
optimizer to improve the solution with respect to trajectory executed on four different robot models, two with six and
duration and the regularization terms will lead to a slightly two with seven joints.
longer duration.
Finer discretization improves the trajectory execution time VI. CONCLUSION
but requires more computation time, as presented in Fig. 5 This paper introduced a novel method to specify sequential
and Fig. 6. This correlation is related to trajectory smooth- manipulation tasks and compute time-optimal motions from
ness. Smaller discretization steps of the motion phase affect such a specification. The key idea is to model manipulation
the performance and reliability of the numerical solver, as a as a sequence of numerical constraint functions that hold in
trajectory is an edge between two discretization steps in the different phases of the manipulation task. From this sequence
motion planning problem. We have to determine the trade-off of constraint functions a multi-phase trajectory optimization
 [4] P. S. Schmitt, W. Neubauer, W. Feiten, K. M. Wurm, G. v. Wichert,
and W. Burgard, “Optimal, sampling-based manipulation planning,” in
Int. Conf. on Robotics and Automation. IEEE, 2017.
[5] A. Ambler and R. Popplestone, “Inferring the positions of bodies from
specified spatial relationships,” Artificial Intelligence, vol. 6, no. 2,
1975.
[6] R. Smits, T. De Laet, K. Claes, H. Bruyninckx, and J. De Schutter,
“iTASC: a tool for multi-sensor integration in robot manipulation,” in
2008 IEEE Int. Conference on Multisensor Fusion and Integration for
Intelligent Systems, 2008.
[7] E. Aertbeliën and J. De Schutter, “eTaSL/eTC: A constraint-based task
specification language and robot controller using expression graphs,”
(a) UR5 (b) Fanuc M-10iA in 2014 IEEE/RSJ Int. Conference on Intelligent Robots and Systems,
2014.
[8] W. Decré, R. Smits, H. Bruyninckx, and J. De Schutter, “Extending
iTaSC to support inequality constraints and non-instantaneous task
specification,” in Int. Conf. on Robotics and Automation. IEEE, 2009.
[9] T. Siméon, J.-P. Laumond, J. Cortés, and A. Sahbani, “Manipulation
planning with probabilistic roadmaps,” The Int. Journal of Robotics
Research, vol. 23, no. 7-8, 2004.
[10] K. Hauser, “Task planning with continuous actions and nondeterminis-
tic motion planning queries,” in AAAI Workshop on Bridging the Gap
between Task and Motion Planning, 2010.
[11] K. Hauser and J.-C. Latombe, “Multi-modal motion planning in non-
expansive spaces,” The Int. Journal of Robotics Research, 2009.
(c) Franka Emika Panda (d) KUKA LBR iiwa [12] W. Vega-Brown and N. Roy, “Asymptotically optimal planning under
piecewise-analytic constraints,” The 12th Int. Workshop on the Algo-
Fig. 7. Time-optimal robot joint trajectories of the same pick-and-place rithmic Foundations of Robotics, 2016.
task for different robot models. Task specifications are independent of the [13] M. Gorner, R. Haschke, H. Ritter, and J. Zhang, “Moveit! task
robot models and kinematic structure, given that Franka Emika Panda and constructor for task-level motion planning,” Proceedings - IEEE Int.
KUKA LBR iiwa have one more joint relative to the UR5 and the Fanuc Conference on Robotics and Automation, vol. 2019-May, 2019.
M-10iA. Each robot must begin and end its motion in its respective home- [14] D. Berenson, S. S. Srinivasa, D. Ferguson, and J. J. Kuffner, “Manip-
position (marked with an orange dot) which results in substantially different ulation planning on constraint manifolds,” in Int. Conf. on Robotics
joint and end-effector trajectories. and Automation. IEEE, 2009.
[15] Z. Kingston, M. Moll, and L. E. Kavraki, “Sampling-based methods
for motion planning with constraints,” Annual Review of Control,
problem is derived to which the solution results in a smooth Robotics, and Autonomous Systems, vol. 1, 2018.
and fast robot motion. [16] J. Mirabel and F. Lamiraux, “Manipulation planning: addressing the
crossed foliation issue,” in Int. Conf. on Robotics and Automation.
Our approach comes with two main advantages. First, by IEEE, 2017.
using a constraint-based model we are able to specify tasks [17] M. Toussaint, “Logic-geometric programming: An optimization-based
in an intuitive and compact fashion. Furthermore, constraints approach to combined task and motion planning,” in IJCAI Int. Joint
Conference on Artificial Intelligence, vol. 2015-January, 2015.
can be composed flexibly in sequence and in parallel and [18] H. G. Bock and K.-J. Plitt, “A multiple shooting algorithm for direct
enable a task specification that is largely independent of solution of optimal control problems,” IFAC Proceedings Volumes,
the robot kinematics. Second, by jointly optimizing all se- vol. 17, no. 2, 1984.
[19] J. A. E. Andersson, J. Gillis, G. Horn, J. B. Rawlings, and M. Diehl,
quential phases the resulting motion uses redundant degrees “CasADi – A software framework for nonlinear optimization and
of freedom within the task to speed up execution. This optimal control,” Mathematical Programming Computation, vol. 11,
leads to the automatic discovery of useful behaviors without no. 1, 2019.
[20] A. Wächter and L. T. Biegler, “On the implementation of a primal-
explicit specification, e. g., blending at the beginning and end dual interior point filter line search algorithm for large-scale nonlinear
of Cartesian straight-line motions or parallel motions of a programming,” Mathematical Programming 2005 106:1, vol. 106,
mobile base and robot arm. no. 1, 2005.
Two current limitations of our approach are the depen-
dency on an initial guess and the limited user feedback
should the optimizer fail. These two also mark potential
areas for future work to develop methods for initialization
and introspection.
R EFERENCES
[1] J. D. Schutter, T. D. Laet, J. Rutgeerts, W. Decré, R. Smits, E. Aert-
beliën, K. Claes, and H. Bruyninckx, “Constraint-based task specifica-
tion and estimation for sensor-based robot systems in the presence of
geometric uncertainty,” The Int. Journal of Robotics Research, vol. 26,
no. 5, 2007.
[2] R. Alami, T. Simeon, and J.-P. Laumond, “A geometrical approach
to planning manipulation tasks. the case of discrete placements and
grasps,” in The fifth Int. Symposium on Robotics Research. MIT
Press, 1990.
[3] J. Mirabel and F. Lamiraux, “Manipulation planning: addressing the
crossed foliation issue,” in Int. Conf. on Robotics and Automation.
IEEE, 2017.