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