45 for(std::size_t i = 0; i < l.size(); i++) \ 47 LOG(#l "[" + std::to_string(i) + "]", l[i]); \ 85 std::vector<double> solution;
88 std::vector<std::pair<fann_type, fann_type>> input_minmax, output_minmax;
90 static void find_minmax(
const std::vector<fann_type>& values, std::vector<std::pair<fann_type, fann_type>>& minmax)
92 for(
size_t i = 0; i < minmax.size(); i++)
94 minmax[i] = std::make_pair(values[i], values[i]);
96 for(
size_t i = 0; i < values.size(); i++)
99 auto& p = minmax[i % minmax.size()];
100 p.first = std::min(p.first, v);
101 p.second = std::max(p.second, v);
105 static void normalize(std::vector<fann_type>& values,
const std::vector<std::pair<fann_type, fann_type>>& minmax)
107 for(
size_t i = 0; i < values.size(); i++)
110 auto& p = minmax[i % minmax.size()];
111 v = (v - p.first) / (p.second - p.first);
115 static void denormalize(std::vector<fann_type>& values,
const std::vector<std::pair<fann_type, fann_type>>& minmax)
117 for(
size_t i = 0; i < values.size(); i++)
120 auto& p = minmax[i % minmax.size()];
121 v = v * (p.second - p.first) + p.first;
125 unsigned int input_count, output_count;
139 input_count = problem.active_variables.size() + problem.tip_link_indices.size() * 6;
140 output_count = problem.active_variables.size();
142 LOG_VAR(input_count);
143 LOG_VAR(output_count);
153 std::vector<unsigned int> levels = {input_count, 50, output_count};
155 net.create_standard_array(levels.size(), levels.data());
157 size_t var_count = params.robot_model->getVariableCount();
158 std::vector<double> state1(var_count), state2(var_count);
159 std::vector<Frame> frames1, frames2;
161 std::vector<fann_type> inputs, outputs;
162 std::vector<fann_type*> input_pp, output_pp;
164 LOG(
"neuro ik generating training data");
166 unsigned int samples = 10000;
168 for(
size_t iter = 0; iter < samples; iter++)
170 for(
size_t ivar = 0; ivar < var_count; ivar++)
172 state1[ivar] = random(modelInfo.getMin(ivar), modelInfo.getMax(ivar));
173 state1[ivar] = modelInfo.clip(state1[ivar], ivar);
175 state2[ivar] = modelInfo.clip(state1[ivar] + random_gauss() * 0.1, ivar);
178 model.applyConfiguration(state1);
179 frames1 = model.getTipFrames();
180 model.applyConfiguration(state2);
181 frames2 = model.getTipFrames();
183 for(
auto ivar : problem.active_variables)
185 inputs.push_back(state1[ivar]);
186 outputs.push_back(state2[ivar] - state1[ivar]);
189 for(
size_t itip = 0; itip < problem.tip_link_indices.size(); itip++)
191 double translational_scale = 1.0;
192 double rotational_scale = 1.0;
196 auto twist = frameTwist(frames1[itip], frames2[itip]);
198 inputs.push_back(frames2[itip].pos.x() - frames1[itip].pos.x());
199 inputs.push_back(frames2[itip].pos.y() - frames1[itip].pos.y());
200 inputs.push_back(frames2[itip].pos.z() - frames1[itip].pos.z());
202 inputs.push_back(twist.rot.x() * rotational_scale);
203 inputs.push_back(twist.rot.y() * rotational_scale);
204 inputs.push_back(twist.rot.z() * rotational_scale);
208 for(
auto& v : inputs)
209 if(!std::isfinite(v))
throw std::runtime_error(
"NAN");
210 for(
auto& v : outputs)
211 if(!std::isfinite(v))
throw std::runtime_error(
"NAN");
213 input_minmax.resize(input_count);
214 output_minmax.resize(output_count);
216 find_minmax(inputs, input_minmax);
217 find_minmax(outputs, output_minmax);
219 normalize(inputs, input_minmax);
220 normalize(outputs, output_minmax);
222 for(
size_t iter = 0; iter < samples; iter++)
224 input_pp.push_back(inputs.data() + iter * input_count);
225 output_pp.push_back(outputs.data() + iter * output_count);
228 LOG(
"neuro ik training");
230 FANN::training_data train;
231 train.set_train_data(samples, input_count, input_pp.data(), output_count, output_pp.data());
233 [](FANN::neural_net& net, FANN::training_data& train,
unsigned int max_epochs,
unsigned int epochs_between_reports,
float desired_error,
unsigned int epochs,
void* user_data) {
234 if(epochs % epochs_between_reports != 0)
return 0;
236 LOG(
"training", epochs, net.get_MSE(), desired_error);
241 net.set_activation_function_hidden(FANN::SIGMOID);
242 net.set_activation_function_output(FANN::SIGMOID);
244 net.init_weights(train);
246 net.train_on_data(train, 1000, 1, 0.0001);
248 fann_type err = net.test_data(train);
249 LOG(
"neuro ik training error:", err);
265 LOG(
"training done");
268 size_t iterations = 0;
270 void initialize(
const Problem& problem)
272 static std::mutex mutex;
273 std::lock_guard<std::mutex> lock(mutex);
274 IKBase::initialize(problem);
275 solution = problem.initial_guess;
276 if(!trained) train();
279 for(
auto& vi : problem.active_variables)
280 solution[vi] = random(modelInfo.getMin(vi), modelInfo.getMax(vi));
283 const std::vector<double>& getSolution()
const {
return solution; }
285 std::vector<fann_type> inputs, outputs;
287 std::vector<Frame> tip_objectives;
364 for(
auto ivar : problem.active_variables)
366 inputs.push_back(solution[ivar]);
369 tip_objectives.resize(model.getTipFrames().size());
370 for(
auto& g : problem.goals)
372 tip_objectives[g.tip_index] = g.frame;
375 model.applyConfiguration(solution);
376 auto& frames1 = model.getTipFrames();
377 auto& frames2 = tip_objectives;
380 for(
size_t itip = 0; itip < tip_objectives.size(); itip++)
382 double translational_scale = 1.0;
383 double rotational_scale = 1.0;
384 auto twist = frameTwist(frames1[itip], frames2[itip]);
385 auto dpos = frames2[itip].pos - frames1[itip].pos;
386 auto drot = Vector3(twist.rot.x() * rotational_scale, twist.rot.y() * rotational_scale, twist.rot.z() * rotational_scale);
408 scale = 1.0 / (0.0000001 + dpos.length() + drot.length());
409 inputs.push_back(dpos.x() * scale);
410 inputs.push_back(dpos.y() * scale);
411 inputs.push_back(dpos.z() * scale);
412 inputs.push_back(drot.x() * scale);
413 inputs.push_back(drot.y() * scale);
414 inputs.push_back(drot.z() * scale);
417 normalize(inputs, input_minmax);
418 auto* oo = net.run(inputs.data());
419 outputs.assign(oo, oo + output_count);
420 denormalize(outputs, output_minmax);
421 auto& vv = problem.active_variables;
422 for(
size_t iout = 0; iout < vv.size(); iout++)
424 size_t ivar = vv[iout];
425 solution[ivar] = modelInfo.clip(solution[ivar] + outputs[iout] * 1 / scale, ivar);
434 std::vector<double> solution;
435 FANN::neural_net net;
437 std::vector<std::pair<fann_type, fann_type>> input_minmax, output_minmax;
454 static void find_minmax(
const std::vector<fann_type>& values, std::vector<std::pair<fann_type, fann_type>>& minmax)
456 std::vector<double> centers(minmax.size(), 0.0);
457 for(
size_t i = 0; i < values.size(); i++)
458 centers[i % minmax.size()] += values[i] * (1.0 * minmax.size() / values.size());
460 std::vector<double> ranges2(minmax.size(), 0.0001);
461 for(
size_t i = 0; i < values.size(); i++)
463 double d = values[i] - centers[i % minmax.size()];
465 ranges2[i % minmax.size()] += d * (1.0 * minmax.size() / values.size());
468 for(
size_t i = 0; i < minmax.size(); i++)
471 p.first = centers[i] - sqrt(ranges2[i]);
472 p.second = centers[i] + sqrt(ranges2[i]);
476 static void normalize(std::vector<fann_type>& values,
const std::vector<std::pair<fann_type, fann_type>>& minmax)
478 for(
size_t i = 0; i < values.size(); i++)
481 auto& p = minmax[i % minmax.size()];
482 v = (v - p.first) / (p.second - p.first);
486 static void denormalize(std::vector<fann_type>& values,
const std::vector<std::pair<fann_type, fann_type>>& minmax)
488 for(
size_t i = 0; i < values.size(); i++)
491 auto& p = minmax[i % minmax.size()];
492 v = v * (p.second - p.first) + p.first;
496 unsigned int input_count, output_count;
510 input_count = problem.tip_link_indices.size() * 7;
511 output_count = problem.active_variables.size();
513 LOG_VAR(input_count);
514 LOG_VAR(output_count);
520 std::vector<unsigned int> levels = {input_count, input_count + output_count, output_count};
526 net.create_standard_array(levels.size(), levels.data());
528 size_t var_count = params.robot_model->getVariableCount();
529 std::vector<double> state = problem.initial_guess;
530 std::vector<Frame> frames;
532 std::vector<fann_type> inputs, outputs;
533 std::vector<fann_type*> input_pp, output_pp;
535 LOG(
"neuro ik generating training data");
537 unsigned int samples = 10000;
539 for(
size_t iter = 0; iter < samples; iter++)
541 for(
size_t ivar : problem.active_variables)
542 state[ivar] = random(modelInfo.getMin(ivar), modelInfo.getMax(ivar));
544 model.applyConfiguration(state);
545 frames = model.getTipFrames();
547 for(
auto ivar : problem.active_variables)
548 outputs.push_back(state[ivar]);
550 for(
size_t itip = 0; itip < problem.tip_link_indices.size(); itip++)
552 inputs.push_back(frames[itip].pos.x());
553 inputs.push_back(frames[itip].pos.y());
554 inputs.push_back(frames[itip].pos.z());
556 auto rot = frames[itip].rot;
559 inputs.push_back(rot.x());
560 inputs.push_back(rot.y());
561 inputs.push_back(rot.z());
562 inputs.push_back(rot.w());
566 for(
auto& v : inputs)
567 if(!std::isfinite(v))
throw std::runtime_error(
"NAN");
568 for(
auto& v : outputs)
569 if(!std::isfinite(v))
throw std::runtime_error(
"NAN");
571 input_minmax.resize(input_count);
572 output_minmax.resize(output_count);
574 find_minmax(inputs, input_minmax);
575 find_minmax(outputs, output_minmax);
577 normalize(inputs, input_minmax);
578 normalize(outputs, output_minmax);
580 for(
size_t iter = 0; iter < samples; iter++)
582 input_pp.push_back(inputs.data() + iter * input_count);
583 output_pp.push_back(outputs.data() + iter * output_count);
586 LOG(
"neuro ik training");
588 FANN::training_data train;
589 train.set_train_data(samples, input_count, input_pp.data(), output_count, output_pp.data());
591 [](FANN::neural_net& net, FANN::training_data& train,
unsigned int max_epochs,
unsigned int epochs_between_reports,
float desired_error,
unsigned int epochs,
void* user_data) {
592 if(epochs % epochs_between_reports != 0)
return 0;
594 LOG(
"training", epochs, net.get_MSE(), desired_error);
599 net.set_activation_function_hidden(FANN::SIGMOID);
600 net.set_activation_function_output(FANN::SIGMOID);
602 net.init_weights(train);
604 net.train_on_data(train, 100, 1, 0.0001);
606 fann_type err = net.test_data(train);
607 LOG(
"neuro ik training error:", err);
623 LOG(
"training done");
626 size_t iterations = 0;
628 void initialize(
const Problem& problem)
630 IKBase::initialize(problem);
631 solution = problem.initial_guess;
632 if(!trained) train();
636 const std::vector<double>& getSolution()
const {
return solution; }
638 std::vector<fann_type> inputs, outputs;
640 std::vector<Frame> tip_objectives;
644 if(iterations > 1)
return;
649 tip_objectives.resize(model.getTipFrames().size());
650 for(
auto& g : problem.goals)
652 tip_objectives[g.tip_index] = g.frame;
655 auto& frames = tip_objectives;
657 for(
size_t itip = 0; itip < tip_objectives.size(); itip++)
659 inputs.push_back(frames[itip].pos.x());
660 inputs.push_back(frames[itip].pos.y());
661 inputs.push_back(frames[itip].pos.z());
663 auto rot = frames[itip].rot;
666 inputs.push_back(rot.x());
667 inputs.push_back(rot.y());
668 inputs.push_back(rot.z());
669 inputs.push_back(rot.w());
672 normalize(inputs, input_minmax);
674 auto* oo = net.run(inputs.data());
676 outputs.assign(oo, oo + output_count);
678 denormalize(outputs, output_minmax);
680 auto& vv = problem.active_variables;
681 for(
size_t iout = 0; iout < vv.size(); iout++)
683 size_t ivar = vv[iout];
684 solution[ivar] = modelInfo.clip(outputs[iout], ivar);