configuration.hh 6.12 KB
 Nicolas Mansard committed Jul 06, 2016 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 // // Copyright (c) 2014 CNRS // Author: Florent Lamiraux // // // 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  Joseph Mirabel committed Sep 07, 2016 23 # include  Nicolas Mansard committed Jul 06, 2016 24 25  namespace hpp {  Joseph Mirabel committed Jul 20, 2016 26  namespace pinocchio {  Joseph Mirabel committed Aug 25, 2016 27 28 29 30 31  /// Saturate joint parameter value so that they are not out of bounds. /// /// \param robot robot that describes the kinematic chain /// \param configuration initial and result configurations /// \retval result configuration reached after saturation.  Joseph Mirabel committed Sep 07, 2016 32 33  void saturate (const DevicePtr_t& robot, ConfigurationOut_t configuration);  Joseph Mirabel committed Aug 25, 2016 34   Nicolas Mansard committed Jul 06, 2016 35 36  /// Integrate a constant velocity during unit time. ///  Joseph Mirabel committed Aug 25, 2016 37  /// \param saturateConfig when true, calls saturate at the end  Nicolas Mansard committed Jul 06, 2016 38 39 40 41 42 43 44 45 46 47 48 49  /// \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.  Joseph Mirabel committed Aug 25, 2016 50  template  Joseph Mirabel committed Sep 07, 2016 51 52 53  void integrate (const DevicePtr_t& robot, ConfigurationIn_t configuration, vectorIn_t velocity, ConfigurationOut_t result);  Joseph Mirabel committed Aug 25, 2016 54 55 56 57 58 59 60  /// Same as integrate inline void integrate (const DevicePtr_t& robot, ConfigurationIn_t configuration, vectorIn_t velocity, ConfigurationOut_t result) { integrate (robot, configuration, velocity, result);  Nicolas Mansard committed Jul 06, 2016 61 62 63 64 65 66 67 68  } /// 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  Joseph Mirabel committed Jan 20, 2017 69  template  Joseph Mirabel committed Sep 07, 2016 70 71 72 73 74  void interpolate (const DevicePtr_t& robot, ConfigurationIn_t q0, ConfigurationIn_t q1, const value_type& u, ConfigurationOut_t result);  Nicolas Mansard committed Jul 06, 2016 75   Joseph Mirabel committed Jan 20, 2017 76 77 78 79 80 81  void interpolate (const DevicePtr_t& robot, ConfigurationIn_t q0, ConfigurationIn_t q1, const value_type& u, ConfigurationOut_t result);  Nicolas Mansard committed Jul 06, 2016 82 83 84 85 86 87 88 89 90 91  /// 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 /// q1 is the result of method integrate from q2 with vector /// \f$\textbf{v}\f$ /// \note If the configuration space is a vector space, this is /// \f$\textbf{v} = q_1 - q_2\f$  Florent Lamiraux committed Jan 24, 2017 92 93 94 95  template void difference (const DevicePtr_t& robot, ConfigurationIn_t q1, ConfigurationIn_t q2, vectorOut_t result);  Joseph Mirabel committed Sep 07, 2016 96 97  void difference (const DevicePtr_t& robot, ConfigurationIn_t q1, ConfigurationIn_t q2, vectorOut_t result);  Nicolas Mansard committed Jul 06, 2016 98   Joseph Mirabel committed Sep 06, 2016 99 100 101 102 103 104 105 106  /// Test that two configurations are close /// /// \param robot robot that describes the kinematic chain /// \param q1 first configuration, /// \param q2 second configuration, /// \param eps numerical threshold /// \return true if the configurations are closer than the numerical /// threshold  Joseph Mirabel committed Sep 07, 2016 107 108 109 110  /// /// \todo add precision argument in se3::isSameConfiguration bool isApprox (const DevicePtr_t& robot, ConfigurationIn_t q1, ConfigurationIn_t q2, value_type eps);  Joseph Mirabel committed Sep 06, 2016 111   Nicolas Mansard committed Jul 06, 2016 112 113 114 115 116  /// Distance between two configuration. /// /// \param robot robot that describes the kinematic chain /// \param q1 first configuration, /// \param q2 second configuration,  Joseph Mirabel committed Sep 07, 2016 117 118  value_type distance (const DevicePtr_t& robot, ConfigurationIn_t q1, ConfigurationIn_t q2);  Nicolas Mansard committed Jul 06, 2016 119 120 121 122 123 124 125  /// 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.  Joseph Mirabel committed Sep 07, 2016 126  void normalize (const DevicePtr_t& robot, Configuration_t& q);  Joseph Mirabel committed Jul 20, 2016 127 128 129  /// For backward compatibility. /// See normalize normalize (const DevicePtr_t&, Configuration_t&)  Nicolas Mansard committed Jul 06, 2016 130 131  inline void normalize (const DevicePtr_t& robot, ConfigurationOut_t q) {  Joseph Mirabel committed Jul 20, 2016 132 133 134  Configuration_t qq = q; normalize(robot, qq); q = qq;  Nicolas Mansard committed Jul 06, 2016 135  }  Joseph Mirabel committed Jul 20, 2016 136   Joseph Mirabel committed Jan 31, 2017 137 138 139 140 141 142  /// Check if a configuration is normalized /// /// It consists in checking that norm of quaternions and complex is one. bool isValidConfiguration (const DevicePtr_t& robot, ConfigurationIn_t q, const value_type& eps);  Nicolas Mansard committed Jul 06, 2016 143 144 145 146 147 148 149 150 151 152 153 154  /// 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 (); } } // namespace model } // namespace hpp #endif // HPP_MODEL_CONFIGURATION_HH