configuration.hh 5.51 KB
 Florent Lamiraux committed Jul 24, 2014 1 2 3 4 5 // // Copyright (c) 2014 CNRS // Author: Florent Lamiraux // //  Florent Lamiraux committed Mar 14, 2014 6 7 8 9 10 11 12 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 // This file is part of hpp-model // hpp-model 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. // // hpp-model 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 // hpp-model If not, see // . #ifndef HPP_MODEL_CONFIGURATION_HH # define HPP_MODEL_CONFIGURATION_HH # include # include # include namespace hpp { namespace model { /// Integrate a constant velocity during unit time. /// /// \param robot robot that describes the kinematic chain /// \param configuration initial and result configurations /// \param velocity velocity vector /// \retval result configuration reached after integration. /// Velocity is dispatched to each joint that integrates according to its /// Lie group structure, i.e. /// \li \f$q_i += v_i\f$ for translation joint and bounded rotation joints, /// \li \f$q_i += v_i \mbox{ modulo } 2\pi\f$ for unbounded rotation joints, /// \li constant rotation velocity for SO(3) joints. /// /// \note bounded degrees of freedom are saturated if the result of the /// above operation is beyond a bound.  Florent Lamiraux committed Jul 25, 2014 43 44 45  inline void integrate (const DevicePtr_t& robot, ConfigurationIn_t configuration, vectorIn_t velocity, ConfigurationOut_t result)  Florent Lamiraux committed Mar 14, 2014 46 47 48 49  { const JointVector_t& jv (robot->getJointVector ()); for (model::JointVector_t::const_iterator itJoint = jv.begin (); itJoint != jv.end (); itJoint++) {  Florent Lamiraux committed Apr 01, 2014 50 51  size_type indexConfig = (*itJoint)->rankInConfiguration (); size_type indexVelocity = (*itJoint)->rankInVelocity ();  Florent Lamiraux committed Mar 14, 2014 52 53 54 55  (*itJoint)->configuration ()->integrate (configuration, velocity, indexConfig, indexVelocity, result); }  Joseph Mirabel committed Dec 10, 2015 56 57  const size_type& dim = robot->extraConfigSpace().dimension(); result.tail (dim) = configuration.tail (dim) + velocity.tail (dim);  Florent Lamiraux committed Mar 14, 2014 58 59  }  Joseph Mirabel committed Oct 23, 2015 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78  /// Interpolate between two configurations of the robot /// \param robot robot that describes the kinematic chain /// \param q0, q1, two configurations to interpolate /// \param u in [0,1] position along the interpolation: q0 for u=0, /// q1 for u=1 /// \retval result interpolated configuration inline void interpolate (const DevicePtr_t& robot, ConfigurationIn_t q0, ConfigurationIn_t q1, const value_type& u, ConfigurationOut_t result) { const JointVector_t& jv (robot->getJointVector ()); for (model::JointVector_t::const_iterator itJoint = jv.begin (); itJoint != jv.end (); itJoint++) { size_type indexConfig = (*itJoint)->rankInConfiguration (); (*itJoint)->configuration ()->interpolate (q0, q1, u, indexConfig, result); }  Joseph Mirabel committed Dec 10, 2015 79 80  const size_type& dim = robot->extraConfigSpace().dimension(); result.tail (dim) = u * q1.tail (dim) + (1-u) * q0.tail (dim);  Joseph Mirabel committed Oct 23, 2015 81 82  }  Florent Lamiraux committed Mar 14, 2014 83 84 85 86 87 88  /// Difference between two configurations as a vector /// /// \param robot robot that describes the kinematic chain /// \param q1 first configuration, /// \param q2 second configuration, /// \retval result difference as a vector \f$\textbf{v}\f$ such that  Joseph Mirabel committed Feb 03, 2016 89  /// q1 is the result of method integrate from q2 with vector  Florent Lamiraux committed Oct 29, 2015 90  /// \f$\textbf{v}\f$  Joseph Mirabel committed Feb 03, 2016 91 92  /// \note If the configuration space is a vector space, this is /// \f$\textbf{v} = q_1 - q_2\f$  Florent Lamiraux committed Jul 25, 2014 93 94  void inline difference (const DevicePtr_t& robot, ConfigurationIn_t q1, ConfigurationIn_t q2, vectorOut_t result)  Florent Lamiraux committed Mar 14, 2014 95 96 97 98  { const JointVector_t& jv (robot->getJointVector ()); for (model::JointVector_t::const_iterator itJoint = jv.begin (); itJoint != jv.end (); itJoint++) {  Florent Lamiraux committed Apr 01, 2014 99 100  size_type indexConfig = (*itJoint)->rankInConfiguration (); size_type indexVelocity = (*itJoint)->rankInVelocity ();  Florent Lamiraux committed Mar 14, 2014 101 102 103  (*itJoint)->configuration ()->difference (q1, q2, indexConfig, indexVelocity, result); }  Joseph Mirabel committed Dec 10, 2015 104 105  const size_type& dim = robot->extraConfigSpace().dimension(); result.tail (dim) = q2.tail (dim) - q1.tail (dim);  Florent Lamiraux committed Mar 14, 2014 106  }  Florent Lamiraux committed Sep 04, 2014 107   Florent Lamiraux committed Sep 01, 2015 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122  /// Normalize configuration /// /// Configuration space is a represented by a sub-manifold of a vector /// space. Normalization consists in projecting a vector on this /// sub-manifold. It mostly consists in normalizing quaternions for /// SO3 joints and 2D-vectors for unbounded rotations. inline void normalize (const DevicePtr_t& robot, ConfigurationOut_t q) { const JointVector_t& jv (robot->getJointVector ()); for (model::JointVector_t::const_iterator itJoint = jv.begin (); itJoint != jv.end (); itJoint++) { size_type indexConfig = (*itJoint)->rankInConfiguration (); (*itJoint)->configuration ()->normalize (indexConfig, q); } }  Florent Lamiraux committed Sep 04, 2014 123 124 125 126 127 128 129 130 131 132 133  /// Write configuration in a string inline std::string displayConfig (ConfigurationIn_t q) { std::ostringstream oss; for (size_type i=0; i < q.size (); ++i) { oss << q [i] << ","; } return oss.str (); }  Florent Lamiraux committed Mar 14, 2014 134 135 136  } // namespace model } // namespace hpp #endif // HPP_MODEL_CONFIGURATION_HH