Commit 23cf9c96 authored by Florent Lamiraux's avatar Florent Lamiraux
Browse files

Fix bug in rotationJoint::Unbounded::difference.

parent bbe82a3c
...@@ -389,7 +389,11 @@ namespace hpp { ...@@ -389,7 +389,11 @@ namespace hpp {
value_type c2 = q2 [indexConfig]; value_type c2 = q2 [indexConfig];
value_type s2 = q2 [indexConfig + 1]; value_type s2 = q2 [indexConfig + 1];
result [indexVelocity] = atan2 (-s1*c2 + s2*c1, c1*c2 + s1*s2); // s1 = sin q1 c1 = cos (q1)
// s2 = sin q2 c2 = cos (q2)
// sin (q1 - q2) = s1*c2 - s2*c1
// cos (q1 - q2) = c1*c2 + s1*s2
result [indexVelocity] = atan2 (s1*c2 - s2*c1, c1*c2 + s1*s2);
} }
void UnBounded::uniformlySample (const size_type& index, void UnBounded::uniformlySample (const size_type& index,
......
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