int main()
{
//////////////////////////////////////////
// sets the initial camera location
vpHomogeneousMatrix cMo(0.3,0.2,3,
vpMath::rad(0),vpMath::rad(0),vpMath::rad(40)) ;
///////////////////////////////////
// initialize the robot
vpRobotCamera robot ;
robot.setSamplingTime(0.04); // 40ms
robot.setPosition(cMo) ;
//initialize the camera parameters
vpCameraParameters cam(800,800,240,180);
//Image definition
unsigned int height = 360 ;
unsigned int width = 480 ;
vpImage<unsigned char> I(height,width);
//Display initialization
vpDisplayGTK disp;
disp.init(I,100,100,"Simulation display");
////////////////////////////////////////
// Desired visual features initialization
// sets the points coordinates in the object frame (in meter)
vpPoint point[4] ;
point[0].setWorldCoordinates(-0.1,-0.1,0) ;
point[1].setWorldCoordinates(0.1,-0.1,0) ;
point[2].setWorldCoordinates(0.1,0.1,0) ;
point[3].setWorldCoordinates(-0.1,0.1,0) ;
// sets the desired camera location
vpHomogeneousMatrix cMo_d(0,0,1,0,0,0) ;
// computes the 3D point coordinates in the camera frame and its 2D coordinates
for (int i = 0 ; i < 4 ; i++)
point[i].project(cMo_d) ;
// creates the associated features
vpFeaturePoint pd[4] ;
for (int i = 0 ; i < 4 ; i++)
vpFeatureBuilder::create(pd[i],point[i]) ;
///////////////////////////////////////
// Current visual features initialization
// computes the 3D point coordinates in the camera frame and its 2D coordinates
for (int i = 0 ; i < 4 ; i++)
point[i].project(cMo) ;
// creates the associated features
vpFeaturePoint p[4] ;
for (int i = 0 ; i < 4 ; i++)
vpFeatureBuilder::create(p[i],point[i]) ;
/////////////////////////////////
// Task defintion
vpServo task ;
// we want an eye-in-hand control law ;
task.setServo(vpServo::EYEINHAND_L_cVe_eJe) ;
task.setInteractionMatrixType(vpServo::DESIRED, vpServo::PSEUDO_INVERSE) ;
// Set the position of the camera in the end-effector frame
vpHomogeneousMatrix cMe ;
vpVelocityTwistMatrix cVe(cMe) ;
task.set_cVe(cVe) ;
// Set the Jacobian (expressed in the end-effector frame)
vpMatrix eJe ;
robot.get_eJe(eJe) ;
task.set_eJe(eJe) ;
// we want to see a point on a point
for (int i = 0 ; i < 4 ; i++)
task.addFeature(p[i],pd[i]) ;
// Set the gain
task.setLambda(1.0) ;
// Print the current information about the task
task.print();
////////////////////////////////////////////////
// The control loop
int k = 0;
while(k++ < 200){
double t = vpTime::measureTimeMs();
// Display the image background
vpDisplay::display(I);
// Update the current features
for (int i = 0 ; i < 4 ; i++)
{
point[i].project(cMo) ;
vpFeatureBuilder::create(p[i],point[i]) ;
}
// Display the task features (current and desired)
vpServoDisplay::display(task,cam,I);
vpDisplay::flush(I);
// Update the robot Jacobian
robot.get_eJe(eJe) ;
task.set_eJe(eJe) ;
// Compute the control law
vpColVector v = task.computeControlLaw() ;
// Send the computed velocity to the robot and compute the new robot position
robot.setVelocity(vpRobot::ARTICULAR_FRAME, v) ;
robot.getPosition(cMo) ;
// Print the current information about the task
task.print();
// Wait 40 ms
vpTime::wait(t,40);
}
task.kill();
return 0;
}
static
void *mainLoop (void *_simu)
{
// pointer copy of the vpSimulator instance
vpSimulator *simu = (vpSimulator *)_simu ;
// Simulation initialization
simu->initMainApplication() ;
///////////////////////////////////
// Set the initial camera location
vpHomogeneousMatrix cMo(0.3,0.2,3,
vpMath::rad(0),vpMath::rad(0),vpMath::rad(40));
///////////////////////////////////
// Initialize the robot
vpRobotCamera robot ;
robot.setSamplingTime(0.04); // 40ms
robot.setPosition(cMo) ;
// Send the robot position to the visualizator
simu->setCameraPosition(cMo) ;
// Initialize the camera parameters
vpCameraParameters cam ;
simu->getCameraParameters(cam);
////////////////////////////////////////
// Desired visual features initialization
// sets the points coordinates in the object frame (in meter)
vpPoint point[4] ;
point[0].setWorldCoordinates(-0.1,-0.1,0) ;
point[1].setWorldCoordinates(0.1,-0.1,0) ;
point[2].setWorldCoordinates(0.1,0.1,0) ;
point[3].setWorldCoordinates(-0.1,0.1,0) ;
// sets the desired camera location
vpHomogeneousMatrix cMo_d(0,0,1,0,0,0) ;
// computes the 3D point coordinates in the camera frame and its 2D coordinates
for (int i = 0 ; i < 4 ; i++)
point[i].project(cMo_d) ;
// creates the associated features
vpFeaturePoint pd[4] ;
for (int i = 0 ; i < 4 ; i++)
vpFeatureBuilder::create(pd[i],point[i]) ;
///////////////////////////////////////
// Current visual features initialization
unsigned int height = simu->getInternalHeight();
unsigned int width = simu->getInternalWidth();
// Create a greyscale image
vpImage<unsigned char> I(height,width);
//Display initialization
#if defined(VISP_HAVE_X11)
vpDisplayX disp;
#elif defined(VISP_HAVE_GDI)
vpDisplayGDI disp;
#elif defined(VISP_HAVE_GTK)
vpDisplayGTK disp;
#endif
disp.init(I,100,100,"Simulation display");
// disp(I);
// Get the current image
vpTime::wait(500); // wait to be sure the image is generated
simu->getInternalImage(I);
// Display the current image
vpDisplay::display(I);
vpDisplay::flush(I);
// Initialize the four dots tracker
std::cout << "A click in the four dots clockwise. " << std::endl;
vpDot2 dot[4];
vpFeaturePoint p[4];
for (int i = 0 ; i < 4 ; i++)
{
dot[i].setGraphics(true);
// Call for a click
std::cout << "A click in the dot " << i << std::endl;
dot[i].initTracking(I);
// Create the associated feature
vpFeatureBuilder::create(p[i],cam,dot[i]);
// flush the display
vpDisplay::flush(I);
}
/////////////////////////////////
// Task defintion
vpServo task ;
// we want an eye-in-hand control law ;
task.setServo(vpServo::EYEINHAND_L_cVe_eJe) ;
task.setInteractionMatrixType(vpServo::DESIRED) ;
// Set the position of the camera in the end-effector frame
vpHomogeneousMatrix cMe ;
vpVelocityTwistMatrix cVe(cMe) ;
task.set_cVe(cVe) ;
// Set the Jacobian (expressed in the end-effector frame)
vpMatrix eJe ;
robot.get_eJe(eJe) ;
task.set_eJe(eJe) ;
// we want to see a point on a point
for (int i = 0 ; i < 4 ; i++)
task.addFeature(p[i],pd[i]) ;
// Set the gain
task.setLambda(1.0) ;
// Print the current information about the task
task.print();
vpTime::wait(500);
////////////////////////////////////////////////
// The control loop
int k = 0;
while(k++ < 200){
double t = vpTime::measureTimeMs();
// Get the current internal camera view and display it
simu->getInternalImage(I);
vpDisplay::display(I);
// Track the four dots and update the associated visual features
for (int i = 0 ; i < 4 ; i++)
{
dot[i].track(I) ;
vpFeatureBuilder::create(p[i],cam,dot[i]) ;
}
// Display the desired and current visual features
vpServoDisplay::display(task,cam,I) ;
vpDisplay::flush(I);
// Update the robot Jacobian
robot.get_eJe(eJe) ;
task.set_eJe(eJe) ;
// Compute the control law
vpColVector v = task.computeControlLaw() ;
// Send the computed velocity to the robot and compute the new robot position
robot.setVelocity(vpRobot::ARTICULAR_FRAME, v) ;
robot.getPosition(cMo) ;
// Send the robot position to the visualizator
simu->setCameraPosition(cMo) ;
// Wait 40 ms
vpTime::wait(t,40);
}
// Print information about the task
task.print();
task.kill();
simu->closeMainApplication() ;
void *a=NULL ;
return a ;
}
static
void *mainLoop (void *_simu)
{
// pointer copy of the vpSimulator instance
vpSimulator *simu = (vpSimulator *)_simu ;
// Simulation initialization
simu->initMainApplication() ;
/////////////////////////////////////////
// sets the initial camera location
vpHomogeneousMatrix cMo(-0.3,-0.2,3,
vpMath::rad(0),vpMath::rad(0),vpMath::rad(40)) ;
///////////////////////////////////
// Initialize the robot
vpRobotCamera robot ;
robot.setSamplingTime(0.04); // 40ms
robot.setPosition(cMo) ;
// Send the robot position to the visualizator
simu->setCameraPosition(cMo) ;
// Initialize the camera parameters
vpCameraParameters cam ;
simu->getCameraParameters(cam);
////////////////////////////////////////
// Desired visual features initialization
// sets the points coordinates in the object frame (in meter)
vpPoint point[4] ;
point[0].setWorldCoordinates(-0.1,-0.1,0) ;
point[1].setWorldCoordinates(0.1,-0.1,0) ;
point[2].setWorldCoordinates(0.1,0.1,0) ;
point[3].setWorldCoordinates(-0.1,0.1,0) ;
// sets the desired camera location
vpHomogeneousMatrix cMo_d(0,0,1,0,0,0) ;
// computes the 3D point coordinates in the camera frame and its 2D coordinates
for (int i = 0 ; i < 4 ; i++)
point[i].project(cMo_d) ;
// creates the associated features
vpFeaturePoint pd[4] ;
for (int i = 0 ; i < 4 ; i++)
vpFeatureBuilder::create(pd[i],point[i]) ;
///////////////////////////////////////
// Current visual features initialization
// computes the 3D point coordinates in the camera frame and its 2D coordinates
for (int i = 0 ; i < 4 ; i++)
point[i].project(cMo) ;
// creates the associated features
vpFeaturePoint p[4] ;
for (int i = 0 ; i < 4 ; i++)
vpFeatureBuilder::create(p[i],point[i]) ;
/////////////////////////////////
// Task defintion
vpServo task ;
// we want an eye-in-hand control law ;
task.setServo(vpServo::EYEINHAND_L_cVe_eJe) ;
task.setInteractionMatrixType(vpServo::DESIRED,vpServo::PSEUDO_INVERSE) ;
// Set the position of the camera in the end-effector frame
vpHomogeneousMatrix cMe ;
vpVelocityTwistMatrix cVe(cMe) ;
task.set_cVe(cVe) ;
// Set the Jacobian (expressed in the end-effector frame)
vpMatrix eJe ;
robot.get_eJe(eJe) ;
task.set_eJe(eJe) ;
// we want to see a point on a point
for (int i = 0 ; i < 4 ; i++)
task.addFeature(p[i],pd[i]) ;
// Set the gain
task.setLambda(1.0) ;
// Print the current information about the task
task.print();
vpTime::wait(500);
////////////////////////////////////////////////
// The control loop
int k = 0;
while(k++ < 200){
double t = vpTime::measureTimeMs();
// Update the current features
for (int i = 0 ; i < 4 ; i++)
{
point[i].project(cMo) ;
vpFeatureBuilder::create(p[i],point[i]) ;
}
// Update the robot Jacobian
robot.get_eJe(eJe) ;
task.set_eJe(eJe) ;
// Compute the control law
vpColVector v = task.computeControlLaw() ;
// Send the computed velocity to the robot and compute the new robot position
robot.setVelocity(vpRobot::ARTICULAR_FRAME, v) ;
robot.getPosition(cMo) ;
// Send the robot position to the visualizator
simu->setCameraPosition(cMo) ;
// Print the current information about the task
task.print();
// Wait 40 ms
vpTime::wait(t,40);
}
task.kill();
simu->closeMainApplication() ;
void *a=NULL ;
return a ;
// return (void *);
}
