Skip to content

Commit

Permalink
Polish Biclops PT unit material
Browse files Browse the repository at this point in the history 
- Introduce an image to explain where the end-effector frame is located
- Update IBVS example to use a realsense camera and an AprilTag
  • Loading branch information
@fspindle
fspindle committed Oct 4, 2023
1 parent c449424 commit 4784eb7
Show file tree
Hide file tree
Showing 5 changed files with 267 additions and 361 deletions.
Binary file added doc/image/tutorial/visual-servo/img-biclops-frames.jpg
View file
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Binary file added doc/image/tutorial/visual-servo/img-biclops-frames.pptx
View file
Binary file not shown.
179 changes: 83 additions & 96 deletions example/servo-biclops/moveBiclops.cpp
View file
Original file line number Diff line number Diff line change
@@ -1,5 +1,4 @@
/****************************************************************************
*
/*
* ViSP, open source Visual Servoing Platform software.
* Copyright (C) 2005 - 2023 by Inria. All rights reserved.
*
Expand Down Expand Up @@ -30,26 +29,17 @@
*
* Description:
* Tests the control law
* Authors:
*
*****************************************************************************/
/*!
\file moveBiclops.cpp
\brief Example of a real robot control, the biclops robot (pan-tilt turret)
by Traclabs. The robot is controlled first in position, then in velocity.
See http://www.traclabs.com/tracbiclops.htm for more details.
*/
*/

/*!
\example moveBiclops.cpp
Example of a real robot control, the biclops robot (pan-tilt turret) by
Traclabs. The robot is controlled first in position, then in velocity.
See http://www.traclabs.com/tracbiclops.htm for more details.
*/
* \file moveBiclops.cpp
* \example moveBiclops.cpp
*
* Example of a real robot control, the biclops robot (pan-tilt turret) by
* Traclabs. The robot is controlled first in position, then in velocity.
*
* See http://www.traclabs.com/tracbiclops.htm for more details.
*/

#include <stdlib.h>
#include <visp3/core/vpColVector.h>
Expand All @@ -64,14 +54,12 @@
#define GETOPTARGS "c:h"

/*
Print the program options.
\param name : Program name.
\param badparam : Bad parameter name.
\param conf : Biclops configuration file.
*/
* Print the program options.
*
* \param name : Program name.
* \param badparam : Bad parameter name.
* \param conf : Biclops configuration file.
*/
void usage(const char *name, const char *badparam, std::string conf)
{
fprintf(stdout, "\n\
Expand All @@ -80,7 +68,7 @@ Move the biclops robot\n\
SYNOPSIS\n\
%s [-c <Biclops configuration file>] [-h]\n\
",
name);
name);

