1 Introduction

Pick-and-place operations are widely encountered tasks assigned to robots in various industries. These operations involve the robot selecting an object from one location and moving it to another designated position. Within the realm of pick-and-place tasks, a fundamental challenge arises when striking a balance between cautious and deliberate movements, which ensure task completion but sacrifice speed, and fast and dynamic movements that increase the risk of failure in grasping the target object. For instance, when transporting a fragile object or an open-top item (e.g., a cup of coffee), if a robot moves its end-effector too fast, it can cause damage or spills. Nonetheless, if transporting an object at a low speed, it may not be able to achieve required productivity in the task. The dilemma between cautious and slow movements versus fast and dynamic movements poses a significant challenge in pick-and-place tasks. Hence, achieving an optimal trade-off between speed and safety is important to maximize the efficiency and reliability of these operations.

The primary objective of this research is to delve into the realm of time-optimized path planning for robot arms, specifically considering the impact of acceleration forces on the transported object during pick-and-place tasks. The overarching goal is to equip robots with the capability to efficiently complete these tasks within a minimum time while simultaneously ensuring safety in object transportation.

Since the inception of robotics research, achieving optimal trajectory duration has been a significant goal. The early work (Bobrow et al., 1985) aimed to minimize the time required to complete a trajectory by selecting accelerations that maximize velocity while adhering to robot constraints. Over the years, there has been significant attention given to time optimization, resulting in the development of techniques like time optimal path parameterization (TOPP) as outlined by Pham (2014); Pham and Pham (2017, 2018); Ma et al. (2021). In fact, TOPP seeks feasible trajectories that satisfy complex constraints related to joint accelerations and velocities while minimizing travel time.

In terms of the grasp optimization, Ichnowski et al. (2020, 2022); Avigal et al. (2023) proposed new approaches that leverage optimization techniques to enhance the efficiency and effectiveness of designing grasping trajectory in various robotic applications. For instance, Ichnowski et al. (2020) introduced the grasp-optimized motion planning (GOMP) approach as a means to optimize grasping for bin-picking robots. In their work, they formulate an optimization problem where the sequence of robot configurations serves as decision variables. The first configuration represents the initial motion state, while the final configuration represents the target destination. GOMP divides the optimization process into fixed time intervals (\(t_{\text {step}}\)) between a predetermined number of steps (H), then gradually narrowing down the number of steps needed to complete the configuration sequence while considering constraints such as joint limits, velocities, accelerations, and collision avoidance, allowing optimization of motion duration at the same time.

Building upon the foundation established by GOMP, the subsequent works have expanded on these concepts to address more complex tasks. For instance, the work (Ichnowski et al., 2022) discussed GOMP for fast inertial transport, which incorporates additional constraints to enable fast inertial transportation of objects. Likewise, Avigal et al. (2023) proposed GOMP for suction transport (GOMP-ST), which focuses on optimizing the grasping motion for suction-based transportation tasks. These works collectively demonstrate the significance of optimization techniques in the context of grasping motion planning for robot arms. By formulating the problem as an optimization task and considering various constraints, these frameworks achieve improved efficiency, stability, and reliability in grasping operations. Nevertheless, a limitation in the GOMP-ST method is that the constraints are proposed to be learned through a learning mechanism, which can be impractical if data is not available.

In this paper, we propose a new approach to efficiently plan trajectories for industrial robots, which concurrently reduces total trajectory time as well as guarantees safety of transporting an object. More specifically, in the total trajectory time optimization problem, in addition to the robot configuration constraints such as joint positions, velocities, and accelerations, we also constrain the acceleration acting on the robot end-effector. In other words, by limiting the robot end-effector acceleration to a safe threshold, an object is guaranteed to be transported safely without damage or spills. On the other hand, we also propose to solve the optimization problem in two steps. In the first step, the optimization problem is solved without the constraint on the robot end-effector acceleration. The obtained results are then used as initial guesses for solving the full optimization problem the second step, where the constraint on the robot end-effector acceleration is applied. The proposed two-step procedure empirically proves that computing time of solving the optimization problem is significantly reduced. It is noted that we propose to exploit the quadratic programming algorithm by Gill et al. (1989) to solve the simplified optimization problem in the first step while employing the sparse sequential quadratic programming method by Gill et al. (2005) to solve the full optimization problem in the second step. As compared with the previous works such as (Ichnowski et al., 2022), our method does not require a machine learning technique, thus eliminating the need for data and the training/learning process, which makes its implementation easier and quicker. Effectiveness of the proposed approach was verified in Drake, where the obtained results are highly promising.

