Commit dd1e12f0 authored by Pierre Fernbach's avatar Pierre Fernbach
Browse files

computeCenterOfContactAtStateForLimb correctly apply the offset when the rotation is not Identity

parent e43905b2
......@@ -2134,9 +2134,8 @@ namespace hpp {
res->length ((_CORBA_ULong)2);
fcl::Vec3f& position = state.contactPositions_.at(limbName);
position += fullBody()->GetLimb(limb)->offset_;
fcl::Transform3f jointT( state.contactRotation_.at(limbName), state.contactPositions_.at(limbName));
fcl::Vec3f position = jointT.transform(fullBody()->GetLimb(limb)->offset_);
_CORBA_ULong size = (_CORBA_ULong) 3;
double* dofArray = hpp::floatSeq::allocbuf(size);
hpp::floatSeq floats (size, size, dofArray, true);
......
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