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;
}
// Semantics operations
bool AngularForceVector3Semantics::changePoint(const PositionSemantics & newPoint,
const LinearForceVector3Semantics & otherLinear,
AngularForceVector3Semantics & resultAngular) const
{
// check semantics
bool semantics_status =
( reportErrorIf(!checkEqualOrUnknown(newPoint.getCoordinateFrame(),this->coordinateFrame),
IDYNTREE_PRETTY_FUNCTION,
"newPoint expressed in a different coordinateFrame\n")
&& reportErrorIf(!checkEqualOrUnknown(newPoint.getReferencePoint(),this->point),
IDYNTREE_PRETTY_FUNCTION,
"newPoint has a reference point different from the original Force vector point\n")/*
&& reportErrorIf(!checkEqualOrUnknown(newPoint.getBody(),newPoint.getRefBody()),
IDYNTREE_PRETTY_FUNCTION,
"newPoint point and reference point are not fixed to the same body\n")
&& reportErrorIf(!checkEqualOrUnknown(newPoint.getRefBody(),this->body),
IDYNTREE_PRETTY_FUNCTION,
"newPoint reference point and original Force vector point are not fixed to the same body\n")*/
&& reportErrorIf(!checkEqualOrUnknown(otherLinear.getCoordinateFrame(),this->coordinateFrame),
IDYNTREE_PRETTY_FUNCTION,
"otherLinear expressed in a different coordinateFrame\n")
&& reportErrorIf(!checkEqualOrUnknown(otherLinear.getBody(),this->body),
IDYNTREE_PRETTY_FUNCTION,
"The bodies defined for both linear and angular force vectors don't match\n")
&& reportErrorIf(!checkEqualOrUnknown(otherLinear.getRefBody(),this->refBody),
IDYNTREE_PRETTY_FUNCTION,
"The reference bodies defined for both linear and angular force vectors don't match\n"));
// compute semantics
resultAngular = *this;
resultAngular.point = newPoint.getPoint();
return semantics_status;
}
bool PositionSemantics::changePoint(const PositionSemantics& newPoint)
{
// check semantics
bool status = this->check_changePoint(newPoint);
// set new semantics
this->point = newPoint.getPoint();
return status;
}
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;
}