The paper is arranged as follows. We first formulate the problem in Sect. 2 while the methodology is discussed in Sect. 3. The preliminary results are presented and analyzed in Sect. 4, and the conclusions are drawn in Sect. 5.

2 Problem Statement

For a pick-and-place task in industrial robots, our problem aims to limit the time required for an end-effector to complete a movement from a predetermined starting point to a predetermined goal while respecting the joint limits of the robot. However, the motion must be able to avoid any damage or spills on a transported object. That is, the acceleration acting on the end-effector is constrained to ensure the success of the transportation task in a timely manner. To this end, we express our problem in general as follows:

$$\begin{aligned} {\mathop {\hbox {min}}\limits _{\textbf{c}}}\quad&{T(\textbf{c})} \end{aligned}$$
(1a)
$$\begin{aligned} \hbox {s.t.}\quad&f_{\textrm{k}}(\tau (0)) = p_{\textrm{start}}, f_{\textrm{k}}(\tau (T)) = p_{\textrm{goal}} \end{aligned}$$
(1b)
$$\begin{aligned}&\tau (t),\dot{\tau }(t),\ddot{\tau }(t) \in \text {configuration limits} \, \forall t \in [0,T] \end{aligned}$$
(1c)
$$\begin{aligned}&f_{\textrm{a}}({\textbf {c}}){\in A,} \end{aligned}$$
(1d)

where T is the duration of the motion, and \({\textbf {c}}=\{{\textbf {c}}_{\textrm{t}}\}\; \forall t \in [0,T]\) is a sequence of the robot states, where \({\textbf {c}}_{\textrm{t}}=(\tau (t),\dot{\tau }(t),\ddot{\tau }(t))\), \(\tau (t)\) is the robot configuration at time t, \(\dot{\tau }(t)\) and \(\ddot{\tau }(t)\) are first and second derivatives of \(\tau (t)\), \(f_{\textrm{k}}\) is the forward kinematic of the robot arm, while \(p_{\textrm{start}}\) and \(p_{\textrm{goal}}\) are the starting and goal points, respectively, \(f_{\textrm{a}}({\textbf {c}})\) is a acceleration function computed at the end-effector, and finally, A is the set of constraints that constrain the acceleration acting on the robot end-effector.

3 Optimization-Based High-Speed Motion Planning

3.1 Trajectory Planning

Since solving the problem (1) is computationally complicated, we propose to approximate the minimal duration of the motion, given its various constraints, which discretizes a motion path into waypoints at time steps 0, 1, \(\cdots \), H, which are separated by an interval \(t_{\text {step}}\). In this context of formulation, the objective function of (1) is now implemented in the value of Horizon H, which will be discussed more below. A new cost function, such as the sum of squares of all the robot joint accelerations, can be used as the objective function to achieve a smoother trajectory, as proposed by Ichnowski et al. (2022). In a discrete domain, let us define \({\textbf{q}}\) as the robot configuration. Hence, we reformulate our optimization problem (1) into the following form:

$$\begin{aligned} {\mathop {{{\textbf{q}}},\dot{\textbf{q}},\ddot{\textbf{q}},{\textbf{H}}}\limits ^{\hbox {min}}} \quad&{\frac{1}{2}\sum _{t=0}^{H}||\ddot{\textbf{q}}||^2_2}\end{aligned}$$
(2a)
$$\begin{aligned} \hbox {s.t}\quad&{{\textbf {q}}_0} = {\textbf {q}}_{\textrm{start}}, {\textbf {q}}_T = {\textbf {q}}_{\textrm{goal}}\end{aligned}$$
(2b)
$$\begin{aligned}&{\dot{\textbf{q}}_0} = {\textbf {0}}\end{aligned}$$
(2c)
$$\begin{aligned}&{{\textbf {q}}_{\textrm{t}},\dot{\textbf{q}}_{\textrm{t}},\ddot{\textbf{q}}_{\textrm{t}}}\in \text {configuration limits}\; \forall t \in \nonumber \\&\quad { [0,1,2,...,H]}\end{aligned}$$
(2d)
$$\begin{aligned}&{{\textbf {q}}_{t+1}} = f({\textbf {q}}_{\textrm{t}},\dot{\textbf{q}}_{\textrm{t}},\ddot{\textbf{q}}_{\textrm{t}}) \; \forall t \in [0,1,2,...,H - 1] \end{aligned}$$
(2e)
$$\begin{aligned}&{\dot{\textbf{q}}_{t+1}} = g(\dot{\textbf{q}}_{\textrm{t}},\ddot{\textbf{q}}_{\textrm{t}}) \; \forall t \in [0,1,2,...,H - 1] \end{aligned}$$
(2f)
$$\begin{aligned}&{||a_{\textrm{ee}}||_{1}}{ \le a_{\textrm{safe}}} \; \forall t \in [0,1,2,...,H] , \end{aligned}$$
(2g)

