void makeReflection(::arma::mat44 & matOut, const ::arma::vec3 & normal) { // Using householder transformation. // See: http://en.wikipedia.org/wiki/Transformation_matrix#Reflection_2 matOut.eye(); matOut.submat(0, 0, 2, 2) = ::arma::eye< ::arma::mat>(3, 3) - 2.0 * normal * normal.t(); }
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); } }
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; }
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; }
void makeZRotation(::arma::mat44 & matOut, const double angle) { matOut.eye(); matOut(0, 0) = cos(angle); matOut(0, 1) = -sin(angle); matOut(1, 0) = sin(angle); matOut(1, 1) = cos(angle); }