regressor.hpp 12.3 KB
 jcarpent committed Jan 18, 2018 1 //  Justin Carpentier committed Nov 01, 2020 2 // Copyright (c) 2018-2020 CNRS INRIA  jcarpent committed Jan 18, 2018 3 4 //  Justin Carpentier committed Nov 01, 2020 5 6 #ifndef __pinocchio_algorithm_regressor_hpp__ #define __pinocchio_algorithm_regressor_hpp__  jcarpent committed Jan 18, 2018 7 8  #include "pinocchio/multibody/model.hpp"  jcarpent committed Jun 01, 2018 9 #include "pinocchio/multibody/data.hpp"  jcarpent committed Jan 18, 2018 10   Justin Carpentier committed Nov 08, 2018 11 namespace pinocchio  jcarpent committed Jan 18, 2018 12 {  Justin Carpentier committed Nov 01, 2020 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72  /// /// \brief Computes the kinematic regressor that links the joint placements variations of the whole kinematic tree /// to the placement variation of the frame rigidly attached to the joint and given by its placement w.r.t. to the joint frame. /// /// \remarks It assumes that the forwardKinematics has been performed first. /// /// \param[in] model The model structure of the rigid body system. /// \param[in] data The data structure of the rigid body system. /// \param[in] joint_id Index of the joint. /// \param[in] placement Relative placement to the joint frame. /// \param[in] rf Reference frame in which the result is expressed (LOCAL, LOCAL_WORLD_ALIGNED or WORLD). /// /// \param[out] kinematic_regressor The kinematic regressor containing the result. Matrix of size 6*(model.njoints-1) initialized to 0. /// template class JointCollectionTpl, typename Matrix6xReturnType> void computeJointKinematicRegressor(const ModelTpl & model, const DataTpl & data, const JointIndex joint_id, const ReferenceFrame rf, const SE3Tpl & placement, const Eigen::MatrixBase & kinematic_regressor); /// /// \brief Computes the kinematic regressor that links the joint placements variations of the whole kinematic tree /// to the placement variation of the joint given as input. /// /// \remarks It assumes that the forwardKinematics has been performed first. /// /// \param[in] model The model structure of the rigid body system. /// \param[in] data The data structure of the rigid body system. /// \param[in] joint_id Index of the joint. /// \param[in] rf Reference frame in which the result is expressed (LOCAL, LOCAL_WORLD_ALIGNED or WORLD). /// \param[out] kinematic_regressor The kinematic regressor containing the result. Matrix of size 6*(model.njoints-1) initialized to 0. /// template class JointCollectionTpl, typename Matrix6xReturnType> void computeJointKinematicRegressor(const ModelTpl & model, const DataTpl & data, const JointIndex joint_id, const ReferenceFrame rf, const Eigen::MatrixBase & kinematic_regressor); /// /// \brief Computes the kinematic regressor that links the joint placements variations of the whole kinematic tree /// to the placement variation of the frame given as input. /// /// \remarks It assumes that the forwardKinematics has been performed first. /// /// \param[in] model The model structure of the rigid body system. /// \param[in] data The data structure of the rigid body system. /// \param[in] frame_id Index of the frame. /// \param[in] rf Reference frame in which the result is expressed (LOCAL, LOCAL_WORLD_ALIGNED or WORLD). /// \param[out] kinematic_regressor The kinematic regressor containing the result. Matrix of size 6*(model.njoints-1) initialized to 0. /// template class JointCollectionTpl, typename Matrix6xReturnType> void computeFrameKinematicRegressor(const ModelTpl & model, const DataTpl & data, const FrameIndex frame_id, const ReferenceFrame rf, const Eigen::MatrixBase & kinematic_regressor);  Gabriele Buondonno committed Jun 21, 2019 73 74 75 76  /// /// \brief Computes the static regressor that links the center of mass positions of all the links /// to the center of mass of the complete model according to the current configuration of the robot. ///  Gabriele Buondonno committed Jun 21, 2019 77 78 79 80 81  /// The result is stored in data.staticRegressor and it corresponds to a matrix \f$Y \f$ such that /// \f$c = Y(q,\dot{q},\ddot{q})\tilde{\pi} \f$ /// where \f$\tilde{\pi} = (\tilde{\pi}_1^T\ \dots\ \tilde{\pi}_n^T)^T \f$ and /// \f$\tilde{\pi}_i = \text{model.inertias[i].toDynamicParameters().head<4>()} \f$ ///  Gabriele Buondonno committed Jun 21, 2019 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96  /// \tparam JointCollection Collection of Joint types. /// \tparam ConfigVectorType Type of the joint configuration vector. /// /// \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). /// /// \return The static regressor of the system. /// template class JointCollectionTpl, typename ConfigVectorType> inline typename DataTpl::Matrix3x & computeStaticRegressor(const ModelTpl & model, DataTpl & data, const Eigen::MatrixBase & q);  jcarpent committed Jan 18, 2018 97 98 99 100 101 102 103  namespace regressor { /// /// \brief Computes the static regressor that links the center of mass positions of all the links /// to the center of mass of the complete model according to the current configuration of the robot. ///  Justin Carpentier committed Oct 29, 2018 104 105 106  /// \tparam JointCollection Collection of Joint types. /// \tparam ConfigVectorType Type of the joint configuration vector. ///  jcarpent committed Jan 18, 2018 107 108 109 110 111 112  /// \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). /// /// \return The static regressor of the system. ///  Gabriele Buondonno committed Jun 21, 2019 113 114  /// \deprecated This function is now in the main pinocchio namespace ///  Justin Carpentier committed Oct 29, 2018 115  template class JointCollectionTpl, typename ConfigVectorType>  Gabriele Buondonno committed Jun 21, 2019 116  inline PINOCCHIO_DEPRECATED typename DataTpl::Matrix3x &  Justin Carpentier committed Oct 29, 2018 117 118  computeStaticRegressor(const ModelTpl & model, DataTpl & data,  Gabriele Buondonno committed Jun 21, 2019 119 120 121 122  const Eigen::MatrixBase & q) { return ::pinocchio::computeStaticRegressor(model,data,q); }  jcarpent committed Jan 18, 2018 123  }  Gabriele Buondonno committed Jun 21, 2019 124   Gabriele Buondonno committed Jun 21, 2019 125 126 127 128 129 130 131 132 133 134 135 136  /// /// \brief Computes the regressor for the dynamic parameters of a single rigid body. /// /// The result is such that /// \f$I a + v \times I v = bodyRegressor(v,a) * I.toDynamicParameters() \f$ /// /// \param[in] v Velocity of the rigid body /// \param[in] a Acceleration of the rigid body /// \param[out] regressor The resulting regressor of the body. /// template inline void  Justin Carpentier committed Nov 01, 2020 137 138 139  bodyRegressor(const MotionDense & v, const MotionDense & a, const Eigen::MatrixBase & regressor);  Gabriele Buondonno committed Jun 21, 2019 140   Gabriele Buondonno committed Jun 21, 2019 141 142 143 144 145 146 147 148 149 150 151 152  /// /// \brief Computes the regressor for the dynamic parameters of a single rigid body. /// /// The result is such that /// \f$I a + v \times I v = bodyRegressor(v,a) * I.toDynamicParameters() \f$ /// /// \param[in] v Velocity of the rigid body /// \param[in] a Acceleration of the rigid body /// /// \return The regressor of the body. /// template  Gabriele Buondonno committed Jun 21, 2019 153  inline Eigen::Matrix  Justin Carpentier committed Nov 01, 2020 154 155  bodyRegressor(const MotionDense & v, const MotionDense & a);  Gabriele Buondonno committed Jun 21, 2019 156 157  ///  Gabriele Buondonno committed Jun 21, 2019 158 159  /// \brief Computes the regressor for the dynamic parameters of a rigid body attached to a given joint, /// puts the result in data.bodyRegressor and returns it.  Gabriele Buondonno committed Jun 21, 2019 160 161 162 163  /// /// This algorithm assumes RNEA has been run to compute the acceleration and gravitational effects. /// /// The result is such that  Gabriele Buondonno committed Jun 21, 2019 164  /// \f$f = \text{jointBodyRegressor(model,data,jointId) * I.toDynamicParameters()} \f$  Gabriele Buondonno committed Jun 21, 2019 165 166 167 168 169 170 171 172 173  /// where \f$f \f$ is the net force acting on the body, including gravity /// /// \param[in] model The model structure of the rigid body system. /// \param[in] data The data structure of the rigid body system. /// \param[in] jointId The id of the joint. /// /// \return The regressor of the body. /// template class JointCollectionTpl>  Gabriele Buondonno committed Jun 21, 2019 174  inline typename DataTpl::BodyRegressorType &  Gabriele Buondonno committed Jun 21, 2019 175 176 177  jointBodyRegressor(const ModelTpl & model, DataTpl & data, JointIndex jointId);  Gabriele Buondonno committed Jun 21, 2019 178 179  ///  Gabriele Buondonno committed Jun 21, 2019 180 181  /// \brief Computes the regressor for the dynamic parameters of a rigid body attached to a given frame, /// puts the result in data.bodyRegressor and returns it.  Gabriele Buondonno committed Jun 21, 2019 182 183 184 185  /// /// This algorithm assumes RNEA has been run to compute the acceleration and gravitational effects. /// /// The result is such that  Gabriele Buondonno committed Jun 21, 2019 186  /// \f$f = \text{frameBodyRegressor(model,data,frameId) * I.toDynamicParameters()} \f$  Gabriele Buondonno committed Jun 21, 2019 187 188 189 190 191 192  /// where \f$f \f$ is the net force acting on the body, including gravity /// /// \param[in] model The model structure of the rigid body system. /// \param[in] data The data structure of the rigid body system. /// \param[in] frameId The id of the frame. ///  Justin Carpentier committed Nov 01, 2020 193  /// \return The dynamic regressor of the body.  Gabriele Buondonno committed Jun 21, 2019 194 195  /// template class JointCollectionTpl>  Gabriele Buondonno committed Jun 21, 2019 196  inline typename DataTpl::BodyRegressorType &  Gabriele Buondonno committed Jun 21, 2019 197 198 199  frameBodyRegressor(const ModelTpl & model, DataTpl & data, FrameIndex frameId);  Gabriele Buondonno committed Jun 21, 2019 200   Gabriele Buondonno committed Jun 21, 2019 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221  /// /// \brief Computes the joint torque regressor that links the joint torque /// to the dynamic parameters of each link according to the current the robot motion. /// /// The result is stored in data.jointTorqueRegressor and it corresponds to a matrix \f$Y \f$ such that /// \f$\tau = Y(q,\dot{q},\ddot{q})\pi \f$ /// where \f$\pi = (\pi_1^T\ \dots\ \pi_n^T)^T \f$ and \f$\pi_i = \text{model.inertias[i].toDynamicParameters()} \f$ /// /// \tparam JointCollection Collection of Joint types. /// \tparam ConfigVectorType Type of the joint configuration vector. /// \tparam TangentVectorType1 Type of the joint velocity vector. /// \tparam TangentVectorType2 Type of the joint acceleration vector. /// /// \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] a The joint acceleration vector (dim model.nv). /// /// \return The joint torque regressor of the system. ///  Gabriele Buondonno committed Jun 21, 2019 222 223  /// \warning This function writes temporary information in data.bodyRegressor. This means if you have valuable data in it it will be overwritten. ///  Gabriele Buondonno committed Jun 21, 2019 224 225 226 227 228 229 230 231  template class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2> inline typename DataTpl::MatrixXs & computeJointTorqueRegressor(const ModelTpl & model, DataTpl & data, const Eigen::MatrixBase & q, const Eigen::MatrixBase & v, const Eigen::MatrixBase & a);  Justin Carpentier committed Nov 08, 2018 232 } // namespace pinocchio  jcarpent committed Jan 18, 2018 233 234 235 236  /* --- Details -------------------------------------------------------------------- */ #include "pinocchio/algorithm/regressor.hxx"  Justin Carpentier committed Nov 01, 2020 237 #endif // ifndef __pinocchio_algorithm_regressor_hpp__