where \({\textbf {q}}_0 = {\textbf {q}}_{\textrm{start}}\) and \({\textbf {q}}_T = {\textbf {q}}_{\textrm{goal}}\) define the initial and destination configurations of the robot in the motion path, \(\dot{\textbf{q}}_0 = {\textbf {0}}\) defines the initial velocity of the robot which is normally equal to zero, \(f({\textbf {q}}_{\textrm{t}},\dot{\textbf{q}}_{\textrm{t}},\ddot{\textbf{q}}_{\textrm{t}})\) and \(g(\dot{\textbf{q}}_{\textrm{t}},\ddot{\textbf{q}}_{\textrm{t}})\) are constraints that describe the kinematic formulas of the robot motion, \(a_{\textrm{ee}}\) is the acceleration acting on the end-effector of the robot due to its immediate kinematic state, not the acceleration of the end-effector itself and \(a_{\textrm{safe}}\) is an acceleration constraint on an object being transported by the robot. In (2g), we consider the first-order norm of \(a_{\textrm{ee}}\) against \(a_{\textrm{safe}}\), under the hypothesis that considering the magnitude of the acting acceleration is sufficient to keep the motion safe. It is assumed that the robot base is fixed, which does not make any movement despite being the constant subject of the gravitational acceleration.

Given the context of the formulation (2), the relations between the duration of the motion T, the horizon H, and the time step \(t_{\text {step}}\) between them can be expressed as follows:

$$\begin{aligned} T = H \times t_{\text {step}}, \end{aligned}$$
(3)

where H, which is an integer value, can be thought of as the number of waypoints linking the starting configuration and the goal configuration. Thus, it is evident that given a fixed \(t_{\text {step}}\), the smaller the value of H, the shorter the time to complete the motion. However, this is all under the condition that (2) must be feasible. Therefore, the general goal is to find the minimal value of H that yields solvable results for (2). It is also noted that the cost function of (2) on the sum of squares of the accelerations is there to help smooth out the motion by keeping the overall acceleration of the trajectory near zero, which minimizes jerks of the robot.

It is well known that planning a trajectory for a manipulator is actually conducting an interpolation through a series of waypoints by using a polynomial function, as highlighted by Lynch and Park (2017). In practice, a third-order (cubic) polynomial function is widely used since it allows to easily implement a zero velocity at a starting point and a full stop at a goal. By utilizing a cubic function, interpolation between two waypoints is a cubic segment (CS). Nonetheless, if all joints of a robot are driven by a CS between two waypoints, it may lead to the waste of accelerating potential on some joints since \(\ddot{\textbf{q}}\) profile is linear. To alleviate this issue, we propose to exploit a second order polynomial function for planning a trajectory. In other words, the kinematic constraints between two consecutive waypoints in (2e) and (2f) are presented as follows:

$$\begin{aligned} \begin{aligned}&{\textbf {q}}_{t+1} = {\textbf {q}}_{\textrm{t}} + \dot{\textbf{q}}_{\textrm{t}} t_{\text {step}} + \frac{1}{2} \ddot{\textbf{q}}_{\textrm{t}} t^2_{\text {step}} \; \forall t \in [0,1,2,...,H - 1]\\&\dot{\textbf{q}}_{t+1} = \dot{\textbf{q}}_{\textrm{t}} + \ddot{\textbf{q}}_{\textrm{t}} t_{\text {step}} \; \forall t \in [0,1,2,...,H - 1]. \end{aligned} \end{aligned}$$
(4)

These constraints allow the joint trajectories to take any necessary shape in order to minimize the objective function in (2). In (4) the robot configuration in time step \(t+1\) depends on that in time step t. Thus, we call it as an adaptive kinematic trajectory.

