dynamics.hpp 7.95 KB
 jcarpent committed Mar 17, 2016 1 //  jcarpent committed Jun 01, 2018 2 // Copyright (c) 2016-2018 CNRS  jcarpent committed Mar 17, 2016 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 // // This file is part of Pinocchio // Pinocchio is free software: you can redistribute it // and/or modify it under the terms of the GNU Lesser General Public // License as published by the Free Software Foundation, either version // 3 of the License, or (at your option) any later version. // // Pinocchio is distributed in the hope that it will be // useful, but WITHOUT ANY WARRANTY; without even the implied warranty // of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU // General Lesser Public License for more details. You should have // received a copy of the GNU Lesser General Public License along with // Pinocchio If not, see // . #ifndef __se3_dynamics_hpp__ #define __se3_dynamics_hpp__ #include "pinocchio/multibody/model.hpp"  jcarpent committed Jun 01, 2018 22 #include "pinocchio/multibody/data.hpp"  jcarpent committed Apr 09, 2016 23 #include "pinocchio/algorithm/compute-all-terms.hpp"  jcarpent committed Mar 17, 2016 24 #include "pinocchio/algorithm/cholesky.hpp"  jcarpent committed Apr 03, 2016 25 #include "pinocchio/algorithm/crba.hpp"  jcarpent committed Mar 30, 2017 26 #include "pinocchio/algorithm/check.hpp"  jcarpent committed Mar 17, 2016 27 28  #include  jcarpent committed Mar 30, 2017 29   jcarpent committed Mar 17, 2016 30 31 32 33 34 35 36 37 38 39 namespace se3 { /// /// \brief Compute the forward dynamics with contact constraints. /// \note It computes the following problem:
