| Directory: | ./ |
|---|---|
| File: | include/hpp/pinocchio/joint.hh |
| Date: | 2025-05-04 12:09:19 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 11 | 13 | 84.6% |
| Branches: | 8 | 10 | 80.0% |
| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | // | ||
| 2 | // Copyright (c) 2016 CNRS | ||
| 3 | // Author: NMansard from Florent Lamiraux | ||
| 4 | // | ||
| 5 | // | ||
| 6 | |||
| 7 | // Redistribution and use in source and binary forms, with or without | ||
| 8 | // modification, are permitted provided that the following conditions are | ||
| 9 | // met: | ||
| 10 | // | ||
| 11 | // 1. Redistributions of source code must retain the above copyright | ||
| 12 | // notice, this list of conditions and the following disclaimer. | ||
| 13 | // | ||
| 14 | // 2. Redistributions in binary form must reproduce the above copyright | ||
| 15 | // notice, this list of conditions and the following disclaimer in the | ||
| 16 | // documentation and/or other materials provided with the distribution. | ||
| 17 | // | ||
| 18 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS | ||
| 19 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT | ||
| 20 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR | ||
| 21 | // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT | ||
| 22 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, | ||
| 23 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT | ||
| 24 | // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, | ||
| 25 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY | ||
| 26 | // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT | ||
| 27 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE | ||
| 28 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH | ||
| 29 | // DAMAGE. | ||
| 30 | |||
| 31 | #ifndef HPP_PINOCCHIO_JOINT_HH | ||
| 32 | #define HPP_PINOCCHIO_JOINT_HH | ||
| 33 | |||
| 34 | #include <cstddef> | ||
| 35 | #include <hpp/pinocchio/config.hh> | ||
| 36 | #include <hpp/pinocchio/fwd.hh> | ||
| 37 | #include <hpp/util/serialization-fwd.hh> | ||
| 38 | |||
| 39 | namespace hpp { | ||
| 40 | namespace pinocchio { | ||
| 41 | /// Robot joint | ||
| 42 | /// | ||
| 43 | /// A joint maps an input vector to a transformation of SE(3) from the | ||
| 44 | /// parent frame to the joint frame. | ||
| 45 | /// | ||
| 46 | /// The input vector is provided through the configuration vector of the | ||
| 47 | /// robot the joint belongs to. The joint input vector is composed of the | ||
| 48 | /// components of the robot configuration starting at | ||
| 49 | /// Joint::rankInConfiguration. | ||
| 50 | /// | ||
| 51 | /// The joint input vector represents an element of a Lie group, either | ||
| 52 | /// \li a vector space for JointTranslation, and bounded JointRotation, | ||
| 53 | /// \li the unit circle for non-bounded JointRotation joints, | ||
| 54 | /// \li an element of SO(3) for JointSO3, represented by a unit quaternion. | ||
| 55 | /// | ||
| 56 | /// This class is a wrapper to pinocchio::JointModelTpl. | ||
| 57 | class HPP_PINOCCHIO_DLLAPI Joint { | ||
| 58 | public: | ||
| 59 | /// \name Construction and copy and destruction | ||
| 60 | /// \{ | ||
| 61 | |||
| 62 | /// Create a new joint | ||
| 63 | /// \param indexInJointList index in pinocchio vector of joints | ||
| 64 | /// (pinocchio::ModelTpl::joints) | ||
| 65 | /// \return shared pointer to result if indexInJointList > 0, empty | ||
| 66 | /// shared pointer if indexInJointList == 0. | ||
| 67 | /// \note indices of device start at 1 since 0 corresponds to "universe". | ||
| 68 | static JointPtr_t create(DeviceWkPtr_t device, JointIndex indexInJointList); | ||
| 69 | |||
| 70 | /// Constructor | ||
| 71 | /// \param indexInJointList index in pinocchio vector of joints | ||
| 72 | /// (pinocchio::ModelTpl::joints). Should be > 0. | ||
| 73 | /// \note indices of device start at 1 since 0 corresponds to "universe". | ||
| 74 | Joint(DeviceWkPtr_t device, JointIndex indexInJointList); | ||
| 75 | |||
| 76 | 1340 | ~Joint() {} | |
| 77 | /// \} | ||
| 78 | // ----------------------------------------------------------------------- | ||
| 79 | /// \name Name | ||
| 80 | /// \{ | ||
| 81 | |||
| 82 | /// Get name | ||
| 83 | const std::string& name() const; | ||
| 84 | |||
| 85 | /// \} | ||
| 86 | // ----------------------------------------------------------------------- | ||
| 87 | /// \name Position | ||
| 88 | /// \{ | ||
| 89 | |||
| 90 | /// Joint transformation | ||
| 91 | ✗ | const Transform3s& currentTransformation() const { | |
| 92 | ✗ | return currentTransformation(data()); | |
| 93 | } | ||
| 94 | |||
| 95 | /// Joint transformation | ||
| 96 | const Transform3s& currentTransformation(const DeviceData& data) const; | ||
| 97 | |||
| 98 | ///\} | ||
| 99 | // ----------------------------------------------------------------------- | ||
| 100 | /// \name Size and rank | ||
| 101 | ///\{ | ||
| 102 | |||
| 103 | /// Return number of degrees of freedom | ||
| 104 | size_type numberDof() const; | ||
| 105 | |||
| 106 | /// Return number of degrees of freedom | ||
| 107 | size_type configSize() const; | ||
| 108 | |||
| 109 | /// Return rank of the joint in the configuration vector | ||
| 110 | size_type rankInConfiguration() const; | ||
| 111 | |||
| 112 | /// Return rank of the joint in the velocity vector | ||
| 113 | size_type rankInVelocity() const; | ||
| 114 | |||
| 115 | ///\} | ||
| 116 | // ----------------------------------------------------------------------- | ||
| 117 | /// \name Kinematic chain | ||
| 118 | /// \{ | ||
| 119 | |||
| 120 | /// Get a pointer to the parent joint (if any). | ||
| 121 | JointPtr_t parentJoint() const; | ||
| 122 | |||
| 123 | /// Number of child joints | ||
| 124 | std::size_t numberChildJoints() const; | ||
| 125 | |||
| 126 | /// Get child joint | ||
| 127 | JointPtr_t childJoint(std::size_t rank) const; | ||
| 128 | |||
| 129 | /// Get (constant) placement of joint in parent frame, i.e. | ||
| 130 | /// model.jointPlacement[idx] | ||
| 131 | const Transform3s& positionInParentFrame() const; | ||
| 132 | |||
| 133 | ///\} | ||
| 134 | // ----------------------------------------------------------------------- | ||
| 135 | /// \name Bounds | ||
| 136 | /// \{ | ||
| 137 | |||
| 138 | /// Set whether given degree of freedom is bounded | ||
| 139 | /// \warning Joint always has bounds. When `bounded == false`, | ||
| 140 | /// the bounds are `-infinity` and `infinity`. | ||
| 141 | void isBounded(size_type rank, bool bounded); | ||
| 142 | /// Get whether given degree of freedom is bounded | ||
| 143 | bool isBounded(size_type rank) const; | ||
| 144 | /// Get lower bound of given degree of freedom | ||
| 145 | value_type lowerBound(size_type rank) const; | ||
| 146 | /// Get upper bound of given degree of freedom | ||
| 147 | value_type upperBound(size_type rank) const; | ||
| 148 | /// Set lower bound of given degree of freedom | ||
| 149 | void lowerBound(size_type rank, value_type lowerBound); | ||
| 150 | /// Set upper bound of given degree of freedom | ||
| 151 | void upperBound(size_type rank, value_type upperBound); | ||
| 152 | /// Set lower bounds | ||
| 153 | void lowerBounds(vectorIn_t lowerBounds); | ||
| 154 | /// Set upper bounds | ||
| 155 | void upperBounds(vectorIn_t upperBounds); | ||
| 156 | |||
| 157 | /// Get upper bound on linear velocity of the joint frame | ||
| 158 | /// \return coefficient \f$\lambda\f$ such that | ||
| 159 | /// \f{equation*} | ||
| 160 | /// \forall \mathbf{q}_{joint}\ \ \ \ \ \ \|\mathbf {v}\| \leq \lambda | ||
| 161 | /// \|\mathbf{\dot{q}}_{joint}\| \f} where \li \f$\mathbf{q}_{joint}\f$ is any | ||
| 162 | /// joint configuration, \li \f$\mathbf{\dot{q}}_{joint}\f$ is the joint | ||
| 163 | /// velocity, and \li \f$\mathbf{v} = J(\mathbf{q})*\mathbf{\dot{q}} \f$ is | ||
| 164 | /// the linear velocity of the joint frame. | ||
| 165 | value_type upperBoundLinearVelocity() const; | ||
| 166 | |||
| 167 | /// Get upper bound on angular velocity of the joint frame | ||
| 168 | /// \return coefficient \f$\lambda\f$ such that | ||
| 169 | /// \f{equation*} | ||
| 170 | /// \forall \mathbf{q}_{joint}\ \ \ \ \ \ \|\omega\| \leq \lambda | ||
| 171 | /// \|\mathbf{\dot{q}}_{joint}\| \f} where \li \f$\mathbf{q}_{joint}\f$ is any | ||
| 172 | /// joint configuration, \li \f$\mathbf{\dot{q}}_{joint}\f$ is the joint | ||
| 173 | /// velocity, and \li \f$\omega = J(\mathbf{q})*\mathbf{\dot{q}}\f$ is the | ||
| 174 | /// angular velocity of the joint frame. | ||
| 175 | value_type upperBoundAngularVelocity() const; | ||
| 176 | |||
| 177 | /// Maximal distance of joint origin to parent origin | ||
| 178 | 81 | const value_type& maximalDistanceToParent() { | |
| 179 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 81 times.
|
81 | if (maximalDistanceToParent_ < 0) computeMaximalDistanceToParent(); |
| 180 | 81 | return maximalDistanceToParent_; | |
| 181 | } | ||
| 182 | |||
| 183 | /// \} | ||
| 184 | protected: | ||
| 185 | /// Compute the maximal distance. \sa maximalDistanceToParent | ||
| 186 | void computeMaximalDistanceToParent(); | ||
| 187 | |||
| 188 | public: | ||
| 189 | // ----------------------------------------------------------------------- | ||
| 190 | /// \name Jacobian | ||
| 191 | /// \{ | ||
| 192 | |||
| 193 | /// Get non const reference to Jacobian | ||
| 194 | /// \param localFrame if true, compute the jacobian (6d) in the local frame, | ||
| 195 | /// whose linear part corresponds to the velocity of the center of the frame. | ||
| 196 | /// If false, the jacobian is expressed in the global frame and its linear | ||
| 197 | /// part corresponds to the value of the velocity vector field at the center | ||
| 198 | /// of the world. | ||
| 199 | JointJacobian_t& jacobian(const bool localFrame = true) const { | ||
| 200 | return jacobian(data(), localFrame); | ||
| 201 | } | ||
| 202 | |||
| 203 | /// Get reference to Jacobian | ||
| 204 | /// \param data a DeviceData (see hpp::pinocchio::DeviceSync for details). | ||
| 205 | /// \param localFrame if true, compute the jacobian (6d) in the local frame, | ||
| 206 | /// whose linear part corresponds to the velocity of the center of the frame. | ||
| 207 | /// If false, the jacobian is expressed in the global frame and its linear | ||
| 208 | /// part corresponds to the value of the velocity vector field at the center | ||
| 209 | /// of the world. | ||
| 210 | JointJacobian_t& jacobian(DeviceData& data, | ||
| 211 | const bool localFrame = true) const; | ||
| 212 | |||
| 213 | /// \} | ||
| 214 | // ----------------------------------------------------------------------- | ||
| 215 | |||
| 216 | /// Access robot owning the object | ||
| 217 | 1714 | DeviceConstPtr_t robot() const { return devicePtr.lock(); } | |
| 218 | /// Access robot owning the object | ||
| 219 | 12327 | DevicePtr_t robot() { return devicePtr.lock(); } | |
| 220 | |||
| 221 | /// \name Body linked to the joint | ||
| 222 | /// \{ | ||
| 223 | |||
| 224 | /// Get linked body | ||
| 225 | BodyPtr_t linkedBody() const; | ||
| 226 | |||
| 227 | /// \} | ||
| 228 | |||
| 229 | /// Display joint | ||
| 230 | std::ostream& display(std::ostream& os) const; | ||
| 231 | |||
| 232 | /// Get configuration space of joint | ||
| 233 | LiegroupSpacePtr_t configurationSpace() const; | ||
| 234 | |||
| 235 | /// Get configuration space of joint. | ||
| 236 | /// Use R^n x SO(n) instead of SE(n). | ||
| 237 | LiegroupSpacePtr_t RnxSOnConfigurationSpace() const; | ||
| 238 | |||
| 239 | /// \name Pinocchio API | ||
| 240 | /// \{ | ||
| 241 | |||
| 242 | 1361 | const JointIndex& index() const { return jointIndex; } | |
| 243 | |||
| 244 | /// Get the index for a given joint | ||
| 245 | /// | ||
| 246 | /// \return 0 if joint is NULL ("universe"), joint->index() otherwise. | ||
| 247 | static inline size_type index(const JointConstPtr_t& joint) { | ||
| 248 | return (joint ? joint->index() : 0); | ||
| 249 | } | ||
| 250 | |||
| 251 | const JointModel& jointModel() const; | ||
| 252 | |||
| 253 | /// \} | ||
| 254 | |||
| 255 | 53 | bool operator==(const Joint& other) const { | |
| 256 |
7/8✓ Branch 2 taken 27 times.
✓ Branch 3 taken 26 times.
✓ Branch 7 taken 27 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 27 times.
✓ Branch 10 taken 26 times.
✓ Branch 12 taken 27 times.
✓ Branch 13 taken 26 times.
|
53 | return index() == other.index() && robot() == other.robot(); |
| 257 | } | ||
| 258 | |||
| 259 | 26 | bool operator!=(const Joint& other) const { return !operator==(other); } | |
| 260 | |||
| 261 | protected: | ||
| 262 | value_type maximalDistanceToParent_; | ||
| 263 | DeviceWkPtr_t devicePtr; | ||
| 264 | JointIndex jointIndex; | ||
| 265 | std::vector<JointIndex> children; | ||
| 266 | |||
| 267 | /// Store list of childrens. | ||
| 268 | void setChildList(); | ||
| 269 | Model& model(); | ||
| 270 | const Model& model() const; | ||
| 271 | DeviceData& data() const; | ||
| 272 | |||
| 273 | /// Assert that the members of the struct are valid (no null pointer, etc). | ||
| 274 | void selfAssert() const; | ||
| 275 | |||
| 276 | friend class Device; | ||
| 277 | |||
| 278 | 7 | Joint() {} | |
| 279 | HPP_SERIALIZABLE(); | ||
| 280 | }; // class Joint | ||
| 281 | |||
| 282 | inline std::ostream& operator<<(std::ostream& os, const Joint& joint) { | ||
| 283 | return joint.display(os); | ||
| 284 | } | ||
| 285 | |||
| 286 | } // namespace pinocchio | ||
| 287 | } // namespace hpp | ||
| 288 | |||
| 289 | #endif // HPP_PINOCCHIO_JOINT_HH | ||
| 290 |