3.2 End-Effector Acceleration via Recursive Newton–Euler Algorithm

In this work, we focus on reducing the total robot motion duration with the success of transporting an object. To ensure the safety, e.g., no damage or spill, of the transporting process, we need to consider the inertial effect of the robot movement acting on the end-effector. This requires taking into account the dynamics of the robot, which involves many complex parameters such as an inertial matrix, Coriolis and centrifugal vectors, gravitational terms, frictional effect, and external forces. However, implementing the full dynamics in our formulas would significantly increase the complexity of the optimization problem, causing computing strain. Therefore, we propose to use the recursive Newton–Euler algorithm (RNEA), a robust algorithm proposed by Luh et al. (1980), to calculate the robot dynamics. It is also known that only the forward of RNEA is sufficient to determine the effect of the robot movement acting on its own end-effector. In other words, by utilizing RNEA, we can determine the acceleration acting on the robot end-effector as \(a_{\textrm{ee}} = \hbox {RNEA}({\textbf {q}},\dot{\textbf{q}}, \ddot{\textbf{q}},g)\). We then implement a safe constraint for our transporting motion in the form (2g), which limits acceleration in any direction of the Cartesian task space. Keeping this constraint satisfied at all time during the transportation would theoretically ensure the success of the task.

3.3 Optimization Approach

To address this issue, the proposition suggests reducing the horizon H until the optimization problem becomes unsolvable, as suggested by Ichnowski et al. (2020, 2022); Avigal et al. (2023). However, it is not easy to initialize H, and in fact the initial H can be obtained through learning, which is beyond this paper. Moreover, with this formulation, the dimensionality of the decision variables in (2) is \(n \times H \times 3\), where n is the degree of freedom of the robot manipulator, and 3 represents the set of position, and its second and third derivatives. Considering this, it is evident that H is proportional to the number of decision variables. Thus, starting with an abundant value of H and then shrinking it down can be computationally straining. In our work, we propose to start a small H, e.g., 1, and then increase H by 1 at every loop until (2) is solvable. However, starting from this minimal value may take time; thus, we alternatively propose to first solve (2) without the last constraint (2g) by using the quadratic programming (QP) algorithm by Gill et al. (1989). The resulting H is then used as the new starting horizon H rather than begin all the way from 1 for solving the full optimization problem (2). Here, it is recommended to check whether the solution of the QP has satisfied the nonlinear constraints or not. This could potentially eliminate the need to solve the full problem at all. And to resolve the full optimization problem (2), we exploit a sparse sequential quadratic programming method of Gill et al. (2005), which is highly effective for solving constrained nonlinear optimization problems. On the other hand, \(t_{\text {step}}\) is also another factor that may affect on solving (2), which will be discussed further in the following sections. We will also analyze how the trade-off between refined results and increased computational strain affects the overall efficiency of the motion planning algorithm.

Algorithm 1
figure a

High-speed motion planning optimization algorithm

Full size image

Overall, comparing to the series of works by Ichnowski et al. (2020, 2022); Avigal et al. (2023), this method does not require machine learning, thus eliminating the need for data and the training/learning process. It makes implementation easier and quicker. The proposed optimization procedure algorithm is summarized in Algorithm 1.

4 Simulation Results and Discussions

To demonstrate effectiveness of our proposed approach, we conducted extensive simulation in a synthetic environment called Drake, developed by Tedrake and the Drake Development Team (2019). We employed a 7-DOF KUKA robot with a model of KUKA LBR IIWA 7 R800 in our simulations. The configuration parameters of the robot are summarized in Table 1. It is noted that the robot configuration and parameters were extracted from a realistic robot model provided by REECO (2023). Furthermore, we will implement this motion planning scheme using two distinct values for \(a_{\textrm{safe}}\): \(11.5\, \hbox {m}/\hbox {s}^2\) and \(11.35\, \hbox {m}/\hbox {s}^2\). The rationale behind selecting these specific values will be discussed in Sect. 4.3. It is remarked that the configuration parameters include joint angles, velocities and accelerations. The initial and end configurations correspond to the starting and goal points of the robot end-effector. To solve the optimization problems, we exploited SNOPT optimizer of Gill et al. (2005).

Table 1 KUKA LBR IIWA 7 R800 Robot Experimental Parameters
Full size table
Fig. 1
figure 1

