| Directory: | ./ |
|---|---|
| File: | include/pinocchio/algorithm/frames.hpp |
| Date: | 2025-02-12 21:03:38 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 25 | 29 | 86.2% |
| Branches: | 4 | 14 | 28.6% |
| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | // | ||
| 2 | // Copyright (c) 2015-2020 CNRS INRIA | ||
| 3 | // | ||
| 4 | |||
| 5 | #ifndef __pinocchio_algorithm_frames_hpp__ | ||
| 6 | #define __pinocchio_algorithm_frames_hpp__ | ||
| 7 | |||
| 8 | #include "pinocchio/multibody/model.hpp" | ||
| 9 | #include "pinocchio/multibody/data.hpp" | ||
| 10 | |||
| 11 | namespace pinocchio | ||
| 12 | { | ||
| 13 | |||
| 14 | /** | ||
| 15 | * @brief Updates the position of each frame contained in the model. | ||
| 16 | * | ||
| 17 | * @tparam JointCollection Collection of Joint types. | ||
| 18 | * | ||
| 19 | * @param[in] model The kinematic model. | ||
| 20 | * @param data Data associated to model. | ||
| 21 | * | ||
| 22 | * @warning One of the algorithms forwardKinematics should have been called first. | ||
| 23 | */ | ||
| 24 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
| 25 | inline void updateFramePlacements( | ||
| 26 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
| 27 | DataTpl<Scalar, Options, JointCollectionTpl> & data); | ||
| 28 | |||
| 29 | /** | ||
| 30 | * @brief Updates the placement of the given frame. | ||
| 31 | * | ||
| 32 | * @param[in] model The kinematic model. | ||
| 33 | * @param data Data associated to model. | ||
| 34 | * @param[in] frame_id Id of the operational Frame. | ||
| 35 | * | ||
| 36 | * @return A reference to the frame placement stored in data.oMf[frame_id] | ||
| 37 | * | ||
| 38 | * @warning One of the algorithms forwardKinematics should have been called first | ||
| 39 | */ | ||
| 40 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
| 41 | inline const typename DataTpl<Scalar, Options, JointCollectionTpl>::SE3 & updateFramePlacement( | ||
| 42 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
| 43 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
| 44 | const FrameIndex frame_id); | ||
| 45 | |||
| 46 | /** | ||
| 47 | * @brief First calls the forwardKinematics on the model, then computes the placement of each | ||
| 48 | * frame. /sa pinocchio::forwardKinematics. | ||
| 49 | * | ||
| 50 | * @tparam JointCollection Collection of Joint types. | ||
| 51 | * @tparam ConfigVectorType Type of the joint configuration vector. | ||
| 52 | * | ||
| 53 | * @param[in] model The kinematic model. | ||
| 54 | * @param data Data associated to model. | ||
| 55 | * @param[in] q Configuration vector. | ||
| 56 | */ | ||
| 57 | template< | ||
| 58 | typename Scalar, | ||
| 59 | int Options, | ||
| 60 | template<typename, int> class JointCollectionTpl, | ||
| 61 | typename ConfigVectorType> | ||
| 62 | inline void framesForwardKinematics( | ||
| 63 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
| 64 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
| 65 | const Eigen::MatrixBase<ConfigVectorType> & q); | ||
| 66 | |||
| 67 | /** | ||
| 68 | * @brief Returns the spatial velocity of the Frame expressed in the desired reference frame. | ||
| 69 | * You must first call pinocchio::forwardKinematics to update placement and velocity | ||
| 70 | * values in data structure. | ||
| 71 | * | ||
| 72 | * @param[in] model The kinematic model | ||
| 73 | * @param[in] data Data associated to model | ||
| 74 | * @param[in] joint_id Id of the parent joint | ||
| 75 | * @param[in] placement frame placement with respect to the parent joint | ||
| 76 | * @param[in] rf Reference frame in which the velocity is expressed. | ||
| 77 | * | ||
| 78 | * @return The spatial velocity of the Frame expressed in the desired reference frame. | ||
| 79 | * | ||
| 80 | * @warning Fist or second order forwardKinematics should have been called first | ||
| 81 | */ | ||
| 82 | |||
| 83 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
| 84 | inline MotionTpl<Scalar, Options> getFrameVelocity( | ||
| 85 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
| 86 | const DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
| 87 | const JointIndex joint_id, | ||
| 88 | const SE3Tpl<Scalar, Options> & placement, | ||
| 89 | const ReferenceFrame rf = LOCAL); | ||
| 90 | |||
| 91 | /** | ||
| 92 | * @brief Returns the spatial velocity of the Frame expressed in the desired reference frame. | ||
| 93 | * You must first call pinocchio::forwardKinematics to update placement and velocity | ||
| 94 | * values in data structure. | ||
| 95 | * | ||
| 96 | * @param[in] model The kinematic model | ||
| 97 | * @param[in] data Data associated to model | ||
| 98 | * @param[in] frame_id Id of the operational Frame | ||
| 99 | * @param[in] rf Reference frame in which the velocity is expressed. | ||
| 100 | * | ||
| 101 | * @return The spatial velocity of the Frame expressed in the desired reference frame. | ||
| 102 | * | ||
| 103 | * @warning Fist or second order forwardKinematics should have been called first | ||
| 104 | */ | ||
| 105 | |||
| 106 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
| 107 | 217 | inline MotionTpl<Scalar, Options> getFrameVelocity( | |
| 108 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
| 109 | const DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
| 110 | const FrameIndex frame_id, | ||
| 111 | const ReferenceFrame rf = LOCAL) | ||
| 112 | { | ||
| 113 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
| 114 | 217 | const typename Model::Frame & frame = model.frames[frame_id]; | |
| 115 | |||
| 116 | 217 | return getFrameVelocity(model, data, frame.parentJoint, frame.placement, rf); | |
| 117 | } | ||
| 118 | |||
| 119 | /** | ||
| 120 | * @brief Returns the spatial acceleration of the Frame expressed in the desired reference | ||
| 121 | * frame. You must first call pinocchio::forwardKinematics to update placement, velocity and | ||
| 122 | * acceleration values in data structure. | ||
| 123 | * | ||
| 124 | * @param[in] model The kinematic model | ||
| 125 | * @param[in] data Data associated to model | ||
| 126 | * @param[in] joint_id Id of the parent joint | ||
| 127 | * @param[in] placement frame placement with respect to the parent joint | ||
| 128 | * @param[in] rf Reference frame in which the acceleration is expressed. | ||
| 129 | * | ||
| 130 | * @return The spatial acceleration of the Frame expressed in the desired reference frame. | ||
| 131 | * | ||
| 132 | * @warning Second order forwardKinematics should have been called first | ||
| 133 | */ | ||
| 134 | |||
| 135 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
| 136 | inline MotionTpl<Scalar, Options> getFrameAcceleration( | ||
| 137 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
| 138 | const DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
| 139 | const JointIndex joint_id, | ||
| 140 | const SE3Tpl<Scalar, Options> & placement, | ||
| 141 | const ReferenceFrame rf = LOCAL); | ||
| 142 | |||
| 143 | /** | ||
| 144 | * @brief Returns the spatial acceleration of the Frame expressed in the desired reference | ||
| 145 | * frame. You must first call pinocchio::forwardKinematics to update placement, velocity and | ||
| 146 | * acceleration values in the data structure. | ||
| 147 | * | ||
| 148 | * @param[in] model The kinematic model | ||
| 149 | * @param[in] data Data associated to model | ||
| 150 | * @param[in] frame_id Id of the operational Frame | ||
| 151 | * @param[in] rf Reference frame in which the acceleration is expressed. | ||
| 152 | * | ||
| 153 | * @return The spatial acceleration of the Frame expressed in the desired reference frame. | ||
| 154 | * | ||
| 155 | * @warning Second order @ref forwardKinematics should have been called first | ||
| 156 | * | ||
| 157 | * @remark In the context of a frame placement constraint \f$J(q) a + \dot{J}(q, v) v = 0\f$, | ||
| 158 | * one way to compute the second term \f$\dot{J}(q, v) v\f$ is to call second-order | ||
| 159 | * @ref forwardKinematics with a zero acceleration, then read the remaining \f$\dot{J}(q, v) v\f$ | ||
| 160 | * by calling this function. This is significantly more efficient than applying the matrix | ||
| 161 | * \f$\dot{J}(q, v)\f$ (from @ref getFrameJacobianTimeVariation) to the velocity vector \f$v\f$. | ||
| 162 | */ | ||
| 163 | |||
| 164 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
| 165 | 310 | inline MotionTpl<Scalar, Options> getFrameAcceleration( | |
| 166 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
| 167 | const DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
| 168 | const FrameIndex frame_id, | ||
| 169 | const ReferenceFrame rf = LOCAL) | ||
| 170 | { | ||
| 171 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
| 172 | 310 | const typename Model::Frame & frame = model.frames[frame_id]; | |
| 173 | 310 | return getFrameAcceleration(model, data, frame.parentJoint, frame.placement, rf); | |
| 174 | } | ||
| 175 | |||
| 176 | /** | ||
| 177 | * @brief Returns the "classical" acceleration of the Frame expressed in the desired | ||
| 178 | * reference frame. This is different from the "spatial" acceleration in that centrifugal effects | ||
| 179 | * are accounted for. You must first call pinocchio::forwardKinematics to update placement, | ||
| 180 | * velocity and acceleration values in data structure. | ||
| 181 | * | ||
| 182 | * @param[in] model The kinematic model | ||
| 183 | * @param[in] data Data associated to model | ||
| 184 | * @param[in] joint_id Id of the parent joint | ||
| 185 | * @param[in] placement frame placement with respect to the parent joint | ||
| 186 | * @param[in] rf Reference frame in which the acceleration is expressed. | ||
| 187 | * | ||
| 188 | * @return The classical acceleration of the Frame expressed in the desired reference frame. | ||
| 189 | * | ||
| 190 | * @warning Second order forwardKinematics should have been called first | ||
| 191 | */ | ||
| 192 | |||
| 193 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
| 194 | inline MotionTpl<Scalar, Options> getFrameClassicalAcceleration( | ||
| 195 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
| 196 | const DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
| 197 | const JointIndex joint_id, | ||
| 198 | const SE3Tpl<Scalar, Options> & placement, | ||
| 199 | const ReferenceFrame rf = LOCAL); | ||
| 200 | |||
| 201 | /** | ||
| 202 | * @brief Returns the "classical" acceleration of the Frame expressed in the desired | ||
| 203 | * reference frame. This is different from the "spatial" acceleration in that centrifugal effects | ||
| 204 | * are accounted for. You must first call pinocchio::forwardKinematics to update placement, | ||
| 205 | * velocity and acceleration values in data structure. | ||
| 206 | * | ||
| 207 | * @param[in] model The kinematic model | ||
| 208 | * @param[in] data Data associated to model | ||
| 209 | * @param[in] frame_id Id of the operational Frame | ||
| 210 | * @param[in] rf Reference frame in which the acceleration is expressed. | ||
| 211 | * | ||
| 212 | * @return The classical acceleration of the Frame expressed in the desired reference frame. | ||
| 213 | * | ||
| 214 | * @warning Second order @ref forwardKinematics should have been called first | ||
| 215 | * | ||
| 216 | * @remark In the context of a frame placement constraint \f$J(q) a + \dot{J}(q, v) v = 0\f$, | ||
| 217 | * one way to compute the second term \f$\dot{J}(q, v) v\f$ is to call second-order | ||
| 218 | * @ref forwardKinematics with a zero acceleration, then read the remaining \f$\dot{J}(q, v) v\f$ | ||
| 219 | * by calling this function. This is significantly more efficient than applying the matrix | ||
| 220 | * \f$\dot{J}(q, v)\f$ (from @ref getFrameJacobianTimeVariation) to the velocity vector \f$v\f$. | ||
| 221 | * | ||
| 222 | */ | ||
| 223 | |||
| 224 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
| 225 | 13 | inline MotionTpl<Scalar, Options> getFrameClassicalAcceleration( | |
| 226 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
| 227 | const DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
| 228 | const FrameIndex frame_id, | ||
| 229 | const ReferenceFrame rf = LOCAL) | ||
| 230 | { | ||
| 231 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
| 232 | 13 | const typename Model::Frame & frame = model.frames[frame_id]; | |
| 233 | 13 | return getFrameClassicalAcceleration(model, data, frame.parentJoint, frame.placement, rf); | |
| 234 | } | ||
| 235 | |||
| 236 | /** | ||
| 237 | * @brief Returns the jacobian of the frame given by its relative placement w.r.t. a joint | ||
| 238 | * frame, and whose columns are either expressed in the LOCAL frame coordinate system, in the | ||
| 239 | * local world aligned (rf = LOCAL_WORLD_ALIGNED) frame or in the WORLD coordinate system, | ||
| 240 | * depending on the value of reference_frame. You must first call | ||
| 241 | * pinocchio::computeJointJacobians. | ||
| 242 | * | ||
| 243 | * @remarks Similarly to pinocchio::getJointJacobian: | ||
| 244 | * - if rf == LOCAL, this function returns the Jacobian of the frame expressed in | ||
| 245 | * the local coordinate system of the frame | ||
| 246 | * - if rf == LOCAL_WORLD_ALIGNED, this function returns the Jacobian of the frame | ||
| 247 | * centered on the frame origin and expressed in a coordinate system aligned with the WORLD. | ||
| 248 | * - if rf == WORLD, this function returns the Jacobian of the frame expressed at | ||
| 249 | * the point coincident with the origin and expressed in a coordinate system aligned with the | ||
| 250 | * WORLD. | ||
| 251 | * | ||
| 252 | * @tparam JointCollection Collection of Joint types. | ||
| 253 | * @tparam Matrix6xLike Type of the matrix containing the joint Jacobian. | ||
| 254 | * | ||
| 255 | * @param[in] model The kinematic model | ||
| 256 | * @param[in] data Data associated to model | ||
| 257 | * @param[in] joint_id Index of the joint. | ||
| 258 | * @param[in] reference_frame Reference frame in which the Jacobian is expressed. | ||
| 259 | * @param[out] J The Jacobian of the Frame expressed in the | ||
| 260 | * reference_frame coordinate system. | ||
| 261 | * | ||
| 262 | * @warning The function pinocchio::computeJointJacobians should have been called first. | ||
| 263 | */ | ||
| 264 | template< | ||
| 265 | typename Scalar, | ||
| 266 | int Options, | ||
| 267 | template<typename, int> class JointCollectionTpl, | ||
| 268 | typename Matrix6xLike> | ||
| 269 | void getFrameJacobian( | ||
| 270 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
| 271 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
| 272 | const JointIndex joint_id, | ||
| 273 | const SE3Tpl<Scalar, Options> & placement, | ||
| 274 | const ReferenceFrame reference_frame, | ||
| 275 | const Eigen::MatrixBase<Matrix6xLike> & J); | ||
| 276 | |||
| 277 | /** | ||
| 278 | * @brief Returns the jacobian of the frame given by its relative placement w.r.t. a joint | ||
| 279 | * frame, and whose columns are either expressed in the LOCAL frame coordinate system, in the | ||
| 280 | * local world aligned frame or in the WORLD coordinate system, depending on the value of | ||
| 281 | * reference_frame. You must first call pinocchio::computeJointJacobians. | ||
| 282 | * | ||
| 283 | * @remarks Similarly to pinocchio::getJointJacobian: | ||
| 284 | * - if rf == LOCAL, this function returns the Jacobian of the frame expressed in | ||
| 285 | * the local coordinate system of the frame | ||
| 286 | * - if rf == LOCAL_WORLD_ALIGNED, this function returns the Jacobian of the frame | ||
| 287 | * centered on the frame origin and expressed in a coordinate system aligned with the WORLD. | ||
| 288 | * - if rf == WORLD, this function returns the Jacobian of the frame expressed at | ||
| 289 | * the point coincident with the origin and expressed in a coordinate system aligned with the | ||
| 290 | * WORLD. | ||
| 291 | * | ||
| 292 | * @tparam JointCollection Collection of Joint types. | ||
| 293 | * | ||
| 294 | * @param[in] model The kinematic model | ||
| 295 | * @param[in] data Data associated to model | ||
| 296 | * @param[in] joint_id Index of the joint. | ||
| 297 | * @param[in] reference_frame Reference frame in which the Jacobian is expressed. | ||
| 298 | * | ||
| 299 | * @warning The function pinocchio::computeJointJacobians should have been called first. | ||
| 300 | */ | ||
| 301 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
| 302 | ✗ | Eigen::Matrix<Scalar, 6, Eigen::Dynamic, Options> getFrameJacobian( | |
| 303 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
| 304 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
| 305 | const JointIndex joint_id, | ||
| 306 | const SE3Tpl<Scalar, Options> & placement, | ||
| 307 | const ReferenceFrame reference_frame) | ||
| 308 | { | ||
| 309 | typedef Eigen::Matrix<Scalar, 6, Eigen::Dynamic, Options> Matrix6x; | ||
| 310 | ✗ | Matrix6x res(Matrix6x::Zero(6, model.nv)); | |
| 311 | |||
| 312 | ✗ | getFrameJacobian(model, data, joint_id, placement, reference_frame, res); | |
| 313 | ✗ | return res; | |
| 314 | } | ||
| 315 | |||
| 316 | /** | ||
| 317 | * @brief Returns the jacobian of the frame expressed either expressed in the local frame | ||
| 318 | * coordinate system, in the local world aligned frame or in the WORLD coordinate system, | ||
| 319 | * depending on the value of reference_frame. You must first call | ||
| 320 | * pinocchio::computeJointJacobians. | ||
| 321 | * | ||
| 322 | * @remarks Similarly to pinocchio::getJointJacobian: | ||
| 323 | * - if rf == LOCAL, this function returns the Jacobian of the frame expressed in | ||
| 324 | * the local coordinate system of the frame | ||
| 325 | * - if rf == LOCAL_WORLD_ALIGNED, this function returns the Jacobian of the frame | ||
| 326 | * centered on the frame origin and expressed in a coordinate system aligned with the WORLD. | ||
| 327 | * - if rf == WORLD, this function returns the Jacobian of the frame expressed at | ||
| 328 | * the point coincident with the origin and expressed in a coordinate system aligned with the | ||
| 329 | * WORLD. | ||
| 330 | * | ||
| 331 | * @tparam JointCollection Collection of Joint types. | ||
| 332 | * @tparam Matrix6xLike Type of the matrix containing the joint Jacobian. | ||
| 333 | * | ||
| 334 | * @param[in] model The kinematic model | ||
| 335 | * @param[in] data Data associated to model | ||
| 336 | * @param[in] frame_id Index of the frame | ||
| 337 | * @param[in] reference_frame Reference frame in which the Jacobian is expressed. | ||
| 338 | * @param[out] J The Jacobian of the Frame expressed in the | ||
| 339 | * coordinates Frame. | ||
| 340 | * | ||
| 341 | * @warning The function pinocchio::computeJointJacobians should have been called first. | ||
| 342 | */ | ||
| 343 | template< | ||
| 344 | typename Scalar, | ||
| 345 | int Options, | ||
| 346 | template<typename, int> class JointCollectionTpl, | ||
| 347 | typename Matrix6xLike> | ||
| 348 | 25 | inline void getFrameJacobian( | |
| 349 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
| 350 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
| 351 | const FrameIndex frame_id, | ||
| 352 | const ReferenceFrame reference_frame, | ||
| 353 | const Eigen::MatrixBase<Matrix6xLike> & J) | ||
| 354 | { | ||
| 355 |
1/4✗ Branch 0 not taken.
✓ Branch 1 taken 25 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
25 | PINOCCHIO_CHECK_INPUT_ARGUMENT( |
| 356 | frame_id < (FrameIndex)model.nframes, "The index of the Frame is outside the bounds."); | ||
| 357 | |||
| 358 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
| 359 | typedef typename Model::Frame Frame; | ||
| 360 | |||
| 361 | 25 | const Frame & frame = model.frames[frame_id]; | |
| 362 |
1/2✓ Branch 4 taken 25 times.
✗ Branch 5 not taken.
|
25 | data.oMf[frame_id] = data.oMi[frame.parentJoint] * frame.placement; |
| 363 | |||
| 364 | 25 | getFrameJacobian( | |
| 365 | 25 | model, data, frame.parentJoint, frame.placement, reference_frame, | |
| 366 | 25 | PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike, J)); | |
| 367 | 25 | } | |
| 368 | |||
| 369 | /** | ||
| 370 | * @brief Returns the jacobian of the frame expressed either expressed in the local frame | ||
| 371 | * coordinate system, in the local world aligned frame or in the WORLD coordinate system, | ||
| 372 | * depending on the value of reference_frame. You must first call | ||
| 373 | * pinocchio::computeJointJacobians. | ||
| 374 | * | ||
| 375 | * @remarks Similarly to pinocchio::getJointJacobian: | ||
| 376 | * - if rf == LOCAL, this function returns the Jacobian of the frame expressed in | ||
| 377 | * the local coordinate system of the frame | ||
| 378 | * - if rf == LOCAL_WORLD_ALIGNED, this function returns the Jacobian of the frame | ||
| 379 | * centered on the frame origin and expressed in a coordinate system aligned with the WORLD. | ||
| 380 | * - if rf == WORLD, this function returns the Jacobian of the frame expressed at | ||
| 381 | * the point coincident with the origin and expressed in a coordinate system aligned with the | ||
| 382 | * WORLD. | ||
| 383 | * | ||
| 384 | * @tparam JointCollection Collection of Joint types. | ||
| 385 | * | ||
| 386 | * @param[in] model The kinematic model | ||
| 387 | * @param[in] data Data associated to model | ||
| 388 | * @param[in] frame_id Index of the frame | ||
| 389 | * @param[in] reference_frame Reference frame in which the Jacobian is expressed. | ||
| 390 | * | ||
| 391 | * @warning The function pinocchio::computeJointJacobians should have been called first. | ||
| 392 | */ | ||
| 393 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
| 394 | 3 | Eigen::Matrix<Scalar, 6, Eigen::Dynamic, Options> getFrameJacobian( | |
| 395 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
| 396 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
| 397 | const FrameIndex frame_id, | ||
| 398 | const ReferenceFrame reference_frame) | ||
| 399 | { | ||
| 400 | typedef Eigen::Matrix<Scalar, 6, Eigen::Dynamic, Options> Matrix6x; | ||
| 401 |
1/2✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
|
3 | Matrix6x res(Matrix6x::Zero(6, model.nv)); |
| 402 | |||
| 403 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | getFrameJacobian(model, data, frame_id, reference_frame, res); |
| 404 | 3 | return res; | |
| 405 | } | ||
| 406 | |||
| 407 | /// | ||
| 408 | /// \brief Computes the Jacobian of a specific Frame expressed in the desired reference_frame | ||
| 409 | /// given as argument. | ||
| 410 | /// | ||
| 411 | /// \tparam JointCollection Collection of Joint types. | ||
| 412 | /// \tparam ConfigVectorType Type of the joint configuration vector. | ||
| 413 | /// \tparam Matrix6xLike Type of the matrix containing the joint Jacobian. | ||
| 414 | /// | ||
| 415 | /// \param[in] model The model structure of the rigid body system. | ||
| 416 | /// \param[in] data The data structure of the rigid body system. | ||
| 417 | /// \param[in] q The joint configuration vector (dim | ||
| 418 | /// model.nq). \param[in] frameId The id of the Frame refering to | ||
| 419 | /// model.frames[frameId]. \param[in] reference_frame Reference frame in which the | ||
| 420 | /// Jacobian is expressed. \param[out] J A reference on the | ||
| 421 | /// Jacobian matrix where the results will be stored in (dim 6 x model.nv). You must fill J with | ||
| 422 | /// zero elements, e.g. J.setZero(). | ||
| 423 | /// | ||
| 424 | /// \return The Jacobian of the specific Frame expressed in the desired reference frame (matrix 6 | ||
| 425 | /// x model.nv). | ||
| 426 | /// | ||
| 427 | /// \remarks The result of this function is equivalent to call first | ||
| 428 | /// computeJointJacobians(model,data,q), then updateFramePlacements(model,data) and then call | ||
| 429 | /// getJointJacobian(model,data,jointId,rf,J), | ||
| 430 | /// but forwardKinematics and updateFramePlacements are not fully computed. | ||
| 431 | /// | ||
| 432 | template< | ||
| 433 | typename Scalar, | ||
| 434 | int Options, | ||
| 435 | template<typename, int> class JointCollectionTpl, | ||
| 436 | typename ConfigVectorType, | ||
| 437 | typename Matrix6xLike> | ||
| 438 | inline void computeFrameJacobian( | ||
| 439 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
| 440 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
| 441 | const Eigen::MatrixBase<ConfigVectorType> & q, | ||
| 442 | const FrameIndex frameId, | ||
| 443 | const ReferenceFrame reference_frame, | ||
| 444 | const Eigen::MatrixBase<Matrix6xLike> & J); | ||
| 445 | |||
| 446 | /// | ||
| 447 | /// \brief Computes the Jacobian of a specific Frame expressed in the LOCAL frame coordinate | ||
| 448 | /// system. | ||
| 449 | /// | ||
| 450 | /// \tparam JointCollection Collection of Joint types. | ||
| 451 | /// \tparam ConfigVectorType Type of the joint configuration vector. | ||
| 452 | /// \tparam Matrix6xLike Type of the matrix containing the joint Jacobian. | ||
| 453 | /// | ||
| 454 | /// \param[in] model The model structure of the rigid body system. | ||
| 455 | /// \param[in] data The data structure of the rigid body system. | ||
| 456 | /// \param[in] q The joint configuration vector (dim model.nq). | ||
| 457 | /// \param[in] frameId The id of the Frame refering to model.frames[frameId]. | ||
| 458 | /// | ||
| 459 | /// \param[out] J A reference on the Jacobian matrix where the results will be stored in (dim 6 x | ||
| 460 | /// model.nv). You must fill J with zero elements, e.g. J.setZero(). | ||
| 461 | /// | ||
| 462 | /// \return The Jacobian of the specific Frame expressed in the LOCAL frame coordinate system | ||
| 463 | /// (matrix 6 x model.nv). | ||
| 464 | /// | ||
| 465 | /// \remarks The result of this function is equivalent to call first | ||
| 466 | /// computeJointJacobians(model,data,q), then updateFramePlacements(model,data) and then call | ||
| 467 | /// getJointJacobian(model,data,jointId,LOCAL,J), | ||
| 468 | /// but forwardKinematics and updateFramePlacements are not fully computed. | ||
| 469 | /// | ||
| 470 | template< | ||
| 471 | typename Scalar, | ||
| 472 | int Options, | ||
| 473 | template<typename, int> class JointCollectionTpl, | ||
| 474 | typename ConfigVectorType, | ||
| 475 | typename Matrix6xLike> | ||
| 476 | 2 | inline void computeFrameJacobian( | |
| 477 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
| 478 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
| 479 | const Eigen::MatrixBase<ConfigVectorType> & q, | ||
| 480 | const FrameIndex frameId, | ||
| 481 | const Eigen::MatrixBase<Matrix6xLike> & J) | ||
| 482 | { | ||
| 483 | 2 | computeFrameJacobian( | |
| 484 | 2 | model, data, q.derived(), frameId, LOCAL, PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike, J)); | |
| 485 | 2 | } | |
| 486 | |||
| 487 | /// | ||
| 488 | /// \brief Computes the Jacobian time variation of a specific frame (given by frame_id) expressed | ||
| 489 | /// either in the WORLD frame (rf = WORLD), in the local world aligned (rf = LOCAL_WORLD_ALIGNED) | ||
| 490 | /// frame or in the LOCAL frame (rf = LOCAL). | ||
| 491 | /// | ||
| 492 | /// \note This jacobian is extracted from data.dJ. You have to run | ||
| 493 | /// pinocchio::computeJointJacobiansTimeVariation before calling it. | ||
| 494 | /// | ||
| 495 | /// \tparam JointCollection Collection of Joint types. | ||
| 496 | /// \tparam Matrix6xLike Type of the matrix containing the joint Jacobian. | ||
| 497 | /// | ||
| 498 | /// \param[in] model The model structure of the rigid body system. | ||
| 499 | /// \param[in] data The data structure of the rigid body system. | ||
| 500 | /// \param[in] frameId The index of the frame. | ||
| 501 | /// | ||
| 502 | /// \param[out] dJ A reference on the Jacobian matrix where the results will be stored in (dim 6 x | ||
| 503 | /// model.nv). You must fill dJ with zero elements, e.g. dJ.fill(0.). | ||
| 504 | /// | ||
| 505 | template< | ||
| 506 | typename Scalar, | ||
| 507 | int Options, | ||
| 508 | template<typename, int> class JointCollectionTpl, | ||
| 509 | typename Matrix6xLike> | ||
| 510 | void getFrameJacobianTimeVariation( | ||
| 511 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
| 512 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
| 513 | const FrameIndex frame_id, | ||
| 514 | const ReferenceFrame rf, | ||
| 515 | const Eigen::MatrixBase<Matrix6xLike> & dJ); | ||
| 516 | |||
| 517 | /** | ||
| 518 | * @brief Compute the inertia supported by a specific frame (given by frame_id) expressed in the | ||
| 519 | * LOCAL frame. The total supported inertia corresponds to the sum of all the inertia after the | ||
| 520 | * given frame, i.e : | ||
| 521 | * * The frame inertia | ||
| 522 | * * The child frames inertia ('Child frames' refers to frames that share the same parent | ||
| 523 | * joint and are placed after the given frame) | ||
| 524 | * * The child joints inertia (if with_subtree == true) | ||
| 525 | * You must first call pinocchio::forwardKinematics to update placement values in data | ||
| 526 | * structure. | ||
| 527 | * | ||
| 528 | * @note Physically speaking, if the robot were to be cut in two parts at that given frame, this | ||
| 529 | * supported inertia would represents the inertia of the part that was after the frame. | ||
| 530 | * with_subtree determines if the childs joints must be taken into consideration (if true) | ||
| 531 | * or only the current joint (if false). | ||
| 532 | * | ||
| 533 | * @note The equivalent function for a joint would be : | ||
| 534 | * * to read `data.Ycrb[joint_id]`, after having called pinocchio::crba (if with_subtree == | ||
| 535 | * true). | ||
| 536 | * * to read `model.inertia[joint_id]` (if with_subtree == false). | ||
| 537 | * | ||
| 538 | * @tparam JointCollection Collection of Joint types. | ||
| 539 | * | ||
| 540 | * @param[in] model The model structure of the rigid body system. | ||
| 541 | * @param[in] data The data structure of the rigid body system. | ||
| 542 | * @param[in] frameId The index of the frame. | ||
| 543 | * @param[in] with_subtree If false, compute the inertia only inside the frame parent joint if | ||
| 544 | * false. If true, include child joints inertia. | ||
| 545 | * | ||
| 546 | * @return The computed inertia. | ||
| 547 | * | ||
| 548 | * @warning forwardKinematics should have been called first | ||
| 549 | */ | ||
| 550 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
| 551 | InertiaTpl<Scalar, Options> computeSupportedInertiaByFrame( | ||
| 552 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
| 553 | const DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
| 554 | const FrameIndex frame_id, | ||
| 555 | bool with_subtree); | ||
| 556 | |||
| 557 | /** | ||
| 558 | * @brief Computes the force supported by a specific frame (given by frame_id) expressed in the | ||
| 559 | * LOCAL frame. The supported force corresponds to the sum of all the forces experienced after the | ||
| 560 | * given frame, i.e : | ||
| 561 | * * The inertial forces and gravity (applied on the supported inertia in body) | ||
| 562 | * * The forces applied by child joints | ||
| 563 | * * (The external forces) | ||
| 564 | * You must first call pinocchio::rnea to update placements, velocities and efforts values | ||
| 565 | * in data structure. | ||
| 566 | * | ||
| 567 | * @note If an external force is applied to the frame parent joint (during rnea), it won't be | ||
| 568 | * taken in consideration in this function (it will be considered to be applied before the frame | ||
| 569 | * in the joint and not after. However external forces applied to child joints will be taken into | ||
| 570 | * account). | ||
| 571 | * | ||
| 572 | * @note Physically speaking, if the robot were to be separated in two parts glued together at | ||
| 573 | * that given frame, the supported force represents the internal forces applide from the part | ||
| 574 | * after the cut/frame to the part before. This compute what a force-torque sensor would measures | ||
| 575 | * if it would be placed at that frame. | ||
| 576 | * | ||
| 577 | * @note The equivalent function for a joint would be to read `data.f[joint_id]`, after having | ||
| 578 | * call pinocchio::rnea. | ||
| 579 | * | ||
| 580 | * @tparam JointCollection Collection of Joint types. | ||
| 581 | * | ||
| 582 | * @param[in] model The model structure of the rigid body system. | ||
| 583 | * @param[in] data The data structure of the rigid body system. | ||
| 584 | * @param[in] frameId The index of the frame. | ||
| 585 | * | ||
| 586 | * @return The computed force. | ||
| 587 | * | ||
| 588 | * @warning pinocchio::rnea should have been called first | ||
| 589 | */ | ||
| 590 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
| 591 | ForceTpl<Scalar, Options> computeSupportedForceByFrame( | ||
| 592 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
| 593 | const DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
| 594 | const FrameIndex frame_id); | ||
| 595 | |||
| 596 | } // namespace pinocchio | ||
| 597 | |||
| 598 | /* --- Details -------------------------------------------------------------------- */ | ||
| 599 | #include "pinocchio/algorithm/frames.hxx" | ||
| 600 | |||
| 601 | #if PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION | ||
| 602 | #include "pinocchio/algorithm/frames.txx" | ||
| 603 | #endif // PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION | ||
| 604 | |||
| 605 | #endif // ifndef __pinocchio_algorithm_frames_hpp__ | ||
| 606 |