示例#1
0
void Communication::publish_objectToGrasp_moveitFormat(){
    ros::Rate r(1);
    ros::NodeHandle node;
    static tf::TransformBroadcaster br;
    Eigen::Matrix4f camToWorldMatrix; camToWorldMatrix.setZero();camToWorldMatrix(0,2) = 1; camToWorldMatrix(1,0) = -1; camToWorldMatrix(2,1) = -1; camToWorldMatrix(3,3) = 1;

    while(node.ok()){
        pcl::PointCloud<PointT>::Ptr object_pc = m_object_ex_ptr->getObjectToGrasp();
        pcl::PointCloud<PointT>::Ptr object_pc_transformed(new pcl::PointCloud<PointT>);
//        Eigen::Vector4f c = m_object_ex_ptr->getGraspCentroid(); c(3)=1;
//        tf::Transform simpleTF;
        tf::StampedTransform camToJacoTf;
        tf::TransformListener listener;
        bool tf_ready = listener.waitForTransform("camera_rgb_frame","root",ros::Time(0),ros::Duration(5.0));
        if(object_pc->size() > 0 && tf_ready){
            listener.lookupTransform("camera_rgb_frame","root",ros::Time(0),camToJacoTf);
            Eigen::Matrix4f camToJacoMatrix;
            pcl_ros::transformAsMatrix(camToJacoTf,camToJacoMatrix);

            Eigen::Matrix4f combinedMatrix = camToJacoMatrix.inverse() * camToWorldMatrix;
//            Eigen::Vector4f result = combinedMatrix * c;
//            Eigen::Matrix4f res;  res(0,3) = result(0); res(1,3) = result(1); res(2,3) = result(2); res(3,3) = 1;
//            simpleTF = tfFromEigen(res);
//            br.sendTransform(tf::StampedTransform(simpleTF, ros::Time::now(), "root", "object_centroid_test"));

            pcl::transformPointCloud(*object_pc, *object_pc_transformed, combinedMatrix);
//            std::cout << "old : " << object_pc->at(100) << std::endl;
//            std::cout << "new : " << object_pc_transformed->at(100) << std::endl;
            ObjectToGrasp_publisher_.publish(object_pc_transformed);
        }
        r.sleep();
    }
}
Eigen::Matrix4f ForwardKinematicsLiego::getDash(Eigen::Vector3f vec) {
	Eigen::Matrix4f res;
	res.setZero();
	res(0, 0) = 0;
	res(0, 1) = -vec[2];
	res(0, 2) = vec[1];
	res(1, 0) = vec[2];
	res(1, 1) = 0;
	res(1, 2) = -vec[0];
	res(2, 0) = -vec[1];
	res(2, 1) = vec[0];
	res(2, 2) = 0;
	return res;
}
示例#3
0
void Scene::DrawScene(ParticleSystem* m, double strainSize, bool drawPoints) {
  int pSize;
  int cSize;
  //float* points = m->GetPositions3d(&pSize);
  //float* colors = m->GetColors(&cSize, strainSize);

  float *points, *colors;
  switch(drawMode) {
    case 0:
      points = m->GetSurfaceTriangles3d(&pSize);
      colors = m->GetTriColors(&cSize, strainSize);
      break;
    case 1:
      points = m->GetPositions3d(&pSize, prevMode);
      colors = m->GetColors(&cSize, strainSize, xpos, ypos, zpos);
      break;
    case 2:
      points = m->GetSurfaceTriangles3d(&pSize);
      colors = m->GetStrainSurfaceTriColors(&cSize, strainSize);
      break;
    case 3:
      points = m->GetTetCenter(&pSize);
      colors = m->GetCenterColors(&cSize, strainSize);
      break;
    case 4:
      points = m->GetAllTriangles3d(&pSize);
      colors = m->GetStrainAllTriColors(&cSize, strainSize);
      break;
  }

  Eigen::Matrix4f rotationMatrix;
  Eigen::Matrix4f projectionMatrix;
  rotationMatrix.setZero();
  projectionMatrix.setZero();
  Eigen::Vector3f pos, target, up;
  pos << xpos, ypos, zpos;
  m->GetCameraPosAndSize(&xtarg, &ytarg, &ztarg);
  target << xtarg, ytarg, ztarg;
  up << 0, -1, 0;
  RotationMatrix(pos, target, up, rotationMatrix);
  PerspectiveMatrix((65*PI)/180.0, ((float)DDWIDTH)/DDHEIGHT, .5, 100, projectionMatrix);
  g_viewMatrix = projectionMatrix * rotationMatrix;


  DrawDelegate::BeginFrame();
  DrawDelegate::SetViewMatrix(g_viewMatrix.data());
  Scene::DrawGrid(1, m->groundLevel);
  if (drawPoints) {
     DrawDelegate::SetLineSize(3);

     switch(drawMode) {
       case 0:
         DrawDelegate::DrawTriangles(points, pSize, colors, cSize);
         break;
       case 1:
         DrawDelegate::DrawLines(points, pSize, colors, cSize);
         break;
       case 2:
         DrawDelegate::DrawTriangles(points, pSize, colors, cSize);
         break;
       case 3:
         DrawDelegate::DrawPoints(points, pSize, colors, cSize);
         break;
       case 4:
         DrawDelegate::DrawTriangles(points, pSize, colors, cSize);
         break;
     }
  }
}