// 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::check_changeRefPoint(const PositionSemantics& newPosition)
{
return ( reportErrorIf(!checkEqualOrUnknown(newPosition.coordinateFrame,this->coordinateFrame),
__PRETTY_FUNCTION__,
"newPosition expressed in a different coordinateFrames\n")
&& reportErrorIf(!checkEqualOrUnknown(newPosition.point,this->refPoint),
__PRETTY_FUNCTION__,
"newPosition point is different from the original reference point\n"));
}
bool PositionSemantics::check_compose(const PositionSemantics& op1, const PositionSemantics& op2)
{
return ( reportErrorIf(!checkEqualOrUnknown(op1.coordinateFrame,op2.coordinateFrame),
__PRETTY_FUNCTION__,
"composing two position expressed in different coordinateFrames\n")
&& reportErrorIf(!checkEqualOrUnknown(op1.refPoint,op2.point) && (op1.point == op2.refPoint),
__PRETTY_FUNCTION__,
"Position(a|A,c|C) = compose(Position(b|B,c|C),Position(a|A,b|B)) is forbidded in iDynTree to avoid ambiguity on compose(Position(b|B,a|A),Position(a|A,b|B))\n")
&& reportErrorIf(!checkEqualOrUnknown(op1.refPoint,op2.point),
__PRETTY_FUNCTION__,
"composing two position expressed in different coordinateFrames\n"));
}
bool PositionSemantics::check_compose(const PositionSemantics& op1, const PositionSemantics& op2)
{
return ( reportErrorIf(!checkEqualOrUnknown(op1.coordinateFrame,op2.coordinateFrame),
IDYNTREE_PRETTY_FUNCTION,
"composing two positions expressed in different coordinateFrames\n")
&& reportErrorIf(!checkEqualOrUnknown(op1.refPoint,op2.point) && (op1.point == op2.refPoint),
IDYNTREE_PRETTY_FUNCTION,
"Position(a|A,c|C) = compose(Position(b|B,c|C),Position(a|A,b|B)) is forbidded in iDynTree to avoid ambiguity on compose(Position(b|B,a|A),Position(a|A,b|B))\n")
&& reportErrorIf(!checkEqualOrUnknown(op1.refPoint,op2.point),
IDYNTREE_PRETTY_FUNCTION,
"Position op1 reference point and Position op2 point don't match\n")
&& reportErrorIf(!checkEqualOrUnknown(op1.refBody,op2.body),
IDYNTREE_PRETTY_FUNCTION,
"Position op1 reference body and Position op2 body don't match\n"));
}
bool PositionSemantics::check_changeRefPoint(const PositionSemantics& newRefPoint)
{
return ( reportErrorIf(!checkEqualOrUnknown(newRefPoint.coordinateFrame,this->coordinateFrame),
IDYNTREE_PRETTY_FUNCTION,
"newRefPoint expressed in a different coordinateFrames\n")
&& reportErrorIf(!checkEqualOrUnknown(newRefPoint.point,this->refPoint),
IDYNTREE_PRETTY_FUNCTION,
"newRefPoint point is different from the original reference point\n")
&& reportErrorIf(!checkEqualOrUnknown(newRefPoint.body,newRefPoint.refBody),
IDYNTREE_PRETTY_FUNCTION,
"newRefPoint point and reference point are not fixed to the same body\n")
&& reportErrorIf(!checkEqualOrUnknown(newRefPoint.body,this->refBody),
IDYNTREE_PRETTY_FUNCTION,
"newRefPoint point and original Position reference point are not fixed to the same body\n"));
}
int DynamicsComputations::getFrameIndex(const std::string& frameName) const
{
// Currently KDL::CoDyCo::UndirectedTree mixes the concepts of frames and links
// see https://github.com/robotology/codyco-modules/issues/39
// Once we have a proper iDynTree::Model, we can properly implement
// the difference between frame and link
int index = this->pimpl->m_robot_model.getLink(frameName)->getLinkIndex();
reportErrorIf(index < 0, "DynamicsComputations::getFrameIndex", "requested frameName not found in model");
return index;
}
bool AngularForceVector3Semantics::compose(const AngularForceVector3Semantics & op1,
const AngularForceVector3Semantics & op2,
AngularForceVector3Semantics & result)
{
// check semantics
bool semantics_status =
( reportErrorIf(!checkEqualOrUnknown(op1.point,op2.point),
IDYNTREE_PRETTY_FUNCTION,
"op1 point and op2 point don't match\n")
&& ForceVector3Semantics<AngularForceVector3Semantics>::compose(op1, op2, result));
// compute semantics;
result.point = op1.point;
return semantics_status;
}
int DynamicsComputations::getLinkIndex(const std::string& linkName) const
{
int index = this->pimpl->m_robot_model.getLink(linkName)->getLinkIndex();
reportErrorIf(index < 0, "DynamicsComputations::getLinkIndex", "requested frameName not found in model");
return index;
}
