configuration.hh 6.12 KB
Newer Older
Nicolas Mansard's avatar
Nicolas Mansard committed
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
// <http://www.gnu.org/licenses/>.

#ifndef HPP_MODEL_CONFIGURATION_HH
# define HPP_MODEL_CONFIGURATION_HH

23
# include <hpp/pinocchio/fwd.hh>
Nicolas Mansard's avatar
Nicolas Mansard committed
24
25

namespace hpp {
26
  namespace pinocchio {
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.
32
33
    void saturate (const DevicePtr_t& robot,
                   ConfigurationOut_t configuration);
34

Nicolas Mansard's avatar
Nicolas Mansard committed
35
36
    /// Integrate a constant velocity during unit time.
    ///
37
    /// \param saturateConfig when true, calls saturate at the end
Nicolas Mansard's avatar
Nicolas Mansard committed
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.
50
    template<bool saturateConfig>
51
52
53
    void integrate (const DevicePtr_t& robot,
                    ConfigurationIn_t configuration,
                    vectorIn_t velocity, ConfigurationOut_t result);
54
55
56
57
58
59
60

    /// Same as integrate<true>
    inline void integrate (const DevicePtr_t& robot,
                           ConfigurationIn_t configuration,
                           vectorIn_t velocity, ConfigurationOut_t result)
    {
      integrate<true> (robot, configuration, velocity, result);
Nicolas Mansard's avatar
Nicolas Mansard committed
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
69
    template <typename LieGroup>
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's avatar
Nicolas Mansard committed
75

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's avatar
Nicolas Mansard committed
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$
92
93
94
95
    template <typename LieGroup>
    void difference (const DevicePtr_t& robot, ConfigurationIn_t q1,
                     ConfigurationIn_t q2, vectorOut_t result);

96
97
    void difference (const DevicePtr_t& robot, ConfigurationIn_t q1,
                     ConfigurationIn_t q2, vectorOut_t result);
Nicolas Mansard's avatar
Nicolas Mansard committed
98

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
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);
111

Nicolas Mansard's avatar
Nicolas Mansard committed
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,
117
118
    value_type distance (const DevicePtr_t& robot, ConfigurationIn_t q1,
                         ConfigurationIn_t q2);
Nicolas Mansard's avatar
Nicolas Mansard committed
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.
126
    void normalize (const DevicePtr_t& robot, Configuration_t& q);
127
128
129

    /// For backward compatibility.
    /// See normalize normalize (const DevicePtr_t&, Configuration_t&)
Nicolas Mansard's avatar
Nicolas Mansard committed
130
131
    inline void normalize (const DevicePtr_t& robot, ConfigurationOut_t q)
    {
132
133
134
      Configuration_t qq = q;
      normalize(robot, qq);
      q = qq;
Nicolas Mansard's avatar
Nicolas Mansard committed
135
    }
136

Joseph Mirabel's avatar
Joseph Mirabel committed
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's avatar
Nicolas Mansard committed
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