Transform DynamicsComputations::getWorldTransform(unsigned int frameIndex)
{
if( frameIndex >= this->getNrOfFrames() )
{
reportError("DynamicsComputations","getWorldTransform","frameIndex out of bound");
return iDynTree::Transform();
}
// compute fwd kinematics (if necessary)
this->computeFwdKinematics();
iDynTree::Transform ret = iDynTree::ToiDynTree(this->pimpl->m_fwdKinematicsResults[frameIndex]);
// Setting position semantics
PositionSemantics posSem;
posSem.setCoordinateFrame(WORLD_INDEX);
posSem.setReferencePoint(WORLD_INDEX);
posSem.setPoint(frameIndex);
ret.getSemantics().setPositionSemantics(posSem);
// Setting rotation semantics
RotationSemantics rotSem;
rotSem.setReferenceOrientationFrame(WORLD_INDEX);
rotSem.setCoordinateFrame(WORLD_INDEX);
rotSem.setOrientationFrame(frameIndex);
ret.getSemantics().setRotationSemantics(rotSem);
return ret;
}
Transform DynamicsComputations::getRelativeTransform(unsigned int refFrameIndex,
unsigned int frameIndex)
{
if( frameIndex >= this->getNrOfFrames() )
{
reportError("DynamicsComputations","getRelativeTransform","frameIndex out of bound");
return iDynTree::Transform();
}
if( refFrameIndex >= this->getNrOfFrames() )
{
reportError("DynamicsComputations","getRelativeTransform","refFrameIndex out of bound");
return iDynTree::Transform();
}
KDL::Frame world_H_frame = this->pimpl->m_fwdKinematicsResults[frameIndex];
KDL::Frame world_H_refFrame = this->pimpl->m_fwdKinematicsResults[refFrameIndex];
KDL::Frame refFrame_H_frame = world_H_refFrame.Inverse()*world_H_frame;
iDynTree::Transform ret = iDynTree::ToiDynTree(refFrame_H_frame);
// set semantics
// Setting position semantics
PositionSemantics posSem;
posSem.setCoordinateFrame(refFrameIndex);
posSem.setReferencePoint(refFrameIndex);
posSem.setPoint(frameIndex);
ret.getSemantics().setPositionSemantics(posSem);
// Setting rotation semantics
RotationSemantics rotSem;
rotSem.setReferenceOrientationFrame(refFrameIndex);
rotSem.setCoordinateFrame(refFrameIndex);
rotSem.setOrientationFrame(frameIndex);
ret.getSemantics().setRotationSemantics(rotSem);
return ret;
}