# Globally Optimal Redundancy Resolution with Dynamic Programming for Robot Planning: A ROS Implementation

^{1}

^{2}

^{*}

## Abstract

**:**

## 1. Introduction

## 2. Materials and Methods

#### 2.1. Discrete Dynamic Programming

#### 2.2. State Space Grids

#### 2.3. DP-Inspired Search Algorithm

## 3. ROS Implementation

#### 3.1. Designing an Extension for MoveIt!

#### 3.2. Requirements

- Req. 1:
- to allow for a seamless integration with the ROS ecosystem, so as to reuse, as much as possible, the available technologies (e.g., visualization and analysis tools);
- Req. 2:
- to support the generation of multiple homogeneous grids;
- Req. 3:
- to perform a search on such grids to find the resolution-optimal joint-space solution [9];
- Req. 4:
- to support the optimization for homogeneous grids [14];
- Req. 5:
- to allow for the addition of user-defined constraints and objective functions;
- Req. 6:
- to allow for the topological analysis of the state space and the resolution-optimal trajectory.

#### 3.3. Context

#### 3.4. Architectural Design

^{++}library containing the algebraic IK solver, able to return all the solutions for given end-effector pose. The off-line process may require several minutes, but is independent from the assigned trajectory and, thus, needs to be executed only once for a given kinematic chain. Currently, IKFast is able to manage open kinematic chains with one degree of redundancy. The value of the redundancy parameter has to be provided at the time the algebraic solver is called, which is the case of the DP grids considered in this communication. Nonetheless, it is worth observing that, in this context, it is not necessary that the IK solver natively supports redundant inverse kinematics, as the redundancy parameters are given for each single grid node and the inverse kinematics always involve a non-redundant kinematic chain. This means that our solution is scalable with respect to an arbitrary redundancy degree and IKFast can always be used, provided a suitable definition of the redundant (i.e., including the redundant joints) and non-redundant (i.e., excluding the redundant joints) planning groups.

#### 3.5. Use of Numeric Solvers

## 4. Results

#### 4.1. Use Case Description

#### 4.2. Grids Computation

^{++}solver generated off-line, which requires to select a redundant joint with respect to which inverse kinematics expressions are computed. In general, the choice of the redundancy parameter is not arbitrary, for two reasons:

- when fixing the redundant joints to specific values, the manipulator must be no longer redundant in order for (10) to return a finite number of solutions: at algorithmic singularities, i.e., configurations nullifying the determinant of the extended Jacobian, this is not the case.

- $|{\mathbf{J}}_{r}^{\left(4\right)}|>0$;
- ${q}_{2}>0$;
- ${q}_{5}>0$.

#### 4.3. Globally Optimal and Pareto-Optimal Solution

^{®}Core

^{TM}i7-2600K CPU @ 3.40GHz × 8. No multi-core execution model has been used in the tests.

## 5. Discussion

## 6. Conclusions

## Author Contributions

## Funding

## Institutional Review Board Statement

## Informed Consent Statement

## Data Availability Statement

## Conflicts of Interest

## References

