vpRobotFrankaSim.cpp
Go to the documentation of this file.
1 /*
2  * This file is part of the ViSP software.
3  * Copyright (C) 2005 - 2021 by INRIA. All rights reserved.
4  *
5  * This software is free software; you can redistribute it and/or
6  * modify it under the terms of the GNU General Public License
7  * ("GPL") version 2 as published by the Free Software Foundation.
8  * See the file LICENSE.txt at the root directory of this source
9  * distribution for additional information about the GNU GPL.
10  *
11  * For using ViSP with software that can not be combined with the GNU
12  * GPL, please contact INRIA about acquiring a ViSP Professional
13  * Edition License.
14  *
15  * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
16  *
17  * This software was developed at:
18  * INRIA Rennes - Bretagne Atlantique
19  * Campus Universitaire de Beaulieu
20  * 35042 Rennes Cedex
21  * France
22  * http://www.irisa.fr/lagadic
23  *
24  * If you have questions regarding the use of this file, please contact
25  * INRIA at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  *
30  * Description:
31  * Franka robot ROS simulator coupled with Coppeliasim.
32  *
33  * Authors:
34  * Alexander Oliva
35  * Fabien Spindler
36  */
37 
39 
40 #include "model/franka_model.h"
41 
42 #if defined( VISP_HAVE_OROCOS_KDL )
43 
48  : m_q( 7, 0 )
49  , m_dq( 7, 0 )
50  , m_tau_J( 7, 0 )
51  , m_mL( 0.0 )
52  , m_flMcom()
53  , m_Il( 3, 3 )
54  , m_flMe()
55  , m_g0( { 0.0, 0.0, -9.80665 } )
56  , m_mutex()
57  , m_q_kdl( 7 )
58  , m_dq_des_kdl( 7 )
59  , m_chain_kdl()
60  , m_q_min_kdl( 7 )
61  , m_q_max_kdl( 7 )
62  , m_stateRobot( vpRobot::STATE_STOP )
63  , m_q_des( 7, 0 )
64  , m_dq_des( 7, 0 )
65  , m_dq_des_filt( 7, 0 )
66  , m_v_cart_des( 6, 0 )
67  , m_tau_J_des( 7, 0 )
68  , m_tau_J_des_filt( 7, 0 )
69  , m_eMc()
70  , m_eVc()
71  , m_verbose( false )
72  , m_toolMounted( false )
73  , m_camMounted( false )
74 {
75 #ifdef VISP_HAVE_OROCOS_KDL
77  KDL::Segment( KDL::Joint( KDL::Joint::None ), KDL::Frame::DH_Craig1989( 0.0, 0.0, 0.333, 0.0 ) ) );
79  KDL::Segment( KDL::Joint( KDL::Joint::RotZ ), KDL::Frame::DH_Craig1989( 0.0, -M_PI_2, 0.0, 0.0 ) ) );
81  KDL::Segment( KDL::Joint( KDL::Joint::RotZ ), KDL::Frame::DH_Craig1989( 0.0, M_PI_2, 0.316, 0.0 ) ) );
83  KDL::Segment( KDL::Joint( KDL::Joint::RotZ ), KDL::Frame::DH_Craig1989( 0.0825, M_PI_2, 0.0, 0.0 ) ) );
85  KDL::Segment( KDL::Joint( KDL::Joint::RotZ ), KDL::Frame::DH_Craig1989( -0.0825, -M_PI_2, 0.384, 0.0 ) ) );
87  KDL::Segment( KDL::Joint( KDL::Joint::RotZ ), KDL::Frame::DH_Craig1989( 0.0, M_PI_2, 0.0, 0.0 ) ) );
89  KDL::Segment( KDL::Joint( KDL::Joint::RotZ ), KDL::Frame::DH_Craig1989( 0.088, M_PI_2, 0.0, 0.0 ) ) );
91  KDL::Segment( KDL::Joint( KDL::Joint::RotZ ), KDL::Frame::DH_Craig1989( 0.0, 0.0, 0.107, 0.0 ) ) );
92 
93  m_q_min_kdl( 0 ) = -2.8973;
94  m_q_min_kdl( 1 ) = -1.7628;
95  m_q_min_kdl( 2 ) = -2.8973;
96  m_q_min_kdl( 3 ) = -3.0718;
97  m_q_min_kdl( 4 ) = -2.8973;
98  m_q_min_kdl( 5 ) = -0.0175;
99  m_q_min_kdl( 6 ) = -2.8973;
100  m_q_max_kdl( 0 ) = 2.8973;
101  m_q_max_kdl( 1 ) = 1.7628;
102  m_q_max_kdl( 2 ) = 2.8973;
103  m_q_max_kdl( 3 ) = -0.0698;
104  m_q_max_kdl( 4 ) = 2.8973;
105  m_q_max_kdl( 5 ) = 3.7525;
106  m_q_max_kdl( 6 ) = 2.8973;
107 
112  *( m_diffIkSolver_kdl ), 100,
113  1e-6 ); // Maximum 100 iterations, stop at accuracy 1e-6
114 #endif
115 }
116 
121 {
122 #ifdef VISP_HAVE_OROCOS_KDL
123  delete m_fksolver_kdl;
124  delete m_jacobianSolver_kdl;
125  delete m_diffIkSolver_kdl;
126  delete m_iksolver_JL_kdl;
127 #endif
128 }
129 
133 vpColVector
135 {
136  return m_dq_des_filt;
137 }
138 
143 void
145 {
146  std::lock_guard< std::mutex > lock( m_mutex );
147  friction = franka_model::friction( m_dq );
148 }
149 
156 void
157 vpRobotFrankaSim::set_eMc( const vpHomogeneousMatrix &eMc )
158 {
159  std::lock_guard< std::mutex > lock( m_mutex );
160  m_eMc = eMc;
161  m_eVc.buildFrom( m_eMc );
162  m_camMounted = true;
163 }
164 
171 void
172 vpRobotFrankaSim::set_flMe( const vpHomogeneousMatrix &flMe )
173 {
174  std::lock_guard< std::mutex > lock( m_mutex );
175 
176 #ifdef VISP_HAVE_OROCOS_KDL
177  KDL::Frame frame8Mfl_kdl;
178  frame8Mfl_kdl = m_chain_kdl.segments[7].getFrameToTip();
179  vpHomogeneousMatrix f8Mfl, f8Me;
180  for ( unsigned int i = 0; i < 3; i++ )
181  {
182  for ( unsigned int j = 0; j < 3; j++ )
183  {
184  f8Mfl[i][j] = frame8Mfl_kdl.M.data[3 * i + j];
185  }
186  f8Mfl[i][3] = frame8Mfl_kdl.p.data[i];
187  }
188  f8Me = f8Mfl * m_flMe.inverse() * flMe;
189 
190  KDL::Rotation f8Re( f8Me[0][0], f8Me[0][1], f8Me[0][2], f8Me[1][0], f8Me[1][1], f8Me[0][2], f8Me[2][0], f8Me[2][1],
191  f8Me[2][2] );
192  KDL::Vector f8te( f8Me[0][3], f8Me[1][3], f8Me[2][3] );
193  KDL::Frame f8Me_kdl( f8Re, f8te );
194 
195  m_chain_kdl.segments[7].setFrameToTip( f8Me_kdl );
196  m_flMe = flMe;
197 #endif
198 }
199 
204 void
205 vpRobotFrankaSim::set_g0( const vpColVector &g0 )
206 {
207  std::lock_guard< std::mutex > lock( m_mutex );
208  m_g0 = g0;
209 }
210 
221 void
222 vpRobotFrankaSim::add_tool( const vpHomogeneousMatrix &flMe, const double mL, const vpHomogeneousMatrix &flMcom,
223  const vpMatrix &I_L )
224 {
225  this->set_flMe( flMe );
226  std::lock_guard< std::mutex > lock( m_mutex );
227  if ( mL < 0.0 )
228  {
229  std::cout << "Mass cannot be negative! \nmL = " << mL << " not assigned \n";
230  }
231  else
232  {
233  m_mL = mL;
234  }
235  m_flMcom = flMcom;
236  m_Il = I_L;
237  m_toolMounted = true;
238 
239  if ( m_verbose )
240  {
241  std::cout << "A tool has been mounted on the robot.\n";
242  std::cout << "Mass: " << m_mL << " [kg]\n";
243  std::cout << "Inertia Tensor in flange frame:\n" << m_Il << " [kg*m^2]\n";
244  std::cout << "CoM position in flange frame: " << m_flMcom.getTranslationVector().t() << " [m]\n";
245  std::cout << "CoM orientation in flange frame: \n" << m_flMcom.getRotationMatrix() << std::endl;
246  }
247 }
248 
253 vpRobot::vpRobotStateType
255 {
256  return m_stateRobot;
257 }
258 
263 void
265 {
266  fJe.reshape( 6, 7 );
267 
268 #ifdef VISP_HAVE_OROCOS_KDL
269  KDL::Jacobian Jac( 7 );
270 
271  m_mutex.lock();
273  m_mutex.unlock();
274 
275  for ( unsigned int i = 0; i < 6; i++ )
276  {
277  for ( unsigned int j = 0; j < 7; j++ )
278  {
279  fJe[i][j] = Jac.data( i, j );
280  }
281  }
282 #endif
283 }
284 
290 void
291 vpRobotFrankaSim::get_fJe( const vpColVector &q, vpMatrix &fJe )
292 {
293  if ( q.size() != 7 )
294  {
295  throw( vpException( vpException::dimensionError, "Joint position vector is not a 7-dim vector (%d)", q.size() ) );
296  }
297 
298  fJe.reshape( 6, 7 );
299 
300 #ifdef VISP_HAVE_OROCOS_KDL
301  KDL::JntArray jnts = KDL::JntArray( 7 );
302  KDL::Jacobian Jac( 7 );
303 
304  for ( unsigned int i = 0; i < 7; i++ )
305  {
306  jnts( i ) = q[i];
307  }
308  m_jacobianSolver_kdl->JntToJac( jnts, Jac );
309 
310  for ( unsigned int i = 0; i < 6; i++ )
311  {
312  for ( unsigned int j = 0; j < 7; j++ )
313  {
314  fJe[i][j] = Jac.data( i, j );
315  }
316  }
317 #endif
318 }
319 
325 void
327 {
328  vpMatrix fJe( 6, 7 );
329  get_fJe( fJe );
330  vpHomogeneousMatrix fMe( get_fMe() );
331  vpMatrix eVf( 6, 6 );
332  eVf.insert( fMe.getRotationMatrix().t(), 0, 0 );
333  eVf.insert( fMe.getRotationMatrix().t(), 3, 3 );
334 
335  eJe = eVf * fJe;
336 }
337 
343 void
344 vpRobotFrankaSim::get_eJe( const vpColVector &q, vpMatrix &eJe )
345 {
346  if ( q.size() != 7 )
347  {
348  throw( vpException( vpException::dimensionError, "Joint position vector is not a 7-dim vector (%d)", q.size() ) );
349  }
350 
351  vpMatrix fJe( 6, 7 );
352  get_fJe( q, fJe );
353  vpHomogeneousMatrix fMe( get_fMe( q ) );
354  vpMatrix eVf( 6, 6 );
355  eVf.insert( fMe.getRotationMatrix().t(), 0, 0 );
356  eVf.insert( fMe.getRotationMatrix().t(), 3, 3 );
357 
358  eJe = eVf * fJe;
359 }
360 
365 vpHomogeneousMatrix
367 {
368  return m_eMc;
369 }
370 
375 vpHomogeneousMatrix
377 {
378  return m_flMe;
379 }
380 
385 vpHomogeneousMatrix
387 {
388  return m_flMcom;
389 }
390 
395 double
397 {
398  return m_mL;
399 }
400 
416 void
417 vpRobotFrankaSim::getPosition( const vpRobot::vpControlFrameType frame, vpColVector &position )
418 {
419  switch ( frame )
420  {
421  case vpRobot::JOINT_STATE:
422  { // Same as ARTICULAR_FRAME
423  position.resize( 7 );
424  std::lock_guard< std::mutex > lock( m_mutex );
425  position = m_q;
426  break;
427  }
428  case vpRobot::END_EFFECTOR_FRAME:
429  {
430  position.resize( 6 );
431 
432  vpPoseVector fPe( get_fMe() );
433  for ( unsigned int i = 0; i < 6; i++ )
434  {
435  position[i] = fPe[i];
436  }
437 
438  break;
439  }
440  case vpRobot::CAMERA_FRAME:
441  { // same as TOOL_FRAME
442  position.resize( 6 );
443  vpPoseVector fPc( get_fMe() * m_eMc );
444  for ( unsigned int i = 0; i < 6; i++ )
445  {
446  position[i] = fPc[i];
447  }
448  break;
449  }
450  default:
451  {
452  throw( vpException( vpException::fatalError, "Cannot get Franka cartesian position: wrong method" ) );
453  }
454  }
455 }
456 
471 void
472 vpRobotFrankaSim::getPosition( const vpRobot::vpControlFrameType frame, vpPoseVector &position )
473 {
474  vpColVector pose( 6, 0 );
475  if ( frame == vpRobot::END_EFFECTOR_FRAME || frame == vpRobot::CAMERA_FRAME )
476  {
477  getPosition( frame, pose );
478  for ( unsigned int i = 0; i < 6; i++ )
479  {
480  position[i] = pose[i];
481  }
482  }
483  else
484  {
485  throw( vpException( vpException::fatalError, "Cannot get a cartesian position for the specified frame" ) );
486  }
487 }
488 
504 void
505 vpRobotFrankaSim::setPosition( const vpRobot::vpControlFrameType frame, const vpColVector &position )
506 {
507  switch ( frame )
508  {
509  case vpRobot::JOINT_STATE:
510  {
511  if ( position.size() != 7 )
512  {
513  throw( vpException( vpException::dimensionError, "Joint position vector is not a 7-dim vector (%d)",
514  position.size() ) );
515  }
516  std::lock_guard< std::mutex > lock( m_mutex );
517  m_q_des = position;
518 
519  break;
520  }
521  case vpRobot::END_EFFECTOR_FRAME:
522  {
523  if ( position.size() != 6 )
524  {
525  throw( vpException( vpException::dimensionError, "Cartesian position vector is not a 6-dim vector (%d)",
526  position.size() ) );
527  }
528  vpHomogeneousMatrix wMe;
529  wMe.buildFrom( position[0], position[1], position[2], position[3], position[4], position[5] );
530  vpColVector q_des = solveIK( wMe );
531  std::lock_guard< std::mutex > lock( m_mutex );
532  m_q_des = q_des;
533 
534  break;
535  }
536  case vpRobot::CAMERA_FRAME:
537  {
538  if ( position.size() != 6 )
539  {
540  throw( vpException( vpException::dimensionError, "Cartesian position vector is not a 6-dim vector (%d)",
541  position.size() ) );
542  }
543  vpHomogeneousMatrix wMc;
544  wMc.buildFrom( position[0], position[1], position[2], position[3], position[4], position[5] );
545 
546  vpHomogeneousMatrix wMe = wMc * m_eMc.inverse();
547  vpColVector q_des = solveIK( wMe );
548  std::lock_guard< std::mutex > lock( m_mutex );
549  m_q_des = q_des;
550 
551  break;
552  }
553  default:
554  {
555  throw( vpException( vpException::fatalError, "Franka positioning frame is not implemented" ) );
556  }
557  }
558 }
559 
565 vpColVector
566 vpRobotFrankaSim::solveIK( const vpHomogeneousMatrix &wMe )
567 {
568  vpColVector q_solved( 7, 0 );
569 #ifdef VISP_HAVE_OROCOS_KDL
570  KDL::JntArray q_out( 7 );
571  KDL::Rotation wRe( wMe[0][0], wMe[0][1], wMe[0][2], wMe[1][0], wMe[1][1], wMe[1][2], wMe[2][0], wMe[2][1],
572  wMe[2][2] );
573 
574  KDL::Vector wte( wMe[0][3], wMe[1][3], wMe[2][3] );
575  KDL::Frame wMe_kdl( wRe, wte );
576  m_mutex.lock();
577  int ret = m_iksolver_JL_kdl->CartToJnt( m_q_kdl, wMe_kdl, q_out );
578  m_mutex.unlock();
579  switch ( ret )
580  {
582  {
583  std::cout << "solveIK: E_NOERROR" << std::endl;
584  break;
585  }
587  {
588  std::cout << "solveIK: E_MAX_ITERATIONS_EXCEEDED" << std::endl;
589  break;
590  }
592  {
593  std::cout << "solveIK: E_NOT_IMPLEMENTED" << std::endl;
594  break;
595  }
596  default:
597  {
598  throw( vpException( vpException::fatalError, "Error: unable to solve ik\n" ) );
599  }
600  }
601 
602  for ( unsigned int i = 0; i < 7; i++ )
603  {
604  q_solved[i] = q_out( i );
605  }
606 #endif
607 
608  return q_solved;
609 }
610 
626 void
627 vpRobotFrankaSim::getVelocity( const vpRobot::vpControlFrameType frame, vpColVector &velocity )
628 {
629  switch ( frame )
630  {
631  case vpRobot::JOINT_STATE:
632  {
633  velocity.resize( 7 );
634  std::lock_guard< std::mutex > lock( m_mutex );
635  velocity = m_dq;
636  break;
637  }
638  case vpRobot::END_EFFECTOR_FRAME:
639  {
640  velocity.resize( 6 );
641  vpMatrix eJe( 6, 7 );
642  this->get_eJe( eJe );
643  std::lock_guard< std::mutex > lock( m_mutex );
644  velocity = eJe * m_dq;
645  break;
646  }
647  case vpRobot::REFERENCE_FRAME:
648  {
649  velocity.resize( 6 );
650  vpMatrix fJe( 6, 7 );
651  this->get_fJe( fJe );
652  std::lock_guard< std::mutex > lock( m_mutex );
653  velocity = fJe * m_dq;
654  break;
655  }
656  default:
657  {
658  throw( vpException( vpException::fatalError, "Cannot get Franka velocity in the specified frame" ) );
659  }
660  }
661 }
662 
682 void
683 vpRobotFrankaSim::setVelocity( const vpRobot::vpControlFrameType frame, const vpColVector &velocity )
684 {
685  if ( vpRobot::STATE_VELOCITY_CONTROL != getRobotState() )
686  {
687  std::cout << "Cannot send a velocity to the robot. "
688  "Use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first. \n";
689  }
690 
691  switch ( frame )
692  {
693  case vpRobot::JOINT_STATE:
694  {
695  if ( velocity.size() != 7 )
696  {
697  std::cout << "Joint velocity vector " << velocity.size() << " is not of size 7 \n";
698  }
699  std::lock_guard< std::mutex > lock( m_mutex );
700  m_dq_des = velocity;
701  break;
702  }
703 
704  case vpRobot::REFERENCE_FRAME:
705  {
706  if ( velocity.size() != 6 )
707  {
708  std::cout << "Cartesian velocity vector " << velocity.size() << " is not of size 6 \n";
709  }
710  vpColVector vel_max( 6 );
711 
712  for ( unsigned int i = 0; i < 3; i++ )
713  {
714  vel_max[i] = 1.7;
715  vel_max[3 + i] = 2.5;
716  }
717  // velocities are expressed in Base frame
718  m_v_cart_des = vpRobot::saturateVelocities( velocity, vel_max, true );
719 
720 #ifdef VISP_HAVE_OROCOS_KDL
721  KDL::Twist v_cart;
722  for ( unsigned int i = 0; i < 3; i++ )
723  {
724  v_cart.vel.data[i] = m_v_cart_des[i];
725  v_cart.rot.data[i] = m_v_cart_des[i + 3];
726  }
727  std::lock_guard< std::mutex > lock( m_mutex );
729  for ( unsigned int i = 0; i < 7; i++ )
730  {
731  m_dq_des[i] = m_dq_des_kdl.data( i );
732  }
733 #endif
734  break;
735  }
736 
737  case vpRobot::END_EFFECTOR_FRAME:
738  {
739  if ( velocity.size() != 6 )
740  {
741  std::cout << "Cartesian velocity vector " << velocity.size() << " is not of size 6 \n";
742  }
743  // Apply Cartesian velocity limits according to the specifications
744  vpColVector vel_max( 6 );
745  for ( unsigned int i = 0; i < 3; i++ )
746  {
747  vel_max[i] = 1.7;
748  vel_max[3 + i] = 2.5;
749  }
750  // Refer End-Effector velocities in Base frame
751  vpHomogeneousMatrix fMe = this->get_fMe();
752  vpVelocityTwistMatrix fVe( fMe, false );
753  m_v_cart_des = fVe * vpRobot::saturateVelocities( velocity, vel_max, true );
754 
755 #ifdef VISP_HAVE_OROCOS_KDL
756  KDL::Twist v_cart;
757  for ( unsigned int i = 0; i < 3; i++ )
758  {
759  v_cart.vel.data[i] = m_v_cart_des[i];
760  v_cart.rot.data[i] = m_v_cart_des[i + 3];
761  }
762 
763  std::lock_guard< std::mutex > lock( m_mutex );
765  for ( unsigned int i = 0; i < 7; i++ )
766  {
767  m_dq_des[i] = m_dq_des_kdl.data( i );
768  }
769 #endif
770  break;
771  }
772  case vpRobot::CAMERA_FRAME:
773  {
774  if ( velocity.size() != 6 )
775  {
776  std::cout << "Cartesian velocity vector " << velocity.size() << " is not of size 6 \n";
777  }
778  // Apply Cartesian velocity limits according to the specifications
779  vpColVector vel_max( 6 );
780  for ( unsigned int i = 0; i < 3; i++ )
781  {
782  vel_max[i] = 1.7;
783  vel_max[3 + i] = 2.5;
784  }
785  // Refer End-Effector velocities in Base frame
786  vpHomogeneousMatrix fMe = this->get_fMe();
787  vpVelocityTwistMatrix fWe( fMe, false );
788  std::lock_guard< std::mutex > lock( m_mutex );
789  m_v_cart_des = vpRobot::saturateVelocities( fWe * m_eVc * velocity, vel_max, true );
790 
791 #ifdef VISP_HAVE_OROCOS_KDL
792  KDL::Twist v_cart;
793  for ( unsigned int i = 0; i < 3; i++ )
794  {
795  v_cart.vel.data[i] = m_v_cart_des[i];
796  v_cart.rot.data[i] = m_v_cart_des[i + 3];
797  }
798 
800  for ( unsigned int i = 0; i < 7; i++ )
801  {
802  m_dq_des[i] = m_dq_des_kdl.data( i );
803  }
804 #endif
805  break;
806  }
807  case vpRobot::MIXT_FRAME:
808  throw( vpException( vpException::functionNotImplementedError, "MIXT_FRAME is not implemented" ) );
809  }
810 }
811 
824 void
825 vpRobotFrankaSim::getForceTorque( const vpRobot::vpControlFrameType frame, vpColVector &force )
826 {
827  switch ( frame )
828  {
829  case vpRobot::JOINT_STATE:
830  {
831  force.resize( 7 );
832  std::lock_guard< std::mutex > lock( m_mutex );
833  force = m_tau_J;
834  break;
835  }
836  case vpRobot::END_EFFECTOR_FRAME:
837  {
838  force.resize( 6 );
839  vpMatrix eJe;
840  this->get_eJe( eJe );
841  std::lock_guard< std::mutex > lock( m_mutex );
842  force = eJe.transpose().pseudoInverse() * m_tau_J;
843  break;
844  }
845  case vpRobot::REFERENCE_FRAME:
846  {
847  force.resize( 6 );
848  vpMatrix fJe;
849  this->get_fJe( fJe );
850  std::lock_guard< std::mutex > lock( m_mutex );
851  force = fJe.transpose().pseudoInverse() * m_tau_J;
852  break;
853  }
854 
855  default:
856  {
857  throw( vpException( vpException::fatalError, "Cannot get Franka position for the specified frame " ) );
858  }
859  }
860 }
861 
874 void
875 vpRobotFrankaSim::setForceTorque( const vpRobot::vpControlFrameType frame, const vpColVector &force )
876 {
877  if ( vpRobot::STATE_FORCE_TORQUE_CONTROL != getRobotState() )
878  {
879  std::cout << "Cannot send a torque command to the robot. "
880  "Use setRobotState(vpRobot::STATE_FORCE_TORQUE_CONTROL) first. \n";
881  }
882 
883  switch ( frame )
884  {
885  // Saturation in joint space
886  case vpRobot::JOINT_STATE:
887  {
888  if ( force.size() != 7 )
889  {
890  std::cout << "Joint velocity vector " << force.size() << " is not of size 7 \n";
891  }
892  std::lock_guard< std::mutex > lock( m_mutex );
893  m_tau_J_des = force;
894 
895  break;
896  }
897 
898  case vpRobot::REFERENCE_FRAME:
899  {
900  if ( force.size() != 6 )
901  {
902  std::cout << "Cartesian velocity vector " << force.size() << " is not of size 6 \n";
903  }
904  vpMatrix fJe( 6, 7 );
905  this->get_fJe( fJe );
906  std::lock_guard< std::mutex > lock( m_mutex );
907  m_tau_J_des = fJe.t() * force;
908  break;
909  }
910 
911  case vpRobot::END_EFFECTOR_FRAME:
912  {
913  if ( force.size() != 6 )
914  {
915  std::cout << "Cartesian velocity vector " << force.size() << " is not of size 6 \n";
916  }
917  vpMatrix eJe( 6, 7 );
918  this->get_eJe( eJe );
919  std::lock_guard< std::mutex > lock( m_mutex );
920  m_tau_J_des = eJe.t() * force;
921  break;
922  }
923  case vpRobot::CAMERA_FRAME:
924  {
925  throw( vpException( vpException::functionNotImplementedError,
926  "force/torque control in camera frame is not implemented" ) );
927  }
928  case vpRobot::MIXT_FRAME:
929  {
930  throw( vpException( vpException::functionNotImplementedError,
931  "force/torque control in mixt frame is not implemented" ) );
932  }
933  }
934 }
935 
941 vpHomogeneousMatrix
943 {
944  vpHomogeneousMatrix fMe;
945 
946 #ifdef VISP_HAVE_OROCOS_KDL
947  KDL::Frame cartpos;
948  // Calculate forward kinematics
949  int kinematics_status;
950  vpRotationMatrix R;
951  vpTranslationVector t;
952  std::lock_guard< std::mutex > lock( m_mutex );
953  kinematics_status = m_fksolver_kdl->JntToCart( m_q_kdl, cartpos );
954  if ( kinematics_status >= 0 )
955  {
956  for ( unsigned int i = 0; i < 3; i++ )
957  {
958  for ( unsigned int j = 0; j < 3; j++ )
959  {
960  R[i][j] = cartpos.M.data[3 * i + j];
961  }
962  t[i] = cartpos.p.data[i];
963  }
964  fMe.buildFrom( t, R );
965  }
966 #endif
967 
968  return fMe;
969 }
970 
977 vpHomogeneousMatrix
978 vpRobotFrankaSim::get_fMe( const vpColVector &q )
979 {
980  if ( q.size() != 7 )
981  {
982  throw( vpException( vpException::dimensionError, "Joint position vector is not a 7-dim vector (%d)", q.size() ) );
983  }
984  vpRotationMatrix R;
985  vpTranslationVector t;
986 
987 #ifdef VISP_HAVE_OROCOS_KDL
988  KDL::Frame cartpos;
989  KDL::JntArray qq( 7 );
990  for ( unsigned int i = 0; i < 7; i++ )
991  {
992  qq( i ) = q[i];
993  }
994  // Calculate forward kinematics
995  int kinematics_status;
996  kinematics_status = m_fksolver_kdl->JntToCart( qq, cartpos );
997  if ( kinematics_status >= 0 )
998  {
999  for ( unsigned int i = 0; i < 3; i++ )
1000  {
1001  for ( unsigned int j = 0; j < 3; j++ )
1002  {
1003  R[i][j] = cartpos.M.data[3 * i + j];
1004  }
1005  t[i] = cartpos.p.data[i];
1006  }
1007  }
1008 #endif
1009 
1010  vpHomogeneousMatrix fMe( t, R );
1011  return fMe;
1012 }
1013 
1018 void
1019 vpRobotFrankaSim::getMass( vpMatrix &mass )
1020 {
1021  std::lock_guard< std::mutex > lock( m_mutex );
1023 }
1024 
1029 void
1030 vpRobotFrankaSim::getGravity( vpColVector &gravity )
1031 {
1032  std::lock_guard< std::mutex > lock( m_mutex );
1034 }
1035 
1041 void
1042 vpRobotFrankaSim::getCoriolis( vpColVector &coriolis )
1043 {
1044  std::lock_guard< std::mutex > lock( m_mutex );
1045  vpMatrix C( 7, 7 );
1047  coriolis = C * m_dq;
1048 }
1049 
1055 void
1057 {
1058  std::lock_guard< std::mutex > lock( m_mutex );
1060 }
1061 
1062 #elif !defined( VISP_BUILD_SHARED_LIBS )
1063 // Work arround to avoid warning: lib*.a(vpRobotFrankaSim.cpp.o) has
1064 // no symbols
1065 void
1066 dummy_vpRobotFrankaSim(){};
1067 #endif
virtual void set_g0(const vpColVector &g0)
vpHomogeneousMatrix get_eMc() const
vpColVector friction(const vpColVector &dq)
void getFriction(vpColVector &friction)
virtual void set_eMc(const vpHomogeneousMatrix &eMc)
virtual int JntToCart(const JntArray &q_in, Frame &p_out, int segmentNr=-1)
virtual void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
virtual void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &position)
vpHomogeneousMatrix get_flMe() const
Vector vel
virtual void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &d_position)
vpColVector m_dq_des_filt
virtual void setForceTorque(const vpRobot::vpControlFrameType frame, const vpColVector &force)
void addSegment(const Segment &segment)
virtual int CartToJnt(const JntArray &q_in, const Twist &v_in, JntArray &qdot_out)
void getCoriolis(vpColVector &coriolis)
vpColVector m_tau_J_des_filt
void getGravity(vpColVector &gravity)
vpColVector gravityVector(const vpColVector &q, const double mL=0, const vpHomogeneousMatrix &flMcom=vpHomogeneousMatrix(), const vpColVector &g0=vpColVector({0.0, 0.0,-9.80665}))
vpColVector m_tau_J_des
KDL::Chain m_chain_kdl
KDL::JntArray m_q_max_kdl
Rotation M
EIGEN_MAKE_ALIGNED_OPERATOR_NEW Eigen::Matrix< double, 6, Eigen::Dynamic > data
vpRobot::vpRobotStateType getRobotState(void)
static Frame DH_Craig1989(double a, double alpha, double d, double theta)
vpMatrix coriolisMatrix(const vpColVector &q, const vpColVector &dq, const double mL, const vpHomogeneousMatrix &flMcom, const vpMatrix &I_L)
KDL::JntArray m_dq_des_kdl
KDL::ChainFkSolverPos_recursive * m_fksolver_kdl
Vector rot
virtual void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position)
double data[3]
virtual void set_flMe(const vpHomogeneousMatrix &flMe)
Eigen::VectorXd data
KDL::JntArray m_q_min_kdl
vpColVector m_dq_des
KDL::ChainIkSolverVel_pinv * m_diffIkSolver_kdl
vpHomogeneousMatrix get_fMe()
void getCoriolisMatrix(vpMatrix &coriolis)
vpVelocityTwistMatrix m_eVc
void get_fJe(vpMatrix &fJe)
vpHomogeneousMatrix get_flMcom() const
KDL::ChainJntToJacSolver * m_jacobianSolver_kdl
std::vector< Segment > segments
vpColVector m_v_cart_des
vpHomogeneousMatrix m_flMcom
vpColVector solveIK(const vpHomogeneousMatrix &edMw)
virtual void getForceTorque(const vpRobot::vpControlFrameType frame, vpColVector &force)
vpMatrix massMatrix(const vpColVector &q, const double mL=0, const vpHomogeneousMatrix &flMcom=vpHomogeneousMatrix(), const vpMatrix &I_L=vpMatrix(3, 3))
Definition: MassMatrix.cpp:25
double get_tool_mass() const
virtual int JntToJac(const JntArray &q_in, Jacobian &jac, int seg_nr=-1)
KDL::JntArray m_q_kdl
vpHomogeneousMatrix m_eMc
vpHomogeneousMatrix m_flMe
void getMass(vpMatrix &mass)
virtual int CartToJnt(const JntArray &q_init, const Frame &p_in, JntArray &q_out)
vpRobot::vpRobotStateType m_stateRobot
void get_eJe(vpMatrix &eJe_)
vpColVector getVelDes()
virtual void add_tool(const vpHomogeneousMatrix &flMe, const double mL, const vpHomogeneousMatrix &flMcom, const vpMatrix &I_L)
KDL::ChainIkSolverPos_NR_JL * m_iksolver_JL_kdl
double data[9]


visp_ros
Author(s): Francois Pasteau, Fabien Spindler, Gatien Gaumerais, Alexander Oliva
autogenerated on Tue Mar 1 2022 00:03:22