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