Comparison of the resulting trajectory and acceleration profiles between two methods (cubic method and our adaptive approach). It is evident that the trajectory duration resulting from our adaptive method is significantly shorter (\(T=2\,\hbox {s}\)) as compared with the cubic method (\(T=3.1 \,\hbox {s}\)), attributed to its ability to utilize the maximum acceleration threshold effectively

Full size image

4.1 Adaptive Kinematic vs Cubic Trajectories

For the comparison purposes, we ran the optimization using both cubic and adaptive kinematic trajectories. The obtained results are shown in Fig. 1. It can be clearly seen that the total motion time on an adaptive kinematic trajectory is smaller than that of a cubic trajectory, e.g., \(T = 2.0\,\hbox {s}\) and \(T = 3.1\,\hbox {s}\), respectively. However, merely considering the execution time does not provide a complete understanding of the advantages offered by our method. We further examined their respective acceleration profiles since it allows us to delve deeper into the underlying factors influencing their performances. By closely inspecting the acceleration profiles, we were able to uncover valuable information about what transpired during the optimization process and how it impacted the overall motion characteristics. It is obvious that the cubic trajectory could not fully utilize the actuators’ max accelerating capability due to the nature of their shape being linear segments. Meanwhile, only constraining kinematic relations between two consecutive waypoints in our approach allows the trajectory to fully utilize the acceleration limits and, hence, remarkably shorten the duration of the motion.

Fig. 2
figure 2

The trajectories at each joint of the robot given different \(t_{\text {step}}\) values. It is evident that smaller \(t_{\text {step}}\) leads to a slightly shorter total motion time T

Full size image
Table 2 Performance obtained by our proposed approach at different \(t_{\text {step}}\) values
Full size table

On the other hand, the proposed approach excels in optimizing the motion trajectory by leveraging its flexibility, allowing all shapes of trajectory as long as they abide the motion’s kinematic conditions. By considering the kinematic constraints and incorporating them into the motion planning process, kinematic formulas constraints enables the trajectory to fully exploit the actuator’s maximum acceleration capabilities. This dynamic optimization leads to more efficient and effective motion execution, resulting in a significantly shorter duration for the entire motion profile. It is also worth to notice that despite the cubic trajectory reaching zero joint velocities at the goal, its acceleration profiles does not converge at zero, which may lead to a certain degree of undesirable deviation from the goal.

4.2 \(t_{\text {step}}\) and H Horizon Impact on Solution Performance

Since \(T = H \times t_{\text {step}}\), by manipulating \(t_{\text {step}}\) and H the resolution and accuracy of the result can be adjusted to meet specific requirements. To gain further insights into the significance of H and \(t_{\text {step}}\), it is essential to consider the dimensionality of the decision variables involved in the optimization process. In our formulation, each of the decision variables (\({\textbf {q}}_{\textrm{t}},\dot{\textbf{q}}_{\textrm{t}},\ddot{\textbf{q}}_{\textrm{t}}\)) possesses dimensions of \(H\times n\), where n denotes the number of joints or degrees of freedom of the robot. That is, when \(t_{\text {step}}\) decreases, the dimensionality of the decision variables also expands along with H.

The relationship between \(t_{\text {step}}\) and the refinement of results as well as its trade-off with computational efficiency are crucial to consider. A smaller \(t_{\text {step}}\) leads to more refined results as it allows for finer steps between waypoints. However, this refinement comes at the cost of increased computational time. To demonstrate the impact of \(t_{\text {step}}\) on the trajectory optimization process, a comparison between \(t_{\text {step1}} = 0.05 \,\hbox {s}\), \(t_{\text {step1}} = 0.1 \,\hbox {s}\), and \(t_{\text {step2}} = 0.2 \,\hbox {s}\) for the threshold \(a_{\text {safe}} = 11.35, \, {\hbox {m}/\hbox {s}}^2\) was conducted. The optimization results are summarized in Fig. 2 and Table 2. It can be seen in Table 2 that the finer \(t_{\text {step}}\) results in a slightly shorter total time to complete the motion. Nevertheless, it comes with higher cost of computation since the algorithm needs to run more iterations in the horizon to find the optimal solution. It is also interesting to note that the trajectories at each joint of the robot, as demonstrated in Fig. 2, are slightly different, which accounts for the slightly different motion duration corresponding to each \(t_{\text {step}}\).

