Abstract
A redundant manipulator can have many trajectories for joints that follow a given end-effector path in the Cartesian space, since it has multiple inverse kinematics solutions per end-effector pose. While maintaining accuracy with the given end-effector path, it is challenging to quickly synthesize a feasible trajectory that satisfies robot-specific constraints and is collision-free against obstacles, especially when the given end-effector path passes around obstacles. In this paper, we present a trajectory optimization of a redundant manipulator to synthesize a trajectory that follows a given end-effector path accurately, while achieving smoothness and collision-free manipulation. Our method holistically incorporates three desired properties into the trajectory optimization process by integrating the Jacobian-based inverse kinematics solving method and an optimization-based motion planning approach. Specifically, we optimize a trajectory using two-stage gradient descent to reduce potential competition between different properties during the update. To avoid falling into local minima, we iteratively explore different candidate trajectories with our local update. We also accelerate our optimizer by adaptively determining the stop of the current exploration based on the observation of optimization results. We compare our method with five prior methods in test scenes, including external obstacles and two non-obstacle problems. Furthermore, we analyze our optimizer performance by experimenting with three different configurations of robots. Our method robustly minimizes the pose error in a progressive manner while satisfying various desirable properties.
This is a preview of subscription content, log in via an institution to check access.
Similar content being viewed by others
Explore related subjects
Discover the latest articles, news and stories from top researchers in related subjects.References
Beeson, P., & Ames, B. (2015). TRAC-IK: An open-source library for improved solving of generic inverse kinematics. In Humanoids (pp. 928–935). IEEE.
Berenson, D., Srinivasa, S. S., Ferguson, D., & Kuffner, J. J. (2009). Manipulation planning on constraint manifolds. In ICRA (pp. 625–632). IEEE.
Bonalli, R., Cauligi, A., Bylard, A., & Pavone, M. (2019). GuSTO: Guaranteed sequential trajectory optimization via sequential convex programming. In 2019 International conference on robotics and automation (ICRA) (pp. 6741–6747). IEEE.
Boyd, S., Boyd, S. P., & Vandenberghe, L. (2004). Convex optimization. Cambridge University Press.
Buss, S. R. (2004). Introduction to inverse kinematics with Jacobian transpose, pseudoinverse and damped least squares methods. IEEE Journal of Robotics and Automation, 17(1–19), 16.
Chiacchio, P., Chiaverini, S., Sciavicco, L., & Siciliano, B. (1991). Closed-loop inverse kinematics schemes for constrained redundant manipulators with task space augmentation and task priority strategy. IJRR, 10(4), 410–425.
Chiaverini, S. (1997). Singularity-robust task-priority redundancy resolution for real-time kinematic control of robot manipulators. Transactions on Robotics and Automation, 13(3), 398–410.
Diankov, R. (2010). Automated construction of robotic manipulation programs.
Douglas, D. H., & Peucker, T. K. (1973). Algorithms for the reduction of the number of points required to represent a digitized line or its caricature. Cartographica: The International Journal for Geographic Information and Geovisualization, 10(2), 112–122.
Flash, T., & Potts, R. B. (1988). Communication: Discrete trajectory planning. IJRR, 7(5), 48–57.
Frank, M., Wolfe, P., et al. (1956). An algorithm for quadratic programming. Naval Research Logistics Quarterly, 3(1–2), 95–110.
Gupta, H., Jin, K. H., Nguyen, H. Q., McCann, M. T., & Unser, M. (2018). CNN-based projected gradient descent for consistent CT image reconstruction. IEEE Transactions on Medical Imaging, 37(6), 1440–1453.
Holladay, R., Salzman, O., & Srinivasa, S. (2019). Minimizing task-space fréchet error via efficient incremental graph search. RA-L, 4(2), 1999–2006.
Holladay, R. M, &Srinivasa, S. S. (2016). Distance metrics and algorithms for task space path optimization. In IROS (pp. 5533–5540). IEEE.
Jiang, X., & Li, S. (2017). BAS: Beetle antennae search algorithm for optimization problems. arXiv:1710.10724
Joulin, A., Tang, K., & Fei-Fei, L. (2014). Efficient image and video co-localization with frank-wolfe algorithm. In European conference on computer vision (pp. 253–268). Springer.
Kalakrishnan, M., Chitta, S., Theodorou, E., Pastor, P., & Schaal, S. (2011). STOMP: Stochastic trajectory optimization for motion planning. In ICRA (pp. 4569–4574). IEEE.
Kang, M., Shin, H., Kim, D., & Yoon, S.-E. (2020). TORM: Fast and accurate trajectory optimization of redundant manipulator given an end-effector path. In IROS. IEEE.
Kanoun, O., Lamiraux, F., & Wieber, P.-B. (2011). Kinematic control of redundant manipulators: Generalizing the task-priority framework to inequality task. T-RO, 27(4), 785–792.
Karaman, S., Walter, M. R., Perez, A., Frazzoli, E., & Teller, S. (2011). Anytime motion planning using the RRT. In ICRA (pp. 1478–1483). IEEE.
Katyal, K. D., Brown, C. Y.,Hechtman, S. A., Para, M. P., McGee, T. G., Wolfe, K. C., Murphy, R. J., Kutzer, M. D. M., Tunstel, E. W., McLoughlin, M. P., et al. (2014). Approaches to robotic teleoperation in a disaster scenario: From supervised autonomy to direct control. In IROS (pp. 1874–1881). IEEE.
Kew, J. C., Ichter, B., Bandari, M., Edward Lee, T.-W., & Faust, A. (2019). Neural collision clearance estimator for fast robot motion planning.
Khan, A. T., Li, S., & Zhou, X. (2021). Trajectory optimization of 5-link biped robot using beetle antennae search. IEEE Transactions on Circuits and Systems II: Express Briefs.
Kim, D., & Fessler, J. A. (2018). Adaptive restart of the optimized gradient method for convex optimization. Journal of Optimization Theory and Applications, 178(1), 240–263.
Kim, B., Um, T. T., Suh, C., & Park, F. C. (2016). Tangent bundle RRT: A randomized algorithm for constrained motion planning. Robotica, 34(1), 202.
Kingston, Z., Moll, M., & Kavraki, L. E. (2019). Exploring implicit spaces for constrained sampling-based planning. IJRR, 38(10–11), 1151–1178.
Kunz, T., & Stilman, M. (2012). Manipulation planning with soft task constraints. In IROS (pp. 1937–1942). IEEE.
Li, L., Miao, Y., Qureshi, A. H., & Yip, M. C. (2021). MPC-MPNet: Model-predictive motion planning networks for fast. Near-optimal planning under kinodynamic constraints. RA-L, 6(3), 4496–4503.
Long, P., Keleştemur, T., Özgün, A., Önol, & Padir, T. (2019). Optimization-based human-in-the-loop manipulation using joint space polytopes. In ICRA (pp. 204–210). IEEE.
Luo, J., & Hauser, K. (2012). Interactive generation of dynamically feasible robot trajectories from sketches using temporal mimicking. In ICRA (pp. 3665–3670). IEEE.
O’donoghue, B., & Candes, E. (2015). Adaptive restart for accelerated gradient schemes. Foundations of Computational Mathematics, 15(3), 715–732.
Praveena, P., Rakita, D., Mutlu, B., & Gleicher, M. (2019). User-guided offline synthesis of robot arm motion from 6-DoF paths. In ICRA (pp. 8825–8831). IEEE.
Rakita, D., Mutlu, B., & Gleicher, M. (2018). RelaxedIK: Real-time synthesis of accurate and feasible robot arm motion. In RSS.
Rakita, D., Mutlu, B., & Gleicher, M. (2019). STAMPEDE: A discrete-optimization method for solving pathwise-inverse kinematics. In ICRA (pp. 3507–3513). IEEE.
Schulman, J., Ho, J., Lee, A. X., Awwal, I., Bradlow, H., & Abbeel, P. (2013). Finding locally optimal, collision-free trajectories with sequential convex optimization. In RSS (Vol. 9, pp. 1–10). Citeseer.
Sinha, A., & Chakraborty, N., (2019). Geometric search-based inverse kinematics of 7-DoF redundant manipulator with multiple joint offsets. In ICRA (pp. 5592–5598). IEEE.
Stilman, M. (2007). Task constrained motion planning in robot joint space. In IROS (pp. 3074–3081). IEEE.
Tong, H., Faloutsos, C., & Pan, J.-Y. (2006). Fast random walk with restart and its applications. In Sixth international conference on data mining (pp. 613–622). IEEE.
Traonmilin, Y., Aujol, J.-F., & Leclaire, A. (2020). Projected gradient descent for non-convex sparse spike estimation. IEEE Signal Processing Letters, 27, 1110–1114.
Vahrenkamp, N., Berenson, D., Asfour, T., Kuffner, J., & Dillmann, R. (2009). Humanoid motion planning for dual-arm manipulation and re-grasping tasks. In IROS (pp. 2464–2470). IEEE.
Van Laarhoven, P. J. M., & Aarts, E. H. L. (1987). Simulated annealing. In Simulated annealing: Theory and applications (pp. 7–15). Springer.
Yoshikawa, T. (1985). Manipulability of robotic mechanisms. IJRR, 4(2), 3–9.
Zhao, Y., Li Yan, Yu., Chen, J. D., & Liu, Y. (2021). Robust and efficient trajectory replanning based on guiding path for quadrotor fast autonomous flight. Remote Sensing, 13(5), 972.
Zinkevich, M. (2003). Online convex programming and generalized infinitesimal gradient ascent. In Proceedings of the 20th international conference on machine learning (pp. 928–936).
Zucker, M., Ratliff, N., Dragan, A. D., Pivtoraiko, M., Klingensmith, M., Dellin, C. M., et al. (2013). CHOMP: Covariant Hamiltonian optimization for motion planning. IJRR, 32(9–10), 1164–1193.
Acknowledgements
We appreciate the anonymous reviewers for constructive comments and insightful suggestions. This work was supported by the National Research Foundation of Korea (NRF) Grant funded by the Korea Government (MSIT) (Nos. 2021R1A4A1032582 and 2019R1A2C3002833).
Author information
Authors and Affiliations
Corresponding author
Additional information
Publisher's Note
Springer Nature remains neutral with regard to jurisdictional claims in published maps and institutional affiliations.
Rights and permissions
About this article
Cite this article
Kang, M., Yoon, SE. Analysis and acceleration of TORM: optimization-based planning for path-wise inverse kinematics. Auton Robot 46, 599–615 (2022). https://doi.org/10.1007/s10514-022-10040-1
Received:
Accepted:
Published:
Issue Date:
DOI: https://doi.org/10.1007/s10514-022-10040-1