bio_ik
MoveIt kinematics_base plugin based on particle optimization & GA
ik_neural.cpp
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2016-2017, Philipp Sebastian Ruppel
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the copyright holder nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 #include "ik_base.h"
36 
37 #include <fann.h>
38 #include <fann_cpp.h>
39 
40 #include <mutex>
41 
42 #define LOG_LIST(l) \
43  { \
44  LOG(#l "[]"); \
45  for(std::size_t i = 0; i < l.size(); i++) \
46  { \
47  LOG(#l "[" + std::to_string(i) + "]", l[i]); \
48  } \
49  }
50 
51 namespace bio_ik
52 {
53 
54 /*
55 static inline KDL::Twist toTwist(const Frame& frame)
56 {
57  KDL::Twist t;
58  t.vel.x(frame.pos.x());
59  t.vel.y(frame.pos.y());
60  t.vel.z(frame.pos.z());
61  auto r = frame.rot.getAxis() * frame.rot.getAngle();
62  t.rot.x(r.x());
63  t.rot.y(r.y());
64  t.rot.z(r.z());
65  return t;
66 }
67 
68 static inline KDL::Twist frameTwist(const Frame& a, const Frame& b)
69 {
70  auto frame = inverse(a) * b;
71  KDL::Twist t;
72  t.vel.x(frame.pos.x());
73  t.vel.y(frame.pos.y());
74  t.vel.z(frame.pos.z());
75  auto r = frame.rot.getAxis() * frame.rot.getAngle();
76  t.rot.x(r.x());
77  t.rot.y(r.y());
78  t.rot.z(r.z());
79  return t;
80 }
81 */
82 
83 struct IKNeural : IKBase
84 {
85  std::vector<double> solution;
86  FANN::neural_net net;
87 
88  std::vector<std::pair<fann_type, fann_type>> input_minmax, output_minmax;
89 
90  static void find_minmax(const std::vector<fann_type>& values, std::vector<std::pair<fann_type, fann_type>>& minmax)
91  {
92  for(size_t i = 0; i < minmax.size(); i++)
93  {
94  minmax[i] = std::make_pair(values[i], values[i]);
95  }
96  for(size_t i = 0; i < values.size(); i++)
97  {
98  auto& v = values[i];
99  auto& p = minmax[i % minmax.size()];
100  p.first = std::min(p.first, v);
101  p.second = std::max(p.second, v);
102  }
103  }
104 
105  static void normalize(std::vector<fann_type>& values, const std::vector<std::pair<fann_type, fann_type>>& minmax)
106  {
107  for(size_t i = 0; i < values.size(); i++)
108  {
109  auto& v = values[i];
110  auto& p = minmax[i % minmax.size()];
111  v = (v - p.first) / (p.second - p.first);
112  }
113  }
114 
115  static void denormalize(std::vector<fann_type>& values, const std::vector<std::pair<fann_type, fann_type>>& minmax)
116  {
117  for(size_t i = 0; i < values.size(); i++)
118  {
119  auto& v = values[i];
120  auto& p = minmax[i % minmax.size()];
121  v = v * (p.second - p.first) + p.first;
122  }
123  }
124 
125  unsigned int input_count, output_count;
126 
127  IKNeural(const IKParams& p)
128  : IKBase(p)
129  {
130  trained = false;
131  }
132 
133  bool trained;
134 
135  void train()
136  {
137  trained = true;
138 
139  input_count = problem.active_variables.size() + problem.tip_link_indices.size() * 6;
140  output_count = problem.active_variables.size();
141 
142  LOG_VAR(input_count);
143  LOG_VAR(output_count);
144 
145  // std::vector<unsigned int> levels = {input_count, input_count, input_count, output_count};
146 
147  // std::vector<unsigned int> levels = {input_count, input_count + output_count, output_count};
148 
149  // std::vector<unsigned int> levels = {input_count, input_count + output_count, input_count + output_count, output_count};
150 
151  // std::vector<unsigned int> levels = {input_count, 100, output_count};
152 
153  std::vector<unsigned int> levels = {input_count, 50, output_count};
154 
155  net.create_standard_array(levels.size(), levels.data());
156 
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;
160 
161  std::vector<fann_type> inputs, outputs;
162  std::vector<fann_type*> input_pp, output_pp;
163 
164  LOG("neuro ik generating training data");
165 
166  unsigned int samples = 10000;
167 
168  for(size_t iter = 0; iter < samples; iter++)
169  {
170  for(size_t ivar = 0; ivar < var_count; ivar++)
171  {
172  state1[ivar] = random(modelInfo.getMin(ivar), modelInfo.getMax(ivar));
173  state1[ivar] = modelInfo.clip(state1[ivar], ivar);
174  // state2[ivar] = modelInfo.clip(state1[ivar] + random_gauss() * modelInfo.getSpan(ivar), ivar);
175  state2[ivar] = modelInfo.clip(state1[ivar] + random_gauss() * 0.1, ivar);
176  }
177 
178  model.applyConfiguration(state1);
179  frames1 = model.getTipFrames();
180  model.applyConfiguration(state2);
181  frames2 = model.getTipFrames();
182 
183  for(auto ivar : problem.active_variables)
184  {
185  inputs.push_back(state1[ivar]);
186  outputs.push_back(state2[ivar] - state1[ivar]);
187  }
188 
189  for(size_t itip = 0; itip < problem.tip_link_indices.size(); itip++)
190  {
191  double translational_scale = 1.0;
192  double rotational_scale = 1.0;
193 
194  // Frame diff = inverse(frames1[itip]) * frames2[itip];
195  // auto twist = toTwist(diff);
196  auto twist = frameTwist(frames1[itip], frames2[itip]);
197 
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());
201 
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);
205  }
206  }
207 
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");
212 
213  input_minmax.resize(input_count);
214  output_minmax.resize(output_count);
215 
216  find_minmax(inputs, input_minmax);
217  find_minmax(outputs, output_minmax);
218 
219  normalize(inputs, input_minmax);
220  normalize(outputs, output_minmax);
221 
222  for(size_t iter = 0; iter < samples; iter++)
223  {
224  input_pp.push_back(inputs.data() + iter * input_count);
225  output_pp.push_back(outputs.data() + iter * output_count);
226  }
227 
228  LOG("neuro ik training");
229 
230  FANN::training_data train;
231  train.set_train_data(samples, input_count, input_pp.data(), output_count, output_pp.data());
232  net.set_callback(
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;
235  // LOG("training", epochs, "/", max_epochs, epochs * 100 / max_epochs, "%");
236  LOG("training", epochs, net.get_MSE(), desired_error);
237  return 0;
238  },
239  0);
240 
241  net.set_activation_function_hidden(FANN::SIGMOID);
242  net.set_activation_function_output(FANN::SIGMOID);
243 
244  net.init_weights(train);
245 
246  net.train_on_data(train, 1000, 1, 0.0001);
247 
248  fann_type err = net.test_data(train);
249  LOG("neuro ik training error:", err);
250 
251  /*std::vector<fann_type> iiv, oov, ttv;
252  for(size_t iter = 0; iter < 10; iter++)
253  {
254  auto* ii = input_pp[iter];
255  auto* oo = net.run(ii);
256  auto* tt = output_pp[iter];
257  iiv.assign(ii, ii + input_count);
258  ttv.assign(tt, tt + output_count);
259  oov.assign(oo, oo + output_count);
260  LOG_LIST(iiv);
261  LOG_LIST(ttv);
262  LOG_LIST(oov);
263  }*/
264 
265  LOG("training done");
266  }
267 
268  size_t iterations = 0;
269 
270  void initialize(const Problem& problem)
271  {
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();
277  iterations = 0;
278  if(thread_index > 0)
279  for(auto& vi : problem.active_variables)
280  solution[vi] = random(modelInfo.getMin(vi), modelInfo.getMax(vi));
281  }
282 
283  const std::vector<double>& getSolution() const { return solution; }
284 
285  std::vector<fann_type> inputs, outputs;
286 
287  std::vector<Frame> tip_objectives;
288 
289  /*void step()
290  {
291  //if(iterations > 1) return;
292  iterations++;
293 
294  inputs.clear();
295  for(auto ivar : problem.active_variables)
296  {
297  inputs.push_back(solution[ivar]);
298  }
299 
300  tip_objectives.resize(model.getTipFrames().size());
301  for(auto& g : problem.goals)
302  {
303  tip_objectives[g.tip_index] = g.frame;
304  }
305 
306  model.applyConfiguration(solution);
307  auto& frames1 = model.getTipFrames();
308  auto& frames2 = tip_objectives;
309 
310  double scale = 1.0;
311 
312  for(size_t itip = 0; itip < tip_objectives.size(); itip++)
313  {
314  double translational_scale = 1.0;
315  double rotational_scale = 1.0;
316 
317  //Frame diff = inverse(frames1[itip]) * frames2[itip];
318  //auto twist = toTwist(diff);
319  auto twist = frameTwist(frames1[itip], frames2[itip]);
320 
321  auto dpos = frames2[itip].pos - frames1[itip].pos;
322  auto drot = Vector3(
323  twist.rot.x() * rotational_scale,
324  twist.rot.y() * rotational_scale,
325  twist.rot.z() * rotational_scale
326  );
327 
328  scale = 1.0 / (0.0000001 + dpos.length() + drot.length());
329 
330  dpos = dpos * scale;
331  drot = drot * scale;
332 
333  inputs.push_back(dpos.x());
334  inputs.push_back(dpos.y());
335  inputs.push_back(dpos.z());
336 
337  inputs.push_back(drot.x());
338  inputs.push_back(drot.y());
339  inputs.push_back(drot.z());
340  }
341 
342  normalize(inputs, input_minmax);
343 
344  auto* oo = net.run(inputs.data());
345 
346  outputs.assign(oo, oo + output_count);
347 
348  denormalize(outputs, output_minmax);
349 
350  auto& vv = problem.active_variables;
351  for(size_t iout = 0; iout < vv.size(); iout++)
352  {
353  size_t ivar = vv[iout];
354  solution[ivar] = modelInfo.clip(solution[ivar] + outputs[iout] * 0.1 / scale, ivar);
355  }
356  }*/
357 
358  void step()
359  {
360  // if(iterations > 1) return;
361  iterations++;
362 
363  inputs.clear();
364  for(auto ivar : problem.active_variables)
365  {
366  inputs.push_back(solution[ivar]);
367  }
368 
369  tip_objectives.resize(model.getTipFrames().size());
370  for(auto& g : problem.goals)
371  {
372  tip_objectives[g.tip_index] = g.frame;
373  }
374 
375  model.applyConfiguration(solution);
376  auto& frames1 = model.getTipFrames();
377  auto& frames2 = tip_objectives;
378 
379  double scale = 1.0;
380  for(size_t itip = 0; itip < tip_objectives.size(); itip++)
381  {
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);
387 
388  /*if(iterations % 2)
389  {
390  scale = 1.0 / (0.0000001 + dpos.length());
391  inputs.push_back(dpos.x() * scale);
392  inputs.push_back(dpos.y() * scale);
393  inputs.push_back(dpos.z() * scale);
394  inputs.push_back(0);
395  inputs.push_back(0);
396  inputs.push_back(0);
397  } else {
398  scale = 1.0 / (0.0000001 + drot.length());
399  inputs.push_back(0);
400  inputs.push_back(0);
401  inputs.push_back(0);
402  inputs.push_back(drot.x() * scale);
403  inputs.push_back(drot.y() * scale);
404  inputs.push_back(drot.z() * scale);
405  }*/
406 
407  {
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);
415  }
416  }
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++)
423  {
424  size_t ivar = vv[iout];
425  solution[ivar] = modelInfo.clip(solution[ivar] + outputs[iout] * 1 / scale, ivar);
426  }
427  }
428 };
429 
430 static IKFactory::Class<IKNeural> neural("neural");
431 
433 {
434  std::vector<double> solution;
435  FANN::neural_net net;
436 
437  std::vector<std::pair<fann_type, fann_type>> input_minmax, output_minmax;
438 
439  /*static void find_minmax(const std::vector<fann_type>& values, std::vector<std::pair<fann_type, fann_type>>& minmax)
440  {
441  for(size_t i = 0; i < minmax.size(); i++)
442  {
443  minmax[i] = std::make_pair(values[i], values[i]);
444  }
445  for(size_t i = 0; i < values.size(); i++)
446  {
447  auto& v = values[i];
448  auto& p = minmax[i % minmax.size()];
449  p.first = std::min(p.first, v);
450  p.second = std::max(p.second, v);
451  }
452  }*/
453 
454  static void find_minmax(const std::vector<fann_type>& values, std::vector<std::pair<fann_type, fann_type>>& minmax)
455  {
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());
459 
460  std::vector<double> ranges2(minmax.size(), 0.0001);
461  for(size_t i = 0; i < values.size(); i++)
462  {
463  double d = values[i] - centers[i % minmax.size()];
464  d = d * d;
465  ranges2[i % minmax.size()] += d * (1.0 * minmax.size() / values.size());
466  }
467 
468  for(size_t i = 0; i < minmax.size(); i++)
469  {
470  auto& p = minmax[i];
471  p.first = centers[i] - sqrt(ranges2[i]);
472  p.second = centers[i] + sqrt(ranges2[i]);
473  }
474  }
475 
476  static void normalize(std::vector<fann_type>& values, const std::vector<std::pair<fann_type, fann_type>>& minmax)
477  {
478  for(size_t i = 0; i < values.size(); i++)
479  {
480  auto& v = values[i];
481  auto& p = minmax[i % minmax.size()];
482  v = (v - p.first) / (p.second - p.first);
483  }
484  }
485 
486  static void denormalize(std::vector<fann_type>& values, const std::vector<std::pair<fann_type, fann_type>>& minmax)
487  {
488  for(size_t i = 0; i < values.size(); i++)
489  {
490  auto& v = values[i];
491  auto& p = minmax[i % minmax.size()];
492  v = v * (p.second - p.first) + p.first;
493  }
494  }
495 
496  unsigned int input_count, output_count;
497 
498  IKNeural2(const IKParams& p)
499  : IKBase(p)
500  {
501  trained = false;
502  }
503 
504  bool trained;
505 
506  void train()
507  {
508  trained = true;
509 
510  input_count = problem.tip_link_indices.size() * 7;
511  output_count = problem.active_variables.size();
512 
513  LOG_VAR(input_count);
514  LOG_VAR(output_count);
515 
516  // std::vector<unsigned int> levels = {input_count, 100, 100, output_count};
517 
518  // std::vector<unsigned int> levels = {input_count, input_count, input_count, output_count};
519 
520  std::vector<unsigned int> levels = {input_count, input_count + output_count, output_count};
521 
522  // std::vector<unsigned int> levels = {input_count, input_count + output_count, input_count + output_count, output_count};
523 
524  // std::vector<unsigned int> levels = {input_count, output_count};
525 
526  net.create_standard_array(levels.size(), levels.data());
527 
528  size_t var_count = params.robot_model->getVariableCount();
529  std::vector<double> state = problem.initial_guess;
530  std::vector<Frame> frames;
531 
532  std::vector<fann_type> inputs, outputs;
533  std::vector<fann_type*> input_pp, output_pp;
534 
535  LOG("neuro ik generating training data");
536 
537  unsigned int samples = 10000;
538 
539  for(size_t iter = 0; iter < samples; iter++)
540  {
541  for(size_t ivar : problem.active_variables)
542  state[ivar] = random(modelInfo.getMin(ivar), modelInfo.getMax(ivar));
543 
544  model.applyConfiguration(state);
545  frames = model.getTipFrames();
546 
547  for(auto ivar : problem.active_variables)
548  outputs.push_back(state[ivar]);
549 
550  for(size_t itip = 0; itip < problem.tip_link_indices.size(); itip++)
551  {
552  inputs.push_back(frames[itip].pos.x());
553  inputs.push_back(frames[itip].pos.y());
554  inputs.push_back(frames[itip].pos.z());
555 
556  auto rot = frames[itip].rot;
557  rot = rot * rot;
558  // rot = tf2::Quaternion(0, 0, 0, 1);
559  inputs.push_back(rot.x());
560  inputs.push_back(rot.y());
561  inputs.push_back(rot.z());
562  inputs.push_back(rot.w());
563  }
564  }
565 
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");
570 
571  input_minmax.resize(input_count);
572  output_minmax.resize(output_count);
573 
574  find_minmax(inputs, input_minmax);
575  find_minmax(outputs, output_minmax);
576 
577  normalize(inputs, input_minmax);
578  normalize(outputs, output_minmax);
579 
580  for(size_t iter = 0; iter < samples; iter++)
581  {
582  input_pp.push_back(inputs.data() + iter * input_count);
583  output_pp.push_back(outputs.data() + iter * output_count);
584  }
585 
586  LOG("neuro ik training");
587 
588  FANN::training_data train;
589  train.set_train_data(samples, input_count, input_pp.data(), output_count, output_pp.data());
590  net.set_callback(
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;
593  // LOG("training", epochs, "/", max_epochs, epochs * 100 / max_epochs, "%");
594  LOG("training", epochs, net.get_MSE(), desired_error);
595  return 0;
596  },
597  0);
598 
599  net.set_activation_function_hidden(FANN::SIGMOID);
600  net.set_activation_function_output(FANN::SIGMOID);
601 
602  net.init_weights(train);
603 
604  net.train_on_data(train, 100, 1, 0.0001);
605 
606  fann_type err = net.test_data(train);
607  LOG("neuro ik training error:", err);
608 
609  /*std::vector<fann_type> iiv, oov, ttv;
610  for(size_t iter = 0; iter < 10; iter++)
611  {
612  auto* ii = input_pp[iter];
613  auto* oo = net.run(ii);
614  auto* tt = output_pp[iter];
615  iiv.assign(ii, ii + input_count);
616  ttv.assign(tt, tt + output_count);
617  oov.assign(oo, oo + output_count);
618  LOG_LIST(iiv);
619  LOG_LIST(ttv);
620  LOG_LIST(oov);
621  }*/
622 
623  LOG("training done");
624  }
625 
626  size_t iterations = 0;
627 
628  void initialize(const Problem& problem)
629  {
630  IKBase::initialize(problem);
631  solution = problem.initial_guess;
632  if(!trained) train();
633  iterations = 0;
634  }
635 
636  const std::vector<double>& getSolution() const { return solution; }
637 
638  std::vector<fann_type> inputs, outputs;
639 
640  std::vector<Frame> tip_objectives;
641 
642  void step()
643  {
644  if(iterations > 1) return;
645  iterations++;
646 
647  inputs.clear();
648 
649  tip_objectives.resize(model.getTipFrames().size());
650  for(auto& g : problem.goals)
651  {
652  tip_objectives[g.tip_index] = g.frame;
653  }
654 
655  auto& frames = tip_objectives;
656 
657  for(size_t itip = 0; itip < tip_objectives.size(); itip++)
658  {
659  inputs.push_back(frames[itip].pos.x());
660  inputs.push_back(frames[itip].pos.y());
661  inputs.push_back(frames[itip].pos.z());
662 
663  auto rot = frames[itip].rot;
664  rot = rot * rot;
665  // rot = tf2::Quaternion(0, 0, 0, 1);
666  inputs.push_back(rot.x());
667  inputs.push_back(rot.y());
668  inputs.push_back(rot.z());
669  inputs.push_back(rot.w());
670  }
671 
672  normalize(inputs, input_minmax);
673 
674  auto* oo = net.run(inputs.data());
675 
676  outputs.assign(oo, oo + output_count);
677 
678  denormalize(outputs, output_minmax);
679 
680  auto& vv = problem.active_variables;
681  for(size_t iout = 0; iout < vv.size(); iout++)
682  {
683  size_t ivar = vv[iout];
684  solution[ivar] = modelInfo.clip(outputs[iout], ivar);
685  }
686  }
687 };
688 
689 static IKFactory::Class<IKNeural2> neural2("neural2");
690 }