예제 #1
0
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;
}
예제 #3
0
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;

}