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

Fix bug in integrate and difference methods of SO3JointConfig

parent 1ab266f5
......@@ -231,7 +231,7 @@ namespace hpp {
q1 [indexConfig + 2], q1 [indexConfig + 3]);
Quaternion_t p2 (q2 [indexConfig + 0], q2 [indexConfig + 1],
q2 [indexConfig + 2], q2 [indexConfig + 3]);
Quaternion_t p (p1); p.conj (); p*=p2;
Quaternion_t p (p2); p.conj (); p = p1*p;
value_type angle; fcl::Vec3f axis;
p.toAxisAngle (axis, angle);
result [indexVelocity + 0] = angle*axis [0];
......
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