\f$\begin{eqnarray} \underset{\ddot{q}}{\min} & & \| \ddot{q} - \ddot{q}_{\text{free}} \|_{M(q)} \\ /// \text{s.t.} & & J (q) \ddot{q} + \gamma (q, \dot{q}) = 0 \end{eqnarray} \f$
/// where \f$\ddot{q}_{\text{free}} \f$ is the free acceleration (i.e. without constraints), /// \f$M \f$ is the mass matrix, \f$J \f$ the constraint Jacobian and \f$\gamma \f$ is the constraint drift.  Rohan Budhiraja committed Aug 09, 2018 40  /// By default, the constraint Jacobian is assumed to be full rank, and undamped Cholesky inverse is performed.  jcarpent committed Mar 17, 2016 41 42 43 44 45 46 47 48  /// /// \param[in] model The model structure of the rigid body system. /// \param[in] data The data structure of the rigid body system. /// \param[in] q The joint configuration (vector dim model.nq). /// \param[in] v The joint velocity (vector dim model.nv). /// \param[in] tau The joint torque vector (dim model.nv). /// \param[in] J The Jacobian of the constraints (dim nb_constraints*model.nv). /// \param[in] gamma The drift of the constraints (dim nb_constraints).  Rohan Budhiraja committed Aug 09, 2018 49  /// \param[in] inv_damping Damping factor for cholesky decomposition of JMinvJt. Set to zero if constraints are full rank.  Rohan Budhiraja committed Aug 09, 2018 50 51  /// \param[in] updateKinematics If true, the algorithm calls first se3::computeAllTerms. Otherwise, it uses the current dynamic values stored in data. \\ /// \note A hint: 1e-12 as the damping factor gave good result in the particular case of redundancy in contact constraints on the two feet.  jcarpent committed Mar 17, 2016 52  ///  Guilhem Saurel committed Mar 25, 2016 53  /// \return A reference to the joint acceleration stored in data.ddq. The Lagrange Multipliers linked to the contact forces are available throw data.lambda_c vector.  jcarpent committed Mar 17, 2016 54 55 56 57 58 59 60 61  /// inline const Eigen::VectorXd & forwardDynamics(const Model & model, Data & data, const Eigen::VectorXd & q, const Eigen::VectorXd & v, const Eigen::VectorXd & tau, const Eigen::MatrixXd & J, const Eigen::VectorXd & gamma,  62  const double inv_damping = 0.,  jcarpent committed Mar 17, 2016 63 64 65 66 67 68 69 70  const bool updateKinematics = true ) { assert(q.size() == model.nq); assert(v.size() == model.nv); assert(tau.size() == model.nv); assert(J.cols() == model.nv); assert(J.rows() == gamma.size());  jcarpent committed Mar 30, 2017 71  assert(model.check(data) && "data is not consistent with model.");  jcarpent committed Mar 17, 2016 72 73  Eigen::VectorXd & a = data.ddq;  Guilhem Saurel committed Mar 25, 2016 74  Eigen::VectorXd & lambda_c = data.lambda_c;  jcarpent committed Mar 17, 2016 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91  if (updateKinematics) computeAllTerms(model, data, q, v); // Compute the UDUt decomposition of data.M cholesky::decompose(model, data); // Compute the dynamic drift (control - nle) data.torque_residual = tau - data.nle; cholesky::solve(model, data, data.torque_residual); data.sDUiJt = J.transpose(); // Compute U^-1 * J.T cholesky::Uiv(model, data, data.sDUiJt); for(int k=0;k  Guilhem Saurel committed Apr 04, 2016 111  ///
\f$\begin{eqnarray} \underset{\dot{q}^{+}}{\min} & & \| \dot{q}^{+} - \dot{q}^{-} \|_{M(q)} \\  jcarpent committed Apr 03, 2016 112  /// \text{s.t.} & & J (q) \dot{q}^{+} = - \epsilon J (q) \dot{q}^{-} \end{eqnarray} \f$
 Guilhem Saurel committed Apr 04, 2016 113  /// where \f$\dot{q}^{-} \f$ is the generalized velocity before impact,  jcarpent committed Apr 03, 2016 114 115 116 117 118 119 120  /// \f$M \f$ is the joint space mass matrix, \f$J \f$ the constraint Jacobian and \f$\epsilon \f$ is the coefficient of restitution (1 for a fully elastic impact or 0 for a rigid impact). /// /// \param[in] model The model structure of the rigid body system. /// \param[in] data The data structure of the rigid body system. /// \param[in] q The joint configuration (vector dim model.nq). /// \param[in] v_before The joint velocity before impact (vector dim model.nv). /// \param[in] J The Jacobian of the constraints (dim nb_constraints*model.nv).  jcarpent committed Apr 04, 2016 121  /// \param[in] r_coeff The coefficient of restitution. Must be in [0;1].  jcarpent committed Apr 03, 2016 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137  /// \param[in] updateKinematics If true, the algorithm calls first se3::crba. Otherwise, it uses the current mass matrix value stored in data. /// /// \return A reference to the generalized velocity after impact stored in data.dq_after. The Lagrange Multipliers linked to the contact impulsed are available throw data.impulse_c vector. /// inline const Eigen::VectorXd & impulseDynamics(const Model & model, Data & data, const Eigen::VectorXd & q, const Eigen::VectorXd & v_before, const Eigen::MatrixXd & J, const double r_coeff = 0., const bool updateKinematics = true ) { assert(q.size() == model.nq); assert(v_before.size() == model.nv); assert(J.cols() == model.nv);  jcarpent committed Mar 30, 2017 138  assert(model.check(data) && "data is not consistent with model.");  jcarpent committed Apr 03, 2016 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168  Eigen::VectorXd & impulse_c = data.impulse_c; Eigen::VectorXd & dq_after = data.dq_after; // Compute the mass matrix if (updateKinematics) crba(model, data, q); // Compute the UDUt decomposition of data.M cholesky::decompose(model, data); data.sDUiJt = J.transpose(); // Compute U^-1 * J.T cholesky::Uiv(model, data, data.sDUiJt); for(int k=0;k