arma::mat TaskOrientation::getJacobianForTask(const KinematicTree &kinematicTree, arma::mat &jacobianWithoutRemovedDOFs, bool normalizeJacobian) const
{
UNUSED(normalizeJacobian); // the jacobian must be normalized within its calculation!
arma::mat jacobian = arma::zeros(getDimensionCnt(), kinematicTree.getMotorCnt());
arma::mat33 forward = arma::eye(3, 3);
arma::mat33 intermediateTransition = arma::eye(3, 3);
for (uint32_t pathIndex = 0; pathIndex < m_invPath.size(); ++pathIndex)
{
const KinematicNode *node = m_invPath[pathIndex].m_node;
arma::colvec3 orientationOfNode = forward.col(m_axis);
if ((false == node->isFixedNode()) &&
(KinematicPathNode::Direction::LINK != m_invPath[pathIndex].m_direction) &&
(pathIndex < m_invPath.size() - 1))
{
kinematics::JacobianValues partialDerivatives = node->getPartialDerivativeOfOrientationToEffector(orientationOfNode);
double sign = 1;
if (KinematicPathNode::Direction::FROM_PARENT == m_invPath[pathIndex].m_direction)
{
/* in this case the rotation axis of the joint is inverted... */
sign = -1;
}
for (kinematics::JacobianValue value : partialDerivatives) {
jacobian.col(value.first) = value.second * sign;
}
}
if (pathIndex < m_invPath.size() - 1)
{
const KinematicNode *nextNode = m_invPath[pathIndex + 1].m_node;
intermediateTransition = node->getInvMatrixToRelative(nextNode).submat(0, 0, 2, 2);
forward = intermediateTransition * forward;
/* update the previously calculated entries in the jacobian according to our current orientation */
jacobian = intermediateTransition * jacobian;
}
}
/* transform the jacobian into the reference coordinate system */
arma::mat33 transformToReferenceSystem = kinematicTree.getTransitionMatrixFromTo(m_referenceCoordinateSystem, m_baseNode).submat(0, 0, 2, 2);
jacobian = transformToReferenceSystem * jacobian;
jacobianWithoutRemovedDOFs = jacobian;
removeDOFfromJacobian(jacobian, kinematicTree);
return jacobian;
}
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;
}