57 #include <visp3/io/vpParseArgv.h> 58 #include <visp3/core/vpConfig.h> 59 #include <visp3/core/vpDebug.h> 60 #include <visp3/core/vpColVector.h> 61 #include <visp3/core/vpTime.h> 63 #ifdef VISP_HAVE_BICLOPS 65 #include <visp3/robot/vpRobotBiclops.h> 68 #define GETOPTARGS "c:h" 79 void usage(
const char *name,
const char *badparam, std::string conf)
82 Move the biclops robot\n\ 85 %s [-c <Biclops configuration file>] [-h]\n \ 90 -c <Biclops configuration file> %s\n\ 91 Sets the biclops robot configuration file.\n\n",
95 fprintf(stderr,
"ERROR: \n" );
96 fprintf(stderr,
"\nBad parameter [%s]\n", badparam);
111 bool getOptions(
int argc,
const char **argv, std::string& conf)
118 case 'c': conf = optarg_;
break;
119 case 'h': usage(argv[0], NULL, conf);
return false;
break;
122 usage(argv[0], optarg_, conf);
return false;
break;
126 if ((c == 1) || (c == -1)) {
128 usage(argv[0], NULL, conf);
129 std::cerr <<
"ERROR: " << std::endl;
130 std::cerr <<
" Bad argument " << optarg_ << std::endl << std::endl;
138 main(
int argc,
const char ** argv)
140 std::string opt_conf =
"/usr/share/BiclopsDefault.cfg";
143 if (getOptions(argc, argv, opt_conf) ==
false) {
159 vpCTRACE <<
"Set position in the articular frame: " 161 <<
" tilt: " <<
vpMath::deg(q[1]) <<
" deg" << std::endl ;
162 robot.setPositioningVelocity(30.) ;
166 vpCTRACE <<
"Position in the articular frame: " 170 vpCTRACE <<
"Velocity in the articular frame: " 174 vpCTRACE <<
"---------------------------------------- " << std::endl;
178 vpCTRACE <<
"Set position in the articular frame: " 180 <<
" tilt: " <<
vpMath::deg(q[1]) <<
" deg" << std::endl ;
181 robot.setPositioningVelocity(10) ;
185 vpCTRACE <<
"Position in the articular frame: " 189 vpCTRACE <<
"Velocity in the articular frame: " 193 vpCTRACE <<
"---------------------------------------- " << std::endl;
195 vpCTRACE <<
"Set STATE_VELOCITY_CONTROL" << std::endl;
199 vpCTRACE <<
"Position in the articular frame: " 201 <<
" tilt: " <<
vpMath::deg(qm[1]) <<
" deg" << std::endl ;
203 vpCTRACE <<
"Velocity in the articular frame: " 207 vpCTRACE <<
"---------------------------------------- " << std::endl;
211 vpCTRACE <<
"Set articular frame velocity " 213 <<
" tilt: " <<
vpMath::deg(qdot[1]) <<
" deg/s" << std::endl ;
220 vpCTRACE <<
"Position in the articular frame: " 222 <<
" tilt: " <<
vpMath::deg(qm[1]) <<
" deg" << std::endl ;
224 vpCTRACE <<
"Velocity in the articular frame: " 229 vpCTRACE <<
"---------------------------------------- " << std::endl;
233 vpCTRACE <<
"Set articular frame velocity " 235 <<
" tilt: " <<
vpMath::deg(qdot[1]) <<
" deg/s" << std::endl ;
242 vpCTRACE <<
"Position in the articular frame: " 244 <<
" tilt: " <<
vpMath::deg(qm[1]) <<
" deg" << std::endl ;
246 vpCTRACE <<
"Velocity in the articular frame: " 251 vpCTRACE <<
"---------------------------------------- " << std::endl;
257 vpCTRACE <<
"Set articular frame velocity " 259 <<
" tilt: " <<
vpMath::deg(qdot[1]) <<
" deg/s" << std::endl ;
266 vpCTRACE <<
"Position in the articular frame: " 268 <<
" tilt: " <<
vpMath::deg(qm[1]) <<
" deg" << std::endl ;
270 vpCTRACE <<
"Velocity in the articular frame: " 274 vpCTRACE <<
"---------------------------------------- " << std::endl;
280 vpCTRACE <<
"Set articular frame velocity " 282 <<
" tilt: " <<
vpMath::deg(qdot[1]) <<
" deg/s" << std::endl ;
289 vpCTRACE <<
"Position in the articular frame: " 291 <<
" tilt: " <<
vpMath::deg(qm[1]) <<
" deg" << std::endl ;
293 vpCTRACE <<
"Velocity in the articular frame: " 306 vpERROR_TRACE(
"You do not have a biclops robot connected to your computer...");
void setPosition(const vpHomogeneousMatrix &wMc)
VISP_EXPORT int wait(double t0, double t)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
static const unsigned int ndof
Initialize the position controller.
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Initialize the velocity controller.
vpHomogeneousMatrix getPosition() const
Interface for the biclops, pan, tilt head control.
static double rad(double deg)
static double deg(double rad)
Implementation of column vector and the associated operations.