tutorial-franka-coppeliasim-joint-impedance-control.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2021 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See https://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  *****************************************************************************/
32 
34 
35 #include <iostream>
36 
37 #include <visp3/gui/vpPlot.h>
39 
40 int
41 main( int argc, char **argv )
42 {
43  bool opt_coppeliasim_sync_mode = false;
44  bool opt_verbose = false;
45  bool opt_save_data = false;
46 
47  for ( int i = 1; i < argc; i++ )
48  {
49  if ( std::string( argv[i] ) == "--enable-coppeliasim-sync-mode" )
50  {
51  opt_coppeliasim_sync_mode = true;
52  }
53  else if ( std::string( argv[i] ) == "--verbose" || std::string( argv[i] ) == "-v" )
54  {
55  opt_verbose = true;
56  }
57  else if ( std::string( argv[i] ) == "--save" )
58  {
59  opt_save_data = true;
60  }
61  else if ( std::string( argv[i] ) == "--help" || std::string( argv[i] ) == "-h" )
62  {
63  std::cout << argv[0] << "[--enable-coppeliasim-sync-mode]"
64  << " [--save]"
65  << " [--verbose] [-v] "
66  << " [--help] [-h]" << std::endl;
67  return EXIT_SUCCESS;
68  }
69  }
70 
72  try
73  {
74  // ROS node //
75  ros::init( argc, argv, "visp_ros" );
76  ros::NodeHandlePtr n = boost::make_shared< ros::NodeHandle >();
77  ros::Rate loop_rate( 1000 );
78  ros::spinOnce();
79 
80  robot.setVerbose( opt_verbose );
81  if ( 0 )
82  {
83  // Instead of setting the tool from Coppeliasim topics, we can set its values manually to e.g. introduce errors in
84  // the parameters This must be done before call robot.connect() The following are the parameters for the Panda
85  // Hand as reported in the datasheet.
86  double mass = 0.73; // [kg]
87  vpHomogeneousMatrix flMe(
88  vpTranslationVector( 0, 0, 0.1034 ),
89  vpRotationMatrix( { std::cos( vpMath::rad( 45 ) ), -std::sin( vpMath::rad( 45 ) ), 0,
90  std::sin( vpMath::rad( 45 ) ), std::cos( vpMath::rad( 45 ) ), 0, 0, 0, 1 } ) );
91  vpHomogeneousMatrix fMcom( vpTranslationVector( -0.01, 0.0, 0.03 ), vpRotationMatrix() );
92  vpMatrix I_l( 3, 3 );
93  I_l.diag( vpColVector( { 0.001, 0.0025, 0.0017 } ) ); // [kg*m^2]
94  robot.add_tool( flMe, mass, fMcom, I_l );
95  }
96 
97  robot.connect();
98 
99  std::cout << "Coppeliasim sync mode enabled: " << ( opt_coppeliasim_sync_mode ? "yes" : "no" ) << std::endl;
100  robot.coppeliasimStopSimulation(); // Allows to reset simulation, moving the robot to initial position
101  robot.setCoppeliasimSyncMode( false );
103 
104  // Move to a secure initial position
105  vpColVector q_init( { 0, vpMath::rad( -45 ), 0, vpMath::rad( -135 ), 0, vpMath::rad( 90 ), vpMath::rad( 45 ) } );
106 
107  robot.setRobotState( vpRobot::STATE_POSITION_CONTROL );
108  robot.setPosition( vpRobot::JOINT_STATE, q_init );
109  vpTime::wait( 500 );
110 
111  vpPlot *plotter = nullptr;
112 
113  plotter = new vpPlot( 4, 800, 800, 10, 10, "Real time curves plotter" );
114  plotter->setTitle( 0, "Joint positions [rad]" );
115  plotter->initGraph( 0, 7 );
116  plotter->setLegend( 0, 0, "q1" );
117  plotter->setLegend( 0, 1, "q2" );
118  plotter->setLegend( 0, 2, "q3" );
119  plotter->setLegend( 0, 3, "q4" );
120  plotter->setLegend( 0, 4, "q5" );
121  plotter->setLegend( 0, 5, "q6" );
122  plotter->setLegend( 0, 6, "q7" );
123 
124  plotter->setTitle( 1, "Joint position error [rad]" );
125  plotter->initGraph( 1, 7 );
126  plotter->setLegend( 1, 0, "e_q1" );
127  plotter->setLegend( 1, 1, "e_q2" );
128  plotter->setLegend( 1, 2, "e_q3" );
129  plotter->setLegend( 1, 3, "e_q4" );
130  plotter->setLegend( 1, 4, "e_q5" );
131  plotter->setLegend( 1, 5, "e_q6" );
132  plotter->setLegend( 1, 6, "e_q7" );
133 
134  plotter->setTitle( 2, "Joint torque measure [Nm]" );
135  plotter->initGraph( 2, 7 );
136  plotter->setLegend( 2, 0, "Tau1" );
137  plotter->setLegend( 2, 1, "Tau2" );
138  plotter->setLegend( 2, 2, "Tau3" );
139  plotter->setLegend( 2, 3, "Tau4" );
140  plotter->setLegend( 2, 4, "Tau5" );
141  plotter->setLegend( 2, 5, "Tau6" );
142  plotter->setLegend( 2, 6, "Tau7" );
143 
144  plotter->setTitle( 3, "Joint error norm [rad]" );
145  plotter->initGraph( 3, 1 );
146  plotter->setLegend( 3, 0, "||qd - d||" );
147 
148  // Create joint array
149  vpColVector q( 7, 0 ), qd( 7, 0 ), dq( 7, 0 ), dqd( 7, 0 ), ddqd( 7, 0 ), tau_d( 7, 0 ), C( 7, 0 ), q0( 7, 0 ),
150  F( 7, 0 ), tau_d0( 7, 0 ), tau_cmd( 7, 0 ), tau( 7, 0 );
151  vpMatrix B( 7, 7 );
152 
153  std::cout << "Reading current joint position" << std::endl;
154  robot.getPosition( vpRobot::JOINT_STATE, q0 );
155  std::cout << "Initial joint position: " << q0.t() << std::endl;
156 
157  robot.setRobotState( vpRobot::STATE_FORCE_TORQUE_CONTROL );
158  robot.setCoppeliasimSyncMode( opt_coppeliasim_sync_mode );
159 
160  qd = q0;
161 
162  bool final_quit = false;
163  bool first_time = false;
164  bool start_trajectory = false;
165 
166  vpMatrix K( 7, 7 ), D( 7, 7 ), I( 7, 7 );
167  K.diag( { 400.0, 400.0, 400.0, 400.0, 400.0, 400.0, 900.0 } );
168  D.diag( { 20.0, 45.0, 45.0, 45.0, 45.0, 45.0, 60.0 } );
169  I.diag( { 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 60.0 } );
170 
171  vpColVector integral( 7, 0 );
172 
173  double mu = 4;
174  double dt = 0;
175 
176  double time_start_trajectory, time_prev, time_cur;
177  double delay_before_trajectory = 0.5; // Start sinusoidal joint trajectory after this delay in [s]
178 
179  // Control loop
180  while ( !final_quit )
181  {
182  time_cur = robot.getCoppeliasimSimulationTime();
183 
184  robot.getPosition( vpRobot::JOINT_STATE, q );
185  robot.getVelocity( vpRobot::JOINT_STATE, dq );
186  robot.getForceTorque( vpRobot::JOINT_STATE, tau );
187  robot.getMass( B );
188  robot.getCoriolis( C );
189  robot.getFriction( F );
190 
191  if ( time_cur < delay_before_trajectory )
192  {
193  time_start_trajectory = time_cur; // To ensure exp() = 1
194  first_time = true;
195  }
196  else if ( !start_trajectory ) // After the delay we start joint trajectory
197  {
198  time_start_trajectory = time_cur;
199  start_trajectory = true;
200  }
201 
202  // Compute joint trajectories
203  // clang-format off
204  qd[0] = q0[0] + ( start_trajectory ? std::sin( 2 * M_PI * 0.1 * ( time_cur - time_start_trajectory ) ) : 0 );
205  dqd[0] = ( start_trajectory ? 2 * M_PI * 0.1 * std::cos( 2 * M_PI * 0.1 * ( time_cur - time_start_trajectory ) ) : 0 );
206  ddqd[0] = ( start_trajectory ? - std::pow( 2 * 0.1 * M_PI, 2 ) * std::sin( 2 * M_PI * 0.1 * ( time_cur - time_start_trajectory ) ) : 0 );
207 
208  qd[2] = q0[2] + ( start_trajectory ? ( M_PI / 16 ) * std::sin( 2 * M_PI * 0.2 * ( time_cur - time_start_trajectory ) ) : 0 );
209  dqd[2] = ( start_trajectory ? M_PI * ( M_PI / 8 ) * 0.2 * std::cos( 2 * M_PI * 0.2 * ( time_cur - time_start_trajectory ) ) : 0 );
210  ddqd[2] = ( start_trajectory ? -M_PI * M_PI * 0.2 * ( M_PI / 4 ) * std::sin( 2 * M_PI * 0.2 * ( time_cur - time_start_trajectory ) ) : 0 );
211 
212  qd[3] = q0[3] + ( start_trajectory ? 0.25 * std::sin( 2 * M_PI * 0.05 * ( time_cur - time_start_trajectory ) ) : 0 );
213  dqd[3] = ( start_trajectory ? 2 * M_PI * 0.05 * 0.25 * std::cos( 2 * M_PI * 0.05 * ( time_cur - time_start_trajectory ) ) : 0 );
214  ddqd[3] = ( start_trajectory ? -0.25 * std::pow( 2 * 0.05 * M_PI, 2 ) * std::sin( 2 * M_PI * 0.05 * ( time_cur - time_start_trajectory ) ) : 0 );
215 
216  //qd[6] = q0[6] + ( start_trajectory ? std::sin( 2 * M_PI * 0.1 * ( time_cur - time_start_trajectory ) ) : 0 );
217  //dqd[6] = ( start_trajectory ? 2 * M_PI * 0.1 * std::cos( 2 * M_PI * 0.1 * ( time_cur - time_start_trajectory ) ) : 0 );
218  //ddqd[6] = ( start_trajectory ? -std::pow( 2 * 0.1 * M_PI, 2 ) * std::sin( 2 * M_PI * 0.1 * ( time_cur - time_start_trajectory ) ) : 0 );
219  // clang-format on
220 
221  dt = time_cur - time_prev;
222  if ( start_trajectory )
223  {
224  integral += ( qd - q ) * dt;
225  }
226 
227  // Compute the control law
228  tau_d = B * ( K * ( qd - q ) + D * ( dqd - dq ) + I * ( integral ) + ddqd ) + C + F;
229 
230  if ( first_time )
231  {
232  tau_d0 = tau_d;
233  }
234 
235  tau_cmd = tau_d - tau_d0 * std::exp( -mu * ( time_cur - time_start_trajectory ) );
236 
237  // Send command to the torque robot
238  robot.setForceTorque( vpRobot::JOINT_STATE, tau_cmd );
239 
240  vpColVector norm( 1, vpMath::deg( std::sqrt( ( qd - q ).sumSquare() ) ) );
241  plotter->plot( 0, time_cur, q );
242  plotter->plot( 1, time_cur, qd - q );
243  plotter->plot( 2, time_cur, tau );
244  plotter->plot( 3, time_cur, norm );
245  vpMouseButton::vpMouseButtonType button;
246  if ( vpDisplay::getClick( plotter->I, button, false ) )
247  {
248  if ( button == vpMouseButton::button3 )
249  {
250  final_quit = true;
251  tau_cmd = 0;
252  std::cout << "Stop the robot " << std::endl;
253  robot.setRobotState( vpRobot::STATE_STOP );
254  }
255  }
256 
257  if ( opt_verbose )
258  {
259  std::cout << "dt: " << dt << std::endl;
260  }
261 
262  time_prev = time_cur;
263  robot.wait( time_cur, 0.001 ); // Sync loop at 1000 Hz (1 ms)
264  }
265 
266  if ( opt_save_data )
267  {
268  plotter->saveData( 0, "sim-joint-position.txt", "# " );
269  plotter->saveData( 1, "sim-joint-position-error.txt", "# " );
270  plotter->saveData( 2, "sim-joint-torque-measure.txt", "# " );
271  plotter->saveData( 3, "sim-joint-error-norm.txt", "# " );
272  }
273 
274  if ( plotter != nullptr )
275  {
276  delete plotter;
277  plotter = nullptr;
278  }
279 
281  }
282  catch ( const vpException &e )
283  {
284  std::cout << "ViSP exception: " << e.what() << std::endl;
285  std::cout << "Stop the robot " << std::endl;
286  robot.setRobotState( vpRobot::STATE_STOP );
287  return EXIT_FAILURE;
288  }
289 
290  return 0;
291 }
vpRobotFrankaSim::getForceTorque
virtual void getForceTorque(const vpRobot::vpControlFrameType frame, vpColVector &force)
Definition: vpRobotFrankaSim.cpp:825
main
int main(int argc, char **argv)
Definition: tutorial-franka-coppeliasim-joint-impedance-control.cpp:41
boost::shared_ptr< NodeHandle >
ros::init
ROSCPP_DECL void init(const M_string &remappings, const std::string &name, uint32_t options=0)
vpRobotFrankaSim::getPosition
virtual void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position)
Definition: vpRobotFrankaSim.cpp:417
vpRobotFrankaSim::add_tool
virtual void add_tool(const vpHomogeneousMatrix &flMe, const double mL, const vpHomogeneousMatrix &flMcom, const vpMatrix &I_L)
Definition: vpRobotFrankaSim.cpp:222
vpROSRobotFrankaCoppeliasim
Definition: vpROSRobotFrankaCoppeliasim.h:87
ros::spinOnce
ROSCPP_DECL void spinOnce()
vpROSRobotFrankaCoppeliasim::setRobotState
vpRobot::vpRobotStateType setRobotState(vpRobot::vpRobotStateType newState)
Definition: vpROSRobotFrankaCoppeliasim.cpp:764
vpROSRobotFrankaCoppeliasim::wait
void wait(double timestamp_second, double duration_second)
Definition: vpROSRobotFrankaCoppeliasim.cpp:976
vpRobotFrankaSim::setForceTorque
virtual void setForceTorque(const vpRobot::vpControlFrameType frame, const vpColVector &force)
Definition: vpRobotFrankaSim.cpp:875
vpRobotFrankaSim::setVerbose
void setVerbose(bool verbose)
Definition: vpRobotFrankaSim.h:111
vpROSRobotFrankaCoppeliasim.h
vpROSRobotFrankaCoppeliasim::coppeliasimStopSimulation
void coppeliasimStopSimulation(double sleep_ms=1000.)
Definition: vpROSRobotFrankaCoppeliasim.cpp:268
vpROSRobotFrankaCoppeliasim::setPosition
void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &position)
Definition: vpROSRobotFrankaCoppeliasim.cpp:947
vpROSRobotFrankaCoppeliasim::getCoppeliasimSimulationTime
double getCoppeliasimSimulationTime()
Definition: vpROSRobotFrankaCoppeliasim.cpp:342
vpRobotFrankaSim::getMass
void getMass(vpMatrix &mass)
Definition: vpRobotFrankaSim.cpp:1019
vpROSRobotFrankaCoppeliasim::coppeliasimStartSimulation
void coppeliasimStartSimulation(double sleep_ms=1000.)
Definition: vpROSRobotFrankaCoppeliasim.cpp:248
vpRobotFrankaSim::getCoriolis
void getCoriolis(vpColVector &coriolis)
Definition: vpRobotFrankaSim.cpp:1042
vpROSRobotFrankaCoppeliasim::setCoppeliasimSyncMode
void setCoppeliasimSyncMode(bool enable, double sleep_ms=1000.)
Definition: vpROSRobotFrankaCoppeliasim.cpp:209
vpRobotFrankaSim::getFriction
void getFriction(vpColVector &friction)
Definition: vpRobotFrankaSim.cpp:144
ros::Rate
vpROSRobotFrankaCoppeliasim::connect
void connect(const std::string &robot_ID="")
Definition: vpROSRobotFrankaCoppeliasim.cpp:132
vpRobotFrankaSim::getVelocity
virtual void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &d_position)
Definition: vpRobotFrankaSim.cpp:627


visp_ros
Author(s): Francois Pasteau, Fabien Spindler, Gatien Gaumerais, Alexander Oliva
autogenerated on Wed Mar 2 2022 01:13:33