aba-derivatives.hpp 3.56 KB
 jcarpent committed Apr 03, 2018 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 ``````// // Copyright (c) 2018 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_aba_derivatives_hpp__ #define __se3_aba_derivatives_hpp__ #include "pinocchio/multibody/model.hpp" `````` jcarpent committed Jun 01, 2018 22 ``````#include "pinocchio/multibody/data.hpp" `````` jcarpent committed Apr 03, 2018 23 24 25 26 27 28 29 30 31 32 33 34 35 `````` namespace se3 { /// /// \brief The derivatives of the Articulated-Body algorithm. /// /// \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[out] aba_partial_dq Partial derivative of the generalized torque vector with respect to the joint configuration. /// \param[out] aba_partial_dv Partial derivative of the generalized torque vector with respect to the joint velocity. `````` jcarpent committed May 24, 2018 36 `````` /// \param[out] aba_partial_dtau Partial derivative of the generalized torque vector with respect to the joint torque. `````` jcarpent committed Apr 03, 2018 37 38 39 40 41 42 43 44 45 46 `````` /// /// \sa se3::aba /// inline void computeABADerivatives(const Model & model, Data & data, const Eigen::VectorXd & q, const Eigen::VectorXd & v, const Eigen::VectorXd & tau, Eigen::MatrixXd & aba_partial_dq, Eigen::MatrixXd & aba_partial_dv, `````` jcarpent committed Apr 03, 2018 47 `````` Data::RowMatrixXd & aba_partial_dtau); `````` jcarpent committed Apr 03, 2018 48 `````` `````` jcarpent committed May 24, 2018 49 50 51 52 53 54 55 56 57 58 `````` /// /// \brief The derivatives of the Articulated-Body algorithm. /// /// \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). /// /// \returns The results are stored in data.ddq_dq, data.ddq_dv and data.Minv which respectively correspond `````` jcarpent committed May 24, 2018 59 `````` /// to the partial derivatives of the joint acceleration vector with respect to the joint configuration, velocity and torque. `````` jcarpent committed May 24, 2018 60 61 62 63 64 65 66 67 68 69 70 71 72 `````` /// And as for se3::computeMinverse, only the upper triangular part of data.Minv is filled. /// /// \sa se3::aba and \sa se3::computeABADerivatives. /// inline void computeABADerivatives(const Model & model, Data & data, const Eigen::VectorXd & q, const Eigen::VectorXd & v, const Eigen::VectorXd & tau) { computeABADerivatives(model,data,q,v,tau, data.ddq_dq,data.ddq_dv,data.Minv); } `````` jcarpent committed Apr 03, 2018 73 74 75 76 77 78 79 `````` } // namespace se3 /* --- Details -------------------------------------------------------------------- */ #include "pinocchio/algorithm/aba-derivatives.hxx" #endif // ifndef __se3_aba_derivatives_hpp__``````