Table 3 Performance comparison between our proposed approach and the simple method H starting from 1, at different \(t_{\text {step}}\) values
Full size table

As discussed in Sect. 3.2, choosing a right initial H can greatly improve the efficiency of solving the optimization problem (2). In our work, we propose first to remove the constraint (2g) from the optimization problem (2), which enables us to easily solve it by using a QP algorithm. The obtained H is then used as a starting horizon for solving the full optimization problem (2). This approach potentially reduces the computing time as the solver can bypass the H values that satisfy only the linear constraints and focus on simultaneously satisfying both the linear and nonlinear constraints. To verify the computational effectiveness of the proposed method, we compared it against solving the optimization problem (2) with an initial \(H = 1\). Both methods used a threshold of \(a_{\textrm{safe}} = 11.35\, \hbox {m}/\hbox {s}^2\), and \(t_{\text {step}}\) varied from \(0.1 \,\hbox {s}\) to \(0.2 \,\hbox {s}\) with a step size of \(0.01\,\hbox {s}\). Each experiment was run 10 times. And one computer was dedicated to only run a solver to solve the problem without doing any other task. The performance and results of both methods are summarized in Table 3. It can clearly seen in the both methods that whenever \(t_{\text {step}}\) is increasing, the horizon H is decreasing, and that the H value is consistent at each \(t_{\text {step}}\), which is reasonable as we utilized the same optimization solver. The total motion time (T) is also consistent at each \(t_{\text {step}}\) in both the approaches though it slightly fluctuates between 2.09 s and 2.34 s as the step size varies. The slight fluctuation is due to numerical error caused by resolutions of \(t_{\text {step}}\) (our choice in this example) and H (which must be an integer number). Of greater significance, the primary difference between the two methodologies is evident in computing time—our proposed method consistently demonstrates lower computing time across all the step sizes in comparison to the simple approach with H starting from 1. This suggests that the proposed method is computationally more efficient, demanding less time for computation. Furthermore, it is noteworthy that in these 11 example scenarios the mean and standard deviation of the computation time for the simple approach with H starting from 1 are \(1289.29\,\hbox {s}\) and \(753.66\,\hbox {s}\), respectively. Conversely, for our proposed method these values stand at \(1054.86\,\hbox {s}\) and \(464.71\,\hbox {s}\), respectively.

Fig. 3
figure 3

Comparison of the trajectories of the robot joints and the end-effector accelerations when the constraint \(a_{\textrm{safe}}\) is set to \(a_{\textrm{safe}} = 11.5\, \hbox {m}/\hbox {s}^2\) and \(a_{\textrm{safe}} = 11.35\, \hbox {m}/\hbox {s}^2\), respectively. The motion duration T is longer (\(T=2.1\,\hbox {s}\)) when \(a_{\textrm{safe}}\) is smaller (\(a_{\textrm{safe}} = 11.35\, \hbox {m}/\hbox {s}^2\)) as compared with \(T=2\,\hbox {s}\) when \(a_{\textrm{safe}} = 11.5\, \hbox {m}/\hbox {s}^2\)

Full size image

4.3 Acceleration Constraint

First, let us briefly discuss the constraint in (2g), wherein the objective is to maintain the first-order norm of the RNEA for each state (\(q,\dot{q},\ddot{q}\)) below a presumed safe threshold. It is noteworthy that the acceleration vector acting on the end-effector, as derived from the RNEA algorithm, already incorporates gravitational acceleration (\(g = 9.81\, \hbox {m}/\hbox {s}^2\)). Given this integration, it is arguable that the selected value \(a_{\textrm{safe}} = 11.5\, \hbox {m}/\hbox {s}^2\) allows minimal leeway for other acceleration components.

In Fig. 3, for the case of \(a_{\textrm{safe}} = 11.5\, \hbox {m}/\hbox {s}^2\), it is evident that the acceleration constraint remains active throughout the motion, underscoring the efficacy of the planning scheme in this context. We then examine the scenario with \(a_{\textrm{safe}} = 11.35\, \hbox {m}/\hbox {s}^2\), representing a further reduction in the threshold. In this instance, the algorithm requires an additional step of H to uphold the (2g) constraint, resulting in a longer motion duration. In conclusion, based on the aforementioned cases, it is apparent that the algorithm’s behavior conforms to the desired outcome. Specifically, it strives to maintain satisfaction of the acceleration constraints and, when deemed necessary, extends the horizon H to do so. Table 4 provides different scenarios with varying \(a_{\text {safe}}\), serving as additional references to our experiment. This table clearly supports our previous statement: for \(a_{\textrm{safe}}\) values of \(12.5\, \hbox {m}/\hbox {s}^2\), \(12\, \hbox {m}/\hbox {s}^2\), and \(11.5\, \hbox {m}/\hbox {s}^2\), both H and T can remain constant at 20 and \(2\,\hbox {s}\), respectively. The consistent values of H and T when \(a_{\textrm{safe}}\) varies between \(11.5 \, \hbox {m}/\hbox {s}^2\) and \(12.5 \, \hbox {m}/\hbox {s}^2\) are due to more relaxation of the constraint in the optimization and the fact that H is also rounded up to an integer number. However, when the constraint \(a_{\textrm{safe}}\) is tighter, decreasing to \(11.35\, \hbox {m}/\hbox {s}^2\) or lower, the algorithm must increase the horizon H in order to keep the constraint (2g) satisfied, and consequently, leading to increment of the total motion time T.

Table 4 Additional scenarios with varying \(a_{\textrm{safe}}\)
Full size table
Fig. 4
figure 4

The optimized and executed trajectories of the joint positions of the simulated 7-DOF KUKA robot. It is noted that the total motion time \(T=2\,\hbox {s}\)

Full size image

4.4 Simulated Execution

In practice, after solving the planning optimization problem (2), the optimized trajectories are employed to control the joints of a robot so that the robot can conduct a required task. When a robot manipulator executes the optimized trajectories, it is highly expected that the executed trajectories match the optimized trajectories. To demonstrate this execution, we implemented the optimized trajectories obtained by our proposed approach in a simulated 7-DOF KUKA robot with a model of KUKA LBR IIWA 7 R800. And we conducted the position control as it is widely used in controlling robot arms nowadays (Tedrake, 2022). For the comparison purposes, we also collected measurements of the positions and velocities of the joints of the simulated robot over time. The measured positions and velocities were then compared with the optimized ones in the same plot for each quantity. The comparison results are illustrated in Figs. 4 and 5 for the joint positions and velocities, respectively.

It can be seen in Fig. 4 that the executed joint trajectories demonstrate remarkable conformity to the input (optimized) trajectories, exhibiting almost no difference between the executed trajectories and the optimized trajectories at each joint. In other words, the optimized trajectories are highly feasible to be executed in a robot manipulator by a control system. Likewise, as demonstrated in Fig. 5, the measured and optimized velocities at each joint of the robot are highly comparable, which attests that the optimized trajectories obtained by our proposed method can be easily digested by the control strategy to get the robot joints followed as planned. The minor deviation between the curves in each plot is likely attributable to the inherent characteristics of the applied controller. Nevertheless, discussing characteristics of control algorithms is beyond the scope of our paper. More importantly, it can be clearly seen that eventually both the optimized and measured trajectories could accurately drive the joints, and then the end-effector, of the robot to reach to and stop at the desired goal, which demonstrates that our proposed approach is highly promising.

Fig. 5
figure 5

The optimized and executed trajectories of the joint velocities of the simulated 7-DOF KUKA robot. It is noted that the total motion time \(T=2\,\hbox {s}\)

Full size image

5 Conclusions

The paper has presented a promising approach for robotic pick and place trajectory planning, specifically focusing on reducing total trajectory time while guaranteeing the acceleration acting on the end-effector less than an expected threshold, which avoids any damage or spills on a transported object. The consideration of robot joint limits, including maximum and minimum joint positions, velocities, and accelerations, is effectively handled in our problem in the form of linear constrains, ensuring that the resulting trajectory remains within the robot’s physical capabilities. Additionally, we have taken into account the kinematic relation along the trajectory, making sure the whole pick-and-place motion is valid and executable for the robot. Another crucial factor we have addressed is the acceleration acting on the end-effector resulting from the robot’s motion. By incorporating this nonlinear constraint into the optimization problem, we have considered and ensured safety of the pick-and-place motion. On the other hand, we have also exploited both the quadratic programming algorithm and sparse sequential quadratic programming method to computationally efficiently solve the optimization in a two-step procedure. Evaluation of the proposed method was conducted on Drake, where the obtained results demonstrate its practicality.