vpHomogeneousMatrix TJoint::getJointTransformation()
{
vpRotationMatrix rm;
vpTranslationVector tv(axis[0] * current_value, axis[1] * current_value, axis[2] * current_value);
cerr << "getJointTransform: " << tv.t() << endl;
return vpHomogeneousMatrix(tv, rm);
}
void
TrackerViewer::callback
(const sensor_msgs::ImageConstPtr& image,
const sensor_msgs::CameraInfoConstPtr& info,
const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& trackingResult,
const visp_tracker::MovingEdgeSites::ConstPtr& sites,
const visp_tracker::KltPoints::ConstPtr& klt)
{
// Copy image.
try
{
rosImageToVisp(image_, image);
}
catch(std::exception& e)
{
ROS_ERROR_STREAM("dropping frame: " << e.what());
}
// Copy moving camera infos, edges sites and optional KLT points.
info_ = info;
sites_ = sites;
klt_ = klt;
// Copy cMo.
cMo_ = vpHomogeneousMatrix();
transformToVpHomogeneousMatrix(*cMo_, trackingResult->pose.pose);
}
vpHomogeneousMatrix Transform::getTransform() {
KDL::Rotation vRek;
vRek=KDL::Rotation::Quaternion(msg_.rotation.x, msg_.rotation.y, msg_.rotation.z, msg_.rotation.w);
vpRotationMatrix vRe;
memcpy(vRe.data, vRek.data,9*sizeof(double));
vpTranslationVector vTe(msg_.translation.x, msg_.translation.y, msg_.translation.z);
return vpHomogeneousMatrix(vTe, vRe);
}
void PerfectHandTOG::computeRollerTurnTOG(ObjectAction *oa, TOGPrimitive **togp, ObjectFrame **of, TaskFrame **tf)
{
cerr << "Computing Roller Turn plan on object " << oa->object->getName() << " for PerfectHand" << endl;
//Compute the facing vector
vpColVector f = getFacingVector(oa->object);
//Set preshape to cylindrical precission
*togp = new TOGPrimitive(new PerfectHandLateral(), new PerfectHandOPhalanx());
((PerfectHandLateral*)(*togp)->preshape)->setClosing(60);
((PerfectHandOPhalanx*)(*togp)->frame)->setFinger(4);
//Set object frame to face which normal matches the container's normal.
//Y direction of the object frame must go in opposite direction to the
//facing vector. Object frame X axis is aligned with container's Y axis
//when possible. However, for this object frame, rotation in Z is not defined.
vpHomogeneousMatrix oMof;
vpColVector n(3), o(3), a(3);
o = -f;
if (fabs(f[2]) == 1 || fabs(f[0]) == 1)
{
n = 0;
n[1] = -1;
}
else
{
n = 0;
n[2] = -f[1];
}
a = vpColVector::cross(n, o);
oMof.setIdentity();
for (int i = 0; i < 3; i++)
{
oMof[i][0] = n[i];
oMof[i][1] = o[i];
oMof[i][2] = a[i];
}
*of = new ObjectFrame(oa->object, oMof);
vpColVector uv(6);
uv = 0;
uv[3] = f[0];
uv[4] = f[1];
uv[5] = f[2];
//TODO: chech turning direction and multiply uv accordingly
*tf = new TaskFrame(oa->object, vpHomogeneousMatrix(0, 0, 0, 0, 0, 0), uv);
}
int test(double x,double y,double z,double alpha){
//intial pose
vpHomogeneousMatrix cMo(x,y,z,-vpMath::rad(0),vpMath::rad(0),alpha);
//Desired pose
vpHomogeneousMatrix cdMo(vpHomogeneousMatrix(0.0,0.0,1.0,vpMath::rad(0),vpMath::rad(0),-vpMath::rad(0)));
//source and destination objects for moment manipulation
vpMomentObject src(6);
vpMomentObject dst(6);
//init and run the simulation
initScene(cMo, cdMo, src, dst); //initialize graphical scene (for interface)
vpMatrix mat = execute(cMo, cdMo, src, dst);
if(fabs(mat[0][0]-(-1)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
if(fabs(mat[0][1]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
if(fabs(mat[0][2]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
if(fabs(mat[1][0]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
if(fabs(mat[1][1]-(-1)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
if(fabs(mat[1][2]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
if(fabs(mat[2][0]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
if(fabs(mat[2][1]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
if(fabs(mat[2][2]-(-1)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
if(fabs(mat[2][5]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
if(fabs(mat[3][0]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
if(fabs(mat[3][1]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
if(fabs(mat[3][2]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
if(fabs(mat[3][5]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
if(fabs(mat[4][0]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
if(fabs(mat[4][1]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
if(fabs(mat[4][2]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
if(fabs(mat[4][5]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
if(fabs(mat[5][0]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
if(fabs(mat[5][1]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
if(fabs(mat[5][2]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
if(fabs(mat[5][5]-(-1)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
return 0;
}
vpHomogeneousMatrix DoorHandleDetectionNode::createTFPlane(const vpColVector coeffs, const double x, const double y, const double z)
{
vpColVector xp(3);
vpColVector yp(3);
vpColVector normal(3);
vpRotationMatrix dRp;
vpTranslationVector P0;
vpHomogeneousMatrix dMp;
geometry_msgs::Pose dMp_msg;
tf::Transform transform;
static tf::TransformBroadcaster br;
//Create a normal to the plan from the coefficients
normal[0] = -coeffs[0];
normal[1] = -coeffs[1];
normal[2] = -coeffs[2];
normal.normalize();
//Create a xp vector that is following the equation of the plane
xp[0] = - (coeffs[1]*y + coeffs[2]*(z+0.05) + coeffs[3]) / (coeffs[0]) - x;
xp[1] = 0;
xp[2] = 0.05;
xp.normalize();
//Create a yp vector with the normal and xp
yp = vpColVector::cross(normal,xp);
//Create the Rotation Matrix
dRp[0][0] = xp[0];
dRp[1][0] = xp[1];
dRp[2][0] = xp[2];
dRp[0][1] = yp[0];
dRp[1][1] = yp[1];
dRp[2][1] = yp[2];
dRp[0][2] = normal[0];
dRp[1][2] = normal[1];
dRp[2][2] = normal[2];
transform.setOrigin( tf::Vector3(x, y, z) );
//Calculate the z0 for the translation vector
double z0 = -(coeffs[0]*x + coeffs[1]*y + coeffs[3])/(coeffs[2]);
//Create the translation Vector
P0 = vpTranslationVector(x, y, z0);
//Create the homogeneous Matrix
dMp = vpHomogeneousMatrix(P0, dRp);
//Publish the tf of the plane
dMp_msg = visp_bridge::toGeometryMsgsPose(dMp);
tf::Quaternion q;
q.setX(dMp_msg.orientation.x);
q.setY(dMp_msg.orientation.y);
q.setZ(dMp_msg.orientation.z);
q.setW(dMp_msg.orientation.w);
transform.setRotation(q);
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), m_parent_depth_tf, "tf_plane"));
return dMp;
}
vpHomogeneousMatrix DoorHandleDetectionNode::createTFLine(const vpColVector direction_axis, vpColVector normal, const double x, const double y, const double z)
{
vpRotationMatrix dRdh;
vpHomogeneousMatrix dMdh;
vpHomogeneousMatrix dMdh_border;
geometry_msgs::Pose dMdh_msg_border;
tf::Transform transformdh;
static tf::TransformBroadcaster br;
vpColVector direction_line(3);
vpColVector centroid;
vpColVector centroidBorder;
centroid.stack(x);
centroid.stack(y);
centroid.stack(z);
centroid.stack(1);
//Normalize the direction axis
direction_line[0]=direction_axis[0];
direction_line[1]=direction_axis[1];
direction_line[2]=direction_axis[2];
direction_line.normalize();
vpColVector y_dh(3);
y_dh = vpColVector::cross(normal,direction_line);
//Create the Rotation Matrix
dRdh[0][0] = direction_line[0];
dRdh[1][0] = direction_line[1];
dRdh[2][0] = direction_line[2];
dRdh[0][1] = y_dh[0];
dRdh[1][1] = y_dh[1];
dRdh[2][1] = y_dh[2];
dRdh[0][2] = normal[0];
dRdh[1][2] = normal[1];
dRdh[2][2] = normal[2];
//Create the pose of the handle with respect to the depth camera in the cog of the handle
dMdh = vpHomogeneousMatrix(vpTranslationVector(x, y, z), dRdh);
//Put the pose of the handle in its rotation axis instead of its cog
centroidBorder = dMdh.inverse() * centroid;
centroidBorder[0] = centroidBorder[0] - (m_lenght_dh / 2) + 0.015;
centroidBorder = dMdh * centroidBorder;
//Create the pose of the handle with respect to the depth camera in the rotation axis of the handle
dMdh_border = vpHomogeneousMatrix(vpTranslationVector(centroidBorder[0], centroidBorder[1], centroidBorder[2]), dRdh);
dMdh_msg_border = visp_bridge::toGeometryMsgsPose(dMdh_border);
//Publish the tf of the handle with respect to the depth camera
transformdh.setOrigin( tf::Vector3(centroidBorder[0], centroidBorder[1], centroidBorder[2] ));
tf::Quaternion qdh;
qdh.setX(dMdh_msg_border.orientation.x);
qdh.setY(dMdh_msg_border.orientation.y);
qdh.setZ(dMdh_msg_border.orientation.z);
qdh.setW(dMdh_msg_border.orientation.w);
transformdh.setRotation(qdh);
br.sendTransform(tf::StampedTransform(transformdh, ros::Time::now(), m_parent_depth_tf, "door_handle_tf"));
return dMdh_border;
}
int
main(int argc, const char ** argv)
{
try {
bool opt_click_allowed = true;
bool opt_display = true;
// Read the command line options
if (getOptions(argc, argv, opt_click_allowed, opt_display) == false) {
exit (-1);
}
// We open two displays, one for the internal camera view, the other one for
// the external view, using either X11, GTK or GDI.
#if defined VISP_HAVE_X11
vpDisplayX displayInt;
#elif defined VISP_HAVE_GDI
vpDisplayGDI displayInt;
#elif defined VISP_HAVE_OPENCV
vpDisplayOpenCV displayInt;
#endif
vpImage<unsigned char> Iint(480, 640, 255);
if (opt_display) {
// open a display for the visualization
displayInt.init(Iint,700,0, "Internal view") ;
}
vpServo task;
std::cout << std::endl ;
std::cout << "----------------------------------------------" << std::endl ;
std::cout << " Test program for vpServo " <<std::endl ;
std::cout << " Eye-in-hand task control, articular velocity are computed"
<< std::endl ;
std::cout << " Simulation " << std::endl ;
std::cout << " task : servo 4 points " << std::endl ;
std::cout << "----------------------------------------------" << std::endl ;
std::cout << std::endl ;
// sets the initial camera location
vpHomogeneousMatrix cMo(-0.05,-0.05,0.7,
vpMath::rad(10), vpMath::rad(10), vpMath::rad(-30));
// sets the point coordinates in the object frame
vpPoint point[4] ;
point[0].setWorldCoordinates(-0.045,-0.045,0) ;
point[3].setWorldCoordinates(-0.045,0.045,0) ;
point[2].setWorldCoordinates(0.045,0.045,0) ;
point[1].setWorldCoordinates(0.045,-0.045,0) ;
// computes the point coordinates in the camera frame and its 2D coordinates
for (unsigned int i = 0 ; i < 4 ; i++)
point[i].track(cMo) ;
// sets the desired position of the point
vpFeaturePoint p[4] ;
for (unsigned int i = 0 ; i < 4 ; i++)
vpFeatureBuilder::create(p[i],point[i]) ; //retrieve x,y and Z of the vpPoint structure
// sets the desired position of the feature point s*
vpFeaturePoint pd[4] ;
// Desired pose
vpHomogeneousMatrix cdMo(vpHomogeneousMatrix(0.0,0.0,0.8,vpMath::rad(0),vpMath::rad(0),vpMath::rad(0)));
// Projection of the points
for (unsigned int i = 0 ; i < 4 ; i++)
point[i].track(cdMo);
for (unsigned int i = 0 ; i < 4 ; i++)
vpFeatureBuilder::create(pd[i], point[i]);
// define the task
// - we want an eye-in-hand control law
// - articular velocity are computed
task.setServo(vpServo::EYEINHAND_CAMERA);
task.setInteractionMatrixType(vpServo::DESIRED) ;
// we want to see a point on a point
for (unsigned int i = 0 ; i < 4 ; i++)
task.addFeature(p[i],pd[i]) ;
// set the gain
task.setLambda(0.8) ;
// Declaration of the robot
vpSimulatorAfma6 robot(opt_display);
// Initialise the robot and especially the camera
robot.init(vpAfma6::TOOL_CCMOP, vpCameraParameters::perspectiveProjWithoutDistortion);
robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
// Initialise the object for the display part*/
robot.initScene(vpWireFrameSimulator::PLATE, vpWireFrameSimulator::D_STANDARD);
// Initialise the position of the object relative to the pose of the robot's camera
robot.initialiseObjectRelativeToCamera(cMo);
// Set the desired position (for the displaypart)
robot.setDesiredCameraPosition(cdMo);
// Get the internal robot's camera parameters
vpCameraParameters cam;
robot.getCameraParameters(cam,Iint);
if (opt_display)
{
//Get the internal view
vpDisplay::display(Iint);
robot.getInternalView(Iint);
vpDisplay::flush(Iint);
}
// Display task information
task.print() ;
unsigned int iter=0 ;
vpTRACE("\t loop") ;
while(iter++<500)
{
std::cout << "---------------------------------------------" << iter <<std::endl ;
vpColVector v ;
// Get the Time at the beginning of the loop
double t = vpTime::measureTimeMs();
// Get the current pose of the camera
cMo = robot.get_cMo();
if (iter==1) {
std::cout <<"Initial robot position with respect to the object frame:\n";
cMo.print();
}
// new point position
for (unsigned int i = 0 ; i < 4 ; i++)
{
point[i].track(cMo) ;
// retrieve x,y and Z of the vpPoint structure
vpFeatureBuilder::create(p[i],point[i]) ;
}
if (opt_display)
{
// Get the internal view and display it
vpDisplay::display(Iint) ;
robot.getInternalView(Iint);
vpDisplay::flush(Iint);
}
if (opt_display && opt_click_allowed && iter == 1)
{
// suppressed for automate test
std::cout << "Click in the internal view window to continue..." << std::endl;
vpDisplay::getClick(Iint) ;
}
// compute the control law
v = task.computeControlLaw() ;
// send the camera velocity to the controller
robot.setVelocity(vpRobot::CAMERA_FRAME, v) ;
std::cout << "|| s - s* || " << ( task.getError() ).sumSquare() <<std::endl ;
// The main loop has a duration of 10 ms at minimum
vpTime::wait(t,10);
}
// Display task information
task.print() ;
task.kill();
std::cout <<"Final robot position with respect to the object frame:\n";
cMo.print();
if (opt_display && opt_click_allowed)
{
// suppressed for automate test
std::cout << "Click in the internal view window to end..." << std::endl;
vpDisplay::getClick(Iint) ;
}
return 0;
}
catch(vpException e) {
std::cout << "Catch a ViSP exception: " << e << std::endl;
return 1;
}
}
int main()
{
#if defined(VISP_HAVE_PTHREAD)
try {
vpHomogeneousMatrix cdMo(0, 0, 0.75, 0, 0, 0);
vpHomogeneousMatrix cMo(0.15, -0.1, 1., vpMath::rad(10), vpMath::rad(-10), vpMath::rad(50));
/*
Top view of the world frame, the camera frame and the object frame
world, also robot base frame : --> w_y
|
\|/
w_x
object :
o_y
/|\
|
o_x <--
camera :
c_y
/|\
|
c_x <--
*/
vpHomogeneousMatrix wMo(vpTranslationVector(0.40, 0, -0.15),
vpRotationMatrix(vpRxyzVector(-M_PI, 0, M_PI/2.)));
std::vector<vpPoint> point;
point.push_back( vpPoint(-0.1,-0.1, 0) );
point.push_back( vpPoint( 0.1,-0.1, 0) );
point.push_back( vpPoint( 0.1, 0.1, 0) );
point.push_back( vpPoint(-0.1, 0.1, 0) );
vpServo task ;
task.setServo(vpServo::EYEINHAND_CAMERA);
task.setInteractionMatrixType(vpServo::CURRENT);
task.setLambda(0.5);
vpFeaturePoint p[4], pd[4] ;
for (unsigned int i = 0 ; i < 4 ; i++) {
point[i].track(cdMo);
vpFeatureBuilder::create(pd[i], point[i]);
point[i].track(cMo);
vpFeatureBuilder::create(p[i], point[i]);
task.addFeature(p[i], pd[i]);
}
vpSimulatorViper850 robot(true);
robot.setVerbose(true);
// Enlarge the default joint limits
vpColVector qmin = robot.getJointMin();
vpColVector qmax = robot.getJointMax();
qmin[0] = -vpMath::rad(180);
qmax[0] = vpMath::rad(180);
qmax[1] = vpMath::rad(0);
qmax[2] = vpMath::rad(270);
qmin[4] = -vpMath::rad(180);
qmax[4] = vpMath::rad(180);
robot.setJointLimit(qmin, qmax);
std::cout << "Robot joint limits: " << std::endl;
for (unsigned int i=0; i< qmin.size(); i ++)
std::cout << "Joint " << i << ": min " << vpMath::deg(qmin[i]) << " max " << vpMath::deg(qmax[i]) << " (deg)" << std::endl;
robot.init(vpViper850::TOOL_PTGREY_FLEA2_CAMERA, vpCameraParameters::perspectiveProjWithoutDistortion);
robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
robot.initScene(vpWireFrameSimulator::PLATE, vpWireFrameSimulator::D_STANDARD);
robot.set_fMo(wMo);
bool ret = true;
#if VISP_VERSION_INT > VP_VERSION_INT(2,7,0)
ret =
#endif
robot.initialiseCameraRelativeToObject(cMo);
if (ret == false)
return 0; // Not able to set the position
robot.setDesiredCameraPosition(cdMo);
// We modify the default external camera position
robot.setExternalCameraPosition(vpHomogeneousMatrix(vpTranslationVector(-0.4, 0.4, 2),
vpRotationMatrix(vpRxyzVector(M_PI/2,0,0))));
vpImage<unsigned char> Iint(480, 640, 255);
#if defined(VISP_HAVE_X11)
vpDisplayX displayInt(Iint, 700, 0, "Internal view");
#elif defined(VISP_HAVE_GDI)
vpDisplayGDI displayInt(Iint, 700, 0, "Internal view");
#elif defined(VISP_HAVE_OPENCV)
vpDisplayOpenCV displayInt(Iint, 700, 0, "Internal view");
#else
std::cout << "No image viewer is available..." << std::endl;
#endif
vpCameraParameters cam(840, 840, Iint.getWidth()/2, Iint.getHeight()/2);
// Modify the camera parameters to match those used in the other simulations
robot.setCameraParameters(cam);
bool start = true;
//for ( ; ; )
for (int iter =0; iter < 275; iter ++)
{
cMo = robot.get_cMo();
for (unsigned int i = 0 ; i < 4 ; i++) {
point[i].track(cMo);
vpFeatureBuilder::create(p[i], point[i]);
}
vpDisplay::display(Iint);
robot.getInternalView(Iint);
if (!start) {
display_trajectory(Iint, point, cMo, cam);
vpDisplay::displayText(Iint, 40, 120, "Click to stop the servo...", vpColor::red);
}
vpDisplay::flush(Iint);
vpColVector v = task.computeControlLaw();
robot.setVelocity(vpRobot::CAMERA_FRAME, v);
// A click to exit
if (vpDisplay::getClick(Iint, false))
break;
if (start) {
start = false;
v = 0;
robot.setVelocity(vpRobot::CAMERA_FRAME, v);
vpDisplay::displayText(Iint, 40, 120, "Click to start the servo...", vpColor::blue);
vpDisplay::flush(Iint);
//vpDisplay::getClick(Iint);
}
vpTime::wait(1000*robot.getSamplingTime());
}
task.kill();
}
catch(vpException e) {
std::cout << "Catch an exception: " << e << std::endl;
}
#endif
}
