A Factor-Graph Approach for Optimization Problems with Dynamics Constraints

In this paper, we introduce dynamics factor graphs as a graphical framework to solve dynamics problems and kinodynamic motion planning problems with full consideration of whole-body dynamics and contacts. A factor graph representation of dynamics problems provides an insightful visualization of their mathematical structure and can be used in conjunction with sparse nonlinear optimizers to solve challenging, high-dimensional optimization problems in robotics. We can easily formulate kinodynamic motion planning as a trajectory optimization problem with factor graphs. We demonstrate the flexibility and descriptive power of dynamics factor graphs by applying them to control various dynamical systems, ranging from a simple cart pole to a 12-DoF quadrupedal robot.

[1]  Maani Ghaffari Jadidi,et al.  Legged Robot State-Estimation Through Combined Forward Kinematic and Preintegrated Contact Factors , 2017, 2018 IEEE International Conference on Robotics and Automation (ICRA).

[2]  Kazuhito Yokoi,et al.  The 3D linear inverted pendulum mode: a simple modeling for a biped walking pattern generation , 2001, Proceedings 2001 IEEE/RSJ International Conference on Intelligent Robots and Systems. Expanding the Societal Role of Robotics in the the Next Millennium (Cat. No.01CH37180).

[3]  Yuval Tassa,et al.  MuJoCo: A physics engine for model-based control , 2012, 2012 IEEE/RSJ International Conference on Intelligent Robots and Systems.

[4]  Rina Dechter,et al.  Network-Based Heuristics for Constraint-Satisfaction Problems , 1987, Artif. Intell..

[5]  Marco Hutter,et al.  Gait and Trajectory Optimization for Legged Systems Through Phase-Based End-Effector Parameterization , 2018, IEEE Robotics and Automation Letters.

[6]  Dinesh K. Pai,et al.  Forward Dynamics, Elimination Methods, and Formulation Stiffness in Robot Simulation , 1997, Int. J. Robotics Res..

[7]  David J. Spiegelhalter,et al.  Local computations with probabilities on graphical structures and their application to expert systems , 1990 .

[8]  Frank Dellaert,et al.  Incremental smoothing and mapping , 2008 .

[9]  Prakash P. Shenoy,et al.  Propagating Belief Functions with Local Computations , 1986, IEEE Expert.

[10]  Frank Dellaert,et al.  On-Manifold Preintegration for Real-Time Visual--Inertial Odometry , 2015, IEEE Transactions on Robotics.

[11]  Eugene C. Freuder.  A Sufficient Condition for Backtrack-Free Search , 1982, JACM.

[12]  Olivier Stasse,et al.  The Pinocchio C++ library : A fast and flexible implementation of rigid body dynamics algorithms and their analytical derivatives , 2019, 2019 IEEE/SICE International Symposium on System Integration (SII).

[13]  Judea Pearl,et al.  Probabilistic reasoning in intelligent systems - networks of plausible inference , 1991, Morgan Kaufmann series in representation and reasoning.

[14]  Catriel Beeri,et al.  Properties of acyclic database schemes , 1981, STOC '81.

[15]  R. Featherstone.  The Calculation of Robot Dynamics Using Articulated-Body Inertias , 1983 .

[16]  P. Heggernes,et al.  Finding Good Column Orderings for Sparse QR Factorization , 1996 .

[17]  Frank Dellaert,et al.  iSAM: Incremental Smoothing and Mapping , 2008, IEEE Transactions on Robotics.

[18]  Ruben Grandia,et al.  Feedback MPC for Torque-Controlled Legged Robots , 2019, 2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS).

[19]  J. Y. S. Luh,et al.  On-Line Computational Scheme for Mechanical Manipulators , 1980 .

[20]  G Rodriguez.  Recursive forward dynamics for two robot arms in a closed chain based on Kalman filtering and Bryson-Frazier smoothing , 1986 .

[21]  Martin L. Felis.  RBDL: an efficient rigid-body dynamics library using recursive algorithms , 2017, Auton. Robots.

[22]  Russ Tedrake,et al.  Whole-body motion planning with centroidal dynamics and full kinematics , 2014, 2014 IEEE-RAS International Conference on Humanoid Robots.

[23]  Byron Boots,et al.  STEAP: simultaneous trajectory estimation and planning , 2018, Autonomous Robots.

[24]  Roger W. Brockett,et al.  Robotic manipulators and the product of exponentials formula , 1984 .

