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