| Directory: | ./ |
|---|---|
| File: | include/pinocchio/algorithm/model.hpp |
| Date: | 2024-08-27 18:20:05 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 8 | 8 | 100.0% |
| Branches: | 3 | 6 | 50.0% |
| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | // | ||
| 2 | // Copyright (c) 2019 CNRS INRIA | ||
| 3 | // | ||
| 4 | |||
| 5 | #ifndef __pinocchio_algorithm_model_hpp__ | ||
| 6 | #define __pinocchio_algorithm_model_hpp__ | ||
| 7 | |||
| 8 | #include "pinocchio/multibody/model.hpp" | ||
| 9 | #include "pinocchio/multibody/geometry.hpp" | ||
| 10 | |||
| 11 | namespace pinocchio | ||
| 12 | { | ||
| 13 | /** | ||
| 14 | * \brief Append a child model into a parent model, after a specific frame given by its index. | ||
| 15 | * | ||
| 16 | * \param[in] modelA the parent model. | ||
| 17 | * \param[in] modelB the child model. | ||
| 18 | * \param[in] frameInModelA index of the frame of modelA where to append modelB. | ||
| 19 | * \param[in] aMb pose of modelB universe joint (index 0) in frameInModelA. | ||
| 20 | * \param[out] model the resulting model. | ||
| 21 | * | ||
| 22 | * The order of the joints in the output models are | ||
| 23 | * - joints of modelA up to the parent of FrameInModelA, | ||
| 24 | * - all the descendents of parent of FrameInModelA, | ||
| 25 | * - the remaining joints of modelA. | ||
| 26 | */ | ||
| 27 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
| 28 | void appendModel( | ||
| 29 | const ModelTpl<Scalar, Options, JointCollectionTpl> & modelA, | ||
| 30 | const ModelTpl<Scalar, Options, JointCollectionTpl> & modelB, | ||
| 31 | const FrameIndex frameInModelA, | ||
| 32 | const SE3Tpl<Scalar, Options> & aMb, | ||
| 33 | ModelTpl<Scalar, Options, JointCollectionTpl> & model); | ||
| 34 | |||
| 35 | /** | ||
| 36 | * \brief Append a child model into a parent model, after a specific frame given by its index. | ||
| 37 | * | ||
| 38 | * \param[in] modelA the parent model. | ||
| 39 | * \param[in] modelB the child model. | ||
| 40 | * \param[in] frameInModelA index of the frame of modelA where to append modelB. | ||
| 41 | * \param[in] aMb pose of modelB universe joint (index 0) in frameInModelA. | ||
| 42 | * | ||
| 43 | * \return A new model containing the fusion of modelA and modelB. | ||
| 44 | * | ||
| 45 | * The order of the joints in the output models are | ||
| 46 | * - joints of modelA up to the parent of FrameInModelA, | ||
| 47 | * - all the descendents of parent of FrameInModelA, | ||
| 48 | * - the remaining joints of modelA. | ||
| 49 | */ | ||
| 50 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
| 51 | 2 | ModelTpl<Scalar, Options, JointCollectionTpl> appendModel( | |
| 52 | const ModelTpl<Scalar, Options, JointCollectionTpl> & modelA, | ||
| 53 | const ModelTpl<Scalar, Options, JointCollectionTpl> & modelB, | ||
| 54 | const FrameIndex frameInModelA, | ||
| 55 | const SE3Tpl<Scalar, Options> & aMb) | ||
| 56 | { | ||
| 57 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
| 58 | 2 | Model model; | |
| 59 | |||
| 60 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | appendModel(modelA, modelB, frameInModelA, aMb, model); |
| 61 | |||
| 62 | 2 | return model; | |
| 63 | } | ||
| 64 | |||
| 65 | /** | ||
| 66 | * \brief Append a child model into a parent model, after a specific frame given by its index. | ||
| 67 | * | ||
| 68 | * \param[in] modelA the parent model. | ||
| 69 | * \param[in] modelB the child model. | ||
| 70 | * \param[in] geomModelA the parent geometry model. | ||
| 71 | * \param[in] geomModelB the child geometry model. | ||
| 72 | * \param[in] frameInModelA index of the frame of modelA where to append modelB. | ||
| 73 | * \param[in] aMb pose of modelB universe joint (index 0) in frameInModelA. | ||
| 74 | * \param[out] model the resulting model. | ||
| 75 | * \param[out] geomModel the resulting geometry model. | ||
| 76 | */ | ||
| 77 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
| 78 | void appendModel( | ||
| 79 | const ModelTpl<Scalar, Options, JointCollectionTpl> & modelA, | ||
| 80 | const ModelTpl<Scalar, Options, JointCollectionTpl> & modelB, | ||
| 81 | const GeometryModel & geomModelA, | ||
| 82 | const GeometryModel & geomModelB, | ||
| 83 | const FrameIndex frameInModelA, | ||
| 84 | const SE3Tpl<Scalar, Options> & aMb, | ||
| 85 | ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
| 86 | GeometryModel & geomModel); | ||
| 87 | |||
| 88 | /** | ||
| 89 | * | ||
| 90 | * \brief Build a reduced model from a given input model and a list of joint to lock. | ||
| 91 | * | ||
| 92 | * \param[in] model the input model to reduce. | ||
| 93 | * \param[in] list_of_joints_to_lock list of joints to lock in the input model. | ||
| 94 | * \param[in] reference_configuration reference configuration. | ||
| 95 | * \param[out] reduced_model the reduced model. | ||
| 96 | * | ||
| 97 | * \remarks All the joints that have been set to be fixed in the new reduced_model now appear in | ||
| 98 | * the kinematic tree as a Frame as FIXED_JOINT. | ||
| 99 | * | ||
| 100 | * \todo At the moment, the joint and geometry order is kept while the frames | ||
| 101 | * are re-ordered in a hard to predict way. Their order could be kept. | ||
| 102 | * | ||
| 103 | */ | ||
| 104 | template< | ||
| 105 | typename Scalar, | ||
| 106 | int Options, | ||
| 107 | template<typename, int> | ||
| 108 | class JointCollectionTpl, | ||
| 109 | typename ConfigVectorType> | ||
| 110 | void buildReducedModel( | ||
| 111 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
| 112 | std::vector<JointIndex> list_of_joints_to_lock, | ||
| 113 | const Eigen::MatrixBase<ConfigVectorType> & reference_configuration, | ||
| 114 | ModelTpl<Scalar, Options, JointCollectionTpl> & reduced_model); | ||
| 115 | |||
| 116 | /** | ||
| 117 | * | ||
| 118 | * \brief Build a reduced model from a given input model and a list of joint to lock. | ||
| 119 | * | ||
| 120 | * \param[in] model the input model to reduce. | ||
| 121 | * \param[in] list_of_joints_to_lock list of joints to lock in the input model. | ||
| 122 | * \param[in] reference_configuration reference configuration. | ||
| 123 | * | ||
| 124 | * \returns A reduce model of the input model. | ||
| 125 | * | ||
| 126 | * \remarks All the joints that have been set to be fixed in the new reduced_model now appear in | ||
| 127 | * the kinematic tree as a Frame as FIXED_JOINT. | ||
| 128 | * | ||
| 129 | */ | ||
| 130 | template< | ||
| 131 | typename Scalar, | ||
| 132 | int Options, | ||
| 133 | template<typename, int> | ||
| 134 | class JointCollectionTpl, | ||
| 135 | typename ConfigVectorType> | ||
| 136 | 8 | ModelTpl<Scalar, Options, JointCollectionTpl> buildReducedModel( | |
| 137 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
| 138 | const std::vector<JointIndex> & list_of_joints_to_lock, | ||
| 139 | const Eigen::MatrixBase<ConfigVectorType> & reference_configuration) | ||
| 140 | { | ||
| 141 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
| 142 | 8 | Model reduced_model; | |
| 143 | |||
| 144 |
2/4✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
|
8 | buildReducedModel(model, list_of_joints_to_lock, reference_configuration, reduced_model); |
| 145 | |||
| 146 | 8 | return reduced_model; | |
| 147 | } | ||
| 148 | |||
| 149 | /** | ||
| 150 | * | ||
| 151 | * \brief Build a reduced model and a rededuced geometry model from a given input model, a given | ||
| 152 | * input geometry model and a list of joint to lock. | ||
| 153 | * | ||
| 154 | * \param[in] model the input model to reduce. | ||
| 155 | * \param[in] geom_model the input geometry model to reduce. | ||
| 156 | * \param[in] list_of_joints_to_lock list of joints to lock in the input model. | ||
| 157 | * \param[in] reference_configuration reference configuration. | ||
| 158 | * \param[out] reduced_model the reduced model. | ||
| 159 | * \param[out] reduced_geom_model the reduced geometry model. | ||
| 160 | * | ||
| 161 | * \remarks All the joints that have been set to be fixed in the new reduced_model now appear in | ||
| 162 | * the kinematic tree as a Frame as FIXED_JOINT. | ||
| 163 | * | ||
| 164 | */ | ||
| 165 | template< | ||
| 166 | typename Scalar, | ||
| 167 | int Options, | ||
| 168 | template<typename, int> | ||
| 169 | class JointCollectionTpl, | ||
| 170 | typename ConfigVectorType> | ||
| 171 | void buildReducedModel( | ||
| 172 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
| 173 | const GeometryModel & geom_model, | ||
| 174 | const std::vector<JointIndex> & list_of_joints_to_lock, | ||
| 175 | const Eigen::MatrixBase<ConfigVectorType> & reference_configuration, | ||
| 176 | ModelTpl<Scalar, Options, JointCollectionTpl> & reduced_model, | ||
| 177 | GeometryModel & reduced_geom_model); | ||
| 178 | |||
| 179 | /** | ||
| 180 | * | ||
| 181 | * \brief Build a reduced model and a rededuced geometry model from a given input model, a given | ||
| 182 | * input geometry model and a list of joint to lock. | ||
| 183 | * | ||
| 184 | * \param[in] model the input model to reduce. | ||
| 185 | * \param[in] list_of_geom_models the input geometry model to reduce (example: visual_model, | ||
| 186 | * collision_model). \param[in] list_of_joints_to_lock list of joints to lock in the input model. | ||
| 187 | * \param[in] reference_configuration reference configuration. | ||
| 188 | * \param[out] reduced_model the reduced model. | ||
| 189 | * \param[out] list_of_reduced_geom_model the list of reduced geometry models. | ||
| 190 | * | ||
| 191 | * \remarks All the joints that have been set to be fixed in the new reduced_model now appear in | ||
| 192 | * the kinematic tree as a Frame as FIXED_JOINT. | ||
| 193 | * | ||
| 194 | */ | ||
| 195 | template< | ||
| 196 | typename Scalar, | ||
| 197 | int Options, | ||
| 198 | template<typename, int> | ||
| 199 | class JointCollectionTpl, | ||
| 200 | typename GeometryModelAllocator, | ||
| 201 | typename ConfigVectorType> | ||
| 202 | void buildReducedModel( | ||
| 203 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
| 204 | const std::vector<GeometryModel, GeometryModelAllocator> & list_of_geom_models, | ||
| 205 | const std::vector<JointIndex> & list_of_joints_to_lock, | ||
| 206 | const Eigen::MatrixBase<ConfigVectorType> & reference_configuration, | ||
| 207 | ModelTpl<Scalar, Options, JointCollectionTpl> & reduced_model, | ||
| 208 | std::vector<GeometryModel, GeometryModelAllocator> & list_of_reduced_geom_models); | ||
| 209 | |||
| 210 | /** | ||
| 211 | * | ||
| 212 | * \brief Computes the common ancestor between two joints belonging to the same kinematic tree. | ||
| 213 | * | ||
| 214 | * \param[in] model the input model. | ||
| 215 | * \param[in] joint1_id index of the first joint. | ||
| 216 | * \param[in] joint2_id index of the second joint. | ||
| 217 | * \param[out] index_ancestor_in_support1 index of the ancestor within model.support[joint1_id]. | ||
| 218 | * \param[out] index_ancestor_in_support2 index of the ancestor within model.support[joint2_id]. | ||
| 219 | * | ||
| 220 | */ | ||
| 221 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
| 222 | JointIndex findCommonAncestor( | ||
| 223 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
| 224 | JointIndex joint1_id, | ||
| 225 | JointIndex joint2_id, | ||
| 226 | size_t & index_ancestor_in_support1, | ||
| 227 | size_t & index_ancestor_in_support2); | ||
| 228 | |||
| 229 | } // namespace pinocchio | ||
| 230 | |||
| 231 | #include "pinocchio/algorithm/model.hxx" | ||
| 232 | |||
| 233 | #if PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION | ||
| 234 | #include "pinocchio/algorithm/model.txx" | ||
| 235 | #endif // PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION | ||
| 236 | |||
| 237 | #endif // ifndef __pinocchio_algorithm_model_hpp__ | ||
| 238 |