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