void KinematicTree::calculateEffectorsPositions(
std::map<NodeID, arma::colvec3> &positions,
const KinematicNode *node,
arma::mat44 const& prev) const
{
const arma::mat44 base = prev * node->getForwardMatrix();
positions[node->getID()] = base.col(3).rows(0, 2);
for (const KinematicNode* const &child : node->getChildren()) {
calculateEffectorsPositions(positions, child, base);
}
}
KinematicEngineTaskFixed::KinematicEngineTaskFixed(std::string name,
MotorID baseNode,
MotorID effectorNode,
const KinematicTree &tree)
: KinematicEngineTask(name, baseNode, effectorNode, tree)
, m_locationTask(name, baseNode, effectorNode, tree)
, m_rotationTaskX(name, baseNode, effectorNode, tree, KinematicEngineTaskOrientation::AXIS_X)
, m_rotationTaskY(name, baseNode, effectorNode, tree, KinematicEngineTaskOrientation::AXIS_Y)
{
m_dimCnt = 9;
m_target = arma::zeros(m_dimCnt);
const arma::mat44 transition = tree.getTransitionMatrixFromTo(baseNode, effectorNode);
m_rotationTaskX.setTarget(transition.col(0).rows(0, 2));
m_rotationTaskY.setTarget(transition.col(1).rows(0, 2));
m_locationTask.setTarget(transition.col(3).rows(0, 2));
m_target.rows(0, 2) = m_rotationTaskX.getTarget();
m_target.rows(3, 5) = m_rotationTaskY.getTarget();
m_target.rows(6, 8) = m_locationTask.getTarget();
m_hasTarget = true;
}
arma::colvec TaskOrientation::getError(const KinematicTree &kinematicTree) const {
/* calculate the orientation of the effector */
const arma::mat44 transition = kinematicTree.getTransitionMatrixFromTo(m_referenceCoordinateSystem, m_effectorNode);
arma::colvec3 value = m_method->getTransform() * transition.col(m_axis).rows(0, 2);
double norm = arma::dot(m_method->getTarget(), value);
if (0. < m_precision)
{
if (norm <= m_precision)
{
/* this means target reached */
value = m_method->getTarget();
}
}
return m_method->getTarget() - value;
}