fprintf(stdout, "\n\
OPTIONS: Default\n\
Expand All @@ -95,16 +83,14 @@ OPTIONS: Default\n\
}

/*!
Set the program options.
\param argc : Command line number of parameters.
\param argv : Array of command line parameters.
\param conf : Biclops configuration file.
\return false if the program has to be stopped, true otherwise.
*/
* Set the program options.
*
* \param argc : Command line number of parameters.
* \param argv : Array of command line parameters.
* \param conf : Biclops configuration file.
*
* \return false if the program has to be stopped, true otherwise.
*/
bool getOptions(int argc, const char **argv, std::string &conf)
{
const char *optarg_;
Expand Down Expand Up @@ -147,7 +133,7 @@ int main(int argc, const char **argv)
return EXIT_FAILURE;
}
try {
vpRobotBiclops robot(opt_conf.c_str());
vpRobotBiclops robot(opt_conf);

vpColVector q(vpBiclops::ndof); // desired position
vpColVector qdot(vpBiclops::ndof); // desired velocity
Expand All @@ -160,122 +146,123 @@ int main(int argc, const char **argv)
q[0] = vpMath::rad(-10);
q[1] = vpMath::rad(-20);
std::cout << "Set position in the articular frame: "
<< " pan: " << vpMath::deg(q[0]) << " deg"
<< " tilt: " << vpMath::deg(q[1]) << " deg" << std::endl;
<< " pan: " << vpMath::deg(q[0]) << " deg"
<< " tilt: " << vpMath::deg(q[1]) << " deg" << std::endl;
robot.setPositioningVelocity(30.);
robot.setPosition(vpRobot::ARTICULAR_FRAME, q);
robot.setPosition(vpRobot::JOINT_STATE, q);

robot.getPosition(vpRobot::ARTICULAR_FRAME, qm);
robot.getPosition(vpRobot::JOINT_STATE, qm);
std::cout << "Position in the articular frame: "
<< " pan: " << vpMath::deg(qm[0]) << " tilt: " << vpMath::deg(qm[1]) << std::endl;
robot.getVelocity(vpRobot::ARTICULAR_FRAME, qm);
<< " pan: " << vpMath::deg(qm[0]) << " tilt: " << vpMath::deg(qm[1]) << std::endl;
robot.getVelocity(vpRobot::JOINT_STATE, qm);
std::cout << "Velocity in the articular frame: "
<< " pan: " << vpMath::deg(qm[0]) << " tilt: " << vpMath::deg(qm[1]) << std::endl;
<< " pan: " << vpMath::deg(qm[0]) << " tilt: " << vpMath::deg(qm[1]) << std::endl;

q[0] = vpMath::rad(10);
q[1] = vpMath::rad(20);
std::cout << "Set position in the articular frame: "
<< " pan: " << vpMath::deg(q[0]) << " deg"
<< " tilt: " << vpMath::deg(q[1]) << " deg" << std::endl;
<< " pan: " << vpMath::deg(q[0]) << " deg"
<< " tilt: " << vpMath::deg(q[1]) << " deg" << std::endl;
robot.setPositioningVelocity(10);
robot.setPosition(vpRobot::ARTICULAR_FRAME, q);
robot.setPosition(vpRobot::JOINT_STATE, q);

robot.getPosition(vpRobot::ARTICULAR_FRAME, qm);
robot.getPosition(vpRobot::JOINT_STATE, qm);
std::cout << "Position in the articular frame: "
<< " pan: " << vpMath::deg(qm[0]) << " tilt: " << vpMath::deg(qm[1]) << std::endl;
robot.getVelocity(vpRobot::ARTICULAR_FRAME, qm);
<< " pan: " << vpMath::deg(qm[0]) << " tilt: " << vpMath::deg(qm[1]) << std::endl;
robot.getVelocity(vpRobot::JOINT_STATE, qm);
std::cout << "Velocity in the articular frame: "
<< " pan: " << vpMath::deg(qm[0]) << " tilt: " << vpMath::deg(qm[1]) << std::endl;
<< " pan: " << vpMath::deg(qm[0]) << " tilt: " << vpMath::deg(qm[1]) << std::endl;

std::cout << "Set STATE_VELOCITY_CONTROL" << std::endl;
robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);

robot.getPosition(vpRobot::ARTICULAR_FRAME, qm);
robot.getPosition(vpRobot::JOINT_STATE, qm);
std::cout << "Position in the articular frame: "
<< " pan: " << vpMath::deg(qm[0]) << " deg"
<< " tilt: " << vpMath::deg(qm[1]) << " deg" << std::endl;
robot.getVelocity(vpRobot::ARTICULAR_FRAME, qm);
<< " pan: " << vpMath::deg(qm[0]) << " deg"
<< " tilt: " << vpMath::deg(qm[1]) << " deg" << std::endl;
robot.getVelocity(vpRobot::JOINT_STATE, qm);
std::cout << "Velocity in the articular frame: "
<< " pan: " << vpMath::deg(qm[0]) << " tilt: " << vpMath::deg(qm[1]) << std::endl;
<< " pan: " << vpMath::deg(qm[0]) << " tilt: " << vpMath::deg(qm[1]) << std::endl;

qdot = 0;
// qdot[0] = vpMath::rad(0.1) ;
qdot[1] = vpMath::rad(25);
std::cout << "Set articular frame velocity "
<< " pan: " << vpMath::deg(qdot[0]) << " deg/s"
<< " tilt: " << vpMath::deg(qdot[1]) << " deg/s" << std::endl;
robot.setVelocity(vpRobot::ARTICULAR_FRAME, qdot);
std::cout << "Set articular frame velocity for 5 sec"
<< " pan: " << vpMath::deg(qdot[0]) << " deg/s"
<< " tilt: " << vpMath::deg(qdot[1]) << " deg/s" << std::endl;
robot.setVelocity(vpRobot::JOINT_STATE, qdot);

// waits 5000ms
vpTime::wait(5000.0);

robot.getPosition(vpRobot::ARTICULAR_FRAME, qm);
robot.getPosition(vpRobot::JOINT_STATE, qm);
std::cout << "Position in the articular frame: "
<< " pan: " << vpMath::deg(qm[0]) << " deg"
<< " tilt: " << vpMath::deg(qm[1]) << " deg" << std::endl;
robot.getVelocity(vpRobot::ARTICULAR_FRAME, qm);
<< " pan: " << vpMath::deg(qm[0]) << " deg"
<< " tilt: " << vpMath::deg(qm[1]) << " deg" << std::endl;
robot.getVelocity(vpRobot::JOINT_STATE, qm);
std::cout << "Velocity in the articular frame: "
<< " pan: " << vpMath::deg(qm[0]) << " tilt: " << vpMath::deg(qm[1]) << std::endl;
<< " pan: " << vpMath::deg(qm[0]) << " tilt: " << vpMath::deg(qm[1]) << std::endl;

qdot = 0;
// qdot[0] = vpMath::rad(0.1) ;
qdot[1] = -vpMath::rad(25);
std::cout << "Set articular frame velocity "
<< " pan: " << vpMath::deg(qdot[0]) << " deg/s"
<< " tilt: " << vpMath::deg(qdot[1]) << " deg/s" << std::endl;
robot.setVelocity(vpRobot::ARTICULAR_FRAME, qdot);
std::cout << "Set articular frame velocity for 3 sec"
<< " pan: " << vpMath::deg(qdot[0]) << " deg/s"
<< " tilt: " << vpMath::deg(qdot[1]) << " deg/s" << std::endl;
robot.setVelocity(vpRobot::JOINT_STATE, qdot);

// waits 3000 ms
vpTime::wait(3000.0);

robot.getPosition(vpRobot::ARTICULAR_FRAME, qm);
robot.getPosition(vpRobot::JOINT_STATE, qm);
std::cout << "Position in the articular frame: "
<< " pan: " << vpMath::deg(qm[0]) << " deg"
<< " tilt: " << vpMath::deg(qm[1]) << " deg" << std::endl;
robot.getVelocity(vpRobot::ARTICULAR_FRAME, qm);
<< " pan: " << vpMath::deg(qm[0]) << " deg"
<< " tilt: " << vpMath::deg(qm[1]) << " deg" << std::endl;
robot.getVelocity(vpRobot::JOINT_STATE, qm);
std::cout << "Velocity in the articular frame: "
<< " pan: " << vpMath::deg(qm[0]) << " tilt: " << vpMath::deg(qm[1]) << std::endl;
<< " pan: " << vpMath::deg(qm[0]) << " tilt: " << vpMath::deg(qm[1]) << std::endl;

qdot = 0;
// qdot[0] = vpMath::rad(0.1) ;
qdot[1] = vpMath::rad(10);
std::cout << "Set articular frame velocity "
<< " pan: " << vpMath::deg(qdot[0]) << " deg/s"
<< " tilt: " << vpMath::deg(qdot[1]) << " deg/s" << std::endl;
robot.setVelocity(vpRobot::ARTICULAR_FRAME, qdot);
std::cout << "Set articular frame velocity for 2 sec"
<< " pan: " << vpMath::deg(qdot[0]) << " deg/s"
<< " tilt: " << vpMath::deg(qdot[1]) << " deg/s" << std::endl;
robot.setVelocity(vpRobot::JOINT_STATE, qdot);

// waits 2000 ms
vpTime::wait(2000.0);

robot.getPosition(vpRobot::ARTICULAR_FRAME, qm);
robot.getPosition(vpRobot::JOINT_STATE, qm);
std::cout << "Position in the articular frame: "
<< " pan: " << vpMath::deg(qm[0]) << " deg"
<< " tilt: " << vpMath::deg(qm[1]) << " deg" << std::endl;
robot.getVelocity(vpRobot::ARTICULAR_FRAME, qm);
<< " pan: " << vpMath::deg(qm[0]) << " deg"
<< " tilt: " << vpMath::deg(qm[1]) << " deg" << std::endl;
robot.getVelocity(vpRobot::JOINT_STATE, qm);
std::cout << "Velocity in the articular frame: "
<< " pan: " << vpMath::deg(qm[0]) << " tilt: " << vpMath::deg(qm[1]) << std::endl;
<< " pan: " << vpMath::deg(qm[0]) << " tilt: " << vpMath::deg(qm[1]) << std::endl;

qdot = 0;
qdot[0] = vpMath::rad(-5);
// qdot[1] = vpMath::rad(-5);

std::cout << "Set articular frame velocity "
<< " pan: " << vpMath::deg(qdot[0]) << " deg/s"
<< " tilt: " << vpMath::deg(qdot[1]) << " deg/s" << std::endl;
robot.setVelocity(vpRobot::ARTICULAR_FRAME, qdot);
std::cout << "Set articular frame velocity for 2 sec"
<< " pan: " << vpMath::deg(qdot[0]) << " deg/s"
<< " tilt: " << vpMath::deg(qdot[1]) << " deg/s" << std::endl;
robot.setVelocity(vpRobot::JOINT_STATE, qdot);

// waits 2000 ms
vpTime::wait(2000.0);

robot.getPosition(vpRobot::ARTICULAR_FRAME, qm);
robot.getPosition(vpRobot::JOINT_STATE, qm);
std::cout << "Position in the articular frame: "
<< " pan: " << vpMath::deg(qm[0]) << " deg"
<< " tilt: " << vpMath::deg(qm[1]) << " deg" << std::endl;
robot.getVelocity(vpRobot::ARTICULAR_FRAME, qm);
<< " pan: " << vpMath::deg(qm[0]) << " deg"
<< " tilt: " << vpMath::deg(qm[1]) << " deg" << std::endl;
robot.getVelocity(vpRobot::JOINT_STATE, qm);
std::cout << "Velocity in the articular frame: "
<< " pan: " << vpMath::deg(qm[0]) << " tilt: " << vpMath::deg(qm[1]) << std::endl;
<< " pan: " << vpMath::deg(qm[0]) << " tilt: " << vpMath::deg(qm[1]) << std::endl;
return EXIT_SUCCESS;
} catch (const vpException &e) {
}
catch (const vpException &e) {
std::cout << "Catch an exception: " << e.getMessage() << std::endl;
return EXIT_FAILURE;
}
Expand Down
Loading

0 comments on commit 4784eb7

Please sign in to comment.