// 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; }