Commit 329c1b96 authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel
Browse files

Update representation of SO3 using quaternions

* a quaternion and its opposite represents the same quaternion. Methods
* difference and distance returns the shortest possible value.
parent 13634b1c
......@@ -159,12 +159,14 @@ namespace hpp {
/// \return angle between both joint configuration
static value_type angleBetweenQuaternions (ConfigurationIn_t q1,
ConfigurationIn_t q2,
const size_type& index)
const size_type& index,
bool& cosIsNegative)
{
value_type innerprod = q1.segment (index, 4).dot (q2.segment (index, 4));
assert (fabs (innerprod) < 1.0001);
if (innerprod < -1) innerprod = -1;
if (innerprod > 1) innerprod = 1;
cosIsNegative = (innerprod < 0);
value_type theta = acos (innerprod);
return theta;
}
......@@ -176,15 +178,20 @@ namespace hpp {
ConfigurationOut_t result)
{
// for rotation part, transform roll pitch yaw into quaternion
value_type theta = angleBetweenQuaternions (q1, q2, index);
if (fabs (theta) > 1e-6) {
bool cosIsNegative;
const value_type angle = angleBetweenQuaternions (q1, q2, index, cosIsNegative);
const int invertor = (cosIsNegative) ? -1 : 1;
const value_type theta = (cosIsNegative) ? (M_PI - angle) : angle;
// theta is between 0 and M_PI/2.
// sin(alpha*theta)/sin(theta) in 0 should be computed differently.
if (fabs (angle) > 1e-6) {
result.segment (index, 4) =
(sin ((1-u)*theta)/sin (theta)) * q1.segment (index, 4) +
(sin ((1-u)*theta)/sin (theta)) * invertor * q1.segment (index, 4) +
(sin (u*theta)/sin (theta)) * q2.segment (index, 4);
} else {
result.segment (index, 4) =
(1-u) * q1.segment (index, 4) + u * q2.segment (index, 4);
(1-u) * invertor * q1.segment (index, 4) + u * q2.segment (index, 4);
}
}
......@@ -192,9 +199,11 @@ namespace hpp {
ConfigurationIn_t q2,
const size_type& index) const
{
value_type theta = angleBetweenQuaternions (q1, q2, index);
bool cosIsNegative;
value_type theta = angleBetweenQuaternions (q1, q2, index, cosIsNegative);
assert (theta >= 0);
return theta;
assert (M_PI - theta >= 0);
return (cosIsNegative) ? M_PI - theta : theta;
}
void SO3JointConfig::integrate (ConfigurationIn_t q,
......@@ -208,11 +217,11 @@ namespace hpp {
value_type angle = .5*omega.norm();
if (angle == 0) {
result.segment (indexConfig, 4) = q.segment (indexConfig, 4);
result.segment<4> (indexConfig) = q.segment <4> (indexConfig);
return;
}
// try to keep norm of quaternion close to 1.
value_type norm2p = q.segment (indexConfig, 4).squaredNorm ();
value_type norm2p = q.segment <4> (indexConfig).squaredNorm ();
Quaternion_t p ((1.5-.5*norm2p) * q [indexConfig + 0],
(1.5-.5*norm2p) * q [indexConfig + 1],
(1.5-.5*norm2p) * q [indexConfig + 2],
......@@ -233,34 +242,24 @@ namespace hpp {
const size_type& indexVelocity,
vectorOut_t result) const
{
const int invertor =
(q1.segment <4> (indexConfig)
.dot (q2.segment <4> (indexConfig)) < 0
) ? -1 : 1;
// Compute rotation vector between q2 and q1.
Quaternion_t p1 (q1 [indexConfig + 0], q1 [indexConfig + 1],
q1 [indexConfig + 2], q1 [indexConfig + 3]);
Quaternion_t p2 (q2 [indexConfig + 0], q2 [indexConfig + 1],
q2 [indexConfig + 2], q2 [indexConfig + 3]);
Quaternion_t p1 (invertor * q1.segment <4> (indexConfig));
Quaternion_t p2 (q2.segment <4> (indexConfig));
Quaternion_t p (p1*p2.conjugate ());
value_type angle; Eigen::Matrix <value_type, 3, 1> axis;
AngleAxis_t angleAxis (p);
angle = angleAxis.angle ();
axis = angleAxis.axis ();
result [indexVelocity + 0] = angle*axis [0];
result [indexVelocity + 1] = angle*axis [1];
result [indexVelocity + 2] = angle*axis [2];
result.segment <3> (indexVelocity) = angleAxis.angle () * angleAxis.axis ();
}
/// 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;
result.segment <4> (index).normalize ();
}
void SO3JointConfig::uniformlySample (const size_type& index,
......
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