- Siciliano, B.; Khatib, O. Springer Handbook of Robotics; Springer International Publishing: Berlin/Heidelberg, Germany, 2016. [Google Scholar] [CrossRef]
- Kazerounian, K.; Wang, Z. Global versus local optimization in redundancy resolution of robotic manipulators. Int. J. Robot. Res.
**1988**, 7, 3–12. [Google Scholar] [CrossRef] - Nakamura, Y.; Hanafusa, H. Optimal redundancy control of robot manipulators. Int. J. Robot. Res.
**1987**, 6, 32–42. [Google Scholar] [CrossRef] - Guigue, A.; Ahmadi, M.; Hayes, M.J.D.; Langlois, R.; Tang, F.C. A dynamic programming approach to redundancy resolution with multiple criteria. In Proceedings of the IEEE International Conference on Robotics and Automation, Rome, Italy, 10–14 April 2007; pp. 1375–1380. [Google Scholar] [CrossRef]
- Gao, J.; Pashkevich, A.; Caro, S. Optimization of the robot and positioner motion in a redundant fiber placement workcell. Mech. Mach. Theory
**2017**, 114, 170–189. [Google Scholar] [CrossRef] - Shen, Y.; Huper, K. Optimal trajectory planning of manipulators subject to motion constraints. In Proceedings of the 12th International Conference on Advanced Robotics, Seattle, WA, USA, 17–20 July 2005. [Google Scholar] [CrossRef]
- Ascher, U.M.; Mattheij, R.M.M.; Russell, R.D. Numerical Solution of Boundary Value Problems for Ordinary Differential Equations; Society for Industrial and Applied Mathematics: Philadelphia, PA, USA, 1995. [Google Scholar] [CrossRef]
- Keller, H.B. Numerical Methods for Two-Point Boundary-Value Problems; Courier Dover Publications: Garden City, NY, USA, 2018. [Google Scholar]
- Ferrentino, E.; Chiacchio, P. A topological approach to globally-optimal redundancy resolution with dynamic programming. In ROMANSY 22–Robot Design, Dynamics and Control; Arakelian, V., Wenger, P., Eds.; Springer: Cham, Switzerland, 2018; Volume 584, pp. 77–85. [Google Scholar] [CrossRef]
- Pashkevich, A.P.; Dolgui, A.B.; Chumakov, O.A. Multiobjective optimization of robot motion for laser cutting applications. Int. J. Comput. Integr. Manuf.
**2004**, 17, 171–183. [Google Scholar] [CrossRef] - Dolgui, A.; Pashkevich, A. Manipulator motion planning for high-speed robotic laser cutting. Int. J. Prod. Res.
**2009**, 47, 5691–5715. [Google Scholar] [CrossRef][Green Version] - Guigue, A.; Ahmadi, M.; Langlois, R.; Hayes, M.J.D. Pareto optimality and multiobjective trajectory planning for a 7-DOF redundant manipulator. IEEE Trans. Robot.
**2010**, 26, 1094–1099. [Google Scholar] [CrossRef] - Cavalcanti Santos, J.; Martins da Silva, M. Redundancy Resolution of Kinematically Redundant Parallel Manipulators Via Differential Dynamic Programing. J. Mech. Robot.
**2017**, 9. [Google Scholar] [CrossRef] - Ferrentino, E.; Chiacchio, P. On the optimal resolution of inverse kinematics for redundant manipulators using a topological analysis. J. Mech. Robot.
**2020**, 12. [Google Scholar] [CrossRef] - What is ROS? Available online: http://www.ros.org/ (accessed on 3 March 2021).
- Ferrentino, E.; Chiacchio, P. Redundancy parametrization in globally-optimal inverse kinematics. In Advances in Robot Kinematics 2018; Lenarcic, J., Parenti-Castelli, V., Eds.; Springer: Cham, Switzerland, 2018; Volume 8, pp. 47–55. [Google Scholar] [CrossRef]
- Burdick, J.W. On the inverse kinematics of redundant manipulators: Characterization of the self-motion manifolds. In Proceedings of the 4th International Conference on Advanced Robotics, Scottsdale, AZ, USA, 14–19 May 1989; Waldron, K.J., Ed.; Springer: Berlin/Heidelberg, Germany, 1989; Volume 4, pp. 25–34. [Google Scholar] [CrossRef]
- Burdick, J.W. On the Inverse Kinematics of Redundant Manipulators: Characterization of the Self-Motion Manifolds. In Proceedings of the IEEE International Conference on Robotics and Automation, Scottsdale, AZ, USA, 14–19 May 1989; pp. 264–270. [Google Scholar] [CrossRef]
- Wenger, P. A New General Formalism for the Kinematic Analysis of All Nonredundant Manipulators. In Proceedings of the IEEE International Conference on Robotics and Automation, Nice, France, 12–14 May 1992; pp. 442–447. [Google Scholar] [CrossRef]
- Pámanes, J.A.; Wenger, P.; Zapata, J.L. Motion Planning of Redundant Manipulators for Specified Trajectory Tasks; Advances in Robot Kinematics; Springer: Dordrecht, The Netherlands, 2002; pp. 203–212. [Google Scholar] [CrossRef]
- Sniedovich, M. Dijkstra’s algorithm revisited: The dynamic programming connexion. Control. Cybern.
**2006**, 35, 599–620. [Google Scholar] - MoveIt. Available online: https://moveit.ros.org/ (accessed on 3 March 2021).
- MoveIt Planners. Available online: https://moveit.ros.org/documentation/planners/ (accessed on 3 March 2021).
- MoveIt Concepts. Available online: https://moveit.ros.org/documentation/concepts/ (accessed on 3 March 2021).
- Ferrentino, E.; Salvioli, F. ROS/MoveIt! Extension for Redundancy Resolution with Dynamic Programming. GitHub/Zenodo. Available online: https://zenodo.org/record/3236880#.YAsFiehKhPY (accessed on 3 March 2021).
- Diankov, R. Automated Construction of Robotic Manipulation Programs. Ph.D. Thesis, Carnegie Mellon University, Pittsburgh, PA, USA, 26 August 2010. Available online: http://www.programmingvision.com/rosen_diankov_thesis.pdf (accessed on 3 March 2021).
- KDL Wiki. Available online: http://www.orocos.org/kdl (accessed on 3 March 2021).
- Ferrentino, E.; Chiacchio, P. Topological analysis of global inverse kinematic solutions for redundant manipulators. In ROMANSY 22–Robot Design, Dynamics and Control; Arakelian, V., Wenger, P., Eds.; Springer: Cham, Switzerland, 2018; Volume 584, pp. 69–76. [Google Scholar] [CrossRef]
- Panda Powertool. Available online: https://www.franka.de/panda (accessed on 3 March 2021).
- Khalil, W.; Kleinfinger, J. A new geometric notation for open and closed-loop robots. In Proceedings of the IEEE International Conference on Robotics and Automation, San Francisco, CA, USA, 7–10 April 1986; pp. 1174–1179. [Google Scholar] [CrossRef]
- Franka Control Interface Documentation. Available online: https://frankaemika.github.io/docs/ (accessed on 3 March 2021).
- Zaplana, I.; Basanez, L. A novel closed-form solution for the inverse kinematics of redundant manipulators through workspace analysis. Mech. Mach. Theory
**2018**, 121, 829–843. [Google Scholar] [CrossRef][Green Version] - Ferrentino, E.; Salvioli, F. Redundancy Resolution for Energy Minimization and Obstacle Avoidance with Franka Emika’s Panda Robot. Available online: https://youtu.be/AxL755_t3_o (accessed on 3 March 2021).

