Endorob  1.4
A C++ Robotics library by P. Berthet-Rayne
endorob.cpp
Go to the documentation of this file.
1 /*MIT License
2 
3 Copyright (c) 2018-2019 Pierre Berthet-Rayne
4 
5 Permission is hereby granted, free of charge, to any person obtaining a copy
6 of this software and associated documentation files (the "Software"), to deal
7 in the Software without restriction, including without limitation the rights
8 to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
9 copies of the Software, and to permit persons to whom the Software is
10 furnished to do so, subject to the following conditions:
11 
12 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
13 IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
14 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
15 AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
16 LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
17 OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
18 SOFTWARE.*/
19 
20 #include "stdafx.h" // For windows/VS users
21 #include "endorob.h"
22 
23 
24 std::mutex mutx; // mutex for critical sections
25 // CPU management static variables
26 uint Endorob::m_CPU_amount = 0;
27 uint Endorob::m_CPU_cpt = 0xFE;
28 uint Endorob::m_instantiations = 0;
29 bool Endorob::m_cpu_info_dispayed = false;
30 uint Motion_RW::m_file_counter = 0;
31 volatile bool Endorob::m_assigning_CPU = false;
32 
33 Endorob::Endorob():
34  m_start_solver(false),
35  m_run(true),
36  m_apply_mask(false),
37  m_converged(false),
38  m_diverged(false),
39  m_iteration_overflow(false),
40  m_apply_joint_coupling(false),
41  m_rolling_joint_present(false),
42  m_check_joint_limit_jacobian(false),
43  m_full_body_needed(false),
44  m_do_not_start_solver(false),
45  m_apply_weighted_solution(true),
46  m_DH_convention(DH_type::DH_modified),
47  m_wait_for_sync(false),
48  m_solver_pending(true),
49  m_thread_running(false),
50  m_apply_fault_tolerance(false)
51 {
52  // Set Matrices and vectors:
53  m_target_T = Mat::Identity(4,4);
54  m_base_frame_T = Mat::Identity(4,4);
55  m_tip_T = Mat::Identity(4,4);
56  m_tool_frame_T = Mat::Identity(4,4);
57  m_mask_vector.resize(6);
58  m_mask_vector << 1, 1, 1, 1, 1, 1;
59  m_error.resize(6);
60  m_prev_error.resize(6);
61  m_lambda = 1;
62  m_joint_index_offset = 0;
63 
64  // Random seed
65  srand ( (uint)std::time(nullptr) );
66 
67  // Set default solver:
68  p_solver_type = &Endorob::pseudo_inverse_simple;
69 
70  // CPU
71  check_for_cpu();
72 
73 }
74 
75 Endorob::Endorob(const Solver_type &solver_type):
76  m_start_solver(false),
77  m_run(true),
78  m_apply_mask(false),
79  m_converged(false),
80  m_diverged(false),
81  m_iteration_overflow(false),
82  m_apply_joint_coupling(false),
83  m_rolling_joint_present(false),
84  m_check_joint_limit_jacobian(false),
85  m_full_body_needed(false),
86  m_do_not_start_solver(true),
87  m_apply_weighted_solution(true),
88  m_DH_convention(DH_type::DH_modified),
89  m_wait_for_sync(false),
90  m_solver_pending(true),
91  m_thread_running(false),
92  m_apply_fault_tolerance(false)
93 {
94  // Set Matrices and vectors:
95  m_target_T = Mat::Identity(4,4);
96  m_base_frame_T = Mat::Identity(4,4);
97  m_tip_T = Mat::Identity(4,4);
98  m_tool_frame_T = Mat::Identity(4,4);
99  m_mask_vector.resize(6);
100  m_mask_vector << 1, 1, 1, 1, 1, 1;
101  //m_error.resize(6);
102  //m_prev_error.resize(6);
103  m_lambda = 1;
104 
105  m_joint_index_offset = 0;
106 
107  // Random seed
108  srand (time(NULL));
109 
110  if(solver_type == Solver_type::Full_body_jacobian)
111  m_full_body_needed = true;
112 
113 
114  // switch(solver_type)
115  // {
116 
117  // case Solver_type::Jacobian_pseudo_inverse:
118 
119  // // Set default solver:
120  // p_solver_type = &Endorob::pseudo_inverse_simple;
121  // psolver = new std::thread(&Endorob::threaded_solver, this, 0, 0);
122  // break;
123 
124  // case Solver_type::Full_body_jacobian:
125  // psolver = new std::thread(&Endorob::threaded_solver_2, this, 0, 0);
126  // m_full_body_needed = true;
127  // break;
128  // }
129 
130  check_for_cpu();
131 }
132 
133 Endorob::~Endorob()
134 {
135  m_start_solver = false;
136  m_run = false;
137 
138  // Wait for thread to finish
139  psolver->join();
140 
141  delete psolver;
142 
143 #ifdef VERBOSE_MODE
144  std::cout << "Solver " << m_instantiation_index << " assigned to CPU " << m_CPU_affinity+1 << " stopped and deleted"<< std::endl;
145 #endif
146 
147  // Clear various arrays
148  m_DH_table.clear();
149  m_robot_path.clear();
150 
151 }
152 
153 void Endorob::set_DH_convention(const DH_type &convention)
154 {
155  m_DH_convention = convention;
156 }
157 
158 void Endorob::save_DH(std::string filename)
159 {
160  std::ofstream output_file;
161  output_file.open(filename);
162  if (!output_file.is_open())
163  throw std::runtime_error("Cannot open File " + filename);
164 
165  output_file << "DH, " << (int)m_DH_convention << std::endl;
166 
167  output_file << "m_robot_frame_T, ";
168  for (auto m = 0; m < m_robot_frame_T.size(); m++)
169  {
170  output_file << std::to_string(m_robot_frame_T(m)) << ", ";
171  }
172  output_file << std::endl;
173 
174  output_file << "m_tool_frame_T, ";
175  for (auto m = 0; m < m_tool_frame_T.size(); m++)
176  {
177  output_file << std::to_string(m_tool_frame_T(m)) << ", ";
178  }
179  output_file << std::endl;
180 
181  output_file << "#id, a, alpha, d, theta, min, max, type" << std::endl;
182  for (size_t i = 0; i < m_DH_table.size(); i++)
183  {
184  output_file << i << ", "
185  << m_DH_table[i].a() << ", "
186  << m_DH_table[i].alpha() << ", "
187  << m_DH_table[i].d() << ", "
188  << m_DH_table[i].theta() << ", "
189  << m_DH_table[i].joint_limit_min() << ", "
190  << m_DH_table[i].joint_limit_max() << ", "
191  << (int)m_DH_table[i].get_joint_type()
192  << std::endl;
193  }
194 
195  output_file.close();
196 }
197 
198 void Endorob::load_DH(std::string filename)
199 {
200  std::ifstream file;
201  file.open(filename);
202  if (!file.is_open())
203  throw std::runtime_error("Cannot open File " + filename);
204 
205  std::string text;
206  bool dh_convention_set = false;
207 
208  while (!file.eof())
209  {
210  // read a line
211  std::getline(file, text, '\n');
212  if (text == "")
213  continue;
214 
215  // split string
216  //std::istringstream iss(text);
217  //std::vector<std::string> results(std::istream_iterator<std::string>{iss},
218  // std::istream_iterator<std::string>());
219 
220  std::vector<std::string> results = split(text, ',', true);
221 
222  if (results[0][0] == '#')
223  continue;
224 
225  if (results[0] == "m_robot_frame_T")
226  {
227  for (size_t i = 0; i < 16; i++)
228  {
229  m_robot_frame_T(i)= std::stod(results[i+1]);
230  }
231  set_robot_reference_frame(m_robot_frame_T);
232 
233  }
234  else if (results[0] == "m_tool_frame_T")
235  {
236  for (size_t i = 0; i < 16; i++)
237  {
238  m_tool_frame_T(i) = std::stod(results[i + 1]);
239  }
240  }
241  else if (results[0] == "DH")
242  {
243  int dh_i = std::stoi(results[1]);
244  DH_type dh_t = (DH_type)dh_i;
245  set_DH_convention(dh_t);
246  dh_convention_set = true;
247  }
248  else
249  {
250  if (!dh_convention_set)
251  throw std::runtime_error("First entry shall be the dh convention");
252  int pos = 0;
253  pos++; //skip the id.
254  double a = std::stod(results[pos++]);
255  double alpha = std::stod(results[pos++]);
256  double d = std::stod(results[pos++]);
257  double theta = std::stod(results[pos++]);
258  double min = std::stod(results[pos++]);
259  double max = std::stod(results[pos++]);
260  int jt_i = std::stoi(results[pos++]);
261  Joint_type jt_t = (Joint_type)jt_i;
262  add_DH_entry(a, alpha, d, theta, jt_t);
263  set_joint_limits(min, max);
264  }
265  }
266 
267  //file << (int)m_DH_convention << std::endl;
268  //for (size_t i = 0; i < m_DH_table.size(); i++)
269  //{
270  // file << i << ", "
271  // << m_DH_table[i].a() << ", "
272  // << m_DH_table[i].alpha() << ", "
273  // << m_DH_table[i].d() << ", "
274  // << m_DH_table[i].theta() << ", "
275  // << m_DH_table[i].joint_limit_min() << ", "
276  // << m_DH_table[i].joint_limit_max()
277  // << std::endl;
278  //}
279 
280  file.close();
281 }
282 
283 void Endorob::add_DH_entry(const double &a, const double &alpha, const double &d, const double &theta, const Joint_type &type, const double &a_bis, const double &theta_bis, const uint &flex)
284 {
285  DH_table_entry new_entry(a, alpha, d, theta, type);
286  m_DH_table.push_back(new_entry);
287 
288  // Check for rolling-joints and add entry
289  if(type == Joint_type::Rolling)
290  {
291  m_rolling_joint_present = true;
292  DH_table_entry new_entry(a_bis, 0, 0, theta_bis, type);
293  m_DH_table.push_back(new_entry);
294  }
295  else if(type == Joint_type::Rolling_flex)
296  {
297  for(unsigned i = 0; i < flex-1; i++)
298  {
299  m_rolling_joint_present = true;
300  DH_table_entry new_entry(a, 0, 0, 0, type);
301  m_DH_table.push_back(new_entry);
302  }
303  }
304 
305  // Update robot's joint amount
306  m_joints = (uint)m_DH_table.size();
307  m_dof = m_joints;
308  m_q_vec.resize(m_joints);
309  for(uint i = 0; i < m_joints; i++)
310  m_q_vec(i) = 0;
311 
312  if(!m_apply_joint_coupling)
313  m_q_vec_for_user = m_q_vec;
314  else
315  m_q_vec_for_user = m_xi_vec;
316 
317 
318 }
319 
320 void Endorob::set_joint_limits(const double &min, const double &max)
321 {
322  int current_index = m_DH_table.size()-1;
323  if(m_DH_table[current_index].get_joint_type() == Joint_type::Rolling)
324  {
325  m_DH_table[current_index].set_joint_limits(min/2, max/2);
326  m_DH_table[current_index-1].set_joint_limits(min/2, max/2);
327  }
328  else
329  m_DH_table[current_index].set_joint_limits(min, max);
330 }
331 
332 void Endorob::set_flex_resolution(const uint &res)
333 {
334 
335 }
336 
337 void Endorob::set_flex_gradient(const Vec &vec)
338 {
339 
340 }
341 
342 void Endorob::set_solver_type(Solver_type solver, const double &lambda)
343 {
344  // pointer to function approach to avoid if statement
345  // https://stackoverflow.com/questions/1485983/calling-c-class-methods-via-a-function-pointer
346  switch(solver)
347  {
348  case Solver_type::Jacobian_pseudo_inverse:
349  p_solver_type = &Endorob::pseudo_inverse_simple;
350  break;
351 
352  case Solver_type::Jacobian_pseudo_inverse_null_space:
353  p_solver_type = &Endorob::pseudo_inverse_null_space;
354  break;
355 
356  case Solver_type::Jacobian_DLS:
357  m_lambda = lambda;
358  p_solver_type = &Endorob::damped_least_square_method;
359  break;
360 
361  case Solver_type::Jacobian_transpose:
362  p_solver_type = &Endorob::jacobian_transpose;
363  break;
364 
365  }
366 }
367 
368 void Endorob::initialize()
369 {
370  // Check that DH table is filled
371  if(m_DH_table.size() == 0)
372  {
373  std::cout << "Error: No DH table...\n";
374  return;
375  }
376  m_q_vec = Mat::Zero(m_q_vec.rows(),1);
377  m_q_vec_for_user = m_q_vec;
378  m_q_vec_rest = m_q_vec;
379 
380  // Forward kinematics matrix
381  m_fwd_k_vec.resize((m_joints+1)*4,4); // +1 for tool frame
382  m_full_body_vec_current.resize(m_joints*3);
383  m_full_body_vec_desired.resize(m_joints*3);
384 
385  // Jacobian calculation related matrices:
386  m_J_vel.resize(3, m_joints);
387  m_J_rot.resize(3, m_joints);
388 
389  if(m_full_body_needed)
390  {
391  m_J.resize((m_joints - m_joint_index_offset)*3 + 3 , m_joints);
392  m_error.resize((m_joints - m_joint_index_offset)*3 + 3);
393  }
394  else
395  m_J.resize(6,m_joints);
396 
397  m_J_inv.resize(m_joints,6);
398  m_phi.resize(m_dof);
399  m_alpha.resize(m_joints);
400  m_identity = Mat::Identity(m_dof, m_dof);
401 
402  // Do initial full forward Kinematics
403  calculate_full_forward_kinematics();
404 
405  // Put into vector from
406  if(m_full_body_needed)
407  convert_fwd_k_into_vec();
408 
409  // Set target as tip location
410  m_target_T = m_tip_T;
411 
412  // Set weights for solver
413  //m_error_weights.resize(m_joints*3,m_joints*3);
414  m_error_weights.resize((m_joints - m_joint_index_offset)*3 + 3, (m_joints - m_joint_index_offset)*3 + 3);
415  m_error_weights_vec.resize(m_dof);
416 
417  m_error_weights = Mat::Identity((m_joints - m_joint_index_offset)*3 + 3, (m_joints - m_joint_index_offset)*3 + 3);
418 
419  for(int i = m_error_weights.rows()-1; i >=2 ; i-=3)
420  //for(int i = m_error_weights.rows()-1; i >=20 ; i-=3)
421  {
422  if(i >= m_error_weights.rows()-7)
423  {
424  m_error_weights(i,i) = 1.0;
425  m_error_weights(i-1,i-1) = 1.0;
426  m_error_weights(i-2,i-2) = 1.0;
427  }
428  else
429  {
430  //m_error_weights(i,i) = m_error_weights(0,0)/2;
431  //m_error_weights(i-1,i-1) = m_error_weights(1,1)/2;
432  //m_error_weights(i-2,i-2) = m_error_weights(2,2)/2;
433  m_error_weights(i,i) = 0;
434  m_error_weights(i-1,i-1) = 0;
435  m_error_weights(i-2,i-2) = 0;
436  }
437 
438  }
439 
440  for(int i = 0; i < m_error_weights_vec.rows(); i++)
441  {
442  m_error_weights_vec(i) = 1;
443  }
444  // m_error_weights_vec(0) = 0;
445  // m_error_weights_vec(1) = 0;
446  // m_error_weights_vec(2) = 0;
447  // m_error_weights_vec(3) = 0;
448  // m_error_weights_vec(4) = 0;
449  // m_error_weights_vec(5) = 0;
450  // m_error_weights_vec(m_error_weights_vec.rows()-1) = 0;
451 
452  // Fault tolerance
453  for(auto i = 0; i < m_joints; i++)
454  {
455  m_faulty_joint_vec.push_back(false);
456  }
457 
458 
459 
460 
461  if(!m_thread_running)
462  {
463 
464  // Start solver
465 
466  m_instantiation_index = m_instantiations;
467  m_instantiations++;
468 
469  if(!m_do_not_start_solver)
470  psolver = new std::thread(&Endorob::threaded_solver, this, 0, 0);
471  else
472  {
473  psolver = new std::thread(&Endorob::threaded_solver_2, this, 0, 0);
474 
475  //psolver->
476  m_full_body_needed = true;
477  }
478  //std::this_thread::sleep_for(std::chrono::milliseconds(10));
479 
480  // Cpu assignement
481  while(m_assigning_CPU)
482  {
483  std::cout << "waiting ";
484  }
485 
486 
487 
488  m_assigning_CPU = true;
489  m_CPU_affinity = m_CPU_cpt;
490 
491  // Change cpu index for other instantiations
492  if(m_CPU_cpt > 0)
493  m_CPU_cpt--;
494  else
495  m_CPU_cpt = m_CPU_amount - 1;
496 
497 
498 #ifdef _WIN32
499 
500  long int res = SetThreadIdealProcessor(psolver->native_handle(), m_CPU_affinity);
501 
502  if(res == -1)
503  {
504 #ifdef VERBOSE_MODE
505  std::cout << "OOOOOOOOOOO____FAILED PROCESSOR ASSIGNMENT____OOOOOOOOOOO" << std::endl;
506  std::cout << "...NOW RUNNING ON DEFAULT CPU..." << std::endl;
507 #endif
508  }
509 
510  //SetThreadPriority(psolver->native_handle(), THREAD_PRIORITY_HIGHEST);
511 
512 #elif __linux__
513 
514  // Create a cpu_set_t object representing a set of CPUs. Clear it and mark
515  // only CPU i as set.
516  cpu_set_t cpuset;
517  CPU_ZERO(&cpuset);
518  CPU_SET(m_CPU_affinity, &cpuset);
519  int rc = pthread_setaffinity_np(psolver->native_handle(), sizeof(cpu_set_t), &cpuset);
520  // if (rc != 0) {
521  // std::cerr << "Error calling pthread_setaffinity_np: " << rc << "\n";
522  // }
523  // Change cpu index for other instantiations
524 
525 #ifdef VERBOSE_MODE
526  if (rc != 0)
527  {
528  std::cout << "OOOOOOOOOOO____FAILED PROCESSOR ASSIGNMENT____OOOOOOOOOOO" << std::endl;
529  std::cout << "...NOW RUNNING ON DEFAULT CPU..." << std::endl;
530  }
531 
532 
533 
534 #endif
535 
536 #endif
537 
538  }
539  std::cout << "Solver " << m_instantiation_index << " assigned to CPU " << m_CPU_affinity+1 << " and running..."<< std::endl;
540 
541  m_assigning_CPU = false;
542 
543  // Enable solver:
544  m_converged = true;
545  m_iteration_overflow = false;
546  m_start_solver = true;
547 
548 
549 }
550 
551 unsigned Endorob::dof()
552 {
553  return m_dof;
554 }
555 
556 unsigned Endorob::joints()
557 {
558  return m_joints;
559 }
560 
561 Mat4d Endorob::get_forward_kinematics()
562 {
563  return m_tip_T;
564 }
565 
566 Mat4d Endorob::get_forward_kinematics(const Vec &q)
567 {
568  // Mutex lock
569  //mutx.lock();
570 
571  Mat4d T_mat;
572  if(!m_apply_joint_coupling)
573  {
574  for(int i = 0; i < q.rows(); i++)
575  {
576  // consider joints offsets if any
577  double joint_offset = 0;
578  if(m_DH_table[i].get_joint_type() == Joint_type::Revolute ||
579  m_DH_table[i].get_joint_type() == Joint_type::Rolling)
580  {
581  joint_offset = m_DH_table[i].theta();
582  }
583  else if(m_DH_table[i].get_joint_type() == Joint_type::Prismatic)
584  {
585  joint_offset = m_DH_table[i].d();
586  }
587 
588 
589  if(i == 0)
590  {
591  // For Standard DH, we need to consider the base matrix
592  if(m_DH_convention == DH_type::DH_standard)
593  {
594  T_mat = m_base_frame_T * base_transformation_matrix(m_DH_table[i], q[i] + joint_offset);
595  }
596  // Modified DH case
597  else
598  T_mat = base_transformation_matrix(m_DH_table[i], q[i] + joint_offset);
599  }
600  else
601  {
602  T_mat = T_mat * base_transformation_matrix(m_DH_table[i], q[i] + joint_offset);
603  }
604  }
605  }
606  else
607  {
608  // Calculate new q_vec based on coupling
609  //Vec tmp_q_vec = m_joint_coupling_increase_dimension * q;
610  Vec tmp_q_vec = m_joint_coupling_matrix * q;
611 
612  for(int i = 0; i < tmp_q_vec.rows(); i++)
613  {
614  // consider joints offsets if any
615  double joint_offset = 0;
616  if(m_DH_table[i].get_joint_type() == Joint_type::Revolute ||
617  m_DH_table[i].get_joint_type() == Joint_type::Rolling)
618  {
619  joint_offset = m_DH_table[i].theta();
620  }
621  else if(m_DH_table[i].get_joint_type() == Joint_type::Prismatic)
622  {
623  joint_offset = m_DH_table[i].d();
624  }
625 
626  if(i == 0)
627  {
628  //T_mat = base_transformation_matrix(m_DH_table[i], tmp_q_vec[i] + joint_offset);
629  // For Standard DH, we need to consider the base matrix
630  if(m_DH_convention == DH_type::DH_standard)
631  {
632  T_mat = m_base_frame_T * base_transformation_matrix(m_DH_table[i], tmp_q_vec[i] + joint_offset);
633  }
634  // Modified DH case
635  else
636  T_mat = base_transformation_matrix(m_DH_table[i], tmp_q_vec[i] + joint_offset);
637  }
638  else
639  {
640  T_mat = T_mat * base_transformation_matrix(m_DH_table[i], tmp_q_vec[i] + joint_offset);
641  }
642  }
643  }
644 
645  // Tool frame (default tool frame is identity)
646  T_mat = T_mat * m_tool_frame_T;
647 
648  // Mutex unlock
649  //mutx.unlock();
650 
651  return T_mat;
652 }
653 
654 Mat Endorob::get_Jacobian()
655 {
656  return m_J_for_user;
657 }
658 
659 Mat Endorob::get_coupling_matrix()
660 {
661  return m_joint_coupling_matrix;
662 
663 }
664 
665 void Endorob::set_tool_frame(const Mat4d &T)
666 {
667  m_tool_frame_T = T;
668 }
669 
670 void Endorob::set_base_frame(const Mat4d &T)
671 {
672  m_base_frame_T = T;
673 }
674 
675 void Endorob::set_mask(const Vec &mask)
676 {
677  if(mask.rows() != 6)
678  return;
679 
680  m_mask_dof = 0;
681 
682  // Count DOF desired in workspace:
683  for(unsigned i = 0; i < 6; i++)
684  {
685  if(mask[i] == 1)
686  m_mask_dof++;
687  }
688 
689  // Resize matrices:
690  // error mastrices
691  m_error_masked.resize(m_mask_dof);
692  m_prev_error_masked.resize(m_mask_dof);
693 
694  // Jacobian
695  m_J_masked.resize(m_mask_dof,m_joints);
696  m_J_inv_masked.resize(m_joints, m_mask_dof);
697 
698 
699  m_mask_vector = mask;
700  m_apply_mask = true;
701 }
702 
703 void Endorob::set_joint_coupling(const Vec &coupling)
704 {
705  m_apply_joint_coupling = true;
706 
707  // if rolling joints are present, modify coupling vector by adding corresponding coupled entries
708  // so we can use the same coupling vector for all joints
709  if(m_rolling_joint_present)
710  {
711  m_coupling_vector.resize(m_joints);
712  unsigned index = 0;
713 
714  for(unsigned i = 0; i < coupling.rows(); i++)
715  {
716  m_coupling_vector[index] = coupling[i];
717 
718  if(m_DH_table[index].get_joint_type() == Joint_type::Rolling)
719  {
720  index++;
721  m_coupling_vector[index] = coupling[i];
722  }
723  // else if(m_DH_table[index].get_joint_type() == Joint_type::Rolling_flex)
724  // {
725  // index++;
726  // m_coupling_vector[index] = coupling[i];
727  // }
728  index++;
729  }
730  }
731  else
732  m_coupling_vector = coupling;
733 
734 #ifdef DEBUG_VERBOSE
735  std::cout << "initial coupling\n" << coupling << std::endl;
736  std::cout << "adjusted coupling\n" << m_coupling_vector;
737 #endif
738 
739  // Calculate DOF of system after coupling
740  calculte_dof_after_coupling();
741 
742  // Calculate coupling matrix
743  calculate_coupling_matrix_increase();
744 
745  // Calculate joint lookup table
746  calculate_joint_LUT();
747 
748  // resize xi_vec
749  m_xi_vec.resize(m_dof);
750  //m_xi_vec.resize(m_joints);
751 
752  // Initialize vector
753  m_q_vec = Mat::Zero(m_q_vec.rows(),1);
754  m_xi_vec = Mat::Zero(m_xi_vec.rows(),1);
755  m_xi_vec_rest = m_xi_vec;
756 
757  // Resize matrices
758  m_J.resize(6,m_joints);
759  m_J_inv.resize(m_joints,6);
760 
761 }
762 
763 
764 
765 void Endorob::calculate_full_forward_kinematics()
766 {
767  // Mutex lock
768  //mutx.lock();
769 
770  Mat4d T_mat;
771 
772  if(!m_apply_joint_coupling)
773  {
774  for(llong i = 0; i < m_q_vec.rows(); i++)
775  {
776  // consider joints offsets if any
777  double joint_offset = 0.0;
778  if((m_DH_table[i].get_joint_type() == Joint_type::Revolute) ||
779  (m_DH_table[i].get_joint_type() == Joint_type::Rolling))
780  {
781  joint_offset = m_DH_table[i].theta();
782  }
783  else if(m_DH_table[i].get_joint_type() == Joint_type::Prismatic)
784  {
785  joint_offset = m_DH_table[i].d();
786  }
787 
788 
789  if(i == 0)
790  {
791  //T_mat = base_transformation_matrix(m_DH_table[i], m_q_vec[i] + joint_offset);
792  // For Standard DH, we need to consider the base matrix
793  if(m_DH_convention == DH_type::DH_standard)
794  {
795  T_mat = m_base_frame_T * base_transformation_matrix(m_DH_table[i], m_q_vec[i] + joint_offset);
796  }
797  // Modified DH case
798  else
799  T_mat = base_transformation_matrix(m_DH_table[i], m_q_vec[i] + joint_offset);
800  }
801  else
802  T_mat = T_mat * base_transformation_matrix(m_DH_table[i], m_q_vec[i] + joint_offset);
803 
804  m_fwd_k_vec.block<4,4>(i*4,0) = T_mat;
805  }
806  }
807  else // Consider joint coupling here
808  {
809  // Calculate new q_vec based on coupling
810  m_q_vec = m_joint_coupling_matrix * m_xi_vec;
811 
812  // Do fwd kinematics
813  for(int i = 0; i < m_q_vec.rows(); i++)
814  {
815  // consider joints offsets if any
816  double joint_offset = 0.0;
817  if((m_DH_table[i].get_joint_type() == Joint_type::Revolute) ||
818  (m_DH_table[i].get_joint_type() == Joint_type::Rolling))
819  {
820  joint_offset = m_DH_table[i].theta();
821  }
822  else if(m_DH_table[i].get_joint_type() == Joint_type::Prismatic)
823  {
824  joint_offset = m_DH_table[i].d();
825  }
826 
827  if(i == 0)
828  {
829  //T_mat = base_transformation_matrix(m_DH_table[i], m_q_vec[i] + joint_offset);
830  // For Standard DH, we need to consider the base matrix
831  if(m_DH_convention == DH_type::DH_standard)
832  {
833  T_mat = m_base_frame_T * base_transformation_matrix(m_DH_table[i], m_q_vec[i] + joint_offset);
834  }
835  // Modified DH case
836  else
837  T_mat = base_transformation_matrix(m_DH_table[i], m_q_vec[i] + joint_offset);
838  }
839  else
840  T_mat = T_mat * base_transformation_matrix(m_DH_table[i], m_q_vec[i] + joint_offset);
841 
842  m_fwd_k_vec.block<4,4>(i*4,0) = T_mat;
843  }
844  }
845 
846  // Tool frame
847  T_mat = T_mat * m_tool_frame_T;
848  //m_fwd_k_vec.push_back(T_mat);
849  m_fwd_k_vec.block<4,4>(m_joints*4,0) = T_mat;
850 
851  // Update tip transform
852  m_tip_T = T_mat;
853 
854  //std::cout << m_fwd_k_vec << std::endl;
855 
856  // Mutex unlock
857  //mutx.unlock();
858 
859 }
860 
861 Mat Endorob::calculate_full_forward_kinematics(const Vec &q)
862 {
863 
864  Mat4d T_mat;
865  Mat fwd_k_vec = m_fwd_k_vec;
866 
867  if(1)
868  {
869  for(llong i = 0; i < q.rows(); i++)
870  {
871  // consider joints offsets if any
872  double joint_offset = 0.0;
873  if((m_DH_table[i].get_joint_type() == Joint_type::Revolute) ||
874  (m_DH_table[i].get_joint_type() == Joint_type::Rolling))
875  {
876  joint_offset = m_DH_table[i].theta();
877  }
878  else if(m_DH_table[i].get_joint_type() == Joint_type::Prismatic)
879  {
880  joint_offset = m_DH_table[i].d();
881  }
882 
883 
884  if(i == 0)
885  {
886  //T_mat = base_transformation_matrix(m_DH_table[i], q[i] + joint_offset);
887  // For Standard DH, we need to consider the base matrix
888  if(m_DH_convention == DH_type::DH_standard)
889  {
890  T_mat = m_base_frame_T * base_transformation_matrix(m_DH_table[i], q[i] + joint_offset);
891  }
892  // Modified DH case
893  else
894  T_mat = base_transformation_matrix(m_DH_table[i], q[i] + joint_offset);
895  }
896  else
897  T_mat = T_mat * base_transformation_matrix(m_DH_table[i], q[i] + joint_offset);
898 
899  fwd_k_vec.block<4,4>(i*4,0) = T_mat;
900  }
901  }
902  else // Consider joint coupling here
903  {
904  // // Calculate new q_vec based on coupling
905  // q = m_joint_coupling_matrix * m_xi_vec;
906 
907  // Do fwd kinematics
908  for(int i = 0; i < q.rows(); i++)
909  {
910  // consider joints offsets if any
911  double joint_offset = 0.0;
912  if((m_DH_table[i].get_joint_type() == Joint_type::Revolute) ||
913  (m_DH_table[i].get_joint_type() == Joint_type::Rolling))
914  {
915  joint_offset = m_DH_table[i].theta();
916  }
917  else if(m_DH_table[i].get_joint_type() == Joint_type::Prismatic)
918  {
919  joint_offset = m_DH_table[i].d();
920  }
921 
922  if(i == 0)
923  {
924  //T_mat = base_transformation_matrix(m_DH_table[i], q[i] + joint_offset);
925  // For Standard DH, we need to consider the base matrix
926  if(m_DH_convention == DH_type::DH_standard)
927  {
928  T_mat = m_base_frame_T * base_transformation_matrix(m_DH_table[i], q[i] + joint_offset);
929  }
930  // Modified DH case
931  else
932  T_mat = base_transformation_matrix(m_DH_table[i], q[i] + joint_offset);
933  }
934  else
935  T_mat = T_mat * base_transformation_matrix(m_DH_table[i], q[i] + joint_offset);
936 
937  fwd_k_vec.block<4,4>(i*4,0) = T_mat;
938  }
939  }
940 
941  // Tool frame
942  T_mat = T_mat * m_tool_frame_T;
943  //fwd_k_vec.push_back(T_mat);
944  fwd_k_vec.block<4,4>(m_joints*4,0) = T_mat;
945 
946 
947  return fwd_k_vec;
948 
949 }
950 
951 
952 Mat4d Endorob::get_tip_pose()
953 {
954  return m_tip_T;
955 }
956 
957 Vec3d Endorob::get_tip_position()
958 {
959  return m_tip_T.block<3,1>(0,3);
960 }
961 
962 void Endorob::calculate_jacobian()
963 {
964  // For math info, check robotics book Siciliano
965 
966  // Mutex lock
967  //mutx.lock();
968 
969  // Empty vector otherwise it will be filled forever...
970  //m_z_vec.clear();
971 
972  // First z vector : z_0 = [0 0 1]^T
973  m_z_tmp << 0, 0, 1;
974  //m_z_vec.push_back(m_z_tmp);
975 
976  // Get full forward kinematics
977  calculate_full_forward_kinematics();
978 
979  // Get end effector transformation matrix
980  // m_p_e = m_fwd_k_vec[ m_joints ].block<3,1>(0,3);
981  m_p_e = m_fwd_k_vec.block<3,1>(m_joints*4,3);
982 
983  // main loop
984  for(unsigned i = 0; i < m_joints; i++)
985  {
986  if(m_DH_convention == DH_type::DH_modified)
987  {
988  // z is extracted from third column of rotation matrix
989  //m_z_tmp = m_fwd_k_vec[i].block<3,1>(0,2);
990  m_z_tmp = m_fwd_k_vec.block<3,1>(i*4,2);
991 
992  // p is extracted from fourth column of transformation matrix
993  //m_p_tmp = m_fwd_k_vec[i].block<3,1>(0,3);
994  m_p_tmp = m_fwd_k_vec.block<3,1>(i*4,3);
995  }
996  else if(m_DH_convention == DH_type::DH_standard)
997  {
998  if(i == 0)
999  {
1000  // z is extracted from third column of rotation matrix
1001  m_z_tmp = m_base_frame_T.block<3,1>(0,2);
1002 
1003  // p is extracted from fourth column of transformation matrix
1004  m_p_tmp = m_base_frame_T.block<3,1>(0,3);
1005  }
1006  else
1007  {
1008  // z is extracted from third column of rotation matrix
1009  m_z_tmp = m_fwd_k_vec.block<3,1>((i-1)*4,2);
1010 
1011  // p is extracted from fourth column of transformation matrix
1012  m_p_tmp = m_fwd_k_vec.block<3,1>((i-1)*4,3);
1013  }
1014  }
1015 
1016  // Revolute joint
1017  if((m_DH_table[i].get_joint_type() == Joint_type::Revolute) ||
1018  (m_DH_table[i].get_joint_type() == Joint_type::Rolling))
1019  {
1020  m_J_vel.block<3,1>(0,i) << m_z_tmp.cross(m_p_e - m_p_tmp);
1021  m_J_rot.block<3,1>(0,i) << m_z_tmp;
1022  }
1023  // Prismatic joint
1024  else if(m_DH_table[i].get_joint_type() == Joint_type::Prismatic)
1025  {
1026  m_J_vel.block<3,1>(0,i) << m_z_tmp;
1027  m_J_rot.block<3,1>(0,i) << 0,0,0;
1028 
1029  }
1030 
1031  // Joint limit Jacobian method check
1032  if(m_check_joint_limit_jacobian && (m_iterations > 10))
1033  {
1034  if(is_geq(m_q_vec(i), m_DH_table[i].joint_limit_max()))
1035  {
1036  m_q_vec(i) = m_DH_table[i].joint_limit_max();
1037  m_J_vel.block<3,1>(0,i) << 0,0,0;
1038  m_J_rot.block<3,1>(0,i) << 0,0,0;
1039 
1040  }
1041 
1042  else if (is_leq(m_q_vec(i), m_DH_table[i].joint_limit_min()))
1043  {
1044  m_q_vec(i) = m_DH_table[i].joint_limit_min();
1045  m_J_vel.block<3,1>(0,i) << 0,0,0;
1046  m_J_rot.block<3,1>(0,i) << 0,0,0;
1047 
1048  }
1049  }
1050  }
1051 
1052 
1053  m_J.resize(6, m_joints);
1054  m_J << m_J_vel, m_J_rot;
1055 
1056  //std::cout << "Jacobian\n" << m_J << std::endl;
1057 
1058  // Mutex unlock
1059  //mutx.unlock();
1060 
1061 }
1062 
1063 void Endorob::calculate_full_body_jacobian()
1064 {
1065  // Empty vector otherwise it will be filled forever...
1066  //m_z_vec.clear();
1067 
1068  // First z vector : z_0 = [0 0 1]^T
1069  m_z_tmp << 0, 0, 1;
1070  //m_z_vec.push_back(m_z_tmp);
1071 
1072  // Get full forward kinematics
1073  calculate_full_forward_kinematics();
1074 
1075  // Put in vector form
1076  convert_fwd_k_into_vec();
1077 
1078  // allocate space for Jacobian
1079  m_J.resize(m_joints*3, m_joints);
1080  m_J = Mat::Zero(m_joints*3, m_joints);
1081 
1082  // Get intermediate end-effector loop
1083  /*static*/ unsigned joint_index;
1084  /*static*/ int jacobian_index;
1085 
1086  joint_index = 0;
1087  jacobian_index = 0;
1088 
1089  for(unsigned j = 0; j < m_joints; j++)
1090  {
1091  // All proximal joints
1092  if(j < m_joints - 1)
1093  {
1094  // Get end effector transformation matrix
1095  //m_p_e = m_fwd_k_vec[ j ].block<3,1>(0,3);
1096  m_p_e = m_fwd_k_vec.block<3,1>(j*4,3);
1097  }
1098  else // final joint need to include tool matrix
1099  {
1100  // Get end effector transformation matrix
1101  //m_p_e = m_fwd_k_vec[ m_joints ].block<3,1>(0,3);
1102  m_p_e = m_fwd_k_vec.block<3,1>(m_joints*4,3);
1103  }
1104 
1105  joint_index++;
1106 
1107  m_J_vel = Mat::Zero(3,m_joints);
1108  // main loop
1109  for(unsigned i = 0; i < joint_index; i++)
1110  {
1111 
1112  // z is extracted from third column of rotation matrix
1113  //m_z_tmp = m_fwd_k_vec[i].block<3,1>(0,2);
1114  m_z_tmp = m_fwd_k_vec.block<3,1>(i*4,2);
1115 
1116 
1117  // p is extracted from fourth column of transformation matrix
1118  //m_p_tmp = m_fwd_k_vec[i].block<3,1>(0,3);
1119  m_p_tmp = m_fwd_k_vec.block<3,1>(i*4,3);
1120 
1121 
1122 
1123  // Revolute joint
1124  if((m_DH_table[i].get_joint_type() == Joint_type::Revolute) ||
1125  (m_DH_table[i].get_joint_type() == Joint_type::Rolling))
1126  {
1127  m_J_vel.block<3,1>(0,i) << m_z_tmp.cross(m_p_e - m_p_tmp);
1128  m_J_rot.block<3,1>(0,i) << m_z_tmp;
1129  }
1130 
1131  // Prismatic joint
1132  else if(m_DH_table[i].get_joint_type() == Joint_type::Prismatic)
1133  {
1134  m_J_vel.block<3,1>(0,i) << m_z_tmp;
1135  m_J_rot.block<3,1>(0,i) << 0,0,0;
1136  }
1137  }
1138 
1139  m_J.block(jacobian_index,0, 3,m_joints) = m_J_vel;
1140  jacobian_index+=3;
1141  }
1142  //std::cout << std::endl << m_J << std::endl;
1143 
1144  //calculate_jacobian();
1145 
1146  //std::cout << std::endl << m_J << std::endl;
1147 
1148  //calculate_pseudo_inverse();
1149 
1150 
1151  //std::cout << "Jacobian\n" << m_J << std::endl;
1152 
1153  // Mutex unlock
1154  //mutx.unlock();
1155 
1156 }
1157 
1158 void Endorob::calculate_full_mobile_body_jacobian()
1159 {
1161  // First z vector : z_0 = [0 0 1]^T
1162  m_z_tmp << 0, 0, 1;
1163 
1164  // Get full forward kinematics
1165  calculate_full_forward_kinematics();
1166 
1167  // Put in vector form
1168  convert_fwd_k_into_vec();
1169 
1170  // allocate space for Jacobian
1171  // 6 DOF mobile base => 3 lin_v + 3 rot_v
1172  // if(first_run)
1173  // {
1174  // m_J.resize((m_joints - m_joint_index_offset)*3 + 3 , m_joints);
1175  // first_run = false;
1176  // }
1177  m_J = Mat::Zero((m_joints - m_joint_index_offset) * 3 + 3, m_joints);
1178 
1179  // Get intermediate end-effector loop
1180  /*static*/ unsigned joint_index;
1181  /*static*/ int jacobian_index;
1182 
1183  joint_index = 0;
1184  jacobian_index = 0;
1185 
1186  // std::cout << "\n\ncurrent\tdesired" << std::endl;
1187  // for(unsigned f = 0; f < 9; f++)
1188  // std::cout << m_full_body_vec_current(f) << " \t" << m_full_body_vec_desired(f) << std::endl;
1189 
1190  for(unsigned j = m_joint_index_offset; j < m_joints; j++)
1191  {
1192  //std::cout << "\n New iteration \n\n";
1193 
1194  // All proximal joints
1195  if(j < m_joints - 1)
1196  {
1197  // Get end effector transformation matrix
1198  m_p_e = m_fwd_k_vec.block<3,1>(j*4,3);
1199  }
1200  else // final joint need to include tool matrix
1201  {
1202  // Get end effector transformation matrix
1203  m_p_e = m_fwd_k_vec.block<3,1>(m_joints*4,3);
1204  }
1205 
1206  joint_index++;
1207 
1208  m_J_vel = Mat::Zero(3,m_joints);
1209  m_J_rot = Mat::Zero(3,m_joints);
1210 
1211  // main loop
1212  for(unsigned i = 0; i < joint_index; i++)
1213  {
1214  // z is extracted from third column of rotation matrix
1215  m_z_tmp = m_fwd_k_vec.block<3,1>(i*4,2);
1216 
1217  // p is extracted from fourth column of transformation matrix
1218  m_p_tmp = m_fwd_k_vec.block<3,1>(i*4,3);
1219 
1220  // Revolute joint
1221  if((m_DH_table[i].get_joint_type() == Joint_type::Revolute) ||
1222  (m_DH_table[i].get_joint_type() == Joint_type::Rolling))
1223  {
1224  m_J_vel.block<3,1>(0,i) << m_z_tmp.cross(m_p_e - m_p_tmp);
1225  m_J_rot.block<3,1>(0,i) << m_z_tmp;
1226  }
1227 
1228  // Prismatic joint
1229  else if(m_DH_table[i].get_joint_type() == Joint_type::Prismatic)
1230  {
1231  m_J_vel.block<3,1>(0,i) << m_z_tmp;
1232  m_J_rot.block<3,1>(0,i) << 0,0,0;
1233  }
1234 
1235  // Fault tolerance
1236  if(m_apply_fault_tolerance)
1237  {
1238  if(m_faulty_joint_vec[i])
1239  {
1240  m_J_vel.block<3,1>(0,i) << 0,0,0;
1241  m_J_rot.block<3,1>(0,i) << 0,0,0;
1242  //m_q_vec(i) = 0;
1243  }
1244  }
1245  }
1246 
1247 
1248 
1249  m_J.block(jacobian_index,0, 3,m_joints) = m_J_vel;
1250  jacobian_index+=3;
1251 
1252  }
1253 
1254  // J_rot added at the end
1255  m_J.block(jacobian_index,0, 3,m_joints) = m_J_rot;
1256 
1257  //std::cout << "\nJ_rot\n" << m_J_rot_tmp << std::endl;
1258  //std::cout << "\nJ\n" << m_J << std::endl;
1259 
1260 
1261  //std::cout << std::endl << m_J << std::endl << std::endl;
1262  m_J_for_user = m_J;
1263 
1264  //calculate_jacobian();
1265 
1266  //std::cout << std::endl << m_J << std::endl;
1267 
1268  //calculate_pseudo_inverse();
1269 
1270 
1271  //std::cout << "Jacobian\n" << m_J << std::endl;
1272 
1273  // Mutex unlock
1274  //mutx.unlock();
1275 
1276 }
1277 
1278 
1279 
1280 Vec3d Endorob::get_position_error(const Mat4d &Td, const Mat4d &Te)
1281 {
1282  Vec3d error = Td.block<3,1>(0,3) - Te.block<3,1>(0,3);
1283  return error;
1284 }
1285 
1286 Vec3d Endorob::get_orientation_error(const Mat4d &Td, const Mat4d &Te)
1287 {
1288  Vec3d result;
1289  // Quaternion approach... to debug
1290  // Eigen::Quaterniond Qd(Td.block<3,3>(0,0)), Qe(Te.block<3,3>(0,0));
1291  // Eigen::Quaterniond delta_Q = Qd * Qe.inverse();
1292  // return delta_Q.vec();
1293 
1294  // Angle axis approach from Siciliano book page 139
1295  Mat3d R = Td.block<3,3>(0,0) * Te.block<3,3>(0,0).transpose();
1296  // Mat3d R = Te.block<3,3>(0,0).transpose() * Td.block<3,3>(0,0) ;
1297  Eigen::AngleAxisd angle_axis(R);
1298 
1299  result = angle_axis.axis() * sin(angle_axis.angle());
1300  //result = angle_axis.axis() * (angle_axis.angle());
1301  return result;
1302 
1303 }
1304 
1305 void Endorob::get_error(const Mat4d &Td, const Mat4d &Te)
1306 {
1307  // Mutex lock
1308  //mutx.lock();
1309 
1310 
1311  //error.resize(6);
1312  m_error.block<3,1>(0,0) = get_position_error(Td, Te);
1313  m_error.block<3,1>(3,0) = get_orientation_error(Td, Te);
1314 
1315  // Mutex unlock
1316  //mutx.unlock();
1317 
1318 
1319 }
1320 
1321 Vec Endorob::get_full_body_error()
1322 {
1323  return m_full_body_vec_desired - m_full_body_vec_current;
1324 }
1325 
1326 void Endorob::get_full_mobile_body_error()
1327 {
1328  // /*static*/ bool first_run = true;
1329 
1330  // if(first_run)
1331  // {
1332  // m_error.resize((m_joints - m_joint_index_offset)*3 + 3);
1333  // first_run = false;
1334  // }
1335 
1336  auto vec_size = m_error.rows() - 3;
1337 
1338  // Position error
1339  m_error.block(0, 0, vec_size, 1) = m_full_body_vec_desired.block(m_joint_index_offset*3, 0, vec_size,1) - m_full_body_vec_current.block(m_joint_index_offset*3, 0, vec_size, 1);
1340 
1341  // Consider that first joints can only move 1 DOF, then 2 DOF, then 3 DOF
1342  //m_error.block<>() <<
1343 
1344  // Orientation error
1345  m_error.block<3,1>(vec_size, 0) = get_orientation_error(m_target_T, m_tip_T);
1346 
1347  //std::cout << error.block<18,1>(0,0) << std::endl << std::endl;
1348 
1349 }
1350 
1351 
1352 
1353 void Endorob::threaded_solver(int a, int b)
1354 {
1355  // Inverse kinematics loop
1356 
1357  m_iterations = 0;
1358 
1359 
1360  if(m_run)
1361  {
1362  m_thread_running = true;
1363  }
1364  else
1365  {
1366 #ifdef VERBOSE_MODE
1367  std::cout << "Solver problem...\n";
1368 #endif
1369  }
1370 
1371  while(m_run)
1372  {
1373  m_prev_error = Mat::Constant(6,1, INF_LIM);
1374 
1375  while(m_start_solver)
1376  {
1377  if(!m_wait_for_sync) // required if the user sets a new q vector
1378  {
1379  m_solver_pending = false;
1380  // Chrono
1381  //auto t0 = chrono_time::now();
1382 
1383 
1384  // Iterative loop:
1385  //while( (!m_converged) && ( m_iterations < 100) )
1386  {
1387  // Solver function
1388  solve();
1389 
1390  // Check for convergence
1391  check_for_convergence();
1392 
1393  m_iterations++;
1394  }
1395 
1396  // Check if exit without convergence
1397  if(m_converged == false)
1398  {
1399 
1400  }
1401  //m_iterations = 0;
1402 
1403  //m_start_solver = false;
1404  }
1405  m_solver_pending = true;
1406  }
1407  }
1408  m_thread_running = false;
1409 
1410 #ifdef VERBOSE_MODE
1411  std::cout << "Solver " << m_instantiation_index << " stopped ...\n";
1412 #endif
1413 }
1414 
1415 void Endorob::solve()
1416 {
1417  // First step calculate jacobian
1418  calculate_jacobian();
1419 
1420  // Apply coupling if required
1421  if(m_apply_joint_coupling)
1422  m_J = m_J * m_joint_coupling_matrix;
1423 
1424  // For user data
1425  m_J_for_user = m_J;
1426 
1427  // Calculate error
1428  get_error(m_target_T, m_tip_T);
1429 
1430 
1431  // Check for mask
1432  if(!m_apply_mask)
1433  {
1434 
1435  // Generic iterative solve equation
1436  (*this.*p_solver_type)();
1437 
1438  // Angle modulus for revolute joints
1439  if(!m_apply_joint_coupling)
1440  {
1441  m_q_vec = angle_modulus(m_q_vec);
1442  }
1443  else
1444  {
1445  m_xi_vec = angle_modulus(m_xi_vec);
1446  }
1447 
1448  // Check if solution is good enough
1449  check_for_convergence();
1450 
1451  }
1452  else
1453  {
1454  solve_for_mask();
1455 
1456  // Check if solution is good enough
1457  if(m_error_masked.norm() < 1E-8)
1458  {
1459  // Joint limit
1460  if(!m_apply_joint_coupling)
1461  {
1462  if(check_joint_in_limit(m_q_vec))
1463  {
1464  m_q_vec = m_prev_q_vec;
1465  }
1466  }
1467  else
1468  {
1469  if(check_joint_in_limit(m_xi_vec))
1470  {
1471  m_xi_vec = m_prev_xi_vec;
1472  }
1473  }
1474  m_converged = true;
1475  //m_start_solver = false;
1476  }
1477  }
1478 
1479  // update previous variables
1480  m_prev_q_vec = m_q_vec;
1481  m_prev_xi_vec = m_xi_vec;
1482  m_prev_error = m_error;
1483 }
1484 
1485 void Endorob::apply_joint_limits()
1486 {
1487  // Joint limit
1488  if(!m_apply_joint_coupling)
1489  {
1490  if(check_joint_in_limit(m_q_vec))
1491  {
1492  m_q_vec_for_user = m_prev_q_vec;
1493  }
1494  }
1495  else
1496  {
1497  if(check_joint_in_limit(m_xi_vec))
1498  {
1499  m_q_vec_for_user = m_prev_xi_vec;
1500  }
1501  }
1502 }
1503 
1504 Vec Endorob::get_faulty_q()
1505 {
1506  Vec q = m_q_vec;
1507 
1508  // Section 4
1509  q[22] = 0.0;
1510  q[23] = 0.0;
1511  q[26] = 0.0;
1512  q[27] = 0.0;
1513 
1514 
1515  return q;
1516 }
1517 
1518 void Endorob::solve_for_mask()
1519 {
1520  double alpha = 0.5;
1521 
1522  // Apply mask to error:
1523  m_error_masked = mask_resize(m_error);
1524 
1525  // Apply mask to Jacobian matrix:
1526  m_J_masked = mask_resize(m_J);
1527 
1528  // For user data
1529  m_J_for_user = m_J_masked;
1530 
1531  // Calculate pseudo inverse
1532  calculate_pseudo_inverse_masked();
1533 
1534  // Iterative equation with mask and coupling
1535  if(m_apply_joint_coupling)
1536  m_xi_vec = m_xi_vec + alpha * m_J_inv_masked * m_error_masked;
1537  else
1538  m_q_vec = m_q_vec + alpha * m_J_inv_masked * m_error_masked;
1539 
1540  // Angle modulus for revolute joints
1541  if(!m_apply_joint_coupling)
1542  {
1543  m_q_vec = angle_modulus(m_q_vec);
1544  }
1545  else
1546  {
1547  m_xi_vec = angle_modulus(m_xi_vec);
1548  }
1549 }
1550 
1551 void Endorob::check_for_convergence()
1552 {
1553  // Check convergence
1554  if(m_error.norm() < 1E-3)
1555  {
1556 
1557 
1558  // Chrono
1559  //auto t1 = chrono_time::now();
1560  //Delta_t elapsed = t1 - t0;
1561 
1562  m_converged = true;
1563  //m_start_solver = false;
1564 
1565  }
1566  else if( abs(m_error.norm() - m_prev_error.norm()) < 1E-3 )
1567  {
1568  m_converged = true;
1569  }
1570  else if(m_error.norm() > 1.5 * m_prev_error.norm())
1571  {
1572  m_diverged = true;
1573  // We diverge... reduce alpha
1574  // if(alpha >0.01)
1575  // {
1576  // alpha = alpha/2;
1577  // q = prev_q;
1578  // m_error = m_prev_error;
1579  // }
1580 
1581  }
1582 }
1583 
1584 
1585 
1586 void Endorob::threaded_solver_2(int a, int b)
1587 {
1588  // Inverse kinematics loop
1589  // Declare variables and assign memory space
1590  double alpha = 1;
1591  m_iterations = 0;
1592 
1593  // Joint vector
1594  Vec prev_q = m_q_vec;
1595  Vec prev_xi = m_xi_vec;
1596 
1597  if(m_run)
1598  {
1599 #ifdef VERBOSE_MODE
1600  std::cout << "Solver " << m_instantiation_index << " assigned to CPU " << m_CPU_affinity +1 << " and running..."<< std::endl;
1601 #endif
1602  m_thread_running = true;
1603  }
1604  else
1605  {
1606 #ifdef VERBOSE_MODE
1607  std::cout << "Solver problem...\n";
1608 #endif
1609  }
1610 
1611  while(m_run)
1612  {
1613  while(m_start_solver)
1614  {
1615  if(!m_wait_for_sync) // required if the user sets a new q vector
1616  {
1617  m_solver_pending = false;
1618  // Chrono
1619  //auto t0 = chrono_time::now();
1620 
1621  m_prev_error = Mat::Constant(m_joints*3,1, INF_LIM);
1622 
1623  // Iterative loop:
1624  //while( (!m_converged) && ( m_iterations < 200) )
1625  {
1626  // First step calculate jacobian
1627  calculate_full_mobile_body_jacobian();
1628  //std::cout << ".";
1629  //calculate_full_body_jacobian();
1630 
1631  // Apply coupling
1632  m_J = m_J * m_joint_coupling_matrix;
1633 
1634  // Calculate error
1635  //m_error = get_full_body_error();
1636  get_full_mobile_body_error();
1637 
1638 
1639 
1640  // Calculate pseudo inverse
1641  calculate_pseudo_inverse();
1642  //calculate_pseudo_inverse_DLS();
1643 
1644  //null_space_constraint_calculation();
1645 
1646 
1647  // Iterative equation
1648  //if(m_apply_weighted_solution)
1649  //modify_error_with_f();
1650 
1651 
1652  // Bare Equation
1653  m_xi_vec = m_xi_vec + alpha * m_J_inv * m_error;
1654 
1655  //m_xi_vec = m_xi_vec + m_J_inv * m_error + (m_identity - m_J_inv * m_J) * m_phi;
1656 
1657  // Weighted Iterative equation
1658  //m_xi_vec = m_xi_vec + alpha * m_J_inv * m_error_weights * m_error;
1659 
1660  // DLS Iterative equation
1661  //m_xi_vec = m_xi_vec + m_J_inv * m_error;
1662 
1663  //m_xi_vec = m_xi_vec + m_J_inv * m_error_weights * m_error;
1664 
1665 
1666  // Null space weighted Iterative equation
1667  //m_xi_vec = m_xi_vec + m_J_inv * m_error_weights * m_error + (m_identity - m_J_inv * m_J) * m_phi;
1668  //m_xi_vec = m_xi_vec + m_J_inv * m_error + (m_identity - m_J_inv.block<18,6>(0,159) * m_J.block<6,18>(159,0)) * m_phi;
1669  //std::cout << (m_identity - m_J_inv * m_J) * m_phi << std::endl << std::endl;
1670 
1671  //for(int l = 0; l < m_error_weights.rows(); l++)
1672  // std::cout << m_error_weights(l,l) << std::endl;
1673 
1674  //std::cout << std::endl << m_phi << std::endl << std::endl;
1675 
1676  // Angle modulus for revolute joints
1677  m_xi_vec = angle_modulus(m_xi_vec);
1678 
1679 
1680  // Fault tolerance
1681  int fault_LUT[54]={0,0,0,0,0,0,6,6,7,7,6,6,7,7,8,8,9,9,8,8,9,9,10,10,11,11,10,10,11,11,12,12,13,13,12,12,13,13,14,14,15,15,14,14,15,15,16,16,17,17,16,16,17,17};
1682  if(m_apply_fault_tolerance)
1683  {
1684  for(auto l = 0; l < 54; l++)
1685  {
1686 
1687  if(m_faulty_joint_vec[ l ])
1688  {
1689  m_xi_vec(fault_LUT[l]) = 0;
1690  }
1691  }
1692  }
1693 
1694 
1695  //std::cout << m_error.block(6, 0, m_error.rows()-7, 1).norm() << std::endl << std::endl;
1696  //std::cout << m_error.norm() << std::endl << std::endl;
1697 
1698 
1699  // Check if solution is good enough
1700  if(m_error.norm() < 1E-3)
1701  {
1702  m_converged = true;
1703  // Joint limit
1704  if(check_joint_in_limit(m_xi_vec))
1705  {
1706  m_xi_vec = prev_xi;
1707  }
1708 
1709  // Zero cleaning
1710  //m_xi_vec = zero_thresholding(m_xi_vec);
1711 
1712  // Chrono
1713  // auto t1 = chrono_time::now();
1714  // Delta_t elapsed = t1 - t0;
1715  // std::cout << "Found solution in " << elapsed.count() << " s" << std::endl;
1716  //std::cout << "\n## " << m_iterations << std::endl;
1717  m_start_solver = false;
1718  }
1719 
1720  // update previous variables
1721  prev_q = m_q_vec;
1722  prev_xi = m_xi_vec;
1723  m_prev_error = m_error;
1724  m_iterations++;
1725 
1726  //std::cout << m_iterations << std::endl;
1727  }
1728 
1729  // Check if exit without convergence
1730  if(m_converged == false)
1731  {
1732  // Joint limit
1733  if(check_joint_in_limit(m_xi_vec))
1734  {
1735  m_xi_vec = prev_xi;
1736  }
1737 
1738  // Angle modulus for revolute joints
1739  //m_q_vec = angle_modulus(m_q_vec);
1740 
1741  // Zero cleaning
1742  //m_q_vec = zero_thresholding(m_q_vec);
1743 
1744  m_iteration_overflow = true;
1745 
1746 
1747  //m_start_solver = false;
1748  }
1749 
1750  m_q_vec_for_user = m_xi_vec;
1751  //m_iterations = 0;
1752 
1753  }
1754  m_solver_pending = true;
1755 
1756  }
1757 
1758  //std::this_thread::sleep_for(std::chrono::microseconds(1));
1759  }
1760 #ifdef VERBOSE_MODE
1761  std::cout << "Solver exited...\n";
1762 #endif
1763 }
1764 
1765 //Vec Endorob::get_inversekinematics(const Mat4d &Td, const Vec &q_vec)
1766 //{
1767 // // Declare variables:
1768 // double alpha = 1;
1769 // unsigned long m_iterations = 0;
1770 // Vec error, m_prev_error;
1771 // m_prev_error.resize(6);
1772 // m_prev_error << INF_LIM, INF_LIM, INF_LIM, INF_LIM, INF_LIM, INF_LIM;
1773 
1774 // Vec q = q_vec, prev_q = q_vec;
1775 
1776 // // Jacobian
1777 // Matrix J, J_inv;
1778 
1779 
1780 // // Iterative loop:
1781 // bool converged = false;
1782 
1783 // while( (!converged) && (m_iterations < 20000) )
1784 // {
1785 // // First step calculate jacobian
1786 // J = calculate_jacobian(q);
1787 // J_inv = calculate_pseudo_inverse(J);
1788 
1789 // // Calculate error
1790 // Mat4d Te = calculate_forward_kinematics(q);
1791 // error = get_error(Td, Te);
1792 
1793 // // Check convergence
1794 // // if(error.norm() > 1.5 * m_prev_error.norm())
1795 // // {
1796 // // // We diverge... reduce alpha
1797 // // if(alpha >0.01)
1798 // // {
1799 // // alpha = alpha/2;
1800 // // q = prev_q;
1801 // // error = m_prev_error;
1802 // // }
1803 
1804 // // }
1805 // // else
1806 // {
1807 // Vec dq = q;
1808 // dq = alpha * J_inv * error;
1809 // // q = q + alpha * J_inv * error;
1810 // q = q + dq;
1811 
1812 // // Angle modulus for revolute joints
1813 // //q = angle_modulus(q);
1814 
1815 
1816 
1817 // }
1818 
1819 // // Check if solution is good enough
1820 // if(error.norm() < 1E-8)
1821 // {
1822 // converged = true;
1823 // }
1824 
1825 // // update previous variables
1826 // prev_q = q;
1827 // m_prev_error = error;
1828 // m_iterations++;
1829 // }
1830 
1831 // // Zero cleaning
1832 // q = zero_thresholding(q);
1833 
1834 // return q;
1835 
1836 //}
1837 
1838 //Vec Endorob::get_inversekinematics(const Mat4d &Td)
1839 //{
1840 // Vec q;
1841 // q.resize(m_DH_table.size());
1842 
1843 // for(unsigned i = 0; i < m_DH_table.size(); i++)
1844 // q (i) = 0;
1845 
1846 // return get_inversekinematics(Td, q);
1847 //}
1848 
1849 Vec Endorob::get_instant_IK_solution()
1850 {
1851  m_converged = false;
1852  m_iteration_overflow = false;
1853  m_iterations = 0;
1854  m_start_solver = true;
1855 
1856  apply_joint_limits();
1857 
1858  return m_q_vec_for_user;
1859 }
1860 
1861 Vec Endorob::get_q_vec()
1862 {
1863  if(!m_apply_joint_coupling)
1864  return m_q_vec;
1865  else
1866  return m_xi_vec;
1867 }
1868 
1869 Vec Endorob::get_full_q_vec()
1870 {
1871  return m_q_vec;
1872 }
1873 
1874 Vec Endorob::get_desired_curve()
1875 {
1876  return m_virtual_snake;
1877 }
1878 
1879 void Endorob::set_desired_curve(const Vec &curve)
1880 {
1881  m_virtual_snake = curve;
1882 }
1883 
1884 void Endorob::set_desired_curve(const Vec &curve, const Mat4d &mat)
1885 {
1886  m_virtual_snake = curve;
1887  m_target_T = mat;
1888 }
1889 
1890 Vec Endorob::get_IK_solution()
1891 {
1892  m_converged = false;
1893  m_iteration_overflow = false;
1894  m_iterations = 0;
1895  m_start_solver = true;
1896 
1897  while(!solver_has_ended())
1898  std::this_thread::sleep_for(std::chrono::microseconds(1));
1899 
1900  apply_joint_limits();
1901 
1902  return m_q_vec_for_user;
1903 }
1904 
1905 
1906 void Endorob::set_q_vec(const Vec &q)
1907 {
1908  m_wait_for_sync = true;
1909  while(!m_solver_pending);
1910 
1911  if(m_apply_joint_coupling)
1912  m_xi_vec = q;
1913  else
1914  m_q_vec = q;
1915 
1916  calculate_full_forward_kinematics();
1917 
1918  // bug fix
1919  m_target_T = m_tip_T;
1920 
1921  if(m_full_body_needed)
1922  convert_fwd_k_into_vec();
1923 
1924  m_wait_for_sync = false;
1925 }
1926 
1927 void Endorob::reset_q_vec()
1928 {
1929  m_wait_for_sync = true;
1930  while(!m_solver_pending);
1931 
1932  m_q_vec = Mat::Zero(m_q_vec.rows(),1);
1933  m_xi_vec = Mat::Zero(m_xi_vec.rows(),1);
1934 
1935  // Do initial full forward Kinematics
1936  calculate_full_forward_kinematics();
1937 
1938  // Put into vector from
1939  if(m_full_body_needed)
1940  convert_fwd_k_into_vec();
1941  m_wait_for_sync = false;
1942 }
1943 
1944 Vec Endorob::get_random_q_vec()
1945 {
1946  Vec result;
1947  result.resize(m_dof);
1948 
1949  for(unsigned i = 0; i < m_dof; i++)
1950  {
1951  int val = rand() % 1000;
1952 
1953  double min = m_DH_table[i].joint_limit_min();
1954  double max = m_DH_table[i].joint_limit_max();
1955  result[i] = ((1 - (double)val/1000) * min) + ((double)val/1000 * max);
1956  }
1957 
1958  return result;
1959 
1960 }
1961 
1962 Vec3d get_rpy(const Mat &mat)
1963 {
1964  Mat3d rot;
1965  Vec3d result(3);
1966 
1967  // Check matrix size, homogeneous or rotation matrix
1968  if(mat.cols() > 3)
1969  {
1970  rot = mat.block<3,3>(0,0);
1971  }
1972  else
1973  rot = mat;
1974 
1975  result = rot.eulerAngles(2,1,0);
1976 
1977  return result;
1978 }
1979 
1980 Mat4d set_rpy(const Mat &mat, Vec3d rpy)
1981 {
1982  Mat4d T = mat;
1983  T.block<3,3>(0,0) = (Eigen::AngleAxisd(rpy[0], Vec3d::UnitZ())
1984  * Eigen::AngleAxisd(rpy[1], Vec3d::UnitY())
1985  * Eigen::AngleAxisd(rpy[2], Vec3d::UnitX())).toRotationMatrix();
1986  return T;
1987 }
1988 
1989 Mat3d rot_x(const double &angle)
1990 {
1991  return Eigen::AngleAxisd(angle, Vec3d::UnitX()).toRotationMatrix();
1992 }
1993 
1994 Mat3d rot_y(const double &angle)
1995 {
1996  return Eigen::AngleAxisd(angle, Vec3d::UnitY()).toRotationMatrix();
1997 }
1998 
1999 Mat3d rot_z(const double &angle)
2000 {
2001  return Eigen::AngleAxisd(angle, Vec3d::UnitZ()).toRotationMatrix();
2002 }
2003 
2004 Mat3d rot_xyz(const double &angle, const Vec3d &axis)
2005 {
2006  return Eigen::AngleAxisd(angle, axis).toRotationMatrix();
2007 }
2008 
2009 Mat4d rotate_transform_locally(const Mat4d &mat, const Mat3d &rot)
2010 {
2011  Mat4d result = mat;
2012 
2013  result.block<3,3>(0,0) = rot * result.block<3,3>(0,0);
2014 
2015  return result;
2016 }
2017 
2018 Mat4d rotate_transform_around_P(const Mat4d &T, const Mat3d &rot, const Vec &P)
2019 {
2020  Mat4d rot4=Eigen::MatrixXd::Identity(4,4), result = T;
2021 
2022  rot4.block<3,3>(0,0) = rot;
2023 
2024  result.block<3,1>(0,3) = result.block<3,1>(0,3) - P;
2025 
2026  result = rot4 * result;
2027 
2028  result.block<3,1>(0,3) = result.block<3,1>(0,3) + P;
2029 
2030  return result;
2031 }
2032 
2033 Mat4d rotate_transform_around_axis_and_P(const Mat4d &T, const double &angle, const Vec3d &axis, const Vec &P)
2034 {
2035  Mat4d rot4 = Eigen::MatrixXd::Identity(4,4), result = T;
2036 
2037  // Arbitrary axis rotation matrix
2038  rot4.block<3,3>(0,0) = rot_xyz(angle, axis);;
2039 
2040  result.block<3,1>(0,3) = result.block<3,1>(0,3) - P;
2041 
2042  result = rot4 * result;
2043 
2044  result.block<3,1>(0,3) = result.block<3,1>(0,3) + P;
2045 
2046  return result;
2047 }
2048 
2049 
2050 
2051 Vec3d project_point_onto_line(const Vec3d &P, const Vec3d &A, const Vec3d &B)
2052 {
2053  // From https://gamedev.stackexchange.com/questions/72528/how-can-i-project-a-3d-point-onto-a-3d-line
2054  Vec3d result;
2055 
2056  result = A + (P-A).dot(B-A)/(B-A).dot(B-A)*(B-A);
2057 
2058  return result;
2059 }
2060 
2061 Vec3d sphere_line_intersection(const double &radius, const Vec3d &center, const Vec3d &A, const Vec3d &B)
2062 {
2063  // from https://en.wikipedia.org/wiki/Line%E2%80%93sphere_intersection
2064  Vec3d l, result;
2065 
2066  l = (B-A).normalized();
2067 
2068  double delta = pow(l.dot(A-center),2) - pow((A-center).norm(),2) + pow(radius,2);
2069 
2070 
2071  if(delta > 0)
2072  {
2073  // d represent distance of intersection to point A
2074  double d1 = - l.dot(A-center) + sqrt( delta );
2075  //double d2 = - l.dot(A-center) - sqrt( delta );
2076 
2077  // We pick the closest point to A
2078  //if(abs(d1) > abs(d2))
2079  //{
2080  // result = A + d2 * l;
2081  //}
2082  //else
2083  //{
2084  result = A + d1 * l;
2085  //}
2086 
2087  }
2088  else if(delta == 0)
2089  {
2090  double d = - l.dot(A-center);
2091  result = A + d * l;
2092  }
2093  else
2094  {
2095  // Error, no intersection
2096  }
2097 
2098 
2099 
2100  return result;
2101 }
2102 
2103 
2104 Vec Endorob::get_body_vec()
2105 {
2106  return m_full_body_vec_current;
2107 }
2108 
2109 Vec Endorob::get_body_vec(const Vec &q)
2110 {
2111  return(convert_fwd_k_into_vec(calculate_full_forward_kinematics(q)));
2112 }
2113 
2114 unsigned Endorob::get_iteration()
2115 {
2116  return m_iterations;
2117 }
2118 
2119 double Endorob::get_error_norm()
2120 {
2121  return m_error.block(0,0,m_error.rows()-3,1).norm();
2122  //return m_error.norm();
2123 }
2124 
2125 double Endorob::get_RMS_error()
2126 {
2127  // RMS error calculation from http://statweb.stanford.edu/~susan/courses/s60/split/node60.html
2128  Vec A = m_error.block(0,0,m_error.rows()-3,1);
2129 
2130  double n = m_error.rows()-3;
2131  Vec B(A.rows());
2132  for(auto i = 0; i < A.rows(); i++)
2133  {
2134  B(i) = A(i) * A(i);
2135  }
2136 
2137  double res = sqrt( B.sum()/n );
2138  return res;
2139 
2140 }
2141 
2142 Vec Endorob::get_error_vector()
2143 {
2144  return m_error;
2145 }
2146 
2147 
2148 
2149 bool Endorob::solver_has_converged()
2150 {
2151  return m_converged;
2152 }
2153 
2154 bool Endorob::solver_has_ended()
2155 {
2156  return (m_iteration_overflow || m_converged);
2157 }
2158 
2159 void Endorob::set_solver_target(const Mat4d &T)
2160 {
2161  m_target_T = T;
2162  m_converged = false;
2163  m_iteration_overflow = false;
2164  m_iterations = 0;
2165  m_start_solver = true;
2166 
2167 }
2168 
2169 void Endorob::set_solver_body_target(const Vec &vec)
2170 {
2171  m_full_body_vec_desired = vec;
2172  m_converged = false;
2173  m_iteration_overflow = false;
2174  m_iterations = 0;
2175  m_start_solver = true;
2176 
2177 }
2178 
2179 void Endorob::set_solver_body_target(const Vec &vec, const Mat4d &T)
2180 {
2181  m_full_body_vec_desired = vec;
2182  m_target_T = T;
2183  m_converged = false;
2184  m_iteration_overflow = false;
2185  m_iterations = 0;
2186  m_start_solver = true;
2187 
2188 }
2189 
2190 Vec Endorob::solve_with_tentacle(Mat T_)
2191 {
2192  // T is the tip position and orientation
2193  Vec result;
2194  return result;
2195 }
2196 
2197 Vec Endorob::follow_the_leader(const std::vector<Vec3d, Eigen::aligned_allocator<Vec3d>> path, const Vec &desired_shape, const double &translation)
2198 {
2199  Vec result = desired_shape;
2200  m_virtual_snake;
2201 
2202  int path_index = path.size() - 1;
2203  if(path.size() > desired_shape.rows()/3)
2204  {
2205  // index for 3D points
2206  int index_point = desired_shape.rows() - 6;
2207 
2208  // Loop over DH table
2209  // First link is controlled by user hence the -3
2210  // All the other links are calculated to follow.
2211  //for(int i = m_DH_table.size() - 6; i > 0 ; i--)
2212  for(int i = desired_shape.rows()/3 - 2; i > 0 ; i--)
2213  {
2214  //if(m_DH_table[i].get_joint_type() == Joint_type::Rolling)
2215  {
2216  // End of link is beginning of next one
2217  Vec3d link_end = result.block<3,1>(index_point,0);
2218 
2219  // Interpolation
2220  Vec3d a = path[path_index-1];
2221  Vec3d b = path[path_index];
2222 
2223 
2224  // double joint_L = m_DH_table[i].a();
2225  double joint_L=0;
2226  if(i==3)
2227  joint_L = 0.01182;
2228  if(i==2)
2229  joint_L = 0.01182;
2230  if(i==1)
2231  joint_L = 0.00618;
2232 
2233 
2234 
2235 
2236  double t = translation;
2237 
2238  if(joint_L > 0)
2239  t = translation / joint_L;
2240 
2241  Vec3d result_inter = a + t * (b - a);
2242 
2243  // The link below points in the correct direction, but the length is not correct
2244  Vec3d tmp_link_start = result_inter;
2245 
2246  // Correct link length
2247  // from https://math.stackexchange.com/questions/897723/how-to-resize-a-vector-to-a-specific-length
2248  Vec3d v = tmp_link_start - link_end;
2249  Vec3d u = m_DH_table[i].a() / v.norm() * v;
2250 
2251  Vec3d link_start = link_end + u;
2252 
2253  result.block<3,1>(index_point-3,0) = link_start;
2254  index_point -= 3;
2255  }
2256  //else
2257  {
2258  // result.block<3,1>(index-3,0) = desired_shape.block<3,1>(index,0);
2259  }
2260  path_index--;
2261 
2262  }
2263  }
2264  return result;
2265 }
2266 
2267 Vec Endorob::follow_the_leader_2(const std::vector<Vec3d, Eigen::aligned_allocator<Vec3d>> path, const Vec &desired_shape, const double &translation)
2268 {
2269  Vec result = desired_shape;
2270  int path_index = path.size() - 1;
2271  int index_point = desired_shape.rows() - 6;
2272 
2273  // For plotting
2274  // static QwtPlot plot;
2275 
2276  // plot.detachItems();
2277 
2278  // plot.setAxisScale( QwtPlot::yLeft, 0.15, 0.25 );
2279  // plot.setAxisScale( QwtPlot::xBottom, -0.5, 0.5 );
2280  // QwtPlotCurve *curve_black = new QwtPlotCurve();
2281  // curve_black->setPen( Qt::black, 8 );
2282  // QwtPlotCurve *curve_blue = new QwtPlotCurve();
2283  // curve_blue->setPen( Qt::blue, 4 );
2284  // QwtPlotCurve *curve_green = new QwtPlotCurve();
2285  // curve_green->setPen( Qt::green, 4 );
2286  // QwtPlotCurve *curve_red = new QwtPlotCurve();
2287  // curve_red->setPen( Qt::red, 4 );
2288 
2289 
2290 
2291 
2292 
2293  if(translation > 0)
2294  {
2295 
2296  // IDEA:
2297  // Look at distance from current link point to next path point
2298  // Compare with link length
2299  // if link shorter interpolate between current points by resizing vector formed by path points
2300 
2301  // if link longer, interpolate with next points
2302  // in both cases need to find intersection between link and path points
2303 
2304  // Loop over all links
2305  // First link is controlled by user hence the -2
2306  // All the other links are calculated to follow.
2307  bool first_iteration = true;
2308  for(int i = desired_shape.rows()/3 - 2; i > 0 ; i--)
2309  {
2310  // End of link point is the start point of the previous link
2311  Vec3d link_end = result.block<3,1>(index_point,0);
2312 
2313  // Current path point
2314  Vec3d current_path_point = path[path_index];
2315 
2316  // Current link length
2317  double link_length = m_DH_table[i].a();
2318  // int modulo = i%2;
2319 
2320  // if(modulo)
2321  // link_length = 0.0061;
2322  // else
2323  // link_length = 0.01182;
2324 
2325 
2326  // if(i==14)
2327  // link_length = 0.0061;
2328  // else if(i==13)
2329  // link_length = 0.01182;
2330  // else if(i==12)
2331  // link_length = 0.0061;
2332  // else if(i==11)
2333  // link_length = 0.01182;
2334  // else if(i==10)
2335  // link_length = 0.0061;
2336  // else if(i==9)
2337  // link_length = 0.01182;
2338  // else if(i==8)
2339  // link_length = 0.0061;
2340  // else if(i==7)
2341  // link_length = 0.01182;
2342  // else if(i==6)
2343  // link_length = 0.0061;
2344  // else if(i==5)
2345  // link_length = 0.01182;
2346  // else if(i==4)
2347  // link_length = 0.0061;
2348  // else if(i==3)
2349  // link_length = 0.01182;
2350  // else if(i==2)
2351  // link_length = 0.0061;
2352  // else if(i==1)
2353  // link_length = 0.01182;
2354 
2355 
2356  // Distance from link point to current path point
2357  double dist1 = (link_end-current_path_point).norm();
2358 
2359  // Explore path while path point is to close (until path point is far enough)
2360  while((dist1 < link_length) && (path_index > 0))
2361  {
2362 
2363  path_index--;
2364 
2365  // Current path point
2366  current_path_point = path[path_index];
2367 
2368  // Distance from link point to current path point
2369  dist1 = (link_end-current_path_point).norm();
2370 
2371  first_iteration = false;
2372  }
2373 
2374  // Now we can find an intersection
2375 
2376  // Previous path point
2377  Vec3d prev_path_point;
2378 
2379  // Init potential risk when the first path point is already out of reach, so prev path point should be link end
2380 
2381  if((path_index == (path.size()-1)) && first_iteration)
2382  {
2383  prev_path_point = link_end;
2384  first_iteration = false;
2385  //path_index--;
2386 
2387  }
2388  else
2389  {
2390  prev_path_point = path[path_index+1];
2391  }
2392 
2393 
2394 
2395  Vec3d link_start = sphere_line_intersection(link_length, link_end, prev_path_point , current_path_point);
2396 
2397 
2398  // Verification stage
2399  double verif_length = (link_end - link_start).norm();
2400  double diff = abs(verif_length - link_length);
2401  if(diff > 0.0001)
2402  {
2403 
2404  std::cout << "PROBLEM WITH LINK LENGTH !!!!!!!!!!!!!!\n";
2405  }
2406 
2407 
2408  // // Distance from link point to current path point
2409  // double dist1 = (link_end-current_path_point).norm();
2410 
2411  // // Distance from link point to next path point
2412  // double dist2 = (link_end-next_path_point).norm();
2413 
2414  // while(dist2 < dist1)
2415  // {
2416  // current_path_point = next_path_point;
2417  // path_index--;
2418  // next_path_point = path[path_index-1];
2419 
2420  // dist1 = (link_end-current_path_point).norm();
2421 
2422  // dist2 = (link_end-next_path_point).norm();
2423  // }
2424 
2425 
2426 
2427 
2428 
2429 
2430 
2431 
2432 
2433 
2434 
2435  // // link is shorter, need to interpolate
2436  // if(link_length < dist1)
2437  // {
2438  // link_start = link_end + dist1 * ( link_end - current_path_point ).normalized();
2439  // // Do not update path index are we are still behind
2440  // }
2441  // // link is longer, sphere line intersection
2442  // else if(link_length > dist1)
2443  // {
2444  // link_start = shpere_line_intersection(link_length, link_end, current_path_point, next_path_point);
2445 
2446  // // update path index
2447  // //path_index--;
2448  // }
2449  // // Perfect match, path point and link end are aligned
2450  // else
2451  // {
2452  // link_start = current_path_point;
2453 
2454  // // update path index
2455  // //path_index--;
2456  // }
2457  result.block<3,1>(index_point-3,0) = link_start;
2458 
2459  index_point -= 3;
2460  }
2461  }
2462 
2463  // // Plot path
2464  // QPolygonF points;
2465  // for(unsigned i = 0 ; i < path.size(); i++)
2466  // {
2467  // points << QPointF( path[i](0), path[i](2) );
2468  // }
2469 
2470  // curve_black->setSamples( points );
2471  // points.clear();
2472  // QwtSymbol *symbol = new QwtSymbol( QwtSymbol::Ellipse,
2473  // QBrush( Qt::black ), QPen( Qt::red, 1 ), QSize( 8, 8 ) );
2474  // curve_black->setSymbol( symbol );
2475  // curve_black->attach( &plot );
2476 
2477 
2478  // // Plot virtual robot
2479  // for(unsigned i = 0 ; i < result.rows(); i+=3)
2480  // {
2481  // points << QPointF( result(i)+0.01, result(i+2) );
2482  // }
2483  // curve_blue->setSamples( points );
2484  // points.clear();
2485  // QwtSymbol *symbolblue = new QwtSymbol( QwtSymbol::Ellipse,
2486  // QBrush( Qt::blue ), QPen( Qt::red, 1 ), QSize( 4, 4 ) );
2487  // curve_blue->setSymbol( symbolblue );
2488  // curve_blue->attach( &plot );
2489 
2490 
2491 
2492 
2493  // curve_green->attach( &plot );
2494  // curve_red->attach( &plot );
2495 
2496 
2497  // plot.resize( 1024, 1024 );
2498  // plot.show();
2499  // plot.replot();
2500 
2501  return result;
2502 }
2503 
2504 Vec Endorob::follow_the_leader_3(Snake_Path path, const Vec &desired_shape, const double &translation)
2505 {
2506  Vec result = desired_shape;
2507  int path_index = path.size() - 1;
2508  int index_point = desired_shape.rows() - 6;
2509 
2510  // // For plotting
2511  // static QwtPlot plot;
2512  // plot.detachItems();
2513  // plot.setAxisScale( QwtPlot::yLeft, 0.0, 0.48 );
2514  // plot.setAxisScale( QwtPlot::xBottom, -0.3, 0.3 );
2515  // QwtPlotCurve *curve_black = new QwtPlotCurve();
2516  // curve_black->setPen( Qt::black, 8 );
2517  // QwtPlotCurve *curve_blue = new QwtPlotCurve();
2518  // curve_blue->setPen( Qt::blue, 4 );
2519  // QwtPlotCurve *curve_green = new QwtPlotCurve();
2520  // curve_green->setPen( Qt::green, 4 );
2521  // QwtPlotCurve *curve_red = new QwtPlotCurve();
2522  // curve_red->setPen( Qt::red, 4 );
2523 
2524 
2525  if(translation > 0)
2526  {
2527 
2528  // IDEA:
2529  // Look at distance from current link point to next path point
2530  // Compare with link length
2531  // if link shorter interpolate between current points by resizing vector formed by path points
2532 
2533  // if link longer, interpolate with next points
2534  // in both cases need to find intersection between link and path points
2535 
2536  // Loop over all links
2537  // First link is controlled by user hence the -2
2538  // All the other links are calculated to follow.
2539  bool first_iteration = true;
2540  for(int i = desired_shape.rows()/3 - 2; i > 0 ; i--)
2541  {
2542 
2543  // End of link point is the start point of the previous link
2544  Vec3d link_end = result.block<3,1>(index_point,0);
2545  Vec3d link_start;
2546 
2547  // Current path point
2548  Vec3d current_path_point = path[path_index];
2549 
2550  // Current link length
2551  double link_length = m_DH_table[i].a();
2552 
2553  if(link_length == 0)
2554  link_length = m_DH_table[i].d();
2555 
2556  // coincident joint, the target should be same as previous
2557  if(link_length != 0)
2558  {
2559 
2560  // Distance from link point to current path point
2561  double dist1 = (link_end-current_path_point).norm();
2562 
2563  // Explore path while path point is to close (until path point is far enough)
2564  while((dist1 < link_length) && (path_index > 0))
2565  {
2566 
2567  path_index--;
2568 
2569  // Current path point
2570  current_path_point = path[path_index];
2571 
2572  // Distance from link point to current path point
2573  dist1 = (link_end-current_path_point).norm();
2574 
2575  first_iteration = false;
2576  }
2577 
2578  // Now we can find an intersection
2579 
2580  // Previous path point
2581  Vec3d prev_path_point;
2582 
2583  // Init potential risk when the first path point is already out of reach, so prev path point should be link end
2584 
2585  if((path_index == (path.size()-1)) && first_iteration)
2586  {
2587  prev_path_point = link_end;
2588  first_iteration = false;
2589  //path_index--;
2590 
2591  }
2592  else
2593  {
2594  prev_path_point = path[path_index+1];
2595  }
2596 
2597 
2598 
2599  link_start = sphere_line_intersection(link_length, link_end, prev_path_point , current_path_point);
2600 
2601 
2602  // Verification stage
2603  double verif_length = (link_end - link_start).norm();
2604  double diff = abs(verif_length - link_length);
2605  if(diff > 0.0001)
2606  {
2607 
2608  std::cout << "PROBLEM WITH LINK LENGTH !!!!!!!!!!!!!!\n";
2609  }
2610  }
2611  else
2612  {
2613  link_start = link_end;
2614  }
2615 
2616  // The base links can only move one dof at a time
2617  if(i == 1)// y axis
2618  {
2619  result.block<1,1>(index_point-3 + 1, 0) = link_start.block<1,1>(1,0);
2620  }
2621  else if(i == 2)// x axis (and y axis)
2622  {
2623  result.block<2,1>(index_point-3, 0) = link_start.block<2,1>(0,0);
2624 
2625  }
2626  else // all other cases
2627  result.block<3,1>(index_point-3, 0) = link_start;
2628 
2629  index_point -= 3;
2630  }
2631  }
2632 
2633  // // // Plot path
2634  // QPolygonF points;
2635  // for(unsigned i = 0 ; i < path.size(); i++)
2636  // {
2637  // points << QPointF( path[i](0), path[i](2) );
2638  // }
2639 
2640  // curve_black->setSamples( points );
2641  // points.clear();
2642  // QwtSymbol *symbol = new QwtSymbol( QwtSymbol::Ellipse,
2643  // QBrush( Qt::black ), QPen( Qt::red, 1 ), QSize( 8, 8 ) );
2644  // curve_black->setSymbol( symbol );
2645  // curve_black->attach( &plot );
2646 
2647 
2648  // // Plot virtual robot
2649  // for(unsigned i = 0 ; i < result.rows(); i+=3)
2650  // {
2651  // points << QPointF( result(i)/*+0.01*/, result(i+2) );
2652  // }
2653  // curve_blue->setSamples( points );
2654  // points.clear();
2655  // QwtSymbol *symbolblue = new QwtSymbol( QwtSymbol::Ellipse,
2656  // QBrush( Qt::blue ), QPen( Qt::red, 1 ), QSize( 4, 4 ) );
2657  // curve_blue->setSymbol( symbolblue );
2658  // curve_blue->attach( &plot );
2659 
2660 
2661 
2662 
2663  // curve_green->attach( &plot );
2664  // curve_red->attach( &plot );
2665 
2666 
2667  // plot.resize( 1024, 1024 );
2668  // plot.show();
2669  // plot.replot();
2670 
2671  return result;
2672 }
2673 
2674 Virtual_Snake Endorob::follow_the_leader_weighted(Snake_Path path, const Virtual_Snake &desired_shape, const double &translation)
2675 {
2676  Virtual_Snake result(desired_shape);
2677  int path_index = path.size() - 1;
2678  int index_point = desired_shape.rows() - 6;
2679 
2680  if(translation > 0)
2681  {
2682 
2683  // IDEA:
2684  // Look at distance from current link point to next path point
2685  // Compare with link length
2686  // if link shorter interpolate between current points by resizing vector formed by path points
2687 
2688  // if link longer, interpolate with next points
2689  // in both cases need to find intersection between link and path points
2690 
2691  // Loop over all links
2692  // First link is controlled by user hence the -2
2693  // All the other links are calculated to follow.
2694  bool first_iteration = true;
2695  for(int i = desired_shape.rows()/3 - 2; i > 0 ; i--)
2696  {
2697 
2698  // End of link point is the start point of the previous link
2699  // Vec3d link_end = result.block<3,1>(index_point,0);
2700  Vec3d link_end = result.block(index_point);
2701  Vec3d link_start;
2702 
2703  // Current path point
2704  Vec3d current_path_point = path[path_index];
2705 
2706  // Current link length
2707  double link_length = m_DH_table[i].a();
2708 
2709  if(link_length == 0)
2710  link_length = m_DH_table[i].d();
2711 
2712  // coincident joint, the target should be same as previous
2713  if(link_length != 0)
2714  {
2715 
2716  // Distance from link point to current path point
2717  double dist1 = (link_end-current_path_point).norm();
2718 
2719  // Explore path while path point is to close (until path point is far enough)
2720  while((dist1 < link_length) && (path_index > 0))
2721  {
2722 
2723  path_index--;
2724 
2725  // Current path point
2726  current_path_point = path[path_index];
2727 
2728  // Distance from link point to current path point
2729  dist1 = (link_end-current_path_point).norm();
2730 
2731  first_iteration = false;
2732  }
2733 
2734  // Now we can find an intersection
2735 
2736  // Previous path point
2737  Vec3d prev_path_point;
2738 
2739  // Init potential risk when the first path point is already out of reach, so prev path point should be link end
2740 
2741  if((path_index == (path.size()-1)) && first_iteration)
2742  {
2743  prev_path_point = link_end;
2744  first_iteration = false;
2745  //path_index--;
2746 
2747  }
2748  else
2749  {
2750  prev_path_point = path[path_index+1];
2751  }
2752 
2753 
2754 
2755  link_start = sphere_line_intersection(link_length, link_end, prev_path_point , current_path_point);
2756 
2757  // Assign corresponding weight
2758  static bool skip_next = false;
2759 
2760 
2761  //result.set_weight(index_point, path.get_weight(path_index));
2762 
2763  if(path.get_weight(path_index) > 0.0)
2764  {
2765  if(!skip_next)
2766  {
2767  result.set_weight(index_point, path.get_weight(path_index));
2768  if(index_point > 0)
2769  result.set_weight(index_point - 1, path.get_weight(path_index));
2770 
2771  skip_next = true;
2772  }
2773  else
2774  {
2775  skip_next = false;
2776  }
2777  }
2778 
2779 
2780  // Verification stage
2781  double verif_length = (link_end - link_start).norm();
2782  double diff = abs(verif_length - link_length);
2783  if(diff > 0.0001)
2784  {
2785 
2786  std::cout << "PROBLEM WITH LINK LENGTH !!!!!!!!!!!!!!\n";
2787  }
2788  }
2789  else
2790  {
2791  link_start = link_end;
2792  }
2793 
2794  // The base links can only move one dof at a time
2795  if(i == 1)// y axis
2796  {
2797  // result.block<1,1>(index_point-3 + 1, 0) = link_start.block<1,1>(1,0);
2798  result.set(index_point-3 + 1, 0, 1, 1, link_start.block<1,1>(1,0));
2799 
2800  }
2801  else if(i == 2)// x axis (and y axis)
2802  {
2803  //result.block<2,1>(index_point-3, 0) = link_start.block<2,1>(0,0);
2804  result.set(index_point-3, 0, 2, 1, link_start.block<2,1>(0,0));
2805 
2806  }
2807  else // all other cases
2808  {
2809  result.set(index_point-3, 0, 3, 1, link_start);
2810  // result.block<3,1>(index_point-3, 0) = link_start;
2811  }
2812 
2813  index_point -= 3;
2814  }
2815  }
2816 
2817  return result;
2818 }
2819 
2820 
2821 std::vector<DH_table_entry> Endorob::get_DH_table()
2822 {
2823  return m_DH_table;
2824 }
2825 
2826 Mat Endorob::get_full_kinematics()
2827 {
2828  return m_fwd_k_vec;
2829 }
2830 
2831 Vec Endorob::set_vec_relative_to_base(const Vec &vec)
2832 {
2833  Vec3d base = vec.block<3,1>(0,0);
2834  Vec result = vec;
2835 
2836  for(unsigned i = 0; i < vec.rows(); i+=3)
2837  {
2838  result.block<3,1>(i,0) = vec.block<3,1>(i,0) - base;
2839  }
2840  return result;
2841 }
2842 
2843 Mat Endorob::get_snake_base()
2844 {
2845  Mat4d T;
2846  Mat4d tmp;
2847 
2848  T = m_fwd_k_vec.block<4,4>(5 * 4,0);
2849 
2850  // tmp = Eigen::MatrixXd::Identity(4,4);
2851  // tmp.block<3,1>(0,3) << 0, 0, -0.06;
2852 
2853  // tmp.block<3,1>(0,3) = T.block<3,3>(0,0) * tmp.block<3,1>(0,3);
2854 
2855  // T.block<3,1>(0,3) = T.block<3,1>(0,3) + tmp.block<3,1>(0,3);
2856 
2857  //tmp_rot = m_fwd_k_vec.block<3,3>(5 * 4,0);
2858 
2859  //std::cout << T << std::endl<< std::endl;
2860 
2861  //T.block<3,3>(0,0) = tmp_rot * T.block<3,3>(0,0);
2862  //T.block<3,1>(0,3) = tmp_rot * T.block<3,1>(0,3);
2863 
2864 
2865  //std::cout << T << std::endl<< std::endl;
2866  //std::cout << tmp << std::endl<< std::endl;
2867 
2868  return T;
2869 }
2870 
2871 void Endorob::set_solver_weight_vec(const uint &index, const double &val)
2872 {
2873  m_error_weights_vec(index) = val;
2874  m_apply_weighted_solution = false;
2875 }
2876 
2877 void Endorob::set_solver_weight_mat(const uint &index, const double &val)
2878 {
2879  for(int i = index * 3; i < index * 3 + 3 ; i++)
2880  {
2881  m_error_weights(i,i) = val;
2882  }
2883  m_apply_weighted_solution = false;
2884 }
2885 
2886 void Endorob::set_solver_weight_mat(const Vec &vec)
2887 {
2888  m_error_weights = vec.asDiagonal();
2889 
2890 }
2891 
2892 void Endorob::set_robot_reference_frame(const Mat4d &T)
2893 {
2894  m_robot_frame_T = T;
2895 
2896  // Calculate increments
2897  m_global_frame_to_robot_increment = get_transform_increment(m_robot_frame_T, Mat4d::Identity(4,4));
2898  m_robot_frame_to_global_increment = get_transform_increment(Mat4d::Identity(4,4), m_robot_frame_T);
2899 
2900 }
2901 
2902 void Endorob::enforce_joint_limits_mode()
2903 {
2904  m_check_joint_limit_jacobian = true;
2905 }
2906 
2907 void Endorob::set_faulty_joint(const uint &joint, const double &val)
2908 {
2909  m_apply_fault_tolerance = true;
2910  m_faulty_joint_vec[joint] = true;
2911 }
2912 
2913 void Endorob::disable_fault_tolerance()
2914 {
2915  m_apply_fault_tolerance = false;
2916 }
2917 
2918 Vec Endorob::angle_modulus(const Vec &q_vec)
2919 {
2920  Vec result = q_vec;
2921  for(unsigned i = 0; i < q_vec.rows(); i++)
2922  {
2923  if((m_DH_table[i].get_joint_type() == Joint_type::Revolute) ||
2924  (m_DH_table[i].get_joint_type() == Joint_type::Rolling))
2925  {
2926  double x = fmod(result[i] + M_PI, 2*M_PI);
2927  if (x < 0)
2928  x += 2 * M_PI;
2929  result[i] = x - M_PI;
2930  // while(result[i] >= M_PI)
2931  // result[i] = result[i] - 2 * M_PI;
2932 
2933  // while(result[i] <= -M_PI)
2934  // result[i] = result[i] + 2 * M_PI;
2935  }
2936 
2937  }
2938  return result;
2939 }
2940 
2941 Mat Endorob::mask_resize(const Mat &mat)
2942 {
2943  Mat result;
2944  result.resize(m_mask_dof, mat.cols());
2945 
2946  unsigned index = 0;
2947 
2948  for(unsigned i = 0; i < 6; i++)
2949  {
2950  if(m_mask_vector[i] == 1)
2951  {
2952  //result.conservativeResize(result.rows()+1, mat.cols());
2953  result.row(index) << mat.row(i);
2954  index++;
2955  }
2956  }
2957  return result;
2958 }
2959 
2960 void Endorob::apply_joint_coupling()
2961 {
2962  // q_vec 6x1
2963  // coupling matrix 26x6
2964  // result 26x6.6x1.26x1
2965 }
2966 
2967 void Endorob::calculate_coupling_matrix_increase()
2968 {
2969  calculte_dof_after_coupling();
2970 
2971  // Define matrix size and intialize to zeros
2972  m_joint_coupling_matrix.resize(m_coupling_vector.rows(),m_dof);
2973  m_joint_coupling_matrix = Mat::Zero(m_coupling_vector.rows(),m_dof);
2974 
2975  // Fill matrix using coupling vector
2976  unsigned index = 0;
2977  for(unsigned i = 0; i < m_coupling_vector.rows(); i++)
2978  {
2979  if(m_DH_table[index].get_joint_type() == Joint_type::Rolling)
2980  m_joint_coupling_matrix(index, m_coupling_vector[index]) = 0.5;
2981 
2982  else if(m_DH_table[index].get_joint_type() == Joint_type::Rolling_flex)
2983  m_joint_coupling_matrix(index, m_coupling_vector[index]) = 1.0/9.0;
2984 
2985  else
2986  m_joint_coupling_matrix(index, m_coupling_vector[index]) = 1.0;
2987  index++;
2988  }
2989  //std::cout << "Increase mat:\n" << m_joint_coupling_matrix << std::endl;
2990 }
2991 
2992 void Endorob::calculate_coupling_matrix_reduce()
2993 {
2994  calculte_dof_after_coupling();
2995 
2996  // Define matrix size and intialize to zeros
2997  m_joint_coupling_reduce_dimension.resize(m_dof, m_coupling_vector.rows());
2998  m_joint_coupling_reduce_dimension = Mat::Zero(m_dof, m_coupling_vector.rows());
2999 
3000  // Search through coupling vector
3001  double prev_val = -1;
3002  unsigned row = 0;
3003  for(unsigned i = 0; i < m_coupling_vector.rows(); i++)
3004  {
3005  double val = m_coupling_vector[i];
3006 
3007  // Found new value
3008  if(val > prev_val)
3009  {
3010  // Fill matrix row
3011  m_joint_coupling_reduce_dimension(row, i) = 1.0;
3012  prev_val = val;
3013  row++;
3014  }
3015  }
3016 }
3017 
3018 void Endorob::calculte_dof_after_coupling()
3019 {
3020  // Calculate resulting DOF after coupling
3021  double dof_val = -1;
3022  for(unsigned i = 0; i < m_coupling_vector.rows(); i++)
3023  {
3024  if(m_coupling_vector[i] > dof_val)
3025  {
3026  dof_val = m_coupling_vector[i];
3027  }
3028  }
3029  // add 1 to consider joint value 0
3030  dof_val+=1;
3031 
3032  // Update robot's DOF
3033  m_dof = dof_val;
3034 }
3035 
3036 std::vector<std::string> Endorob::split(const std::string & s, char delimiter, bool remove_empty)
3037 {
3038  std::vector<std::string> tokens;
3039  std::string token;
3040  std::istringstream tokenStream(s);
3041  while (std::getline(tokenStream, token, delimiter))
3042  {
3043  if (remove_empty && token == "")
3044  continue;
3045  tokens.push_back(token);
3046  }
3047  return tokens;
3048 }
3049 
3050 bool Endorob::check_joint_in_limit(const Vec &q)
3051 {
3052  bool result = false;
3053  Vec truc = q;
3054 
3055  if(!m_apply_joint_coupling)
3056  {
3057  for(unsigned i = 0; i < q.rows(); i++)
3058  {
3059  if(q[i] > m_DH_table[i].joint_limit_max())
3060  result = true;
3061 
3062  else if(q[i] < m_DH_table[i].joint_limit_min())
3063  result = true;
3064 
3065  }
3066  }
3067  else
3068  {
3069  for(unsigned i = 0; i < q.rows(); i++)
3070  {
3071  if(q[i] > m_DH_table[m_joint_lookup_table[i]].joint_limit_max())
3072  result = true;
3073 
3074  else if(q[i] < m_DH_table[m_joint_lookup_table[i]].joint_limit_min())
3075  result = true;
3076 
3077  }
3078 
3079  }
3080 
3081  return result;
3082 }
3083 
3084 void Endorob::calculate_joint_LUT()
3085 {
3086  m_joint_lookup_table.resize(m_dof);
3087  unsigned index = 0;
3088  double prev_val = -1;
3089 
3090  for(unsigned i = 0; i < m_joints; i++)
3091  {
3092  if(m_coupling_vector[i] > prev_val)
3093  {
3094  m_joint_lookup_table[index] = i;
3095  index++;
3096  prev_val = m_coupling_vector[i];
3097  }
3098  }
3099 }
3100 
3101 void Endorob::convert_fwd_k_into_vec()
3102 {
3103  int cpt_index = 0;
3104  for(unsigned i = 0; i < m_joints-1; i++)
3105  {
3106  m_full_body_vec_current.block<3,1>(cpt_index,0) = m_fwd_k_vec.block<3,1>(i*4,3);
3107  cpt_index += 3;
3108 
3109  }
3110  // Final point, we add tool mat directly as it is a rigid transformation and is dependent on the previous link
3111  m_full_body_vec_current.block<3,1>(cpt_index,0) = m_fwd_k_vec.block<3,1>(m_joints*4,3);
3112 }
3113 
3114 Vec Endorob::convert_fwd_k_into_vec(Mat fwd_k)
3115 {
3116  Vec body_vec = m_full_body_vec_current;
3117  int cpt_index = 0;
3118  for(unsigned i = 0; i < m_joints-1; i++)
3119  {
3120  body_vec.block<3,1>(cpt_index,0) = fwd_k.block<3,1>(i*4,3);
3121  cpt_index += 3;
3122 
3123  }
3124  // Final point, we add tool mat directly as it is a rigid transformation and is dependent on the previous link
3125  body_vec.block<3,1>(cpt_index,0) = fwd_k.block<3,1>(m_joints*4,3);
3126 
3127  return body_vec;
3128 }
3129 
3130 Vec Endorob::get_joint_limit_scalar()
3131 {
3132  Vec result;
3133  result.resize(m_joints);
3134 
3135  // Check joint values in q
3136 
3137  // create vector of 1 if joint not in limit, and 0 if joint in limit
3138  for( unsigned i = 0; i < m_joints; i++)
3139  {
3140  // default case, joint not in limit
3141  result(i) = 1;
3142 
3143  if(m_q_vec(i) > m_DH_table[i].joint_limit_max())
3144  result(i) = 0;
3145 
3146  else if (m_q_vec(i) < m_DH_table[i].joint_limit_min())
3147  result(i) = 0;
3148  }
3149  return result;
3150 }
3151 
3152 void Endorob::pseudo_inverse_simple()
3153 {
3154  double alpha = 0.5;
3155 
3156  // Calculate pseudo inverse
3157  calculate_pseudo_inverse();
3158 
3159  // Iterative equation
3160  if(m_apply_joint_coupling)
3161  m_xi_vec = m_xi_vec + alpha * m_J_inv * m_error;
3162  else
3163  m_q_vec = m_q_vec + alpha * m_J_inv * m_error;
3164 }
3165 
3166 void Endorob::pseudo_inverse_null_space()
3167 {
3168  double alpha = 1.0;
3169 
3170  // Calculate pseudo inverse
3171  calculate_pseudo_inverse();
3172 
3173  // Constraint calculation
3174  null_space_constraint_calculation();
3175 
3176 
3177  // Iterative equation
3178  if(m_apply_joint_coupling)
3179  m_xi_vec = m_xi_vec + alpha * m_J_inv * m_error + (m_identity - m_J_inv * m_J) * m_phi;
3180  else
3181  m_q_vec = m_q_vec + alpha * m_J_inv * m_error + (m_identity - m_J_inv * m_J) * m_phi;
3182 
3183 }
3184 
3185 void Endorob::null_space_constraint_calculation()
3186 {
3187  // From "Computational modeling for the computer animation of legged figures"
3188  for(int i = 0; i < m_error_weights_vec.rows(); i++)
3189  m_phi(i) = 0.001 * (m_xi_vec(i) - m_xi_vec_rest(i));
3190  //m_phi(i) = 2 * m_error_weights_vec(i) * (m_xi_vec(i) - m_xi_vec_rest(i));
3191  //m_phi(i) = m_error_weights_vec(i) * ((m_xi_vec(i) - m_xi_vec_rest(i)) * (m_xi_vec(i) - m_xi_vec_rest(i)) );
3192 
3193 }
3194 
3195 void Endorob::damped_least_square_method()
3196 {
3197  // Calculate pseudo inverse
3198  calculate_pseudo_inverse_DLS();
3199 
3200  // Iterative equation
3201  if(m_apply_joint_coupling)
3202  m_xi_vec = m_xi_vec + m_J_inv * m_error;
3203  else
3204  m_q_vec = m_q_vec + m_J_inv * m_error;
3205 }
3206 
3207 void Endorob::jacobian_transpose()
3208 {
3209  double alpha = 0.5;
3210 
3211  // Iterative equation
3212  if(m_apply_joint_coupling)
3213  m_xi_vec = m_xi_vec + m_J.transpose() * m_error;
3214  else
3215  m_q_vec = m_q_vec + m_J.transpose() * m_error;
3216 }
3217 
3218 void Endorob::iterative_solve_equation()
3219 {
3220 
3221 }
3222 
3223 void Endorob::check_for_cpu()
3224 {
3225  m_CPU_amount = std::thread::hardware_concurrency();
3226 
3227  // If m_CPU_cpt not assigned
3228  if(m_CPU_cpt == 0xFE)
3229  m_CPU_cpt = m_CPU_amount - 1;
3230 
3231  if(!m_cpu_info_dispayed)
3232  {
3233 #ifdef VERBOSE_MODE
3234  if((m_CPU_amount > 1))
3235  std::cout << "Endorob found: " << m_CPU_amount << " CPUs on your machine." << std::endl << std::endl;
3236  else
3237  std::cout << "Endorob found only " << m_CPU_amount << " CPU on your machine." << std::endl << std::endl;
3238 #endif
3239 
3240  m_cpu_info_dispayed = true;
3241  }
3242  else
3243  std::this_thread::sleep_for(std::chrono::milliseconds(100)); // for proper display, not messed up between threads
3244 }
3245 
3246 void Endorob::modify_error_with_f()
3247 {
3248 
3249  for(unsigned i = 0; i < m_error.rows()-9; i++)
3250  {
3251 
3252  if(m_error(i) > 0)
3253  {
3254  //m_error(i) = (1.0 - m_error_weights(i,i)) * exp(-1.0/(m_error(i)*10.0)) * m_error(i);
3255  m_error(i) = (1.0 - m_error_weights(i,i)) * exp(-1.0/pow(m_error(i)*250.0, 3)) * m_error(i); // paper values 1.0 - 25.0 - 250.0, 600 for sub-mm
3256 
3257  }
3258  else if(m_error(i) < 0)
3259  {
3260  //m_error(i) = -((1.0 - m_error_weights(i,i)) * exp(-1.0/(abs(m_error(i))*10.0)) * abs(m_error(i)));
3261  m_error(i) = -((1.0 - m_error_weights(i,i)) * exp(-1.0/pow(abs(m_error(i))*250.0, 3)) * abs(m_error(i))); // paper values 1.0 - 25.0 - 250.0, 600 for sub-mm
3262  }
3263  else
3264  m_error(i) = 0;
3265  // if(abs(m_error(i)) > 0.01)
3266  // {
3267 
3268  // }
3269  // else
3270  // {
3271  // m_error(i) = 0.0;
3272  // }
3273  }
3274 }
3275 
3276 
3277 Vec Endorob::zero_thresholding(const Vec &q_vec, const double &val)
3278 {
3279  Vec result = q_vec;
3280  for(unsigned i = 0; i < q_vec.rows(); i++)
3281  {
3282  if(abs(result[i]) < val)
3283  {
3284  result[i] = 0;
3285 
3286  }
3287  }
3288  return result;
3289 }
3290 
3291 Mat4d Endorob::base_transformation_matrix(DH_table_entry DH, double q)
3292 {
3293  Mat4d mat;
3294  double a = DH.a(), alpha = DH.alpha(), d = DH.d(), theta = DH.theta();
3295  Joint_type joint_type = DH.get_joint_type();
3296 
3297  //std::cout << "a: "<< a << " alpha: " << alpha << " d: " << d << " theta: " << q << std::endl;
3298 
3299 
3300  switch(joint_type)
3301  {
3302  case Joint_type::Prismatic:
3303  d = q;
3304  break;
3305 
3306  case Joint_type::Revolute:
3307  case Joint_type::Rolling:
3308  theta = q;
3309  break;
3310  }
3311 
3312  switch(m_DH_convention)
3313  {
3314  case DH_type::DH_modified:
3315  mat << cos(theta), -sin(theta), 0, a,
3316  sin(theta)*cos(alpha), cos(theta)*cos(alpha), -sin(alpha), -sin(alpha)*d,
3317  sin(theta)*sin(alpha), cos(theta)*sin(alpha), cos(alpha), cos(alpha)*d,
3318  0, 0, 0, 1;
3319 
3320  break;
3321 
3322  case DH_type::DH_standard:
3323  mat << cos(theta), -sin(theta)*cos(alpha), sin(theta)*sin(alpha), a*cos(theta),
3324  sin(theta), cos(theta)*cos(alpha), -cos(theta)*sin(alpha), a*sin(theta),
3325  0, sin(alpha), cos(alpha), d,
3326  0, 0, 0, 1;
3327 
3328  break;
3329  }
3330 
3331  return mat;
3332 }
3333 
3334 void Endorob::calculate_pseudo_inverse()
3335 {
3336  // Good article about pseudo inverse: https://www.johndcook.com/blog/2018/05/05/svd/
3337  // Mutex lock
3338  //mutx.lock();
3339 
3340  // double tolerance = 1.E-15;
3341  double tolerance = 1.E-6;
3342 
3343  // SVD decomposition
3344  Eigen::JacobiSVD<Mat> svd(m_J, Eigen::ComputeThinU | Eigen::ComputeThinV);
3345 
3346  // Extract singular values
3347  Mat singular_values = svd.singularValues();
3348  Mat inv_singular_values = singular_values;
3349 
3350  // Tolerance and zero value management
3351  unsigned int cpt_zero = 0;
3352  for(int i = 0; i < singular_values.rows(); i++)
3353  {
3354  if(singular_values(i) > tolerance)
3355  {
3356  inv_singular_values(i) = 1.0/singular_values(i);
3357  }
3358  else
3359  {
3360  inv_singular_values(i) = 0;
3361  cpt_zero++;
3362  }
3363  }
3364 
3365  // Redimensioning of the dataset
3366  Mat Umat, Vmat, newU, newV, newinv;
3367  Umat = svd.matrixU();
3368  Vmat = svd.matrixV();
3369 
3370  newV = Vmat.block(0, 0, Vmat.rows(), Vmat.cols()-cpt_zero);
3371  newU = Umat.block(0, 0, Umat.rows(), Umat.cols()-cpt_zero);
3372  newinv = inv_singular_values.block(0, 0, (inv_singular_values.rows()-cpt_zero), 1);
3373 
3374  // Finally calculate pseudo-inverse
3375  // p_inv = svd.matrixV() * inv_singular_values.asDiagonal() * svd.matrixU().transpose();
3376  m_J_inv = newV * newinv.asDiagonal() * newU.transpose();
3377 
3378  // Pseudo-inverse formula:
3379  //p_inv = J.transpose() * (J * J.transpose()).inverse();
3380 
3381  // Mutex unlock
3382  //mutx.unlock();
3383 }
3384 
3385 void Endorob::calculate_pseudo_inverse_masked()
3386 {
3387  // Mutex lock
3388  //mutx.lock();
3389 
3390  double tolerance = 1.E-15;
3391 
3392  // SVD decomposition
3393  Eigen::JacobiSVD<Mat> svd(m_J_masked, Eigen::ComputeThinU | Eigen::ComputeThinV);
3394 
3395  // Extract singular values
3396  Mat singular_values = svd.singularValues();
3397  Mat inv_singular_values = singular_values;
3398 
3399  // Tolerance and zero value management
3400  unsigned int cpt_zero = 0;
3401  for(int i = 0; i < singular_values.rows(); i++)
3402  {
3403  if(singular_values(i) > tolerance)
3404  {
3405  inv_singular_values(i) = 1.0/singular_values(i);
3406  }
3407  else
3408  {
3409  inv_singular_values(i) = 0;
3410  cpt_zero++;
3411  }
3412  }
3413 
3414  // Redimensioning of the dataset
3415  Mat Umat, Vmat, newU, newV, newinv;
3416  Umat = svd.matrixU();
3417  Vmat = svd.matrixV();
3418 
3419  newV = Vmat.block(0, 0, Vmat.rows(), Vmat.cols()-cpt_zero);
3420  newU = Umat.block(0, 0, Umat.rows(), Umat.cols()-cpt_zero);
3421  newinv = inv_singular_values.block(0, 0, (inv_singular_values.rows()-cpt_zero), 1);
3422 
3423  // Finally calculate pseudo-inverse
3424  // p_inv = svd.matrixV() * inv_singular_values.asDiagonal() * svd.matrixU().transpose();
3425  m_J_inv_masked = newV * newinv.asDiagonal() * newU.transpose();
3426 
3427 }
3428 
3429 void Endorob::calculate_pseudo_inverse_DLS()
3430 {
3431  // Good article about pseudo inverse: https://www.johndcook.com/blog/2018/05/05/svd/
3432 
3433  // SVD decomposition
3434  Eigen::JacobiSVD<Mat> svd(m_J, Eigen::ComputeThinU | Eigen::ComputeThinV);
3435 
3436  // Extract singular values
3437  Mat singular_values = svd.singularValues();
3438 
3439  // Tolerance and zero value management
3440  //unsigned int cpt_zero = 0;
3441  Mat E = singular_values.asDiagonal();
3442 
3443  for(int i = 0; i < singular_values.rows(); i++)
3444  {
3445  // if(singular_values(i) > tolerance)
3446  // {
3447  E(i,i) = singular_values(i) / ( pow(singular_values(i),2) + pow(m_lambda,2) );
3448  // }
3449  // else
3450  // {
3451  // E(i,i) = 0;
3452  // //cpt_zero++;
3453  // }
3454  }
3455 
3456 
3457  // Finally calculate pseudo-inverse
3458  // From S. R. Buss inverse kinematics survey
3459  // m_J_inv = newV * newE * newU.transpose();
3460  m_J_inv = svd.matrixV() * E * svd.matrixU().transpose();
3461 
3462 }
3463 
3464 DH_table_entry::DH_table_entry(const double &a, const double &alpha, const double &d, const double &theta, const Joint_type &type)
3465 {
3466  m_a = a;
3467  m_alpha = alpha;
3468  m_d = d;
3469  m_theta = theta;
3470 
3471  m_joint_type = type;
3472 
3473  m_joint_limit_max = 1.047;
3474  m_joint_limit_min = -1.047;
3475 }
3476 
3477 DH_table_entry::DH_table_entry(const double &a, const double &a_bis, const double &alpha, const double &d, const double &theta, const Joint_type &type)
3478 {
3479  m_a = a;
3480  m_a_bis = a_bis;
3481  m_alpha = alpha;
3482  m_d = d;
3483  m_theta = theta;
3484 
3485  m_joint_type = type;
3486 
3487  m_joint_limit_max = 1.047/2;
3488  m_joint_limit_min = -1.047/2;
3489 }
3490 
3491 void DH_table_entry::set_offset(const double &offset)
3492 {
3493 
3494 }
3495 
3496 void DH_table_entry::set_joint_limits(const double &min, const double &max)
3497 {
3498  m_joint_limit_max = max;
3499  m_joint_limit_min = min;
3500 }
3501 
3502 void DH_table_entry::set_flex_resolution(const uint &res)
3503 {
3504  m_flex_resolution = res;
3505 }
3506 
3507 void DH_table_entry::set_flex_gradient(const Vec &vec)
3508 {
3509  m_flex_gradient = vec;
3510 }
3511 
3512 double DH_table_entry::a()
3513 {
3514  return m_a;
3515 }
3516 
3517 double DH_table_entry::alpha()
3518 {
3519  return m_alpha;
3520 }
3521 
3522 double DH_table_entry::d()
3523 {
3524  return m_d;
3525 }
3526 
3527 double DH_table_entry::theta()
3528 {
3529  return m_theta;
3530 }
3531 
3532 double DH_table_entry::a_bis()
3533 {
3534  return m_a_bis;
3535 }
3536 
3537 uint DH_table_entry::get_flex_resolution()
3538 {
3539  return m_flex_resolution;
3540 }
3541 
3542 Vec DH_table_entry::get_flex_gradient()
3543 {
3544  return m_flex_gradient;
3545 }
3546 
3547 Joint_type DH_table_entry::get_joint_type()
3548 {
3549  return m_joint_type;
3550 }
3551 
3552 double DH_table_entry::joint_limit_max()
3553 {
3554  return m_joint_limit_max;
3555 }
3556 
3557 double DH_table_entry::joint_limit_min()
3558 {
3559  return m_joint_limit_min;
3560 }
3561 
3562 
3563 
3564 
3565 
3566 
3567 
3568 
3569 Motion_RW::Motion_RW():
3570  pthread(nullptr),
3571  m_overwrite_file_period(false),
3572  m_header_written(false),
3573  m_run(true),
3574  m_start_loop(false),
3575  m_runonce(true)
3576 {
3577  m_period_write = 0.001;
3578  m_period_read = 0.001;
3579  m_file_name_write = "output_data_00.csv";
3580  m_file_name_read = "input_data.csv";
3581 
3582  // Thread initialization
3583  if(pthread == nullptr)
3584  {
3585  pthread = new std::thread(&Motion_RW::motion_RW_loop, this, 0, 0);
3586  //m_start_loop = true;
3587 
3588  m_mode = Motion_mode::Idle;
3589  }
3590 
3591 
3592 }
3593 
3594 Motion_RW::Motion_RW(Endorob *robot):
3595  probot(robot),
3596  pthread(nullptr),
3597  m_overwrite_file_period(false),
3598  m_header_written(false),
3599  m_run(true),
3600  m_start_loop(false),
3601  m_runonce(true)
3602 {
3603  m_period_write = 0.001;
3604  m_period_read = 0.001;
3605  m_file_name_write = "output_data_00.csv";
3606  m_file_name_read = "input_data.csv";
3607 
3608  // Thread initialization
3609  if(pthread == nullptr)
3610  {
3611  pthread = new std::thread(&Motion_RW::motion_RW_loop, this, 0, 0);
3612  //m_start_loop = true;
3613 
3614  m_mode = Motion_mode::Idle;
3615  }
3616 
3617 
3618 }
3619 
3620 Motion_RW::~Motion_RW()
3621 {
3622  m_start_loop = false;
3623  m_run = false;
3624 
3625  if(pthread != nullptr)
3626  {
3627  pthread->join();
3628  delete pthread;
3629  pthread = nullptr;
3630  }
3631  if(m_file_write.is_open())
3632  m_file_write.close();
3633 
3634  if(m_file_read.is_open())
3635  m_file_read.close();
3636 
3637 
3638 
3639 
3640 #ifdef VERBOSE_MODE
3641  std::cout << "Motion RW stopped...\n";
3642 #endif
3643 }
3644 
3645 void Motion_RW::set_endorob_pointer(Endorob *robot)
3646 {
3647  probot = robot;
3648 }
3649 
3650 void Motion_RW::set_virt_snake_pointer(Virtual_Snake *virt_snake)
3651 {
3652  pvirt_snake = virt_snake;
3653 }
3654 
3655 void Motion_RW::set_write_frequency(const double &freq)
3656 {
3657  m_period_write = 1/freq;
3658 }
3659 
3660 void Motion_RW::set_read_frequency(const double &freq)
3661 {
3662  m_overwrite_file_period = true;
3663  m_period_read = 1/freq;
3664 }
3665 
3666 void Motion_RW::start_recording()
3667 {
3668  // Check if file already exist
3669  std::ifstream file_checker(m_file_name_write);
3670  if(file_checker.good())
3671  {
3672  // File already exist
3673  std::string filenumber = m_file_name_write.substr(m_file_name_write.size()-6,2);
3674  int filenbr = std::stoi(filenumber);
3675  if(filenbr < 10)
3676  m_file_name_write = m_file_name_write.substr(0,m_file_name_write.size()-6) + "0" + std::to_string(filenbr+1) + ".csv";
3677  else
3678  m_file_name_write = m_file_name_write.substr(0,m_file_name_write.size()-6) + std::to_string(filenbr+1) + ".csv";
3679 
3680  }
3681 
3682  // Open file
3683  m_file_write.open(m_file_name_write, std::ios::out);
3684  if(!m_file_write.is_open())
3685  {
3686  std::cout << "ERROR opening output file, check that file is not in use...\n";
3687  return;
3688  }
3689 
3690  // Output date and data format
3691  output_header();
3692 
3693  m_mode = Motion_mode::Write;
3694  m_start_loop = true;
3695 
3696 
3697 }
3698 
3699 void Motion_RW::stop_recording()
3700 {
3701  m_start_loop = false;
3702  m_file_write.close();
3703 #ifdef VERBOSE_MODE
3704  std::cout << "\nRecording stopped.\n";
3705 #endif
3706 
3707 }
3708 
3709 void Motion_RW::set_file_name_write(const std::string &file)
3710 {
3711  m_file_name_write = file;
3712 }
3713 
3714 void Motion_RW::set_file_name_read(const std::string &file)
3715 {
3716  m_file_name_read = file;
3717 }
3718 
3719 void Motion_RW::load_file(const std::string &file)
3720 {
3721  m_file_name_read = file;
3722 
3723  // Open file
3724  m_file_read.open(m_file_name_read, std::ios::in);
3725  if(!m_file_read.is_open())
3726  {
3727  std::cout << "ERROR opening input file, check that file exist and is not in use...\n";
3728  return;
3729  }
3730 
3731  // Parse file
3732 #ifdef VERBOSE_MODE
3733  std::cout << "Parsing file " << m_file_name_read << "... ";
3734 #endif
3735 
3736  // Read header
3737  m_file_read.ignore(550,'!');
3738 
3739  std::string tmp;
3740 
3741  while(!m_file_read.eof())
3742  {
3743  // Time stamp
3744  double tmp_val;
3745  m_file_read >> tmp_val;
3746  m_input_timing.push_back(tmp_val);
3747  //std::cout << tmp_t << std::endl;
3748 
3749  // Comma
3750  char trash;
3751  m_file_read >> trash;
3752  //std::cout << trash << std::endl;
3753 
3754  // Iteration
3755  m_file_read >> tmp_val;
3756 
3757  // Comma
3758  m_file_read >> trash;
3759 
3760  // Transform
3761  Mat4d tmp_mat;
3762  for(unsigned r = 0; r < 4; r++)
3763  {
3764  for(unsigned c = 0; c < 4; c++)
3765  {
3766  m_file_read >> tmp_val;
3767  tmp_mat(r,c) = tmp_val;
3768 
3769  // Comma
3770  m_file_read >> trash;
3771  }
3772  }
3773  m_T_data.push_back(tmp_mat);
3774 
3775  // q vec
3776  Vec q;
3777  q.resize(probot->dof());
3778  //m_file_read >> tmp_val;
3779  for(int i = 0; i < probot->dof(); i++)
3780  {
3781  m_file_read >> tmp_val;
3782  q(i) = tmp_val;
3783 
3784  // Comma
3785  m_file_read >> trash;
3786  }
3787  m_q_data.push_back(q);
3788 
3789  // error norm
3790  m_file_read >> tmp_val;
3791  m_error_data.push_back(tmp_val);
3792 
3793  //std::cout << trash << std::endl;
3794  }
3795 
3796  // File verification
3797  if(m_q_data.size() != m_input_timing.size())
3798  {
3799  std::cout << "File read error, the file is not compatible with your robot...\n";
3800  return;
3801  }
3802 
3803 #ifdef VERBOSE_MODE
3804  std::cout << "\nReading done... Size: " << m_q_data.size() << std::endl;
3805 #endif
3806 
3807  // Optional file cleaning (delete duplicates for smoother replay)
3808  static long int cpt_delete = 0;
3809  for(long unsigned int i = 0; i < m_q_data.size()-1; i++)
3810  {
3811  // Check for duplicate entries
3812  Vec tmp_q = m_q_data[i].block<3,1>(0,0), tmp_q_next = m_q_data[i+1].block<3,1>(0,0);
3813 
3814  if(tmp_q == tmp_q_next)
3815  {
3816 
3817  // delete entries
3818  m_q_data.erase(m_q_data.begin() + i);
3819  m_error_data.erase(m_error_data.begin() + i);
3820  m_T_data.erase(m_T_data.begin() + i);
3821  //std::cout << ".";
3822  cpt_delete++;
3823 
3824  }
3825  if(m_error_data[i] == m_error_data[i+1])
3826  {
3827 
3828  // delete entries
3829  m_q_data.erase(m_q_data.begin() + i);
3830  m_error_data.erase(m_error_data.begin() + i);
3831  m_T_data.erase(m_T_data.begin() + i);
3832  //std::cout << ".";
3833  cpt_delete++;
3834 
3835  }
3836  }
3837  std::cout << cpt_delete;
3838 
3839  // Optional retracting
3840  long unsigned int sz = m_q_data.size();
3841 
3842  for(long int i = sz-2; i >= 0; i--)
3843  {
3844 
3845  // copy entries
3846  m_q_data.push_back(m_q_data[i]);
3847  m_error_data.push_back(m_error_data[i]);
3848  m_T_data.push_back(m_T_data[i]);
3849 
3850  }
3851 
3852 
3853  // Run file
3854  if(!m_overwrite_file_period)
3855  m_period_read = m_input_timing[0];
3856 
3857  // Thread initialization
3858  //pthread = new std::thread(&Motion_RW::motion_RW_loop, this, 0, 0);
3859  //m_start_loop = true;
3860  //m_run = true;
3861  m_file_type = File_type::Joint;
3862  m_mode = Motion_mode::Read;
3863  m_start_loop = true;
3864 
3865 }
3866 
3867 void Motion_RW::load_virtual_snake_file(const std::string &file)
3868 {
3869  m_file_name_read = file;
3870 
3871  // Open file
3872  m_file_read.open(m_file_name_read, std::ios::in);
3873  if(!m_file_read.is_open())
3874  {
3875  std::cout << "ERROR opening input file, check that file exist and is not in use...\n";
3876  return;
3877  }
3878 
3879  // Parse file
3880 #ifdef VERBOSE_MODE
3881  std::cout << "Parsing file " << m_file_name_read << "... ";
3882 #endif
3883 
3884 
3885  std::string tmp;
3886 
3887  // Read data size
3888  // q size
3889  long qsize;
3890  m_file_read >> qsize;
3891 
3892  // Comma
3893  char txt;
3894  m_file_read >> txt;
3895 
3896  while(!m_file_read.eof())
3897  {
3898  // Time stamp
3899  double tmp_val;
3900  m_file_read >> tmp_val;
3901  m_input_timing.push_back(tmp_val);
3902 
3903  // Comma
3904  m_file_read >> txt;
3905 
3906  // data
3907  // q vec
3908  Vec q;
3909  q.resize(qsize);
3910 
3911  // read virtual snake
3912  for(auto i = 0; i < qsize; i++)
3913  {
3914 
3915  m_file_read >> tmp_val;
3916  q(i) = tmp_val;
3917 
3918  // Comma
3919  m_file_read >> txt;
3920  }
3921  m_virtual_snake_data.push_back(q);
3922 
3923  // read tip
3924  Mat4d tmptip = Mat4d::Identity();
3925  for(auto c = 0; c < 4; c++)
3926  {
3927  for(auto r = 0; r < 3; r++)
3928  {
3929  m_file_read >> tmp_val;
3930  tmptip(r,c) = tmp_val;
3931 
3932  // Comma
3933  m_file_read >> txt;
3934  }
3935 
3936 
3937  }
3938  m_tip_data.push_back(tmptip);
3939 
3940 
3941  }
3942 
3943  // erase last point
3944  m_tip_data.pop_back();
3945  m_virtual_snake_data.pop_back();
3946 
3947 
3948 #ifdef VERBOSE_MODE
3949  std::cout << "\nReading done... Size: " << m_virtual_snake_data.size() << std::endl;
3950 #endif
3951 
3952  // Run file
3953  if(!m_overwrite_file_period)
3954  m_period_read = m_input_timing[0];
3955 
3956  m_file_type = File_type::Virtual_robot;
3957  m_mode = Motion_mode::Read;
3958  m_start_loop = true;
3959 }
3960 
3961 void Motion_RW::benchmark_virtual_snake_file(const std::string &file)
3962 {
3963  m_file_name_read = file;
3964 
3965  // Open file
3966  m_file_read.open(m_file_name_read, std::ios::in);
3967  if(!m_file_read.is_open())
3968  {
3969  std::cout << "ERROR opening input file, check that file exist and is not in use...\n";
3970  return;
3971  }
3972 
3973  // Parse file
3974 #ifdef VERBOSE_MODE
3975  std::cout << "Parsing file " << m_file_name_read << "... ";
3976 #endif
3977 
3978 
3979  std::string tmp;
3980 
3981  // Read data size
3982  // q size
3983  long qsize;
3984  m_file_read >> qsize;
3985 
3986  // Comma
3987  char txt;
3988  m_file_read >> txt;
3989 
3990  while(!m_file_read.eof())
3991  {
3992  // Time stamp
3993  double tmp_val;
3994  m_file_read >> tmp_val;
3995  m_input_timing.push_back(tmp_val);
3996 
3997  // Comma
3998  m_file_read >> txt;
3999 
4000  // data
4001  // q vec
4002  Vec q;
4003  q.resize(qsize);
4004 
4005  // read virtual snake
4006  for(auto i = 0; i < qsize; i++)
4007  {
4008 
4009  m_file_read >> tmp_val;
4010  q(i) = tmp_val;
4011 
4012  // Comma
4013  m_file_read >> txt;
4014  }
4015  m_virtual_snake_data.push_back(q);
4016 
4017  // read tip
4018  Mat4d tmptip = Mat4d::Identity();
4019  for(auto c = 0; c < 4; c++)
4020  {
4021  for(auto r = 0; r < 3; r++)
4022  {
4023  m_file_read >> tmp_val;
4024  tmptip(r,c) = tmp_val;
4025 
4026  // Comma
4027  m_file_read >> txt;
4028  }
4029 
4030 
4031  }
4032  m_tip_data.push_back(tmptip);
4033 
4034 
4035  }
4036 
4037  // erase last point
4038  m_tip_data.pop_back();
4039  m_virtual_snake_data.pop_back();
4040 
4041 
4042 #ifdef VERBOSE_MODE
4043  std::cout << "\nReading done... Size: " << m_virtual_snake_data.size() << std::endl;
4044 #endif
4045 
4046  // Run file
4047  if(!m_overwrite_file_period)
4048  m_period_read = m_input_timing[0];
4049 
4050  m_file_type = File_type::Virtual_robot;
4051  m_mode = Motion_mode::Benchmark;
4052  m_start_loop = true;
4053 }
4054 
4055 void Motion_RW::benchmark_virtual_snake_vec(const std::vector<Vec> &virtual_vec, const std::vector<Mat4d> &Td_vec)
4056 {
4057 
4058  m_tip_data = Td_vec;
4059  m_virtual_snake_data = virtual_vec;
4060 
4061  // erase last point
4062  //m_tip_data.pop_back();
4063  //m_virtual_snake_data.pop_back();
4064 
4065 
4066  // Run file
4067 
4068  m_file_type = File_type::Virtual_robot;
4069  m_mode = Motion_mode::Benchmark;
4070  m_start_loop = true;
4071 }
4072 
4073 void Motion_RW::write_once(const Virtual_Snake &curv, const Mat4d &Ttip)
4074 {
4075  static bool first_run = true;
4076 
4077  // Write Timing
4078  auto now = chrono_time::now();
4079  Delta_t elapsed = now - m_t0_write;
4080 
4081  if(elapsed.count() >= m_period_write)
4082  {
4083  if(first_run)
4084  {
4085  // Check if file already exist
4086  std::ifstream file_checker(m_file_name_write);
4087  if(file_checker.good())
4088  {
4089  // File already exist
4090  std::string filenumber = m_file_name_write.substr(m_file_name_write.size()-6,2);
4091  int filenbr = std::stoi(filenumber);
4092  if(filenbr < 10)
4093  m_file_name_write = m_file_name_write.substr(0,m_file_name_write.size()-6) + "0" + std::to_string(filenbr+1) + ".csv";
4094  else
4095  m_file_name_write = m_file_name_write.substr(0,m_file_name_write.size()-6) + std::to_string(filenbr+1) + ".csv";
4096 
4097  }
4098 
4099  // Open file
4100  m_file_write.open(m_file_name_write, std::ios::out);
4101  if(!m_file_write.is_open())
4102  {
4103  std::cout << "ERROR opening output file, check that file is not in use...\n";
4104  return;
4105  }
4106 
4107  // Output data size
4108  m_file_write << curv.rows() << "," << std::endl;
4109 
4110  m_start_time = now;
4111  first_run = false;
4112  }
4113  m_t0_write = chrono_time::now();
4114  elapsed = now - m_start_time;
4115  m_file_write << elapsed.count() << ", ";
4116 
4117  // Body targets
4118  for(auto i = 0; i < curv.rows(); i++)
4119  {
4120  m_file_write << curv(i) << ",";
4121  }
4122 
4123  // Tip pose
4124  for(auto c = 0; c < 4; c++)
4125  {
4126  for(auto r = 0; r < 3; r++)
4127  {
4128  m_file_write << Ttip(r,c) << ",";
4129  }
4130  }
4131 
4132 
4133  m_file_write << std::endl;
4134  }
4135 }
4136 
4137 void Motion_RW::write_once(const char &key)
4138 {
4139  static bool first_run = true;
4140 
4141  if(first_run)
4142  {
4143  first_run = false;
4144 
4145  // Open file
4146  m_file_write.open(m_file_name_write, std::ios::out);
4147  if(!m_file_write.is_open())
4148  {
4149  std::cout << "ERROR opening key stroke file, check that file is not in use...\n";
4150  return;
4151  }
4152  }
4153 
4154  // Output data size
4155  m_file_write << key << std::endl;
4156 
4157 
4158 
4159 }
4160 
4161 bool Motion_RW::is_reading_finished()
4162 {
4163  bool result = false;
4164 
4165  switch(m_file_type)
4166  {
4167  case File_type::Joint:
4168  if(m_q_data.size() == 0)
4169  result = true;
4170 
4171  break;
4172  case File_type::Virtual_robot:
4173  if(m_virtual_snake_data.size() == 0)
4174  result = true;
4175 
4176  break;
4177  }
4178  return result;
4179 }
4180 
4181 bool Motion_RW::is_running()
4182 {
4183  return m_run;
4184 }
4185 
4186 
4187 
4188 void Motion_RW::motion_RW_loop(int a, int b)
4189 {
4190  static bool first_run = true;
4191  static double prev_time = 0.0;
4192  Vec pe,p0;
4193 
4194 #ifdef VERBOSE_MODE
4195  std::cout << "Motion RW started...\n";
4196 #endif
4197 
4198  while(m_run)
4199  {
4200 
4201  m_t0_write = chrono_time::now();
4202  m_t0_read = chrono_time::now();
4203  m_start_time = m_t0_write;
4204 
4205  while(m_start_loop)
4206  {
4207  if(m_mode == Motion_mode::Write || m_mode == Motion_mode::Read_Write)
4208  {
4209  if(m_header_written)
4210  {
4211  // Write Timing
4212  auto now = chrono_time::now();
4213  Delta_t elapsed = now - m_t0_write;
4214 
4215  if(elapsed.count() >= m_period_write)
4216  {
4217  if(first_run)
4218  {
4219  m_start_time = now;
4220  first_run = false;
4221  }
4222  m_t0_write = chrono_time::now();
4223  elapsed = now - m_start_time;
4224  m_file_write << elapsed.count() << ", ";
4225  output_data();
4226 
4227  }
4228  }
4229  }
4230  if(m_mode == Motion_mode::Read || m_mode == Motion_mode::Read_Write)
4231  {
4232  // Read Timing
4233  auto now = chrono_time::now();
4234  Delta_t elapsed = now - m_t0_read;
4235 
4236  if(elapsed.count() >= m_period_read)
4237  {
4238  m_t0_read = chrono_time::now();
4239 
4240  switch(m_file_type)
4241  {
4242  case File_type::Joint:
4243  if(m_q_data.size() > 0)
4244  {
4245  if(!m_overwrite_file_period)
4246  {
4247  if(m_input_timing.size() > 0)
4248  {
4249  m_period_read = m_input_timing[0] - prev_time;
4250  prev_time = m_input_timing[0];
4251  m_input_timing.erase(m_input_timing.begin());
4252  }
4253 
4254 
4255  }
4256  probot->set_q_vec(m_q_data[0]);
4257  //std::cout << m_input_data[0] << std::endl;
4258  m_q_data.erase(m_q_data.begin());
4259 
4260 
4261  }
4262 
4263  break;
4264  case File_type::Virtual_robot:
4265  if(m_virtual_snake_data.size() > 0)
4266  {
4267  if(!m_overwrite_file_period)
4268  {
4269  if(m_input_timing.size() > 0)
4270  {
4271  m_input_timing.erase(m_input_timing.begin());
4272  if(m_input_timing.size() > 0)
4273  {
4274  m_period_read = m_input_timing[0] - prev_time;
4275  prev_time = m_input_timing[0];
4276  }
4277 
4278  }
4279  }
4280 
4281  // update curve
4282  // Vec pe = m_tip_data[0].block<3,1>(0,3);
4283  // Vec p0 = m_virtual_snake_data[0].block<3,1>(3,0);
4284  // pvirt_snake->set_pe(pe);
4285  // curve.set_p0(p0);
4286  // probot.set_desired_curve(pvirt_snake->curve());
4287 
4288  // Save path for plotting
4289  p0 = m_virtual_snake_data[0].block<3,1>(m_virtual_snake_data[0].rows()-6,0);
4290  pe = m_virtual_snake_data[0].block<3,1>(m_virtual_snake_data[0].rows()-3,0);
4291  m_robot_path_for_plot.push_back(p0);
4292 
4293  // control robot
4294  probot->set_solver_body_target(m_virtual_snake_data[0], m_tip_data[0]);
4295 
4296 
4297 
4298 
4299 
4300  //std::cout << m_virtual_snake_data[0] << std::endl << std::endl;
4301  //std::cout << "data ";
4302 
4303  m_virtual_snake_data.erase(m_virtual_snake_data.begin());
4304  m_tip_data.erase(m_tip_data.begin());
4305 
4306 
4307  }
4308  else // trajectory accomplished, we save the data // very ugly coding to be replaced
4309  {
4310  // We wait a little for convergence
4311  std::this_thread::sleep_for(std::chrono::seconds(2));
4312 
4313  static bool save_once_only = true;
4314  if(save_once_only)
4315  {
4316  save_once_only = false;
4317  // Update curve/virtual robot/ftl
4318  Virtual_Snake curve;
4319  curve.set_curve(probot->get_body_vec());
4320  curve.set_pe(pe);
4321  curve.set_p0(p0);
4322  curve = probot->follow_the_leader_weighted(m_robot_path_for_plot, curve, 0.000001);
4323 
4324  std::ofstream m_file_write;
4325 
4326  // Open file
4327  m_file_write.open("C:/Box Sync/Matlab/journal_paper/result_for_matlab_replay.csv", std::ios::out);
4328  if(!m_file_write.is_open())
4329  {
4330  std::cout << "ERROR opening output file, check that file is not in use...\n";
4331  return;
4332  }
4333 
4334 
4335  // Output data
4336  m_file_write << "path, virtual, rob1, rob2, q, xi, error\n" ;
4337  Vec rob = probot->get_body_vec();
4338  Vec robis = probot->get_body_vec();
4339  Vec q = probot->get_q_vec();
4340  Vec xi = probot->get_full_q_vec();
4341  // TRO Revision
4342  Vec error = probot->get_error_vector();
4343 
4344  int qidx = 0;
4345  for(unsigned i = 0 ; i < m_robot_path_for_plot.size(); i++)
4346  {
4347  for(unsigned j = 0; j < 3; j++)
4348  {
4349  m_file_write << m_robot_path_for_plot[i](j,0) << ", ";
4350  if(i < rob.rows() / 3)
4351  {
4352  m_file_write << curve(i*3+j) << ", ";
4353  m_file_write << rob(i*3+j) << ", ";
4354  m_file_write << robis(i*3+j) << ", ";
4355 
4356 
4357  if(qidx < q.rows())
4358  {
4359  m_file_write << q(qidx) << ", ";
4360  }
4361  else
4362  m_file_write << " , ";
4363 
4364  if(qidx < xi.rows())
4365  {
4366  m_file_write << xi(qidx) << ", ";
4367  }
4368  else
4369  m_file_write << " , ";
4370 
4371  // TRO revision
4372 
4373  m_file_write << error(i*3+j) << "\n";
4374 
4375 
4376  qidx++;
4377 
4378  }
4379  else
4380  m_file_write << "\n ";
4381  }
4382 
4383  }
4384 
4385  m_file_write.close();
4386  std::cout << "Data output successfully" << std::endl;
4387 
4389  // Now we save the faulty data
4390  // Save current state
4391 
4392  // Open file
4393 
4394  m_file_write.open("C:/Box Sync/Matlab/journal_paper/fault_result_for_matlab.csv", std::ios::out);
4395  if(!m_file_write.is_open())
4396  {
4397  std::cout << "ERROR opening output file, check that file is not in use...\n";
4398  return;
4399  }
4400 
4401 
4402  // Output data
4403  m_file_write << "path, virtual, rob1, rob2, q, xi, error\n" ;
4404 
4405 
4406  // get faulty joint vector
4407  q = probot->get_faulty_q();
4408 
4409  // calculate fwd K
4410  rob = probot->get_body_vec(q);
4411 
4412  robis = rob;
4413 
4414  xi = q;
4415 
4416 
4417  error = probot->get_error_vector();
4418 
4419  qidx = 0;
4420  for(unsigned i = 0 ; i < m_robot_path_for_plot.size(); i++)
4421  {
4422  for(unsigned j = 0; j < 3; j++)
4423  {
4424  m_file_write << m_robot_path_for_plot[i](j,0) << ", ";
4425  if(i < rob.rows() / 3)
4426  {
4427  m_file_write << curve(i*3+j) << ", ";
4428  m_file_write << rob(i*3+j) << ", ";
4429  m_file_write << robis(i*3+j) << ", ";
4430 
4431 
4432  if(qidx < q.rows())
4433  {
4434  m_file_write << q(qidx) << ", ";
4435  }
4436  else
4437  m_file_write << " , ";
4438 
4439  if(qidx < xi.rows())
4440  {
4441  m_file_write << xi(qidx) << ", ";
4442  }
4443  else
4444  m_file_write << " , ";
4445 
4446  // TRO revision
4447 
4448  m_file_write << error(i*3+j) << "\n";
4449 
4450 
4451  qidx++;
4452 
4453  }
4454  else
4455  m_file_write << "\n ";
4456  }
4457 
4458  }
4459 
4460  m_file_write.close();
4461  std::cout << "Data output successfully" << std::endl;
4462  }
4463  }
4464 
4465  break;
4466  }
4467 
4468 
4469  }
4470  }
4471  if(m_mode == Motion_mode::Benchmark)
4472  {
4473 
4474  if(m_runonce)
4475  {
4476  m_benchmark_file.open("benchmarkingn_" + std::to_string(m_file_counter) + "_" + std::to_string(probot->get_full_q_vec().rows()) +".csv", std::ios::out);
4477  m_benchmark_check_file.open("benchmark_path_" + std::to_string(m_file_counter) + "_" + std::to_string(probot->get_full_q_vec().rows()) +".csv", std::ios::out);
4478 
4479  // Output data size
4480  m_benchmark_check_file << m_virtual_snake_data[0].rows() << "," << std::endl;
4481 
4482 
4483  m_file_counter++;
4484  m_runonce = false;
4485  }
4486 
4487  // Read Timing
4488  auto now = chrono_time::now();
4489  Delta_t elapsed = now - m_t0_read;
4490 
4491  if(elapsed.count() >= m_period_read)
4492  {
4493  m_t0_read = chrono_time::now();
4494 
4495  if(m_virtual_snake_data.size() > 0)
4496  {
4497  if(!m_overwrite_file_period)
4498  {
4499  if(m_input_timing.size() > 0)
4500  {
4501  m_input_timing.erase(m_input_timing.begin());
4502  if(m_input_timing.size() > 0)
4503  {
4504  m_period_read = m_input_timing[0] - prev_time;
4505  prev_time = m_input_timing[0];
4506  }
4507 
4508  }
4509  }
4510 
4511  // Get Robot state
4512 
4513  //probot->get_q_vec();
4514  // double error = probot->get_error_norm();
4515  double error = probot->get_RMS_error();
4516  m_benchmark_file << elapsed.count() << ", " << error << std::endl;
4518 
4519 
4520  m_file_write << elapsed.count() << ", ";
4521 
4522  // Body targets
4523  for(auto i = 0; i < m_virtual_snake_data[0].rows(); i++)
4524  {
4525  m_benchmark_check_file << m_virtual_snake_data[0](i) << ",";
4526  }
4527 
4528  // Tip pose
4529  for(auto c = 0; c < 4; c++)
4530  {
4531  for(auto r = 0; r < 3; r++)
4532  {
4533  m_benchmark_check_file << m_tip_data[0](r,c) << ",";
4534  }
4535  }
4536 
4537  m_benchmark_check_file << std::endl;
4538 
4539 
4540 
4541 
4542  // control robot
4543  probot->set_solver_body_target(m_virtual_snake_data[0], m_tip_data[0]);
4544  m_virtual_snake_data.erase(m_virtual_snake_data.begin());
4545  m_tip_data.erase(m_tip_data.begin());
4546  }
4547  else
4548  {
4549  m_benchmark_file.close();
4550  m_benchmark_check_file.close();
4551  m_start_loop = false;
4552  m_run = false;
4553  std::cout << "Benchmark path finished\n";
4554  }
4555 
4556 
4557 
4558 
4559 
4560  }
4561  }
4562  }
4563  //while(!m_start_loop)
4564  std::this_thread::sleep_for(std::chrono::microseconds(250));
4565  }
4566 }
4567 
4568 void Motion_RW::output_header()
4569 {
4570  auto today = std::chrono::system_clock::now();
4571  std::time_t t = std::chrono::system_clock::to_time_t ( today );
4572  m_file_write << "# Endorob recorded motion: " << std::ctime(&t);
4573  m_file_write << "# Robot information, " ;
4574  m_file_write << "Joints: " << probot->joints();
4575  m_file_write << ", DOF: " << probot->dof() << std::endl;
4576  m_file_write << "# Data structure: " << std::endl;
4577  m_file_write << "Time, ";
4578  m_file_write << "iteration, ";
4579  for(unsigned r = 0; r < 4; r++)
4580  for(unsigned c = 0; c < 4; c++)
4581  m_file_write << "T(" << r << "|" << c << "), ";
4582 
4583  for(unsigned i = 0; i < probot->dof(); i++)
4584  m_file_write << "qvec_" << i << ", ";
4585  m_file_write << "error norm";
4586  m_file_write << ", !" << std::endl;
4587 
4588  m_header_written = true;
4589 }
4590 
4591 void Motion_RW::output_data()
4592 {
4593  Vec q = probot->get_q_vec();
4594  Mat4d T = probot->get_snake_base();
4595 
4596  m_file_write << probot->get_iteration() << ", ";
4597 
4598  for(unsigned r = 0; r < 4; r++)
4599  {
4600  for(unsigned c = 0; c < 4; c++)
4601  {
4602  m_file_write << T(r,c) << ", ";
4603  }
4604  }
4605 
4606  for(unsigned i = 0; i < probot->dof(); i++)
4607  m_file_write << q(i) << ", ";
4608 
4609  m_file_write << probot->get_error_norm();
4610 
4611  m_file_write << std::endl;
4612 
4613 }
4614 
4615 
4616 Snake_Path::Snake_Path()
4617 {}
4618 
4619 Snake_Path::~Snake_Path()
4620 {
4621  m_path.clear();
4622  m_weights.clear();
4623 }
4624 
4625 void Snake_Path::push_back(const Vec3d &point)
4626 {
4627  push_back(point, 0);
4628 }
4629 
4630 void Snake_Path::push_back(const Vec3d &point, const double &weight)
4631 {
4632  m_path.push_back(point);
4633  m_weights.push_back(weight);
4634 }
4635 
4636 void Snake_Path::pop_back()
4637 {
4638  m_path.pop_back();
4639  m_weights.pop_back();
4640 }
4641 
4642 uint Snake_Path::size()
4643 {
4644  return m_path.size();
4645 }
4646 
4647 void Snake_Path::set_weight(const double &weight, const int &index)
4648 {
4649  m_weights[m_weights.size()-1] = weight;
4650 }
4651 
4652 Vec Snake_Path::get_weights_vector(const uint &size)
4653 {
4654  Vec result;
4655  result.resize(size - 1);
4656  // uint index = m_weights.size() - size - 1;
4657  uint index = m_weights.size() - (size - 1);
4658 
4659  for(uint i = 0; i < (size-1); i++)
4660  {
4661  result(i) = m_weights[index + i];
4662  }
4663 
4664  return result;
4665 }
4666 
4667 double Snake_Path::get_weight(const uint &index)
4668 {
4669  return m_weights[index];
4670 }
4671 
4672 void Snake_Path::clear()
4673 {
4674  m_path.clear();
4675  m_weights.clear();
4676 }
4677 
4678 Virtual_Snake::Virtual_Snake()
4679 {
4680 
4681 }
4682 
4683 Virtual_Snake::Virtual_Snake(Endorob *robot)
4684 {
4685  m_curve = robot->get_body_vec();
4686  m_weights = Vec::Zero(m_curve.rows() + 3); // +3 for orientation
4687 
4688  // Assign weights of 1 to tip
4689  for(llong i = m_weights.rows()-1; i > m_weights.rows()-1-6; i--)
4690  m_weights(i) = 1.0;
4691 
4692 }
4693 
4694 Virtual_Snake::Virtual_Snake(const Virtual_Snake &snake)
4695 {
4696  m_curve = snake.curve();
4697  m_weights = snake.weights();
4698 }
4699 
4700 Virtual_Snake::~Virtual_Snake()
4701 {
4702 
4703 }
4704 
4705 Vec3d Virtual_Snake::pe()
4706 {
4707  return m_curve.block<3,1>(m_curve.rows()-3,0);
4708 }
4709 
4710 void Virtual_Snake::set_pe(const Vec3d &pe)
4711 {
4712  m_curve.block<3,1>(m_curve.rows()-3,0) = pe;
4713 }
4714 
4715 Vec3d Virtual_Snake::p0()
4716 {
4717  return m_curve.block<3,1>(m_curve.rows()-6,0);
4718 }
4719 
4720 void Virtual_Snake::set_p0(const Vec3d &p0)
4721 {
4722  m_curve.block<3,1>(m_curve.rows()-6,0) = p0;
4723 }
4724 
4725 Vec3d Virtual_Snake::pb()
4726 {
4727  return m_curve.block<3,1>(m_curve.rows()-9,0);
4728 }
4729 
4730 Vec Virtual_Snake::base()
4731 {
4732  return m_curve.block<3,1>(0,0);
4733 }
4734 
4735 Vec Virtual_Snake::curve() const
4736 {
4737  return m_curve;
4738 }
4739 
4740 void Virtual_Snake::set_curve(const Vec &curve)
4741 {
4742  m_curve = curve;
4743  m_weights = Vec::Zero(m_curve.rows() + 3); // +3 for orientation
4744 
4745  // Assign weights of 1 to tip
4746  for(int i = m_weights.rows()-1; i > m_weights.rows()-1-6; i--)
4747  m_weights(i) = 1.0;
4748 }
4749 
4750 void Virtual_Snake::set_weights(const Vec &weights)
4751 {
4752  for(uint i = 0; i < weights.rows()-1; i++)
4753  {
4754  for(uint j = i*3; j < i*3 + 3; j++)
4755  {
4756  m_weights(j) = weights(i);
4757 
4758  }
4759  }
4760 }
4761 
4762 void Virtual_Snake::set_weight(const uint &index, const double &val)
4763 {
4764  for(int i = index ; i < index + 3; i++)
4765  m_weights(i) = val;
4766 
4767 }
4768 
4769 Vec Virtual_Snake::weights() const
4770 {
4771  return m_weights;
4772 }
4773 
4774 uint Virtual_Snake::get_links()
4775 {
4776  return m_curve.rows()/3-1;
4777 }
4778 
4779 uint Virtual_Snake::rows() const
4780 {
4781  return m_curve.rows();
4782 }
4783 
4784 Vec3d Virtual_Snake::block(const uint &index)
4785 {
4786  return m_curve.block<3,1>(index,0);
4787 }
4788 
4789 void Virtual_Snake::set(const uint &index1, const uint &index2, const uint &block1, const uint &block2, const Vec &val)
4790 {
4791  m_curve.block(index1, index2, block1, block2) = val;
4792 }
4793 
4794 
4795 Mat3d get_rotation_increment(const Mat3d &start, const Mat3d &goal)
4796 {
4797  return goal * start.transpose();
4798 }
4799 
4800 Vec get_vector_increment(const Vec &start, const Vec &goal)
4801 {
4802  return goal-start;
4803 }
4804 
4805 Mat4d get_transform_increment(const Mat4d &start, const Mat4d &goal)
4806 {
4807  Mat4d result = Mat4d::Identity(4,4);
4808  result.block<3,3>(0,0) = get_rotation_increment(start.block<3,3>(0,0), goal.block<3,3>(0,0));
4809  result.block<3,1>(0,3) = get_vector_increment(start.block<3,1>(0,3), goal.block<3,1>(0,3));
4810  return result;
4811 
4812 }
4813 
4814 Vec3d get_Euler(const Mat &mat)
4815 {
4816  Mat3d rot;
4817  Vec3d result(3);
4818 
4819  // Check matrix size, homogeneous or rotation matrix
4820  if(mat.cols() > 3)
4821  {
4822  rot = mat.block<3,3>(0,0);
4823  }
4824  else
4825  rot = mat;
4826 
4827  result = rot.eulerAngles(0,1,2);
4828 
4829  return result;
4830 }
4831 
4832 Mat4d invert_transform(const Mat4d &mat)
4833 {
4834  Mat4d result = mat;
4835  result.block<3,3>(0,0) = mat.block<3,3>(0,0).transpose();
4836  result.block<3,1>(0,3) = - result.block<3,3>(0,0) * mat.block<3,1>(0,3);
4837  return result;
4838 }
4839 
4840 double rad_to_degree(const double &val)
4841 {
4842  return val * 180 / M_PI;
4843 }
4844 
4845 double degree_to_rad(const double &val)
4846 {
4847  return val * M_PI / 180;
4848 }
4849 
4850 bool is_almost_equal(const double &a, const double &b, const double &tolerance)
4851 {
4852  return abs(a-b) <= tolerance;
4853 }
4854 
4855 
4856 
4857 bool is_geq(const double &a, const double &b, const double &tolerance)
4858 {
4859  if(is_almost_equal(a, b, tolerance))
4860  return true;
4861 
4862  return (a > b);
4863 }
4864 
4865 bool is_leq(const double &a, const double &b, const double &tolerance)
4866 {
4867  if(is_almost_equal(a, b, tolerance))
4868  return true;
4869 
4870  return (a < b);
4871 }
Endorob::m_robot_frame_T
Mat4d m_robot_frame_T
Definition: endorob.h:1018
Virtual_Snake::set_curve
void set_curve(const Vec &curve)
Definition: endorob.cpp:4740
Endorob::m_CPU_affinity
uint m_CPU_affinity
Definition: endorob.h:1030
Endorob::get_Euler
Vec3d get_Euler(const Mat &mat)
Get Euler angles.
Definition: endorob.cpp:4814
Motion_RW::is_running
bool is_running()
Definition: endorob.cpp:4181
Snake_Path::size
uint size()
Definition: endorob.cpp:4642
Endorob::m_iterations
unsigned long m_iterations
Definition: endorob.h:988
Endorob::set_solver_body_target
void set_solver_body_target(const Vec &vec)
Set solver's full body target (for full body control)
Definition: endorob.cpp:2169
Endorob::m_virtual_snake
Vec m_virtual_snake
Definition: endorob.h:1016
Endorob::threaded_solver_2
void threaded_solver_2(int a, int b)
Definition: endorob.cpp:1586
Endorob::m_apply_joint_coupling
bool m_apply_joint_coupling
Definition: endorob.h:976
Endorob::get_full_kinematics
Mat get_full_kinematics()
Get the full and complete forward kinematics as a nx4 matrix of homogeneous matrix for each link.
Definition: endorob.cpp:2826
Virtual_Snake::curve
Vec curve() const
Definition: endorob.cpp:4735
Endorob::set_DH_convention
void set_DH_convention(const DH_type &convention)
Set DH convention: modified or standard.
Definition: endorob.cpp:153
Motion_RW::set_virt_snake_pointer
void set_virt_snake_pointer(Virtual_Snake *virt_snake)
Definition: endorob.cpp:3650
DH_table_entry::alpha
double alpha()
Definition: endorob.cpp:3517
Endorob::set_flex_gradient
void set_flex_gradient(const Vec &vec)
Setup flex gradient for continuum robots; used if the material is not uniformely bending.
Definition: endorob.cpp:337
Endorob::m_mask_vector
Vec m_mask_vector
Definition: endorob.h:996
Endorob::m_J_inv
Mat m_J_inv
Definition: endorob.h:1000
Solver_type::Jacobian_pseudo_inverse
Simple pseudo-inverse approach.
Solver_type::Jacobian_pseudo_inverse_null_space
Pseudo iverse with null space.
Motion_RW::pthread
std::thread * pthread
Definition: endorob.h:1142
Endorob::rotate_transform_locally
Mat4d rotate_transform_locally(const Mat4d &mat, const Mat3d &rot)
Rotate transform locally.
Definition: endorob.cpp:2009
DH_table_entry::m_flex_gradient
Vec m_flex_gradient
Definition: endorob.h:371
Vec
Eigen::VectorXd Vec
Definition: endorob.h:165
Motion_RW::m_robot_path_for_plot
Snake_Path m_robot_path_for_plot
Definition: endorob.h:1174
Motion_RW::start_recording
void start_recording()
Definition: endorob.cpp:3666
Virtual_Snake::set_p0
void set_p0(const Vec3d &p0)
Definition: endorob.cpp:4720
Endorob::add_DH_entry
void add_DH_entry(const double &a, const double &alpha, const double &d, const double &theta, const Joint_type &type, const double &a_bis=0, const double &theta_bis=0, const uint &flex=0)
Setup robot DH table one entry at a time (Attention, modified DH convention by default)
Definition: endorob.cpp:283
Virtual_Snake::get_links
uint get_links()
Definition: endorob.cpp:4774
Motion_RW::m_start_time
Time_point m_start_time
Definition: endorob.h:1151
Endorob::m_target_T
Mat4d m_target_T
Definition: endorob.h:961
Endorob::solve_for_mask
void solve_for_mask()
Definition: endorob.cpp:1518
Endorob::m_p_tmp
Vec3d m_p_tmp
Definition: endorob.h:1004
rad_to_degree
double rad_to_degree(const double &val)
Definition: endorob.cpp:4840
Endorob::m_do_not_start_solver
bool m_do_not_start_solver
Definition: endorob.h:989
Endorob::get_position_error
Vec3d get_position_error(const Mat4d &Td, const Mat4d &Te)
Definition: endorob.cpp:1280
Endorob::get_forward_kinematics
Mat4d get_forward_kinematics()
Get the tip pose / homogeneous matrix (forward kinematics)
Definition: endorob.cpp:561
Endorob::m_z_tmp
Vec3d m_z_tmp
Definition: endorob.h:1004
Endorob::m_full_body_needed
bool m_full_body_needed
Definition: endorob.h:1017
Endorob::damped_least_square_method
void damped_least_square_method()
Definition: endorob.cpp:3195
Motion_RW::m_file_name_write
std::string m_file_name_write
Definition: endorob.h:1152
Endorob::rot_x
Mat3d rot_x(const double &angle)
Get the desired rotation matrix around the X axis rotated by a desired angle:
Definition: endorob.cpp:1989
Motion_RW::m_period_read
double m_period_read
Definition: endorob.h:1146
Endorob::set_solver_weight_vec
void set_solver_weight_vec(const uint &index, const double &val)
Definition: endorob.cpp:2871
Endorob::joints
unsigned joints()
Get robot number of joints (different to the number of DOF if coupling is used)
Definition: endorob.cpp:556
Snake_Path::set_weight
void set_weight(const double &weight, const int &index=-1)
Definition: endorob.cpp:4647
Joint_type::Rolling
Rolling-joint (two circular surfaces rolling against each other, changing the contact point)
Endorob::follow_the_leader_3
Vec follow_the_leader_3(Snake_Path path, const Vec &desired_shape, const double &translation)
Definition: endorob.cpp:2504
endorob.h
Motion_RW::m_runonce
bool m_runonce
Definition: endorob.h:1169
Endorob::get_faulty_q
Vec get_faulty_q()
Definition: endorob.cpp:1504
Endorob::convert_fwd_k_into_vec
void convert_fwd_k_into_vec()
Definition: endorob.cpp:3101
Endorob::m_tool_frame_T
Mat4d m_tool_frame_T
Definition: endorob.h:959
M_PI
#define M_PI
Definition: endorob.h:131
Endorob::m_error_masked
Vec m_error_masked
Definition: endorob.h:1021
Endorob::get_iteration
unsigned get_iteration()
Get number of iterations.
Definition: endorob.cpp:2114
Endorob::m_thread_running
volatile bool m_thread_running
Definition: endorob.h:992
Endorob::reset_q_vec
void reset_q_vec()
Reset robot to startup state.
Definition: endorob.cpp:1927
degree_to_rad
double degree_to_rad(const double &val)
Definition: endorob.cpp:4845
Motion_RW::m_q_data
std::vector< Mat, Eigen::aligned_allocator< Mat > > m_q_data
Definition: endorob.h:1154
Motion_RW::is_reading_finished
bool is_reading_finished()
Definition: endorob.cpp:4161
Motion_RW::output_data
void output_data()
Definition: endorob.cpp:4591
Endorob::m_xi_vec
Vec m_xi_vec
Definition: endorob.h:978
Endorob::m_fwd_k_vec
Mat m_fwd_k_vec
Definition: endorob.h:1014
Virtual_Snake::set_pe
void set_pe(const Vec3d &pe)
Definition: endorob.cpp:4710
Endorob::get_IK_solution
Vec get_IK_solution()
Request for solver's converged solution, whether target is reached or not.
Definition: endorob.cpp:1890
Endorob::m_global_frame_to_robot_increment
Mat4d m_global_frame_to_robot_increment
Definition: endorob.h:1018
Motion_RW::set_file_name_read
void set_file_name_read(const std::string &file)
Definition: endorob.cpp:3714
Endorob::zero_thresholding
Vec zero_thresholding(const Vec &q_vec, const double &val=M_PI/180/20)
Definition: endorob.cpp:3277
Virtual_Snake::p0
Vec3d p0()
Definition: endorob.cpp:4715
Endorob::follow_the_leader_2
Vec follow_the_leader_2(const std::vector< Vec3d, Eigen::aligned_allocator< Vec3d >> path, const Vec &desired_shape, const double &translation)
Definition: endorob.cpp:2267
DH_table_entry::m_joint_type
Joint_type m_joint_type
Definition: endorob.h:367
llong
long long llong
Definition: endorob.h:157
Motion_RW::m_file_name_read
std::string m_file_name_read
Definition: endorob.h:1152
Motion_RW::m_file_write
std::ofstream m_file_write
Definition: endorob.h:1149
Endorob::mask_resize
Mat mask_resize(const Mat &mat)
Definition: endorob.cpp:2941
Snake_Path
Snake Path Class.
Definition: endorob.h:380
Endorob::m_apply_weighted_solution
bool m_apply_weighted_solution
Definition: endorob.h:1010
Virtual_Snake::m_weights
Vec m_weights
Definition: endorob.h:1088
Virtual_Snake
Virtual Snake Robot Class.
Definition: endorob.h:1052
Endorob::get_Jacobian
Mat get_Jacobian()
Get the Jacobian matrix of the robot in the current configuration.
Definition: endorob.cpp:654
Endorob::set_mask
void set_mask(const Vec &mask)
Set DOF reduction mask if your robot/instrument has less than 6 DOF. The mask is a 6x1 vector x,...
Definition: endorob.cpp:675
Endorob::iterative_solve_equation
void iterative_solve_equation()
Definition: endorob.cpp:3218
Endorob::m_start_solver
volatile bool m_start_solver
Definition: endorob.h:985
Virtual_Snake::pb
Vec3d pb()
Definition: endorob.cpp:4725
Endorob::sphere_line_intersection
Vec3d sphere_line_intersection(const double &radius, const Vec3d &center, const Vec3d &A, const Vec3d &B)
Sphere line intersection.
Definition: endorob.cpp:2061
Endorob::m_instantiation_index
uint m_instantiation_index
Definition: endorob.h:1035
Endorob
This is the main Class: EndoRob that you can use to control your own weird looking robot....
Definition: endorob.h:420
Endorob::get_coupling_matrix
Mat get_coupling_matrix(void)
Get the joint coupling matrix.
Definition: endorob.cpp:659
Endorob::m_q_vec_for_user
Vec m_q_vec_for_user
Definition: endorob.h:964
Endorob::m_full_body_vec_desired
Vec m_full_body_vec_desired
Definition: endorob.h:1015
Endorob::project_point_onto_line
Vec3d project_point_onto_line(const Vec3d &P, const Vec3d &A, const Vec3d &B)
Project point P onto line defined by AB (orthogonal projection)
Definition: endorob.cpp:2051
Endorob::m_prev_q_vec
Vec m_prev_q_vec
Definition: endorob.h:963
Endorob::get_rpy
Vec3d get_rpy(const Mat &mat)
Get Roll Pitch and Yaw angles.
Definition: endorob.cpp:1962
Motion_RW::m_input_timing
std::vector< double > m_input_timing
Definition: endorob.h:1158
Solver_type::Full_body_jacobian
Full body Jacobian approach for full joint control.
Endorob::m_base_frame_T
Mat4d m_base_frame_T
Definition: endorob.h:959
Motion_RW::m_header_written
bool m_header_written
Definition: endorob.h:1144
Endorob::solver_has_converged
bool solver_has_converged()
Definition: endorob.cpp:2149
uint
unsigned int uint
Definition: endorob.h:156
Endorob::solve_with_tentacle
Vec solve_with_tentacle(Mat T_)
Definition: endorob.cpp:2190
Endorob::set_desired_curve
void set_desired_curve(const Vec &curve)
Set desired curve.
Definition: endorob.cpp:1879
Endorob::m_CPU_cpt
static uint m_CPU_cpt
Definition: endorob.h:1029
DH_table_entry::m_d
double m_d
Definition: endorob.h:366
Endorob::load_DH
void load_DH(std::string filename)
Load DH entries from file.
Definition: endorob.cpp:198
Snake_Path::pop_back
void pop_back()
Definition: endorob.cpp:4636
Vec3d
Eigen::Vector3d Vec3d
Definition: endorob.h:163
DH_table_entry::theta
double theta()
Definition: endorob.cpp:3527
Endorob::m_J_masked
Mat m_J_masked
Definition: endorob.h:1000
DH_table_entry::joint_limit_max
double joint_limit_max()
Definition: endorob.cpp:3552
Endorob::m_q_vec
Vec m_q_vec
Definition: endorob.h:962
Endorob::calculate_joint_LUT
void calculate_joint_LUT()
Definition: endorob.cpp:3084
Endorob::m_J_rot
Mat m_J_rot
Definition: endorob.h:1002
Endorob::get_full_mobile_body_error
void get_full_mobile_body_error()
Definition: endorob.cpp:1326
Endorob::set_flex_resolution
void set_flex_resolution(const uint &res)
Setup flex resolution for continuum robots: sampling/division into smaller revolute joints.
Definition: endorob.cpp:332
Endorob::get_transform_increment
Mat4d get_transform_increment(const Mat4d &start, const Mat4d &goal)
Get increment between two transformation matrices (rotation and translation)
Definition: endorob.cpp:4805
Motion_RW::m_overwrite_file_period
bool m_overwrite_file_period
Definition: endorob.h:1161
Endorob::get_error
void get_error(const Mat4d &Td, const Mat4d &Te)
Definition: endorob.cpp:1305
Endorob::m_cpu_info_dispayed
static bool m_cpu_info_dispayed
Definition: endorob.h:1034
DH_table_entry::a_bis
double a_bis()
Definition: endorob.cpp:3532
Endorob::follow_the_leader_weighted
Virtual_Snake follow_the_leader_weighted(Snake_Path path, const Virtual_Snake &desired_shape, const double &translation)
Definition: endorob.cpp:2674
Motion_RW::load_file
void load_file(const std::string &file)
Definition: endorob.cpp:3719
Endorob::follow_the_leader
Vec follow_the_leader(const std::vector< Vec3d, Eigen::aligned_allocator< Vec3d >> path, const Vec &desired_shape, const double &translation)
Definition: endorob.cpp:2197
Endorob::calculte_dof_after_coupling
void calculte_dof_after_coupling()
Definition: endorob.cpp:3018
Endorob::calculate_full_body_jacobian
void calculate_full_body_jacobian()
Definition: endorob.cpp:1063
DH_table_entry::m_a
double m_a
Definition: endorob.h:366
Endorob::pseudo_inverse_null_space
void pseudo_inverse_null_space()
Definition: endorob.cpp:3166
Joint_type::Prismatic
Standard prismatic joint.
Snake_Path::get_weights_vector
Vec get_weights_vector(const uint &size)
Definition: endorob.cpp:4652
Motion_RW::stop_recording
void stop_recording()
Definition: endorob.cpp:3699
Virtual_Snake::Virtual_Snake
Virtual_Snake()
Definition: endorob.cpp:4678
Endorob::m_J
Mat m_J
Definition: endorob.h:1000
Endorob::split
std::vector< std::string > split(const std::string &s, char delimiter, bool remove_empty)
Definition: endorob.cpp:3036
Endorob::get_full_body_error
Vec get_full_body_error()
Definition: endorob.cpp:1321
Endorob::m_wait_for_sync
volatile bool m_wait_for_sync
Definition: endorob.h:990
Endorob::get_RMS_error
double get_RMS_error()
Get the RMS per joint error between the desired and current position.
Definition: endorob.cpp:2125
Endorob::modify_error_with_f
void modify_error_with_f()
Definition: endorob.cpp:3246
Endorob::m_q_vec_rest
Vec m_q_vec_rest
Definition: endorob.h:962
Virtual_Snake::set
void set(const uint &index1, const uint &index2, const uint &block1, const uint &block2, const Vec &val)
Definition: endorob.cpp:4789
Endorob::m_solver_pending
volatile bool m_solver_pending
Definition: endorob.h:991
DH_table_entry::get_flex_gradient
Vec get_flex_gradient()
Definition: endorob.cpp:3542
Endorob::p_solver_type
void(Endorob::* p_solver_type)(void)
Definition: endorob.h:969
Endorob::set_solver_type
void set_solver_type(Solver_type solver, const double &lambda=1)
Set iterative solver type.
Definition: endorob.cpp:342
Endorob::set_robot_reference_frame
void set_robot_reference_frame(const Mat4d &T)
Definition: endorob.cpp:2892
Endorob::rot_y
Mat3d rot_y(const double &angle)
Get the desired rotation matrix around the Y axis rotated by a desired angle:
Definition: endorob.cpp:1994
Endorob::get_body_vec
Vec get_body_vec()
Get full-body vector.
Definition: endorob.cpp:2104
Virtual_Snake::rows
uint rows() const
Definition: endorob.cpp:4779
Snake_Path::~Snake_Path
~Snake_Path()
Definition: endorob.cpp:4619
Endorob::m_mask_dof
unsigned m_mask_dof
Definition: endorob.h:995
Endorob::apply_joint_limits
void apply_joint_limits()
Definition: endorob.cpp:1485
Joint_type::Rolling_flex
Multi-stage flexible joint with Rolling-joints (two circular surfaces rolling against each other,...
Endorob::m_robot_path
std::vector< Vec3d, Eigen::aligned_allocator< Vec3d > > m_robot_path
Definition: endorob.h:1024
Motion_RW::m_file_counter
static uint m_file_counter
Definition: endorob.h:1168
Motion_RW::motion_RW_loop
void motion_RW_loop(int a, int b)
Definition: endorob.cpp:4188
DH_table_entry::m_joint_limit_min
double m_joint_limit_min
Definition: endorob.h:369
Endorob::set_joint_coupling
void set_joint_coupling(const Vec &coupling)
Set joint coupling if multiple joint are moving together (coupled) whether for mechanical coupling or...
Definition: endorob.cpp:703
is_leq
bool is_leq(const double &a, const double &b, const double &tolerance)
Definition: endorob.cpp:4865
DH_table_entry::a
double a()
Definition: endorob.cpp:3512
Endorob::base_transformation_matrix
Mat4d base_transformation_matrix(DH_table_entry DH, double q)
Definition: endorob.cpp:3291
Endorob::m_iteration_overflow
bool m_iteration_overflow
Definition: endorob.h:987
Endorob::Endorob
Endorob()
Default constructor.
Definition: endorob.cpp:33
Endorob::jacobian_transpose
void jacobian_transpose()
Definition: endorob.cpp:3207
Motion_RW::m_error_data
std::vector< double > m_error_data
Definition: endorob.h:1156
Motion_RW::write_once
void write_once(const Virtual_Snake &curv, const Mat4d &Ttip)
Definition: endorob.cpp:4073
Endorob::set_q_vec
void set_q_vec(const Vec &q)
Set robot's joint vector.
Definition: endorob.cpp:1906
Endorob::m_phi
Vec m_phi
Definition: endorob.h:1007
Motion_RW::set_file_name_write
void set_file_name_write(const std::string &file)
Definition: endorob.cpp:3709
Endorob::m_full_body_vec_current
Vec m_full_body_vec_current
Definition: endorob.h:1015
Virtual_Snake::~Virtual_Snake
~Virtual_Snake()
Definition: endorob.cpp:4700
Motion_RW::m_start_loop
volatile bool m_start_loop
Definition: endorob.h:1145
Motion_RW::m_period_write
double m_period_write
Definition: endorob.h:1146
Endorob::calculate_pseudo_inverse_masked
void calculate_pseudo_inverse_masked()
Definition: endorob.cpp:3385
Endorob::m_prev_xi_vec
Vec m_prev_xi_vec
Definition: endorob.h:979
Endorob::m_converged
volatile bool m_converged
Definition: endorob.h:986
DH_table_entry::get_flex_resolution
uint get_flex_resolution()
Definition: endorob.cpp:3537
Mat
Eigen::MatrixXd Mat
Definition: endorob.h:160
stdafx.h
Virtual_Snake::set_weights
void set_weights(const Vec &weights)
Definition: endorob.cpp:4750
Motion_RW::m_t0_write
Time_point m_t0_write
Definition: endorob.h:1151
Endorob::m_run
volatile bool m_run
Definition: endorob.h:985
Solver_type
Solver_type
Currently supported solvers.
Definition: endorob.h:148
is_almost_equal
bool is_almost_equal(const double &a, const double &b, const double &tolerance)
Definition: endorob.cpp:4850
Endorob::solve
void solve()
Definition: endorob.cpp:1415
Motion_RW::set_write_frequency
void set_write_frequency(const double &freq)
Definition: endorob.cpp:3655
DH_modified
Modified DH approach (better than standard!)
Definition: endorob.h:333
Motion_RW::m_file_read
std::ifstream m_file_read
Definition: endorob.h:1150
Endorob::m_CPU_amount
static uint m_CPU_amount
Definition: endorob.h:1028
Delta_t
std::chrono::duration< double > Delta_t
Definition: endorob.h:175
Motion_RW::~Motion_RW
~Motion_RW()
Definition: endorob.cpp:3620
Endorob::disable_fault_tolerance
void disable_fault_tolerance()
Disable joint fault tolerance.
Definition: endorob.cpp:2913
Endorob::calculate_pseudo_inverse
void calculate_pseudo_inverse()
Definition: endorob.cpp:3334
Snake_Path::Snake_Path
Snake_Path()
Definition: endorob.cpp:4616
Endorob::m_error_weights
Mat m_error_weights
Definition: endorob.h:1005
is_geq
bool is_geq(const double &a, const double &b, const double &tolerance)
Definition: endorob.cpp:4857
DH_table_entry::m_a_bis
double m_a_bis
Definition: endorob.h:366
Endorob::m_joint_lookup_table
Vec m_joint_lookup_table
Definition: endorob.h:980
Snake_Path::get_weight
double get_weight(const uint &index)
Definition: endorob.cpp:4667
Endorob::check_for_convergence
void check_for_convergence()
Definition: endorob.cpp:1551
Endorob::get_joint_limit_scalar
Vec get_joint_limit_scalar()
Definition: endorob.cpp:3130
Motion_RW::set_read_frequency
void set_read_frequency(const double &freq)
Definition: endorob.cpp:3660
Endorob::initialize
void initialize()
Once all parameters set, initialize robot and start solver.
Definition: endorob.cpp:368
Endorob::threaded_solver
void threaded_solver(int a, int b)
Definition: endorob.cpp:1353
Endorob::get_error_vector
Vec get_error_vector()
Get the current error between the desired and current position.
Definition: endorob.cpp:2142
Endorob::calculate_full_mobile_body_jacobian
void calculate_full_mobile_body_jacobian()
Definition: endorob.cpp:1158
DH_table_entry::m_joint_limit_max
double m_joint_limit_max
Definition: endorob.h:369
Endorob::get_random_q_vec
Vec get_random_q_vec()
Get random feasible joint vector.
Definition: endorob.cpp:1944
DH_table_entry
DH Table Class.
Definition: endorob.h:343
Motion_RW::m_T_data
std::vector< Mat, Eigen::aligned_allocator< Mat > > m_T_data
Definition: endorob.h:1155
Endorob::m_apply_fault_tolerance
std::atomic< bool > m_apply_fault_tolerance
Definition: endorob.h:1039
Virtual_Snake::block
Vec3d block(const uint &index)
Definition: endorob.cpp:4784
DH_table_entry::set_joint_limits
void set_joint_limits(const double &min, const double &max)
Definition: endorob.cpp:3496
Endorob::m_joint_coupling_matrix
Mat m_joint_coupling_matrix
Definition: endorob.h:974
Endorob::calculate_full_forward_kinematics
void calculate_full_forward_kinematics()
Definition: endorob.cpp:765
Endorob::dof
unsigned dof()
Get robot number of DOF (different to the number of joints if coupling is used)
Definition: endorob.cpp:551
Endorob::m_faulty_joint_vec
std::vector< bool > m_faulty_joint_vec
Definition: endorob.h:1038
DH_type
DH_type
DH two typical standards:
Definition: endorob.h:331
Endorob::calculate_coupling_matrix_increase
void calculate_coupling_matrix_increase()
Definition: endorob.cpp:2967
Motion_RW::m_virtual_snake_data
std::vector< Vec > m_virtual_snake_data
Definition: endorob.h:1159
Endorob::m_assigning_CPU
static volatile bool m_assigning_CPU
Definition: endorob.h:1031
INF_LIM
#define INF_LIM
Definition: endorob.h:169
Endorob::set_joint_limits
void set_joint_limits(const double &min, const double &max)
Set joint limits for current joint, radians for revolute or rolling joints, meters for prismatic.
Definition: endorob.cpp:320
Endorob::set_solver_target
void set_solver_target(const Mat4d &T)
Set solver's target 4x4 homegenous matrix.
Definition: endorob.cpp:2159
Endorob::m_J_for_user
Mat m_J_for_user
Definition: endorob.h:1001
Endorob::set_vec_relative_to_base
Vec set_vec_relative_to_base(const Vec &vec)
Definition: endorob.cpp:2831
Endorob::m_prev_error
Vec m_prev_error
Definition: endorob.h:1021
Motion_RW::m_t0_read
Time_point m_t0_read
Definition: endorob.h:1151
Motion_RW::benchmark_virtual_snake_file
void benchmark_virtual_snake_file(const std::string &file)
Definition: endorob.cpp:3961
DH_table_entry::d
double d()
Definition: endorob.cpp:3522
Endorob::m_DH_table
std::vector< DH_table_entry > m_DH_table
Definition: endorob.h:966
Motion_RW::m_benchmark_check_file
std::ofstream m_benchmark_check_file
Definition: endorob.h:1171
Endorob::set_solver_weight_mat
void set_solver_weight_mat(const uint &index, const double &val)
Set the solver's weights value for a desired joint at a specific index.
Definition: endorob.cpp:2877
Motion_RW::m_benchmark_file
std::ofstream m_benchmark_file
Definition: endorob.h:1170
Endorob::m_error_weights_vec
Vec m_error_weights_vec
Definition: endorob.h:1006
Endorob::calculate_coupling_matrix_reduce
void calculate_coupling_matrix_reduce()
Definition: endorob.cpp:2992
Endorob::m_coupling_vector
Vec m_coupling_vector
Definition: endorob.h:973
Endorob::solver_has_ended
bool solver_has_ended()
Definition: endorob.cpp:2154
DH_table_entry::get_joint_type
Joint_type get_joint_type()
Definition: endorob.cpp:3547
Motion_RW::File_type::Joint
Joint vector data.
Endorob::get_q_vec
Vec get_q_vec()
Get joint vector.
Definition: endorob.cpp:1861
Endorob::calculate_jacobian
void calculate_jacobian()
Definition: endorob.cpp:962
DH_table_entry::DH_table_entry
DH_table_entry(const double &a, const double &alpha, const double &d, const double &theta, const Joint_type &type)
Definition: endorob.cpp:3464
Endorob::angle_modulus
Vec angle_modulus(const Vec &q_vec)
Definition: endorob.cpp:2918
Motion_RW::Motion_RW
Motion_RW()
Definition: endorob.cpp:3569
Motion_RW::load_virtual_snake_file
void load_virtual_snake_file(const std::string &file)
Definition: endorob.cpp:3867
Endorob::~Endorob
~Endorob()
Default destructor.
Definition: endorob.cpp:133
Endorob::get_orientation_error
Vec3d get_orientation_error(const Mat4d &Td, const Mat4d &Te)
Definition: endorob.cpp:1286
Endorob::set_base_frame
void set_base_frame(const Mat4d &T)
Set base frame.
Definition: endorob.cpp:670
DH_table_entry::set_flex_gradient
void set_flex_gradient(const Vec &vec)
Definition: endorob.cpp:3507
Endorob::get_instant_IK_solution
Vec get_instant_IK_solution()
Request for solver's current solution,if solver is still running, the solution may not be valid.
Definition: endorob.cpp:1849
Endorob::m_joint_coupling_reduce_dimension
Mat m_joint_coupling_reduce_dimension
Definition: endorob.h:975
Motion_RW::benchmark_virtual_snake_vec
void benchmark_virtual_snake_vec(const std::vector< Vec > &virtual_vec, const std::vector< Mat4d > &Td_vec)
Definition: endorob.cpp:4055
Endorob::invert_transform
Mat4d invert_transform(const Mat4d &mat)
Invert homogeneous matrix for opposite transformation.
Definition: endorob.cpp:4832
Endorob::get_rotation_increment
Mat3d get_rotation_increment(const Mat3d &start, const Mat3d &goal)
Get increment between two rotation matrices.
Definition: endorob.cpp:4795
Endorob::save_DH
void save_DH(std::string filename)
Save DH entries to file.
Definition: endorob.cpp:158
Virtual_Snake::weights
Vec weights() const
Definition: endorob.cpp:4769
Snake_Path::m_path
std::vector< Vec3d, Eigen::aligned_allocator< Vec3d > > m_path
Definition: endorob.h:400
Endorob::m_joints
uint m_joints
Definition: endorob.h:957
Motion_RW::set_endorob_pointer
void set_endorob_pointer(Endorob *robot)
Definition: endorob.cpp:3645
Mat4d
Eigen::Matrix4d Mat4d
Definition: endorob.h:161
Motion_RW::pvirt_snake
Virtual_Snake * pvirt_snake
Definition: endorob.h:1148
DH_table_entry::m_flex_resolution
uint m_flex_resolution
Definition: endorob.h:370
mutx
std::mutex mutx
Definition: endorob.cpp:24
Endorob::m_tip_T
Mat4d m_tip_T
Definition: endorob.h:961
Motion_RW::m_file_type
File_type m_file_type
Definition: endorob.h:1167
Endorob::m_robot_frame_to_global_increment
Mat4d m_robot_frame_to_global_increment
Definition: endorob.h:1018
Endorob::check_for_cpu
void check_for_cpu()
Definition: endorob.cpp:3223
Snake_Path::clear
void clear()
Definition: endorob.cpp:4672
Snake_Path::m_weights
std::vector< double > m_weights
Definition: endorob.h:401
Endorob::m_check_joint_limit_jacobian
bool m_check_joint_limit_jacobian
Definition: endorob.h:967
Endorob::m_p_e
Vec3d m_p_e
Definition: endorob.h:1003
Endorob::pseudo_inverse_simple
void pseudo_inverse_simple()
Definition: endorob.cpp:3152
Endorob::m_apply_mask
bool m_apply_mask
Definition: endorob.h:997
Endorob::m_lambda
double m_lambda
Definition: endorob.h:968
Motion_RW::m_tip_data
std::vector< Mat4d > m_tip_data
Definition: endorob.h:1160
Endorob::get_error_norm
double get_error_norm()
Get the error norm between the desired and current position.
Definition: endorob.cpp:2119
Endorob::rotate_transform_around_axis_and_P
Mat4d rotate_transform_around_axis_and_P(const Mat4d &T, const double &angle, const Vec3d &axis, const Vec &P)
Rotate transform around arbitrary axis and point.
Definition: endorob.cpp:2033
DH_table_entry::joint_limit_min
double joint_limit_min()
Definition: endorob.cpp:3557
Endorob::check_joint_in_limit
bool check_joint_in_limit(const Vec &q)
Definition: endorob.cpp:3050
Endorob::apply_joint_coupling
void apply_joint_coupling()
Definition: endorob.cpp:2960
Solver_type::Jacobian_transpose
Jacobian transpose.
Motion_RW::m_run
volatile bool m_run
Definition: endorob.h:1143
Endorob::set_rpy
Mat4d set_rpy(const Mat &mat, Vec3d rpy)
Set the roll pitch and yaw angles into a 4x4 homogeneous matrix.
Definition: endorob.cpp:1980
DH_table_entry::m_alpha
double m_alpha
Definition: endorob.h:366
DH_standard
Standard DH approach.
Definition: endorob.h:332
Solver_type::Jacobian_DLS
Damped least square approach.
Endorob::calculate_pseudo_inverse_DLS
void calculate_pseudo_inverse_DLS()
Definition: endorob.cpp:3429
Endorob::get_tip_pose
Mat4d get_tip_pose()
Get the tip pose / homogeneous matrix (forward kinematics)
Definition: endorob.cpp:952
Endorob::get_desired_curve
Vec get_desired_curve()
Get desired curve.
Definition: endorob.cpp:1874
Endorob::set_tool_frame
void set_tool_frame(const Mat4d &T)
Set tool frame.
Definition: endorob.cpp:665
Endorob::m_dof
uint m_dof
Definition: endorob.h:958
Endorob::rot_z
Mat3d rot_z(const double &angle)
Get the desired rotation matrix around the Z axis rotated by a desired angle:
Definition: endorob.cpp:1999
DH_table_entry::set_offset
void set_offset(const double &offset)
Definition: endorob.cpp:3491
Endorob::m_DH_convention
DH_type m_DH_convention
Definition: endorob.h:960
Virtual_Snake::base
Vec base()
Definition: endorob.cpp:4730
Endorob::get_DH_table
std::vector< DH_table_entry > get_DH_table()
Get the full DH table of the robot.
Definition: endorob.cpp:2821
Mat3d
Eigen::Matrix3d Mat3d
Definition: endorob.h:162
Endorob::get_tip_position
Vec3d get_tip_position()
Get the tip position / 3x1 vector (forward kinematics)
Definition: endorob.cpp:957
Endorob::m_alpha
Vec m_alpha
Definition: endorob.h:1007
Endorob::enforce_joint_limits_mode
void enforce_joint_limits_mode(void)
Enforce joint limit mode. In this mode the joint cannot exceed their joint limit.
Definition: endorob.cpp:2902
Motion_RW::File_type::Virtual_robot
virtual robot input data
Endorob::get_full_q_vec
Vec get_full_q_vec()
Get complete joint vector before coupling.
Definition: endorob.cpp:1869
Motion_RW::m_mode
Motion_mode m_mode
Definition: endorob.h:1153
Endorob::get_vector_increment
Vec get_vector_increment(const Vec &start, const Vec &goal)
Get increment between two vectors.
Definition: endorob.cpp:4800
Virtual_Snake::pe
Vec3d pe()
Definition: endorob.cpp:4705
Endorob::m_identity
Mat m_identity
Definition: endorob.h:1008
Endorob::m_joint_index_offset
unsigned m_joint_index_offset
Definition: endorob.h:1009
Joint_type
Joint_type
Currently supported joint types:
Definition: endorob.h:138
Endorob::rotate_transform_around_P
Mat4d rotate_transform_around_P(const Mat4d &T, const Mat3d &rot, const Vec &P)
Rotate transform around arbitrary point.
Definition: endorob.cpp:2018
Endorob::rot_xyz
Mat3d rot_xyz(const double &angle, const Vec3d &axis)
Get the desired rotation matrix formed by the angle and axis representation.
Definition: endorob.cpp:2004
Endorob::set_faulty_joint
void set_faulty_joint(const uint &joint, const double &val)
Set a joint as faulty for Jacobian Compensation.
Definition: endorob.cpp:2907
Endorob::m_xi_vec_rest
Vec m_xi_vec_rest
Definition: endorob.h:978
Endorob::psolver
std::thread * psolver
Definition: endorob.h:984
Endorob::m_instantiations
static uint m_instantiations
Definition: endorob.h:1033
Motion_RW::output_header
void output_header()
Definition: endorob.cpp:4568
Endorob::m_error
Vec m_error
Definition: endorob.h:1021
DH_table_entry::m_theta
double m_theta
Definition: endorob.h:366
Snake_Path::push_back
void push_back(const Vec3d &point)
Definition: endorob.cpp:4625
Endorob::m_rolling_joint_present
bool m_rolling_joint_present
Definition: endorob.h:977
Virtual_Snake::set_weight
void set_weight(const uint &index, const double &val)
Definition: endorob.cpp:4762
DH_table_entry::set_flex_resolution
void set_flex_resolution(const uint &res)
Definition: endorob.cpp:3502
Endorob::m_diverged
volatile bool m_diverged
Definition: endorob.h:986
Joint_type::Revolute
Standard revolute joint.
Virtual_Snake::m_curve
Vec m_curve
Definition: endorob.h:1087
Endorob::null_space_constraint_calculation
void null_space_constraint_calculation()
Definition: endorob.cpp:3185
Motion_RW::probot
Endorob * probot
Definition: endorob.h:1147
Endorob::m_prev_error_masked
Vec m_prev_error_masked
Definition: endorob.h:1021
Endorob::m_J_inv_masked
Mat m_J_inv_masked
Definition: endorob.h:1000
Endorob::m_J_vel
Mat m_J_vel
Definition: endorob.h:1002
Endorob::get_snake_base
Mat get_snake_base()
Definition: endorob.cpp:2843