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