**Figure 6.**Workspace path assigned to the Panda arm, together with the base reference frame and obstacle.

**Figure 7.**Panda grids (each corresponding to a different extended aspect) representing ${q}_{1}$ for the trajectory described in Section 4.1 considering joint limits.

**Figure 8.**Panda grids (each corresponding to a different extended aspect) representing ${q}_{1}$ for the trajectory described in Section 4.1 neglecting joint limits.

**Figure 9.**Discrete globally optimal (

**left**) and Pareto-optimal (

**right**) solution for the Panda example.

${\mathit{d}}_{\mathit{i}}$ | ${\mathit{\theta}}_{\mathit{i}}$ | ${\mathit{a}}_{\mathit{i}}$ | ${\mathit{\alpha}}_{\mathit{i}}$ | |
---|---|---|---|---|

J1 | $0.333$ | ${q}_{1}$ | 0 | 0 |

J2 | 0 | ${q}_{2}$ | 0 | $-\pi /2$ |

J3 | $0.316$ | ${q}_{3}$ | 0 | $\pi /2$ |

J4 | 0 | ${q}_{4}$ | $0.0825$ | $\pi /2$ |

J5 | $0.384$ | ${q}_{5}$ | $-0.0825$ | $-\pi /2$ |

J6 | 0 | ${q}_{6}$ | 0 | $\pi /2$ |

J7 | 0 | ${q}_{7}$ | $0.088$ | $\pi /2$ |

Flange | $0.107$ | 0 | 0 | 0 |

**Table 2.**Cost function and performance of DP redundancy resolution algorithm for the Panda example, minimizing the square norm of velocities.

Publisher’s Note: MDPI stays neutral with regard to jurisdictional claims in published maps and institutional affiliations. |

© 2021 by the authors. Licensee MDPI, Basel, Switzerland. This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution (CC BY) license (http://creativecommons.org/licenses/by/4.0/).

## Share and Cite

**MDPI and ACS Style**

Ferrentino, E.; Salvioli, F.; Chiacchio, P. Globally Optimal Redundancy Resolution with Dynamic Programming for Robot Planning: A ROS Implementation. *Robotics* **2021**, *10*, 42.
https://doi.org/10.3390/robotics10010042

**AMA Style**

Ferrentino E, Salvioli F, Chiacchio P. Globally Optimal Redundancy Resolution with Dynamic Programming for Robot Planning: A ROS Implementation. *Robotics*. 2021; 10(1):42.
https://doi.org/10.3390/robotics10010042

**Chicago/Turabian Style**

Ferrentino, Enrico, Federico Salvioli, and Pasquale Chiacchio. 2021. "Globally Optimal Redundancy Resolution with Dynamic Programming for Robot Planning: A ROS Implementation" *Robotics* 10, no. 1: 42.
https://doi.org/10.3390/robotics10010042