tutorial-franka-coppeliasim-pbvs-apriltag.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>
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 );
}
}
}
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;
bool opt_task_sequencing = false;
double convergence_threshold_t = 0.0005, convergence_threshold_tu = vpMath::rad( 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] ) == "--task_sequencing" )
{
opt_task_sequencing = 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();
vpROSRobotFrankaCoppeliasim robot;
robot.setVerbose( opt_verbose );
robot.connect();
std::cout << "Coppeliasim sync mode enabled: " << ( opt_coppeliasim_sync_mode ? "yes" : "no" ) << std::endl;
robot.coppeliasimStopSimulation(); // Allows to reset simulation, moving the robot to initial position
robot.setCoppeliasimSyncMode( false );
robot.coppeliasimStartSimulation();
if ( 0 )
{
robot.setRobotState( vpRobot::STATE_POSITION_CONTROL );
vpColVector q;
robot.getPosition( vpRobot::JOINT_STATE, q );
std::cout << "Initial joint position: " << q.t() << std::endl;
q[0] += vpMath::rad( 10 ); // Add 10 deg axis 1
std::cout << "Move to joint position: " << q.t() << std::endl;
robot.setPosition( vpRobot::JOINT_STATE, q );
}
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 cdMc, cMo, oMo;
// Desired pose used to compute the desired features
vpHomogeneousMatrix cdMo( vpTranslationVector( 0, 0.0, opt_tagSize * 3 ),
vpRotationMatrix( { 1, 0, 0, 0, -1, 0, 0, 0, -1 } ) );
cdMc = cdMo * cMo.inverse();
// Create visual features
vpFeatureTranslation t( vpFeatureTranslation::cdMc );
vpFeatureThetaU tu( vpFeatureThetaU::cdRc );
t.buildFrom( cdMc );
tu.buildFrom( cdMc );
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;
if ( opt_plot )
{
plotter = new vpPlot( 2, static_cast< int >( 250 * 2 ), 500, static_cast< int >( I.getWidth() ) + 80, 10,
"Real time curves plotter" );
plotter->setTitle( 0, "Visual features error" );
plotter->setTitle( 1, "Camera velocities" );
plotter->initGraph( 0, 6 );
plotter->initGraph( 1, 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" );
}
bool final_quit = false;
bool has_converged = false;
bool send_velocities = false;
bool servo_started = false;
std::vector< vpImagePoint > *traj_corners = nullptr; // To memorize point trajectory
double sim_time = robot.getCoppeliasimSimulationTime();
double sim_time_prev = sim_time;
double sim_time_init_servo = sim_time;
double sim_time_img = sim_time;
if ( 0 )
{
// Instead of setting eMc from /coppeliasim/franka/eMc topic, we can set its value to introduce noise for example
vpHomogeneousMatrix eMc;
eMc.buildFrom( 0.05, -0.05, 0, 0, 0, M_PI_4 );
robot.set_eMc( eMc );
}
std::cout << "eMc:\n" << robot.get_eMc() << std::endl;
robot.setRobotState( vpRobot::STATE_VELOCITY_CONTROL );
robot.setCoppeliasimSyncMode( opt_coppeliasim_sync_mode );
while ( !final_quit )
{
sim_time = robot.getCoppeliasimSimulationTime();
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_velocities ? "stop the robot" : "servo the robot" )
<< ", right click to quit.";
vpDisplay::displayText( I, 20, 20, ss.str(), vpColor::red );
}
vpColVector v_c( 6 );
// Only one tag is detected
if ( cMo_vec.size() == 1 )
{
cMo = cMo_vec[0];
static bool first_time = true;
if ( first_time )
{
// 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
}
} // end first_time
// Update visual features
cdMc = cdMo * oMo * cMo.inverse();
t.buildFrom( cdMc );
tu.buildFrom( cdMc );
if ( opt_task_sequencing )
{
if ( !servo_started )
{
if ( send_velocities )
{
servo_started = true;
}
sim_time_init_servo = robot.getCoppeliasimSimulationTime();
}
v_c = task.computeControlLaw( robot.getCoppeliasimSimulationTime() - sim_time_init_servo );
}
else
{
v_c = task.computeControlLaw();
}
// 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, 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 ( first_time )
{
traj_corners = new std::vector< vpImagePoint >[corners.size()];
}
// Display the trajectory of the points used as features
display_point_trajectory( I, corners, traj_corners );
if ( opt_plot )
{
plotter->plot( 0, static_cast< double >( sim_time ), task.getError() );
plotter->plot( 1, static_cast< double >( sim_time ), v_c );
}
if ( opt_verbose )
{
std::cout << "v_c: " << v_c.t() << std::endl;
}
vpTranslationVector cd_t_c = cdMc.getTranslationVector();
vpThetaUVector cd_tu_c = cdMc.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: " << error_tr;
vpDisplay::displayText( I, 20, static_cast< int >( I.getWidth() ) - 150, ss.str(), vpColor::red );
ss.str( "" );
ss << "error_tu: " << error_tu;
vpDisplay::displayText( I, 40, static_cast< int >( I.getWidth() ) - 150, ss.str(), vpColor::red );
if ( opt_verbose )
std::cout << "error translation: " << error_tr << " ; error rotation: " << error_tu << std::endl;
if ( !has_converged && error_tr < convergence_threshold_t && error_tu < convergence_threshold_tu )
{
has_converged = true;
std::cout << "Servo task has converged"
<< "\n";
vpDisplay::displayText( I, 100, 20, "Servo task has converged", vpColor::red );
}
if ( first_time )
{
first_time = false;
}
} // end if (cMo_vec.size() == 1)
else
{
v_c = 0; // Stop the robot
}
if ( !send_velocities )
{
v_c = 0; // Stop the robot
}
robot.setVelocity( vpRobot::CAMERA_FRAME, v_c );
std::stringstream ss;
ss << "Loop time [s]: " << std::round( ( sim_time - sim_time_prev ) * 1000. ) / 1000.;
ss << " Simulation time [s]: " << sim_time;
sim_time_prev = sim_time;
vpDisplay::displayText( I, 40, 20, ss.str(), vpColor::red );
vpMouseButton::vpMouseButtonType button;
if ( vpDisplay::getClick( I, button, false ) )
{
switch ( button )
{
case vpMouseButton::button1:
send_velocities = !send_velocities;
break;
case vpMouseButton::button3:
final_quit = true;
v_c = 0;
break;
default:
break;
}
}
vpDisplay::flush( I );
robot.wait( sim_time, 0.020 ); // Slow down the loop to simulate a camera at 50 Hz
} // end while
if ( opt_plot && plotter != nullptr )
{
delete plotter;
plotter = nullptr;
}
robot.coppeliasimStopSimulation();
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 ( 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