Commit de18e056 by Justin Carpentier

### Merge pull request #132 from jcarpent/devel

Add forward dynamics with contacts algorithm
parents 9d42031d 3c047ae3
 ... ... @@ -144,6 +144,7 @@ SET(${PROJECT_NAME}_ALGORITHM_HEADERS algorithm/cholesky.hxx algorithm/kinematics.hpp algorithm/kinematics.hxx algorithm/dynamics.hpp algorithm/center-of-mass.hpp algorithm/center-of-mass.hxx algorithm/non-linear-effects.hpp ... ...  Subproject commit 7cdd2146508a3431adb8e5a0d4233704080299cd Subproject commit 9f9a90007704d679c631431ab36189f7ba61f323  ... ... @@ -40,6 +40,8 @@ namespace se3 /// \param[in] model The model structure of the rigid body system. /// \param[in] data The data structure of the rigid body system. /// /// \return A reference to the upper triangular matrix \f$U\f$. /// inline const Eigen::MatrixXd & decompose(const Model & model, Data & data); ... ... @@ -54,7 +56,7 @@ namespace se3 /// /// \param[in] model The model structure of the rigid body system. /// \param[in] data The data structure of the rigid body system. /// \param[inout] y The input vector to inverse which also contains the result \f$x\f$of the inversion. /// \param[inout] y The input matrix to inverse which also contains the result \f$x\f$of the inversion. /// template Mat & solve(const Model & model, const Data & data, ... ... @@ -65,7 +67,7 @@ namespace se3 /// /// \param[in] model The model structure of the rigid body system. /// \param[in] data The data structure of the rigid body system. /// \param[inout] v The input vector to multiply with data.M and storing the result. /// \param[inout] v The input matrix to multiply with data.M and storing the result. /// \param[in] usingCholesky If true, use the Cholesky decomposition stored in data. Exploit the sparsity of the kinematic tree. /// /// \return A reference to the result of \f$ Mv \f$. ... ... @@ -80,7 +82,7 @@ namespace se3 /// /// \param[in] model The model structure of the rigid body system. /// \param[in] data The data structure of the rigid body system. /// \param[inout] v The input vector to multiply with data.U and also storing the result. /// \param[inout] v The input matrix to multiply with data.U and also storing the result. /// /// \return A reference to the result of \f$ Uv \f$stored in v. /// ... ... @@ -94,7 +96,7 @@ namespace se3 /// /// \param[in] model The model structure of the rigid body system. /// \param[in] data The data structure of the rigid body system. /// \param[inout] v The input vector to multiply with data.U.tranpose() and also storing the result. /// \param[inout] v The input matrix to multiply with data.U.tranpose() and also storing the result. /// /// \return A reference to the result of \f$ U^{\top}v \f$stored in v. /// ... ... @@ -108,7 +110,7 @@ namespace se3 /// /// \param[in] model The model structure of the rigid body system. /// \param[in] data The data structure of the rigid body system. /// \param[inout] v The input vector to multiply with data.U^{-1} and also storing the result. /// \param[inout] v The input matrix to multiply with data.U^{-1} and also storing the result. /// /// \return A reference to the result of \f$ U^{-1}v \f$stored in v. /// ... ... @@ -124,7 +126,7 @@ namespace se3 /// /// \param[in] model The model structure of the rigid body system. /// \param[in] data The data structure of the rigid body system. /// \param[inout] v The input vector to multiply with data.U^{-\top} and also storing the result. /// \param[inout] v The input matrix to multiply with data.U^{-\top} and also storing the result. /// /// \return A reference to the result of \f$ U^{-\top}v \f$stored in v. /// ... ... @@ -140,7 +142,7 @@ namespace se3 /// /// \param[in] model The model structure of the rigid body system. /// \param[in] data The data structure of the rigid body system. /// \param[inout] v The input vector to multiply with data.M^{-1} and also storing the result. /// \param[inout] v The input matrix to multiply with data.M^{-1} and also storing the result. /// /// \return A reference to the result of \f$ M^{-1}v \f$stored in v. /// ... ...  ... ... @@ -71,13 +71,13 @@ namespace se3 const Data & data, Eigen::MatrixBase & v) { assert(v.size() == model.nv); EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat); assert(v.rows() == model.nv); const Eigen::MatrixXd & U = data.U; const std::vector & nvt = data.nvSubtree_fromRow; for(int k=0;k < model.nv-1;++k) // You can stop one step before nv v[k] += U.row(k).segment(k+1,nvt[(Model::Index)k]-1) * v.segment(k+1,nvt[(Model::Index)k]-1); v.row(k) += U.row(k).segment(k+1,nvt[(Model::Index)k]-1) * v.middleRows(k+1,nvt[(Model::Index)k]-1); return v.derived(); } ... ... @@ -88,13 +88,12 @@ namespace se3 const Data & data, Eigen::MatrixBase & v) { assert(v.size() == model.nv); EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat); assert(v.rows() == model.nv); const Eigen::MatrixXd & U = data.U; const std::vector & nvt = data.nvSubtree_fromRow; for( int i=model.nv-2;i>=0;--i ) // You can start from nv-2 (no child in nv-1) v.segment(i+1,nvt[(Model::Index)i]-1) += U.row(i).segment(i+1,nvt[(Model::Index)i]-1).transpose()*v[i]; for( int k=model.nv-2;k>=0;--k ) // You can start from nv-2 (no child in nv-1) v.middleRows(k+1,nvt[(Model::Index)k]-1) += U.row(k).segment(k+1,nvt[(Model::Index)k]-1).transpose()*v.row(k); return v.derived(); } ... ... @@ -109,14 +108,13 @@ namespace se3 { /* We search y s.t. v = U y. * For any k, v_k = y_k + U_{k,k+1:} y_{k+1:} */ assert(v.size() == model.nv); EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat); assert(v.rows() == model.nv); const Eigen::MatrixXd & U = data.U; const std::vector & nvt = data.nvSubtree_fromRow; for( int k=model.nv-2;k>=0;--k ) // You can start from nv-2 (no child in nv-1) v[k] -= U.row(k).segment(k+1,nvt[(Model::Index)k]-1) * v.segment(k+1,nvt[(Model::Index)k]-1); v.row(k) -= U.row(k).segment(k+1,nvt[(Model::Index)k]-1) * v.middleRows(k+1,nvt[(Model::Index)k]-1); return v.derived(); } ... ... @@ -127,13 +125,12 @@ namespace se3 { /* We search y s.t. v = U' y. * For any k, v_k = y_k + sum_{m \in parent{k}} U(m,k) v(k). */ assert(v.size() == model.nv); EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat); assert(v.rows() == model.nv); const Eigen::MatrixXd & U = data.U; const std::vector & nvt = data.nvSubtree_fromRow; for( int i=0;i & v) { assert(v.size() == model.nv); EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat); assert(v.rows() == model.nv); const Eigen::MatrixXd & M = data.M; const std::vector & nvt = data.nvSubtree_fromRow; ... ... @@ -154,8 +150,8 @@ namespace se3 for(int k=model.nv-1;k>=0;--k) { res[k] = M.row(k).segment(k,nvt[(Model::Index)k]) * v.segment(k,nvt[(Model::Index)k]); res.segment(k+1,nvt[(Model::Index)k]-1) += M.row(k).segment(k+1,nvt[(Model::Index)k]-1).transpose()*v[k]; res.row(k) = M.row(k).segment(k,nvt[(Model::Index)k]) * v.middleRows(k,nvt[(Model::Index)k]); res.middleRows(k+1,nvt[(Model::Index)k]-1) += M.row(k).segment(k+1,nvt[(Model::Index)k]-1).transpose()*v.row(k); } return res; ... ... @@ -167,7 +163,7 @@ namespace se3 Eigen::MatrixBase & v) { Utv(model,data,v); for( int k=0;k & v) { Uiv(model,data,v); for(int k=0;k  // // Copyright (c) 2016 CNRS // // 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" #include "pinocchio/simulation/compute-all-terms.hpp" #include "pinocchio/algorithm/cholesky.hpp" #include 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. /// /// \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). /// \param[in] updateKinematics If true, the algorithm calls first se3::computeAllTerms. Otherwise, it uses the current dynamic values stored in data. /// /// \return A reference to the joint acceleration stored in data.ddq. The Lagrange Multipliers linked to the contact forces are available throw data.lambda vector. /// 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, 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()); Eigen::VectorXd & a = data.ddq; Eigen::VectorXd & lambda = data.lambda; 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  ... ... @@ -29,6 +29,7 @@ #include "pinocchio/multibody/joint/joint-variant.hpp" #include "pinocchio/tools/string-generator.hpp" #include #include EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(se3::SE3) EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(se3::Inertia) ... ... @@ -472,6 +473,23 @@ namespace se3 /// \brief Potential energy of the model. double potential_energy; // Temporary variables used in forward dynamics /// \brief Inverse of the operational-space inertia matrix Eigen::MatrixXd JMinvJt; /// \brief Cholesky decompostion of \JMinvJt. Eigen::LLT llt_JMinvJt; /// \brief Lagrange Multipliers corresponding to contact forces. Eigen::VectorXd lambda; /// \brief Temporary corresponding to \f$ \sqrt{D} U^{-1} J^{\top} \f$. Eigen::MatrixXd sDUiJt; /// \brief Temporary corresponding to the residual torque \f$ \tau - b(q,\dot{q}) \f\$. Eigen::VectorXd torque_residual; /// /// \brief Default constructor of se3::Data from a se3::Model. /// ... ...
 ... ... @@ -279,6 +279,11 @@ namespace se3 ,velocityLimit(ref.nv) ,lowerPositionLimit(ref.nq) ,upperPositionLimit(ref.nq) ,JMinvJt() ,llt_JMinvJt() ,lambda() ,sDUiJt(ref.nv,ref.nv) ,torque_residual(ref.nv) { /* Create data strcture associated to the joints */ for(Model::Index i=0;i<(Model::JointIndex)(model.nbody);++i) ... ...
 ... ... @@ -28,6 +28,7 @@ #include "pinocchio/algorithm/non-linear-effects.hpp" #include "pinocchio/algorithm/crba.hpp" #include "pinocchio/algorithm/aba.hpp" #include "pinocchio/algorithm/dynamics.hpp" #include "pinocchio/algorithm/kinematics.hpp" #include "pinocchio/algorithm/jacobian.hpp" #include "pinocchio/algorithm/operational-frames.hpp" ... ... @@ -94,6 +95,19 @@ namespace se3 aba(*model,*data,q,v,tau); return data->ddq; } static Eigen::MatrixXd fd_llt_proxy(const ModelHandler & model, DataHandler & data, const VectorXd_fx & q, const VectorXd_fx & v, const VectorXd_fx & tau, const eigenpy::MatrixXd_fx & J, const VectorXd_fx & gamma, const bool update_kinematics = true) { forwardDynamics(*model,*data,q,v,tau,J,gamma,update_kinematics); return data->ddq; } static SE3::Vector3 com_0_proxy(const ModelHandler& model, ... ... @@ -327,6 +341,16 @@ namespace se3 "Joint torque tau (size Model::nv)"), "Compute ABA, put the result in Data::ddq and return it."); bp::def("forwardDynamics",fd_llt_proxy, bp::args("Model","Data", "Joint configuration q (size Model::nq)", "Joint velocity v (size Model::nv)", "Joint torque tau (size Model::nv)", "Contact Jacobian J (size nb_constraint * Model::nv)", "Contact drift gamma (size nb_constraint)", "Update kinematics (if true, it updates the dynamic variable according to the current state)"), "Solve the forward dynamics problem with contacts, put the result in Data::ddq and return it."); bp::def("centerOfMass",com_0_proxy, bp::args("Model","Data", "Configuration q (size Model::nq)", ... ...
 ... ... @@ -112,6 +112,8 @@ namespace se3 .ADD_DATA_PROPERTY_CONST(double,kinetic_energy,"Kinetic energy in [J] computed by kineticEnergy(model,data,q,v,True/False)") .ADD_DATA_PROPERTY_CONST(double,potential_energy,"Potential energy in [J] computed by potentialEnergy(model,data,q,True/False)") .ADD_DATA_PROPERTY_CONST(Eigen::VectorXd,lambda,"Lagrange Multipliers linked to contact forces") ; } ... ... @@ -159,6 +161,8 @@ namespace se3 IMPL_DATA_PROPERTY_CONST(double,kinetic_energy,"Kinetic energy in [J] computed by kineticEnergy(model,data,q,v,True/False)") IMPL_DATA_PROPERTY_CONST(double,potential_energy,"Potential energy in [J] computed by potentialEnergy(model,data,q,True/False)") IMPL_DATA_PROPERTY_CONST(Eigen::VectorXd,lambda,"Lagrange Multipliers linked to contact forces") /* --- Expose --------------------------------------------------------- */ static void expose() { ... ...
 // // Copyright (c) 2015 CNRS // Copyright (c) 2015-2016 CNRS // // This file is part of Pinocchio // Pinocchio is free software: you can redistribute it ... ... @@ -109,9 +109,9 @@ namespace se3 return derived().__equal__(other); } bool isApprox (const Derived_t & other) const bool isApprox (const Derived_t & other, const Scalar_t & prec = Eigen::NumTraits::dummy_precision()) const { return derived().isApprox_impl(other); return derived().isApprox_impl(other, prec); } friend std::ostream & operator << (std::ostream & os,const SE3Base & X) ... ... @@ -164,12 +164,15 @@ namespace se3 SE3Tpl(const Eigen::MatrixBase & R, const Eigen::MatrixBase & p) : rot(R), trans(p) { EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(v3,3) EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M3,3,3) } template SE3Tpl(const Eigen::MatrixBase & m) : rot(m.template block<3,3>(LINEAR,LINEAR)), trans(m.template block<3,1>(LINEAR,ANGULAR)) { EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M4,4,4); } SE3Tpl(int) : rot(Matrix3::Identity()), trans(Vector3::Zero()) {} ... ... @@ -216,8 +219,7 @@ namespace se3 return *this; } public: Matrix4 toHomogeneousMatrix_impl() const { Matrix4 M; ... ... @@ -244,7 +246,6 @@ namespace se3 return M; } void disp_impl(std::ostream & os) const { os << " R =\n" << rot << std::endl ... ... @@ -265,8 +266,8 @@ namespace se3 return d.se3ActionInverse(*this); } Vector3 act_impl (const Vector3& p) const { return (rot*p+trans).eval(); } Vector3 actInv_impl(const Vector3& p) const { return (rot.transpose()*(p-trans)).eval(); } Vector3 act_impl (const Vector3& p) const { return rot*p+trans; } Vector3 actInv_impl(const Vector3& p) const { return rot.transpose()*(p-trans); } SE3Tpl act_impl (const SE3Tpl& m2) const { return SE3Tpl( rot*m2.rot,trans+rot*m2.trans);} SE3Tpl actInv_impl (const SE3Tpl& m2) const { return SE3Tpl( rot.transpose()*m2.rot, rot.transpose()*(m2.trans-trans));} ... ... @@ -279,15 +280,11 @@ namespace se3 return (rotation_impl() == m2.rotation() && translation_impl() == m2.translation()); } bool isApprox_impl( const SE3Tpl & m2 ) const bool isApprox_impl (const SE3Tpl & m2, const Scalar_t & prec = Eigen::NumTraits::dummy_precision()) const { Matrix4 diff( toHomogeneousMatrix_impl() - m2.toHomogeneousMatrix_impl()); return (diff.isMuchSmallerThan(toHomogeneousMatrix_impl(), 1e-14) && diff.isMuchSmallerThan(m2.toHomogeneousMatrix_impl(), 1e-14) ); return rot.isApprox(m2.rot, prec) && trans.isApprox(m2.trans, prec); } public: const Angular_t & rotation_impl() const { return rot; } Angular_t & rotation_impl() { return rot; } void rotation_impl(const Angular_t & R) { rot = R; } ... ...