GCC Code Coverage Report

Line	Branch	Exec	Source
1			//
2			// Copyright (c) 2014 CNRS
3			// Author: 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_CONFIGURATION_HH
32			#define HPP_PINOCCHIO_CONFIGURATION_HH
33
34			#include <hpp/pinocchio/fwd.hh>
35			#include <iomanip>
36
37			namespace hpp {
38			namespace pinocchio {
39			/// Saturate joint parameter value so that they are not out of bounds.
40			///
41			/// \param robot robot that describes the kinematic chain
42			/// \param[in,out] configuration initial and result configurations
43			/// \retval configuration reached after saturation.
44			void saturate(const DevicePtr_t& robot, ConfigurationOut_t configuration);
45
46			/// Saturate joint parameter value so that they are not out of bounds.
47			///
48			/// \param robot robot that describes the kinematic chain
49			/// \param[in,out] configuration initial and result configurations
50			/// \retval saturation an array of boolean saying who saturated (size
51			/// robot.numberDof()).
52			bool saturate(const DevicePtr_t& robot, ConfigurationOut_t configuration,
53			ArrayXb& saturation);
54
55			/// Integrate a constant velocity during unit time.
56			///
57			/// \tparam saturateConfig when true, calls
58			/// \c hpp::pinocchio::saturate(robot, result) at the end
59			/// \tparam LieGroup is a compile-time map of joint type to Lie group type,
60			/// that defines the joint-wise operations.
61			/// \param robot robot that describes the kinematic chain
62			/// \param configuration initial and result configurations
63			/// \param velocity velocity vector
64			/// \retval result configuration reached after integration.
65			/// Velocity is dispatched to each joint that integrates according to its
66			/// Lie group structure, i.e.
67			/// \li \f$q_i += v_i\f$ for translation joint and bounded rotation joints,
68			/// \li \f$q_i += v_i \mbox{ modulo } 2\pi\f$ for unbounded rotation joints,
69			/// \li constant rotation velocity for SO(3) joints.
70			///
71			/// \note The default template arguments correspond to
72			/// \c integrate<true,DefaultLieGroupMap>.
73			template <bool saturateConfig, typename LieGroup>
74			void integrate(const DevicePtr_t& robot, ConfigurationIn_t configuration,
75			vectorIn_t velocity, ConfigurationOut_t result);
76
77			void integrate(const DevicePtr_t& robot, ConfigurationIn_t configuration,
78			vectorIn_t velocity, ConfigurationOut_t result);
79
80			/// Interpolate between two configurations of the robot
81			///
82			/// \tparam LieGroup is a compile-time map of joint type to Lie group type,
83			/// that defines the joint-wise operations.
84			/// \param robot robot that describes the kinematic chain
85			/// \param q0, q1 two configurations to interpolate
86			/// \param u in [0,1] position along the interpolation: q0 for u=0,
87			/// q1 for u=1
88			/// \retval result interpolated configuration
89			///
90			/// \note The default template arguments correspond to
91			/// \c interpolate<RnxSOnLieGroupMap>.
92			template <typename LieGroup>
93			void interpolate(const DevicePtr_t& robot, ConfigurationIn_t q0,
94			ConfigurationIn_t q1, const value_type& u,
95			ConfigurationOut_t result);
96
97			void interpolate(const DevicePtr_t& robot, ConfigurationIn_t q0,
98			ConfigurationIn_t q1, const value_type& u,
99			ConfigurationOut_t result);
100
101			/// Difference between two configurations as a vector
102			///
103			/// \tparam LieGroup is a compile-time map of joint type to Lie group type,
104			/// that defines the joint-wise operations.
105			/// \param robot robot that describes the kinematic chain
106			/// \param q1 first configuration,
107			/// \param q2 second configuration,
108			/// \retval result difference as a vector \f$\textbf{v}\f$ such that
109			/// q1 is the result of method integrate from q2 with vector
110			/// \f$\textbf{v}\f$
111			/// \note If the configuration space is a vector space, this is
112			/// \f$\textbf{v} = q_1 - q_2\f$
113			///
114			/// \note The default template arguments correspond to
115			/// \c difference<RnxSOnLieGroupMap>.
116			template <typename LieGroup>
117			void difference(const DevicePtr_t& robot, ConfigurationIn_t q1,
118			ConfigurationIn_t q2, vectorOut_t result);
119
120			void difference(const DevicePtr_t& robot, ConfigurationIn_t q1,
121			ConfigurationIn_t q2, vectorOut_t result);
122
123			/// Test that two configurations are close
124			///
125			/// \param robot robot that describes the kinematic chain
126			/// \param q1 first configuration,
127			/// \param q2 second configuration,
128			/// \param eps numerical threshold
129			/// \return true if the configurations are closer than the numerical
130			/// threshold
131			bool isApprox(const DevicePtr_t& robot, ConfigurationIn_t q1,
132			ConfigurationIn_t q2, value_type eps);
133
134			/// Distance between two configuration.
135			///
136			/// \param robot robot that describes the kinematic chain
137			/// \param q1 first configuration,
138			/// \param q2 second configuration,
139			value_type distance(const DevicePtr_t& robot, ConfigurationIn_t q1,
140			ConfigurationIn_t q2);
141
142			/// Normalize configuration
143			///
144			/// Configuration space is a represented by a sub-manifold of a vector
145			/// space. Normalization consists in projecting a vector on this
146			/// sub-manifold. It mostly consists in normalizing quaternions for
147			/// SO3 joints and 2D-vectors for unbounded rotations.
148			void normalize(const DevicePtr_t& robot, Configuration_t& q);
149
150			/// For backward compatibility.
151			/// See normalize normalize (const DevicePtr_t&, Configuration_t&)
152			inline void normalize(const DevicePtr_t& robot, ConfigurationOut_t q) {
153			Configuration_t qq = q;
154			normalize(robot, qq);
155			q = qq;
156			}
157
158			/// Check if a configuration is normalized
159			///
160			/// It consists in checking that norm of quaternions and complex is one.
161			bool isNormalized(const DevicePtr_t& robot, ConfigurationIn_t q,
162			const value_type& eps);
163
164			/// Write configuration in a string
165		48	inline std::string displayConfig(ConfigurationIn_t q, int precision = 20) {
166	1/2 ✓ Branch 1 taken 48 times. ✗ Branch 2 not taken.	48	std::ostringstream oss;
167	2/4 ✓ Branch 1 taken 48 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 48 times. ✗ Branch 6 not taken.	48	oss << "(" << std::setprecision(precision);
168	2/2 ✓ Branch 1 taken 928 times. ✓ Branch 2 taken 48 times.	976	for (size_type i = 0; i < q.size(); ++i) {
169	3/6 ✓ Branch 1 taken 928 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 928 times. ✗ Branch 5 not taken. ✓ Branch 7 taken 928 times. ✗ Branch 8 not taken.	928	oss << q[i] << ",";
170			}
171	1/2 ✓ Branch 1 taken 48 times. ✗ Branch 2 not taken.	48	oss << ")";
172	1/2 ✓ Branch 1 taken 48 times. ✗ Branch 2 not taken.	96	return oss.str();
173		48	}
174
175			/// Write a SE3 taking into account the indentation
176			std::ostream& display(std::ostream& os, const SE3& m);
177			} // namespace pinocchio
178			} // namespace hpp
179			#endif // HPP_PINOCCHIO_CONFIGURATION_HH
180