[25]  Brendan J. Frey,et al.  Factor Graphs and Algorithms , 2008 .

[26]  Jonas Buchli,et al.  Fast Trajectory Optimization for Legged Robots Using Vertex-Based ZMP Constraints , 2017, IEEE Robotics and Automation Letters.

[27]  A. George,et al.  Row-ordering schemes for sparse givens transformations. II. implicit graph model , 1984 .

[28]  Siddhartha S. Srinivasa,et al.  DART: Dynamic Animation and Robotics Toolkit , 2018, J. Open Source Softw..

[29]  Richard M. Murray,et al.  A Mathematical Introduction to Robotic Manipulation , 1994 .

[30]  Ahmad A. Masoud,et al.  Kinodynamic Motion Planning , 2010, IEEE Robotics & Automation Magazine.

[31]  R. Blickhan.  The spring-mass model for running and hopping. , 1989, Journal of biomechanics.

[32]  David E. Orin,et al.  Efficient Dynamic Computer Simulation of Robotic Mechanisms , 1982 .

[33]  F. Dellaert.  Factor Graphs and GTSAM: A Hands-on Introduction , 2012 .

[34]  Frank Dellaert,et al.  Factor Graphs for Robot Perception , 2017, Found. Trends Robotics.

[35]  Robert Michael Tanner,et al.  A recursive approach to low complexity codes , 1981, IEEE Trans. Inf. Theory.

[36]  Umberto Bertelè,et al.  On the Theory of the Elimination Process , 1971 .

[37]  Frank Dellaert,et al.  iSAM2: Incremental smoothing and mapping using the Bayes tree , 2012, Int. J. Robotics Res..

[38]  Maurice Fallon,et al.  Robust Legged Robot State Estimation Using Factor Graph Optimization , 2019, IEEE Robotics and Automation Letters.

[39]  Surya P. N. Singh,et al.  V-REP: A versatile and scalable robot simulation framework , 2013, 2013 IEEE/RSJ International Conference on Intelligent Robots and Systems.

[40]  Zhihong Man,et al.  Dynamics modelling and linear control of quadcopter , 2016, 2016 International Conference on Advanced Mechatronic Systems (ICAMechS).

[41]  David E. Orin,et al.  Robot dynamics: equations and algorithms , 2000, Proceedings 2000 ICRA. Millennium Conference. IEEE International Conference on Robotics and Automation. Symposia Proceedings (Cat. No.00CH37065).

[42]  Nathan Goodman,et al.  Tree queries: a simple class of relational queries , 1982, TODS.

[43]  E. Ng,et al.  Predicting structure in nonsymmetric sparse matrix factorizations , 1993 .

[44]  Zoran Popovic,et al.  Discovery of complex behaviors through contact-invariant optimization , 2012, ACM Trans. Graph..

[45]  Abhinandan Jain,et al.  Graph theoretic foundations of multibody dynamics , 2011, Multibody system dynamics.

[46]  Russ Tedrake,et al.  Direct Trajectory Optimization of Rigid Body Dynamical Systems through Contact , 2012, WAFR.

[47]  Abhinandan Jain.  Unified formulation of dynamics for serial rigid multibody systems , 1991 .

[48]  Abhinandan Jain,et al.  A Spatial Operator Algebra for Manipulator Modeling and Control , 1989, Proceedings, 1989 International Conference on Robotics and Automation.

[49]  Maani Ghaffari Jadidi,et al.  Hybrid Contact Preintegration for Visual-Inertial-Contact State Estimation Using Factor Graphs , 2018, 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS).

[50]  Umberto Bertelè,et al.  Nonserial Dynamic Programming , 1972 .

[51]  David E. Orin,et al.  Kinematic and kinetic analysis of open-chain linkages utilizing Newton-Euler methods , 1979 .

[52]  Byron Boots,et al.  Continuous-time Gaussian process motion planning via probabilistic inference , 2017, Int. J. Robotics Res..

[53]  Raimund Seidel,et al.  A New Method for Solving Constraint Satisfaction Problems , 1981, IJCAI.

[54]  山田 祐,et al.  Open Dynamics Engine を用いたスノーボードロボットシミュレータの開発 , 2007 .

[55]  Brendan J. Frey,et al.  Factor graphs and the sum-product algorithm , 2001, IEEE Trans. Inf. Theory.

[56]  H.-A. Loeliger,et al.  An introduction to factor graphs , 2004, IEEE Signal Process. Mag..