Commit df761e9d authored by Florent Lamiraux's avatar Florent Lamiraux
Browse files

In JointConfiguration, make sure that distance and difference are zero

  - whenever configurations are equal.
parent 0f53ce3b
......@@ -162,6 +162,10 @@ namespace hpp {
const size_type& index,
bool& cosIsNegative)
{
if (q1.segment (index, 4) == q2.segment (index, 4)) {
cosIsNegative = false;
return 0;
}
value_type innerprod = q1.segment (index, 4).dot (q2.segment (index, 4));
assert (fabs (innerprod) < 1.0001);
if (innerprod < -1) innerprod = -1;
......@@ -242,6 +246,10 @@ namespace hpp {
const size_type& indexVelocity,
vectorOut_t result) const
{
if (q1.segment <4> (indexConfig) == q2.segment <4> (indexConfig)) {
result.segment <3> (indexVelocity).setZero ();
return;
}
const int invertor =
(q1.segment <4> (indexConfig)
.dot (q2.segment <4> (indexConfig)) < 0
......@@ -397,14 +405,9 @@ namespace hpp {
ConfigurationIn_t q2,
const size_type& index) const
{
// distance on the unit circle
value_type innerprod =
q1.segment (index, 2).dot (q2.segment (index, 2));
assert (fabs (innerprod) < 1.0001);
if (innerprod < -1) innerprod = -1;
if (innerprod > 1) innerprod = 1;
value_type theta = acos (innerprod);
return theta;
Eigen::Matrix <value_type, 1, 1> result;
difference (q1, q2, index, 0, result);
return fabs (result [0]);
}
void UnBounded::integrate (ConfigurationIn_t q, vectorIn_t v,
......
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