Commit 7b27288c authored by Florent Lamiraux's avatar Florent Lamiraux
Browse files

Add method normalize in JointConfiguration class

  - add inline function to normalize a configuration.
parent c79e0e18
......@@ -76,6 +76,21 @@ namespace hpp {
}
}
/// 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);
}
}
/// Write configuration in a string
inline std::string displayConfig (ConfigurationIn_t q)
{
......
......@@ -108,12 +108,16 @@ namespace hpp {
const size_type& indexVelocity,
vectorOut_t result) const = 0;
/// Normalize configuration of joint
virtual void normalize (const size_type& index,
ConfigurationOut_t result) const = 0;
/// Uniformly sample the configuration space of the joint
/// \param index index of first component of q corresponding to the joint.
/// \retval result write joint configuration in
/// result [index:index+nb dofs]
virtual void uniformlySample (const size_type& index,
ConfigurationOut_t result) const = 0;
/// \name Bounds
/// @{
/// Set whether given degree of freedom is bounded
......@@ -167,6 +171,9 @@ namespace hpp {
const size_type& indexConfig,
const size_type& indexVelocity,
vectorOut_t result) const;
/// Normalize configuration of joint
virtual void normalize (const size_type& index,
ConfigurationOut_t result) const;
virtual void uniformlySample (const size_type& index,
ConfigurationOut_t result) const;
}; // class AnchorJointConfig
......@@ -218,6 +225,9 @@ namespace hpp {
const size_type& indexConfig,
const size_type& indexVelocity,
vectorOut_t result) const;
/// Normalize configuration of joint
virtual void normalize (const size_type& index,
ConfigurationOut_t result) const;
virtual void uniformlySample (const size_type& index,
ConfigurationOut_t result) const;
}; // class SO3JointConfig
......@@ -281,6 +291,9 @@ namespace hpp {
const size_type& indexVelocity,
vectorOut_t result) const = 0;
/// Normalize configuration of joint
virtual void normalize (const size_type& index,
ConfigurationOut_t result) const;
virtual void uniformlySample (const size_type& index,
ConfigurationOut_t result) const = 0;
}; // class RotationJointConfig
......@@ -306,6 +319,9 @@ namespace hpp {
const size_type& indexConfig,
const size_type& indexVelocity,
vectorOut_t result) const;
/// Normalize configuration of joint
virtual void normalize (const size_type& index,
ConfigurationOut_t result) const;
void uniformlySample (const size_type& index,
ConfigurationOut_t result) const;
}; // class UnBounded
......@@ -330,6 +346,9 @@ namespace hpp {
const size_type& indexConfig,
const size_type& indexVelocity,
vectorOut_t result) const;
/// Normalize configuration of joint
virtual void normalize (const size_type& index,
ConfigurationOut_t result) const;
void uniformlySample (const size_type& index,
ConfigurationOut_t result) const;
}; // class Bounded
......@@ -383,6 +402,9 @@ namespace hpp {
const size_type& indexVelocity,
vectorOut_t result) const;
/// Normalize configuration of joint
virtual void normalize (const size_type& index,
ConfigurationOut_t result) const;
virtual void uniformlySample (const size_type& index,
ConfigurationOut_t result) const;
}; // class TranslationJointConfig
......
......@@ -140,6 +140,12 @@ namespace hpp {
{
}
/// Normalize configuration of joint
void AnchorJointConfig::normalize (const size_type&,
ConfigurationOut_t) const
{
}
void AnchorJointConfig::uniformlySample (const size_type&,
ConfigurationOut_t) const
{
......@@ -242,6 +248,21 @@ namespace hpp {
result [indexVelocity + 2] = angle*axis [2];
}
/// Normalize configuration of joint
void SO3JointConfig::normalize (const size_type& index,
ConfigurationOut_t result) const
{
value_type p0 = result [index];
value_type p1 = result [index + 1];
value_type p2 = result [index + 2];
value_type p3 = result [index + 3];
value_type d = sqrt (p0*p0 + p1*p1 + p2*p2 + p3*p3);
result [index] /= d;
result [index + 1] /= d;
result [index + 2] /= d;
result [index + 3] /= d;
}
void SO3JointConfig::uniformlySample (const size_type& index,
ConfigurationOut_t result) const
{
......@@ -306,6 +327,14 @@ namespace hpp {
q2.segment <dimension> (indexConfig);
}
/// Normalize configuration of joint
/// Do nothing
template <size_type dimension>
void TranslationJointConfig <dimension>::normalize
(const size_type&, ConfigurationOut_t) const
{
}
template <size_type dimension>
void TranslationJointConfig <dimension>::uniformlySample
(const size_type& index, ConfigurationOut_t result) const
......@@ -399,6 +428,17 @@ namespace hpp {
result [indexVelocity] = atan2 (s1*c2 - s2*c1, c1*c2 + s1*s2);
}
/// Normalize configuration of joint
void UnBounded::normalize (const size_type& index,
ConfigurationOut_t result) const
{
value_type c = result [index];
value_type s = result [index + 1];
value_type d = sqrt (c*c + s*s);
result [index] /= d;
result [index + 1] /= d;
}
void UnBounded::uniformlySample (const size_type& index,
ConfigurationOut_t result) const
{
......@@ -448,6 +488,13 @@ namespace hpp {
result [indexVelocity] = q1 [indexConfig] - q2 [indexConfig];
}
/// Normalize configuration of joint
/// Do nothing.
void Bounded::normalize (const size_type&,
ConfigurationOut_t) const
{
}
void Bounded::uniformlySample (const size_type& index,
ConfigurationOut_t result) const
{
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment