std::map<MotorID, double> KinematicEngine::solveIKStepSpeeds(kinematics::MotorValuesMap curValues, kinematics::MotorValuesMap curSpeeds, Tasks& tasks, Constraints const& constraints) { fillTreeWithValues(tasks.getTree(), curValues, curSpeeds, false); std::map<MotorID, double> newSpeeds; m_inverseKinematic.calculateSpeeds(tasks.getTree(), newSpeeds, tasks.getTasks(), constraints, tasks.getIdleTask()); return newSpeeds; }
std::map<MotorID, double> KinematicEngine::solveIKStepValues(kinematics::MotorValuesMap curValues, kinematics::MotorValuesMap curSpeeds, Tasks& tasks, Constraints const& constraints, int iterationCnt, double& error) { std::map<MotorID, double> newValues = curValues; for (int i(0); i < iterationCnt; ++i) { fillTreeWithValues(tasks.getTree(), newValues, curSpeeds, (i == 0)); error = m_inverseKinematic.iterationStep(tasks.getTree(), newValues, tasks.getTasks(), constraints, tasks.getIdleTask()); } return newValues; }
std::map<MotorID, double> KinematicEngine::solveIKGravityOvershoot(kinematics::MotorValuesMap curValues, kinematics::MotorValuesMap curSpeeds, Tasks& tasks, kinematicEngine::Task& task, double& error) { fillTreeWithValues(tasks.getTree(), curValues, curSpeeds, false); std::map<MotorID, double> torques; error = m_inverseKinematic.iterationStepGravitation(tasks.getTree(), torques, task); for (std::pair<kinematics::NodeID, KinematicNode*> const& node : tasks.getTree().getNodes()) { kinematics::MotorIDs const& motorIDs = node.second->getMotors(); for (MotorID motor : motorIDs) { torques[motor] = utils::limited(torques[motor], -m_overshootValues[motor], m_overshootValues[motor]); } } return torques; }