46 #include <visp3/core/vpConfig.h>
48 #ifdef VISP_HAVE_BICLOPS
50 #include <visp3/robot/vpRobotBiclops.h>
51 #include <visp3/io/vpParseArgv.h>
54 #define GETOPTARGS "c:h"
63 void usage(
const char *name,
const char *badparam, std::string conf)
66 Move the Biclops robot\n\
69 %s [-c <Biclops configuration file>] [-h]\n\
75 -c <Biclops configuration file> %s\n\
76 Sets the Biclops robot configuration file.\n\n",
80 fprintf(stderr,
"ERROR: \n");
81 fprintf(stderr,
"\nBad parameter [%s]\n", badparam);
94 bool getOptions(
int argc,
const char **argv, std::string &conf)
105 usage(argv[0],
nullptr, conf);
110 usage(argv[0], optarg_, conf);
116 if ((c == 1) || (c == -1)) {
118 usage(argv[0],
nullptr, conf);
119 std::cerr <<
"ERROR: " << std::endl;
120 std::cerr <<
" Bad argument " << optarg_ << std::endl << std::endl;
127 int main(
int argc,
const char **argv)
129 std::string opt_conf =
"/usr/share/BiclopsDefault.cfg";
132 if (getOptions(argc, argv, opt_conf) ==
false) {
148 std::cout <<
"Set Joint position "
150 <<
" tilt: " <<
vpMath::deg(q[1]) <<
" deg" << std::endl;
151 robot.setPositioningVelocity(30.);
155 std::cout <<
" Joint position "
158 std::cout <<
" Joint velocity "
163 std::cout <<
"Set Joint position "
165 <<
" tilt: " <<
vpMath::deg(q[1]) <<
" deg" << std::endl;
166 robot.setPositioningVelocity(10);
170 std::cout <<
" Joint position: "
173 std::cout <<
" Joint velocity: "
176 std::cout <<
"Set STATE_VELOCITY_CONTROL" << std::endl;
180 std::cout <<
" Joint position: "
182 <<
" tilt: " <<
vpMath::deg(qm[1]) <<
" deg" << std::endl;
184 std::cout <<
" Joint velocity "
190 std::cout <<
"Set joint velocity for 5 sec"
192 <<
" tilt: " <<
vpMath::deg(qdot[1]) <<
" deg/s" << std::endl;
199 std::cout <<
" Joint position "
201 <<
" tilt: " <<
vpMath::deg(qm[1]) <<
" deg" << std::endl;
203 std::cout <<
" Joint velocity "
209 std::cout <<
"Set joint velocity for 3 sec"
211 <<
" tilt: " <<
vpMath::deg(qdot[1]) <<
" deg/s" << std::endl;
218 std::cout <<
" Joint position "
220 <<
" tilt: " <<
vpMath::deg(qm[1]) <<
" deg" << std::endl;
222 std::cout <<
" Joint velocity "
228 std::cout <<
"Set joint velocity for 2 sec"
230 <<
" tilt: " <<
vpMath::deg(qdot[1]) <<
" deg/s" << std::endl;
237 std::cout <<
" Joint position "
239 <<
" tilt: " <<
vpMath::deg(qm[1]) <<
" deg" << std::endl;
241 std::cout <<
" Joint velocity "
248 std::cout <<
"Set joint velocity for 2 sec"
250 <<
" tilt: " <<
vpMath::deg(qdot[1]) <<
" deg/s" << std::endl;
257 std::cout <<
" Joint position "
259 <<
" tilt: " <<
vpMath::deg(qm[1]) <<
" deg" << std::endl;
261 std::cout <<
" Joint velocity "
266 std::cout <<
"Catch an exception: " << e.
getMessage() << std::endl;
273 std::cout <<
"You do not have an Biclops PT robot connected to your computer..." << std::endl;
static const unsigned int ndof
Number of dof.
Implementation of column vector and the associated operations.
error that can be emitted by ViSP classes.
const char * getMessage() const
static double rad(double deg)
static double deg(double rad)
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
Interface for the Biclops, pan, tilt head control.
void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel) vp_override
@ STATE_POSITION_CONTROL
Initialize the position controller.
@ STATE_VELOCITY_CONTROL
Initialize the velocity controller.
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
VISP_EXPORT int wait(double t0, double t)