tutorial-franka-coppeliasim-dual-arm.cpp
/****************************************************************************
*
* ViSP, open source Visual Servoing Platform software.
* Copyright (C) 2005 - 2021 by Inria. All rights reserved.
*
* This software is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
* For using ViSP with software that can not be combined with the GNU
* GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
* See https://visp.inria.fr for more information.
*
* This software was developed at:
* Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
*
* If you have questions regarding the use of this file, please contact
* Inria at visp@inria.fr
*
* This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
* WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
*
*****************************************************************************/
#include <iostream>
#include <visp3/core/vpCameraParameters.h>
#include <visp3/detection/vpDetectorAprilTag.h>
#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/gui/vpPlot.h>
#include <visp3/io/vpImageIo.h>
#include <visp3/visual_features/vpFeatureThetaU.h>
#include <visp3/visual_features/vpFeatureTranslation.h>
#include <visp3/vs/vpServo.h>
#include <visp3/vs/vpServoDisplay.h>
#include <visp_ros/vpROSGrabber.h>
#include <visp_ros/vpROSRobotFrankaCoppeliasim.h>
#include <geometry_msgs/WrenchStamped.h>
void
display_point_trajectory( const vpImage< unsigned char > &I, const std::vector< vpImagePoint > &vip,
std::vector< vpImagePoint > *traj_vip )
{
for ( size_t i = 0; i < vip.size(); i++ )
{
if ( traj_vip[i].size() )
{
// Add the point only if distance with the previous > 1 pixel
if ( vpImagePoint::distance( vip[i], traj_vip[i].back() ) > 1. )
{
traj_vip[i].push_back( vip[i] );
}
}
else
{
traj_vip[i].push_back( vip[i] );
}
}
for ( size_t i = 0; i < vip.size(); i++ )
{
for ( size_t j = 1; j < traj_vip[i].size(); j++ )
{
vpDisplay::displayLine( I, traj_vip[i][j - 1], traj_vip[i][j], vpColor::green, 2 );
}
}
}
vpMatrix
Ta( const vpHomogeneousMatrix &edMe )
{
vpMatrix Lx( 6, 6 ), Lw( 3, 3 ), skew_u( 3, 3 );
Lx.eye();
vpThetaUVector tu( edMe );
vpColVector u;
double theta;
tu.extract( theta, u );
skew_u = vpColVector::skew( u );
Lw.eye();
if ( theta != 0.0 )
{
Lw -= 0.5 * theta * skew_u;
Lw += ( 1 - ( ( vpMath::sinc( theta ) ) / ( vpMath::sqr( vpMath::sinc( theta * 0.5 ) ) ) ) ) * skew_u * skew_u;
}
Lx.insert( Lw, 3, 3 );
return Lx;
}
vpColVector We( 6, 0 );
std::mutex shared_data;
void
Wrench_callback( const geometry_msgs::WrenchStamped &sensor_wrench )
{
std::lock_guard< std::mutex > lock( shared_data );
We[0] = sensor_wrench.wrench.force.x;
We[1] = sensor_wrench.wrench.force.y;
We[2] = sensor_wrench.wrench.force.z;
We[3] = sensor_wrench.wrench.torque.x;
We[4] = sensor_wrench.wrench.torque.y;
We[5] = sensor_wrench.wrench.torque.z;
return;
}
vpMatrix
compute_interaction_matrix_3D( const vpHomogeneousMatrix &cdMc )
{
vpMatrix Lx( 6, 6 ), Lw( 3, 3 ), skew_u( 3, 3 );
vpRotationMatrix cdRc( cdMc );
vpThetaUVector tu( cdMc );
vpColVector u;
double theta;
tu.extract( theta, u );
skew_u = vpColVector::skew( u );
Lw.eye();
Lw += 0.5 * theta * skew_u;
Lw += ( 1 - ( ( vpMath::sinc( theta ) ) / ( vpMath::sqr( vpMath::sinc( theta * 0.5 ) ) ) ) ) * skew_u * skew_u;
Lx.insert( cdRc, 0, 0 );
Lx.insert( Lw, 3, 3 );
return Lx;
}
int
main( int argc, char **argv )
{
double opt_tagSize = 0.08;
bool display_tag = true;
int opt_quad_decimate = 2;
bool opt_verbose = false;
bool opt_plot = false;
bool opt_adaptive_gain = false;
double convergence_threshold_t = 0.005, convergence_threshold_tu = 0.5;
bool opt_coppeliasim_sync_mode = false;
for ( int i = 1; i < argc; i++ )
{
if ( std::string( argv[i] ) == "--tag_size" && i + 1 < argc )
{
opt_tagSize = std::stod( argv[i + 1] );
}
else if ( std::string( argv[i] ) == "--verbose" || std::string( argv[i] ) == "-v" )
{
opt_verbose = true;
}
else if ( std::string( argv[i] ) == "--plot" )
{
opt_plot = true;
}
else if ( std::string( argv[i] ) == "--adaptive_gain" )
{
opt_adaptive_gain = true;
}
else if ( std::string( argv[i] ) == "--quad_decimate" && i + 1 < argc )
{
opt_quad_decimate = std::stoi( argv[i + 1] );
}
else if ( std::string( argv[i] ) == "--no-convergence-threshold" )
{
convergence_threshold_t = 0.;
convergence_threshold_tu = 0.;
}
else if ( std::string( argv[i] ) == "--enable-coppeliasim-sync-mode" )
{
opt_coppeliasim_sync_mode = true;
}
else if ( std::string( argv[i] ) == "--help" || std::string( argv[i] ) == "-h" )
{
std::cout << argv[0] << "[--tag_size <marker size in meter; default " << opt_tagSize << ">] "
<< "[--quad_decimate <decimation; default " << opt_quad_decimate << ">] "
<< "[--adaptive_gain] "
<< "[--plot] "
<< "[--task_sequencing] "
<< "[--no-convergence-threshold] "
<< "[--enable-coppeliasim-sync-mode] "
<< "[--verbose] [-v] "
<< "[--help] [-h]" << std::endl;
;
return EXIT_SUCCESS;
}
}
try
{
//------------------------------------------------------------------------//
//------------------------------------------------------------------------//
// ROS node
ros::init( argc, argv, "visp_ros" );
ros::NodeHandlePtr n = boost::make_shared< ros::NodeHandle >();
ros::Rate loop_rate( 1000 );
ros::spinOnce();
ros::Subscriber sub_wrench = n->subscribe( "/coppeliasim/franka/right_arm/ft_sensor", 1, Wrench_callback );
vpROSRobotFrankaCoppeliasim right_arm, left_arm;
right_arm.setVerbose( opt_verbose );
right_arm.connect( "right_arm" );
left_arm.setVerbose( opt_verbose );
left_arm.connect( "left_arm" );
std::cout << "Coppeliasim sync mode enabled: " << ( opt_coppeliasim_sync_mode ? "yes" : "no" ) << std::endl;
right_arm.coppeliasimStopSimulation(); // Allows to reset simulation, moving the robot to initial position
right_arm.setCoppeliasimSyncMode( false );
left_arm.setCoppeliasimSyncMode( false );
right_arm.coppeliasimStartSimulation();
vpHomogeneousMatrix bMfra( vpTranslationVector( 0.040, -0.03, 0.17 ),
vpRotationMatrix( { 1, 0, 0, 0, 0, -1, 0, 1, 0 } ) );
vpHomogeneousMatrix bMfla( vpTranslationVector( 0.040, 0.03, 0.17 ),
vpRotationMatrix( { 1, 0, 0, 0, 0, 1, 0, -1, 0 } ) );
vpImage< unsigned char > I;
vpROSGrabber g;
g.setImageTopic( "/coppeliasim/franka/camera/image" );
g.setCameraInfoTopic( "/coppeliasim/franka/camera/camera_info" );
g.open( argc, argv );
g.acquire( I );
std::cout << "Image size: " << I.getWidth() << " x " << I.getHeight() << std::endl;
vpCameraParameters cam;
g.getCameraInfo( cam );
std::cout << cam << std::endl;
vpDisplayOpenCV dc( I, 10, 10, "Color image" );
vpDetectorAprilTag::vpAprilTagFamily tagFamily = vpDetectorAprilTag::TAG_36h11;
vpDetectorAprilTag::vpPoseEstimationMethod poseEstimationMethod = vpDetectorAprilTag::HOMOGRAPHY_VIRTUAL_VS;
vpDetectorAprilTag detector( tagFamily );
detector.setAprilTagPoseEstimationMethod( poseEstimationMethod );
detector.setDisplayTag( display_tag );
detector.setAprilTagQuadDecimate( opt_quad_decimate );
// Servo
vpHomogeneousMatrix ccMo, cMo, oMo, ccMc, cdMcc, wMo, wMt;
// Desired pose used to compute the desired features
vpHomogeneousMatrix cdMo( vpTranslationVector( 0.0, 0.0, 0.2 ),
vpRotationMatrix( { 1, 0, 0, 0, -1, 0, 0, 0, -1 } ) );
// Create visual features
vpFeatureTranslation t( vpFeatureTranslation::cdMc );
vpFeatureThetaU tu( vpFeatureThetaU::cdRc );
t.buildFrom( ccMc );
tu.buildFrom( ccMc );
vpFeatureTranslation td( vpFeatureTranslation::cdMc );
vpFeatureThetaU tud( vpFeatureThetaU::cdRc );
vpServo task;
// Add the visual features
task.addFeature( t, td );
task.addFeature( tu, tud );
task.setServo( vpServo::EYEINHAND_CAMERA );
task.setInteractionMatrixType( vpServo::CURRENT );
if ( opt_adaptive_gain )
{
std::cout << "Enable adaptive gain" << std::endl;
vpAdaptiveGain lambda( 4, 1.2, 25 ); // lambda(0)=4, lambda(oo)=1.2 and lambda'(0)=25
task.setLambda( lambda );
}
else
{
task.setLambda( 1.2 );
}
vpPlot *plotter = nullptr;
vpPlot *plotter_left = nullptr;
if ( opt_plot )
{
plotter = new vpPlot( 3, static_cast< int >( 250 * 2 ), 600, static_cast< int >( I.getWidth() ) + 80, 10,
"Real time curves plotter" );
plotter->setTitle( 0, "Visual features error" );
plotter->setTitle( 1, "Camera velocities" );
plotter->setTitle( 2, "Measured wrench" );
plotter->initGraph( 0, 6 );
plotter->initGraph( 1, 6 );
plotter->initGraph( 2, 6 );
plotter->setLegend( 0, 0, "error_feat_tx" );
plotter->setLegend( 0, 1, "error_feat_ty" );
plotter->setLegend( 0, 2, "error_feat_tz" );
plotter->setLegend( 0, 3, "error_feat_theta_ux" );
plotter->setLegend( 0, 4, "error_feat_theta_uy" );
plotter->setLegend( 0, 5, "error_feat_theta_uz" );
plotter->setLegend( 1, 0, "vc_x" );
plotter->setLegend( 1, 1, "vc_y" );
plotter->setLegend( 1, 2, "vc_z" );
plotter->setLegend( 1, 3, "wc_x" );
plotter->setLegend( 1, 4, "wc_y" );
plotter->setLegend( 1, 5, "wc_z" );
plotter->setLegend( 2, 0, "F_x" );
plotter->setLegend( 2, 1, "F_y" );
plotter->setLegend( 2, 2, "F_z" );
plotter->setLegend( 2, 3, "Tau_x" );
plotter->setLegend( 2, 4, "Tau_y" );
plotter->setLegend( 2, 5, "Tau_z" );
plotter_left = new vpPlot( 4, 500, 600, 10, 10, "Real time curves plotter" );
plotter_left->setTitle( 0, "EE Pose [m] - [rad]" );
plotter_left->initGraph( 0, 6 );
plotter_left->setLegend( 0, 0, "x" );
plotter_left->setLegend( 0, 1, "y" );
plotter_left->setLegend( 0, 2, "z" );
plotter_left->setLegend( 0, 3, "tu_x" );
plotter_left->setLegend( 0, 4, "tu_y" );
plotter_left->setLegend( 0, 5, "tu_z" );
plotter_left->setTitle( 1, "EE pose error [m] - [rad]" );
plotter_left->initGraph( 1, 6 );
plotter_left->setLegend( 1, 0, "e_x" );
plotter_left->setLegend( 1, 1, "e_y" );
plotter_left->setLegend( 1, 2, "e_z" );
plotter_left->setLegend( 1, 3, "e_tu_x" );
plotter_left->setLegend( 1, 4, "e_tu_y" );
plotter_left->setLegend( 1, 5, "e_tu_z" );
plotter_left->setTitle( 2, "Joint torque command [Nm]" );
plotter_left->initGraph( 2, 7 );
plotter_left->setLegend( 2, 0, "Tau1" );
plotter_left->setLegend( 2, 1, "Tau2" );
plotter_left->setLegend( 2, 2, "Tau3" );
plotter_left->setLegend( 2, 3, "Tau4" );
plotter_left->setLegend( 2, 4, "Tau5" );
plotter_left->setLegend( 2, 5, "Tau6" );
plotter_left->setLegend( 2, 6, "Tau7" );
plotter_left->setTitle( 3, "Pose error norm [rad]" );
plotter_left->initGraph( 3, 1 );
plotter_left->setLegend( 3, 0, "||e||" ); // change this
}
bool final_quit = false;
bool has_converged = false;
bool send_cmd = false;
bool restart = false;
std::vector< vpImagePoint > *traj_corners = nullptr; // To memorize point trajectory
double sim_time = right_arm.getCoppeliasimSimulationTime();
double sim_time_prev = sim_time;
double sim_time_init_servo = sim_time;
double sim_time_img = sim_time;
double sim_time_img_prev = sim_time;
double sim_time_start = sim_time;
double sim_time_convergence = sim_time;
double dt = 0.0;
vpMatrix K( 6, 6 ), D( 6, 6 );
double wp = 50;
double wo = 20;
K.diag( { wp * wp, wp * wp, wp * wp, wo * wo, wo * wo, wo * wo } );
D.diag( { 2 * wp, 2 * wp, 2 * wp, 2 * wo, 2 * wo, 2 * wo } );
double c_time = 0.0;
double mu = 2.0;
std::cout << "eMc:\n" << right_arm.get_eMc() << std::endl;
vpColVector v_c( 6, 0 ), q( 7, 0 ), dq( 7, 0 ), tau_d( 7, 0 ), C( 7, 0 ), pos( 6, 0 ), F( 7, 0 ), tau_d0( 7, 0 ),
tau_cmd( 7, 0 ), x_e( 6, 0 ), dx_e( 6, 0 ), dx_ed( 6, 0 ), ddx_ed( 6, 0 );
vpMatrix J( 6, 7 ), Ja( 6, 7 ), dJa( 6, 7 ), Ja_old( 6, 7 ), B( 7, 7 ), I7( 7, 7 ), Ja_pinv_B_t( 6, 7 ), Ls( 6, 6 );
I7.eye();
right_arm.setRobotState( vpRobot::STATE_VELOCITY_CONTROL );
left_arm.setRobotState( vpRobot::STATE_FORCE_TORQUE_CONTROL );
right_arm.setCoppeliasimSyncMode( opt_coppeliasim_sync_mode );
left_arm.setCoppeliasimSyncMode( opt_coppeliasim_sync_mode );
vpRotationMatrix wRed0( { 1, 0, 0, 0, -1, 0, 0, 0, -1 } );
vpHomogeneousMatrix wMed0( vpTranslationVector( 0.5, 0.0, 0.0 ), wRed0 );
vpHomogeneousMatrix wMed;
wMed = wMed0;
std::atomic_bool admittance_thread_running{ true };
bool init_done = false;
vpHomogeneousMatrix ftTee;
// the FT sensor frame is coincident with the flange frame in Coppeliasim model
// then the transformation between flange and end-effector is the same than
// the transformation between the FT sensor and the end-effector
ftTee = right_arm.get_flMe();
std::cout << "ftTee:\n" << ftTee << "\n";
vpVelocityTwistMatrix cTe;
cTe.buildFrom( right_arm.get_eMc().inverse() );
vpForceTwistMatrix cFee, eeFft, eeFc;
cFee.buildFrom( right_arm.get_eMc().inverse() );
eeFc.buildFrom( right_arm.get_eMc() );
eeFft.buildFrom( ftTee.inverse() );
// Admittance parameters
vpColVector dde_s( 6, 0 ), de_s( 6, 0 ), e_s( 6, 0 ), d_err( 6, 0 ), err( 6, 0 ), old_err( 6, 0 );
vpMatrix Ks( 6, 6 ), Ds( 6, 6 ), Be( 6, 6 ), Bc_inv( 6, 6 ), I3( 3, 3 );
Ks.diag( 100 );
Ds.diag( 100 );
I3.eye();
Be.insert( 1 * I3, 0, 0 ); // mass
Be.insert( 0.1 * I3, 3, 3 ); // inertia
// in this way we achieve Isotropic behavior on the ee frame
Bc_inv = cTe * Be.inverseByCholesky() * ( (vpMatrix)cTe ).t();
vpColVector r( 3, 0 ), wFcog( 3, 0 ), ft_load( 6, 0 ), ft_meas( 6, 0 ), cfc_meas( 6, 0 ), Fs( 6, 0 );
double m = right_arm.get_tool_mass(); // [kg]
wFcog[2] = -9.81 * m;
r = right_arm.get_flMcom().getTranslationVector();
std::thread fast_thread( [&]() {
std::cout << "fast_thread: started... \n" << std::endl;
try
{
while ( admittance_thread_running )
{
if ( init_done )
{
sim_time = right_arm.getCoppeliasimSimulationTime();
dt = sim_time - sim_time_prev;
sim_time_prev = sim_time;
// std::cout << "dt: " << dt<< std::endl;
vpColVector aux( 3, 0 );
aux = ( ( bMfra * right_arm.get_fMe() * right_arm.get_flMe().inverse() ).inverse() ).getRotationMatrix() *
wFcog;
ft_load[0] = aux[0];
ft_load[1] = aux[1];
ft_load[2] = aux[2];
aux = vpColVector::skew( r ) * aux;
ft_load[3] = aux[0];
ft_load[4] = aux[1];
ft_load[5] = aux[2];
ft_meas = We - ft_load;
shared_data.lock();
cfc_meas = cFee * eeFft * ft_meas;
task.computeControlLaw();
Ls = compute_interaction_matrix_3D( cdMcc );
Fs = Ls * Bc_inv * cfc_meas;
// admittance law
dde_s = -Ks * e_s - Ds * de_s + Fs;
de_s += dde_s * dt;
e_s += de_s * dt;
cdMcc.insert( (vpTranslationVector)e_s.extract( 0, 3 ) );
if ( sqrt( e_s.extract( 3, 3 ).sumSquare() ) != 0.0 )
{
vpRotationMatrix R_aux;
R_aux.buildFrom( (vpThetaUVector)e_s.extract( 3, 3 ) );
cdMcc.insert( R_aux );
}
else
{
cdMcc.insert( vpRotationMatrix() );
}
ccMo = cdMcc.inverse() * cdMo;
ccMc = ccMo * oMo * cMo.inverse();
t.buildFrom( ccMc );
tu.buildFrom( ccMc );
v_c = task.computeControlLaw();
left_arm.getPosition( vpRobot::JOINT_STATE, q );
left_arm.getVelocity( vpRobot::JOINT_STATE, dq );
left_arm.getMass( B );
left_arm.getCoriolis( C );
left_arm.getFriction( F );
left_arm.get_fJe( J );
if ( has_converged )
{
wMed[0][3] = wMed0[0][3] + 0.05 * sin( 2 * M_PI * 0.3 * ( sim_time - sim_time_convergence ) );
wMed[1][3] = wMed0[1][3] + 0.05 * ( 1 - cos( 2 * M_PI * 0.3 * ( sim_time - sim_time_convergence ) ) );
// wMed[2][3] = wMed0[2][3] + 0.05*sin(2*M_PI*0.5*(sim_time - sim_time_convergence));
dx_ed[0] = 2 * M_PI * 0.3 * 0.05 * cos( 2 * M_PI * 0.3 * ( sim_time - sim_time_convergence ) );
dx_ed[1] = 2 * M_PI * 0.3 * 0.05 * sin( 2 * M_PI * 0.3 * ( sim_time - sim_time_convergence ) );
// dx_ed[2] = 2*M_PI*0.5*0.05*cos(2*M_PI*0.5*(sim_time - sim_time_convergence));
ddx_ed[0] =
-2 * M_PI * 0.3 * 2 * M_PI * 0.3 * 0.05 * sin( 2 * M_PI * 0.3 * ( sim_time - sim_time_convergence ) );
ddx_ed[1] =
2 * M_PI * 0.3 * 2 * M_PI * 0.3 * 0.05 * cos( 2 * M_PI * 0.3 * ( sim_time - sim_time_convergence ) );
// ddx_ed[2] = -2*M_PI*0.5*2*M_PI*0.5*0.05*sin(2*M_PI*0.5*(sim_time -
// sim_time_convergence));
}
vpMatrix RR( 6, 6 );
RR.insert( wMed.getRotationMatrix().t(), 0, 0 );
RR.insert( wMed.getRotationMatrix().t(), 3, 3 );
x_e = (vpColVector)vpPoseVector( wMed.inverse() * left_arm.get_fMe() );
Ja = Ta( wMed.inverse() * left_arm.get_fMe() ) * RR * J;
dx_e = Ta( wMed.inverse() * left_arm.get_fMe() ) * RR * ( dx_ed - J * dq );
if ( dt != 0 )
{
dJa = ( Ja - Ja_old ) / dt;
}
else
{
dJa = 0;
}
Ja_old = Ja;
Ja_pinv_B_t = ( Ja * B.inverseByCholesky() * Ja.t() ).inverseByCholesky() * Ja * B.inverseByCholesky();
// Compute the control law
tau_d = B * Ja.pseudoInverse() * ( -K * ( x_e ) + D * (dx_e)-dJa * dq + ddx_ed ) + C + F -
( I7 - Ja.t() * Ja_pinv_B_t ) * B * dq * 100;
if ( !send_cmd )
{
v_c = 0; // Stop the robot
tau_cmd = 0;
restart = true;
}
else
{
if ( restart )
{
c_time = sim_time;
tau_d0 = tau_d;
restart = false;
}
tau_cmd = tau_d - tau_d0 * std::exp( -mu * ( sim_time - c_time ) );
}
right_arm.setVelocity( vpRobot::CAMERA_FRAME, v_c );
left_arm.setForceTorque( vpRobot::JOINT_STATE, tau_cmd );
shared_data.unlock();
} // end init_done
right_arm.wait( sim_time, 0.002 );
} // end while
}
catch ( const vpException &e )
{
std::cout << "ViSP exception: " << e.what() << std::endl;
admittance_thread_running = false;
}
catch ( ... )
{
std::cout << "Something wrong happens: " << std::endl;
admittance_thread_running = false;
}
std::cout << "fast_thread: exiting..." << std::endl;
final_quit = true;
} );
// control loop
while ( !final_quit )
{
g.acquire( I, sim_time_img );
vpDisplay::display( I );
std::vector< vpHomogeneousMatrix > cMo_vec;
detector.detect( I, opt_tagSize, cam, cMo_vec );
{
std::stringstream ss;
ss << "Left click to " << ( send_cmd ? "stop the robot" : "servo the robot" ) << ", right click to quit.";
vpDisplay::displayText( I, 20, 20, ss.str(), vpColor::red );
}
// Only one tag is detected
if ( cMo_vec.size() == 1 )
{
shared_data.lock();
cMo = cMo_vec[0];
if ( !init_done )
{
// Introduce security wrt tag positionning in order to avoid PI rotation
std::vector< vpHomogeneousMatrix > v_oMo( 2 ), v_cdMc( 2 );
v_oMo[1].buildFrom( 0, 0, 0, 0, 0, M_PI );
for ( size_t i = 0; i < 2; i++ )
{
v_cdMc[i] = cdMo * v_oMo[i] * cMo.inverse();
}
if ( std::fabs( v_cdMc[0].getThetaUVector().getTheta() ) <
std::fabs( v_cdMc[1].getThetaUVector().getTheta() ) )
{
oMo = v_oMo[0];
}
else
{
std::cout << "Desired frame modified to avoid PI rotation of the camera" << std::endl;
oMo = v_oMo[1]; // Introduce PI rotation
}
sim_time_start = sim_time;
ccMo = cdMo;
} // end first_time
// Update visual features
ccMc = ccMo * oMo * cMo.inverse();
t.buildFrom( ccMc );
tu.buildFrom( ccMc );
shared_data.unlock();
// Display the current and desired feature points in the image display
// Display desired and current pose features
vpDisplay::displayFrame( I, cdMo * oMo, cam, opt_tagSize / 1.5, vpColor::yellow, 2 );
vpDisplay::displayFrame( I, ccMo * oMo, cam, opt_tagSize / 2, vpColor::none, 3 );
vpDisplay::displayFrame( I, cMo, cam, opt_tagSize / 2, vpColor::none, 3 );
// Get tag corners
std::vector< vpImagePoint > corners = detector.getPolygon( 0 );
// Get the tag cog corresponding to the projection of the tag frame in the image
corners.push_back( detector.getCog( 0 ) );
// Display the trajectory of the points
if ( !init_done )
{
traj_corners = new std::vector< vpImagePoint >[corners.size()];
}
// Display the trajectory of the points used as features
// display_point_trajectory( I, corners, traj_corners );
vpTranslationVector cd_t_c = ccMc.getTranslationVector();
vpThetaUVector cd_tu_c = ccMc.getThetaUVector();
double error_tr = sqrt( cd_t_c.sumSquare() );
double error_tu = vpMath::deg( sqrt( cd_tu_c.sumSquare() ) );
std::stringstream ss;
ss << "error_t [m]: " << error_tr;
vpDisplay::displayText( I, 20, static_cast< int >( I.getWidth() ) - 160, ss.str(), vpColor::red );
ss.str( "" );
ss << "error_tu [deg]: " << error_tu;
vpDisplay::displayText( I, 40, static_cast< int >( I.getWidth() ) - 160, ss.str(), vpColor::red );
if ( !has_converged && error_tr < convergence_threshold_t && error_tu < convergence_threshold_tu )
{
has_converged = true;
sim_time_convergence = sim_time;
std::cout << "Servo task has converged \n";
vpDisplay::displayText( I, 100, 20, "Servo task has converged", vpColor::red );
}
if ( !init_done )
{
init_done = true;
}
} // end if (cMo_vec.size() == 1)
else
{
v_c = 0; // Stop the robot
tau_cmd = 0;
}
if ( opt_plot )
{
plotter->plot( 0, static_cast< double >( sim_time ), task.getError() );
plotter->plot( 1, static_cast< double >( sim_time ), v_c );
plotter->plot( 2, static_cast< double >( sim_time ), cfc_meas );
plotter_left->plot( 0, sim_time, (vpColVector)vpPoseVector( bMfla * left_arm.get_fMe() ) );
plotter_left->plot( 1, sim_time, x_e );
plotter_left->plot( 2, sim_time, tau_cmd );
plotter_left->plot( 3, sim_time, vpColVector( 1, sqrt( x_e.sumSquare() ) ) );
}
std::stringstream ss;
ss << "Loop time [s]: " << std::round( ( sim_time_img - sim_time_img_prev ) * 1000. ) / 1000.;
ss << " Simulation time [s]: " << sim_time_img;
sim_time_img_prev = sim_time_img;
vpDisplay::displayText( I, 40, 20, ss.str(), vpColor::red );
vpMouseButton::vpMouseButtonType button;
if ( vpDisplay::getClick( I, button, false ) )
{
switch ( button )
{
case vpMouseButton::button1:
send_cmd = !send_cmd;
break;
case vpMouseButton::button3:
final_quit = true;
v_c = 0;
tau_cmd = 0;
admittance_thread_running = false;
break;
default:
break;
}
}
vpDisplay::flush( I );
} // end while
if ( opt_plot && plotter != nullptr && plotter_left != nullptr )
{
delete plotter;
plotter = nullptr;
delete plotter_left;
plotter_left = nullptr;
}
right_arm.coppeliasimStopSimulation();
right_arm.setRobotState( vpRobot::STATE_STOP );
left_arm.setRobotState( vpRobot::STATE_STOP );
if ( !final_quit )
{
while ( !final_quit )
{
g.acquire( I );
vpDisplay::display( I );
vpDisplay::displayText( I, 20, 20, "Click to quit the program.", vpColor::red );
vpDisplay::displayText( I, 40, 20, "Visual servo converged.", vpColor::red );
if ( vpDisplay::getClick( I, false ) )
{
final_quit = true;
}
vpDisplay::flush( I );
}
}
if ( fast_thread.joinable() )
{
fast_thread.join();
std::cout << "fast thread joined.. \n ";
}
if ( traj_corners )
{
delete[] traj_corners;
}
}
catch ( const vpException &e )
{
std::cout << "ViSP exception: " << e.what() << std::endl;
std::cout << "Stop the robot " << std::endl;
return EXIT_FAILURE;
}
return 0;
}

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