Commit b213f61d authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel
Browse files

Fix bug in jointRotation::UnBounded::integrate

parent 93f95889
......@@ -370,11 +370,11 @@ namespace hpp {
value_type omega = v [indexVelocity];
value_type cosOmega = cos (omega);
value_type sinOmega = sin (omega);
value_type cosq = q [indexConfig];
value_type sinq = q [indexConfig + 1];
value_type norm2p = q.segment (indexConfig, 2).squaredNorm ();
result [indexConfig] = (1.5-.5*norm2p) *
(cosOmega * q [indexConfig] - sinOmega * q [indexConfig + 1]);
result [indexConfig + 1] = (1.5-.5*norm2p) *
sinOmega * q [indexConfig] + cosOmega * q [indexConfig + 1];
result [indexConfig ] = (1.5-.5*norm2p) * (cosOmega * cosq - sinOmega * sinq);
result [indexConfig + 1] = (1.5-.5*norm2p) * (sinOmega * cosq + cosOmega * sinq);
}
void UnBounded::difference (ConfigurationIn_t q1, ConfigurationIn_t q2,
......
Supports Markdown
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