Add API for easily adding markers/ghost robots and other visualizations

[costashatz]

We should add helper functions to RobotDARTSimu for easily adding visualization things. Here's a list:

• Ghost robots (clone of robots with different colors/shading that do not interact with the environment and are there just for visualization purposes) --- maybe this should go to Robot class
• Add visualization spheres
• Add visualization cubes/rectangles
• ...

@jbmouret @dalinel @PedroDesRobots let me know if you have something to add..

[PedroDesRobots]

we could also add :

• Convex Hull visualization (based on multiple spheres or better with 2D polygon with colors/fillings)
• CoM visualization (based on one sphere)
• Capture Point visualization (based on one sphere)
• ...

From my point of view, those quantities are not humanoid robot related but more floating base robot.

And I don't know if it's already in the API, but we need to add a collisionFilter to avoid ghost robot and sphere to collide with the real robot.

[jbmouret]

The convex hull, capture point, etc. might require additional libraries (KDL) and I am not sure we want to impose this by default. I am OK if we can compute it ourselves, however.

One option is to leave these vizualisation in the 'client code' (the code that uses the simulator) and simply provide the API to display any marker.

[costashatz]

The convex hull, capture point, etc. might require additional libraries (KDL) and I am not sure we want to impose this by default. I am OK if we can compute it ourselves, however.

Indeed I am not sure if this can be done without additional libraries. @PedroDesRobots , can you show some code for this?

One option is to leave these vizualisation in the 'client code' (the code that uses the simulator) and simply provide the API to display any marker.

Yes the idea is to provide this 'generic' marker visualization and then the user can do whatever he/she wants. But it'd be nice to provide a few "frequently-used" ones.

And I don't know if it's already in the API, but we need to add a collisionFilter to avoid ghost robot and sphere to collide with the real robot.

It's not already, but this is not related to visualizations. For the ghost robots/visualizations, we should/will remove all the dynamics/collision aspects (DART-related) and then it automatically does not collide/update for dynamics.

[PedroDesRobots]

Indeed I am not sure if this can be done without additional libraries. can you show some code for this?

Maybe the easiest is CoM :

void createComVisualization(const Eigen::Vector3d& CoM,const std::string& name,  const Eigen::Vector4d& color){
  Eigen::Vector6d p;
  p << 0, 0, 0, CoM(0), CoM(1), 0;
  dart::dynamics::SkeletonPtr com = addEllipsoid(p, Eigen::Vector3d(0.02, 0.02, 0.02), "free",1.0, color, "CoM_projection");
  com_map_[name] = com;
  graphObjects_.push_back(com);
}

For Convex Hull :

void createConvexHullVisualization(const std::vector<KDL::Vector>& ch,const std::string& name,  const Eigen::Vector4d& color){
  std::vector<dart::dynamics::SkeletonPtr> convex_hull_vec;
  for(int i =0; i<ch.size() ; i++)
  {
    Eigen::Vector6d p;
    KDL::Vector ch_pos = ch[i];
    p << 0, 0, 0, ch_pos.x(), ch_pos.y(), 0;
    convex_hull_vec.push_back(addEllipsoid(p, Eigen::Vector3d(0.02, 0.02, 0.02), "free",1.0, color));
  }
  convex_hull_map_[name] = convex_hull_vec;
  graphObjects_.insert(graphObjects_.end(),convex_hull_vec.begin(),convex_hull_vec.end());
}

The user has to provide std::vector<KDL::Vector> but it could be replace by an Eigen vector.

If we want to do thing smarter, we can :

1. add a collision dector on the floor
2. compute the larger polygon
3. extract point's position of the polygon
4. createConvexHull

But like @jbmouret said, they might require additional libraries to perform that.

[costashatz]

The user has to provide std::vector<KDL::Vector> but it could be replace by an Eigen vector.

I was talking about code to compute the convex hull. Not visualize it. How do you compute this ch vector?

[PedroDesRobots]

There is no real computation at the moment. We stack a vector with contact points which exist inside the URDF (here it's the trick), then we took the transform directly of those points...
Not super elegant but it was working well for your application since we know which foot is on the floor or not.

I'm using a function inside the OpenSoT framework with PCL (not working with TSID yet).

this->convex_hull_ =
      boost::make_shared<OpenSoT::constraints::velocity::ConvexHull>(
          q_, *(this->model_ptr().get()), linksInContact, 0.03);

#ifdef USE_PCL
std::vector<KDL::Vector> control::opensot_controller::getConvexHull(std::string frame){
  std::vector<KDL::Vector> ch;
  ch = this->convex_hull_->getConvexHull(frame);
  return ch;
}
#endif

and getConvexHull is computed like that :

bool convex_hull::getConvexHull(const std::list<KDL::Vector>& points,
                                      std::vector<KDL::Vector>& convex_hull)
{
    fromSTDList2PCLPointCloud(points, _pointCloud);

    //Filtering
    projectPCL2Plane(_pointCloud, _ransac_distance_thr, _projectedPointCloud);


    pcl::PointCloud<pcl::PointXYZ> pointsInConvexHull;
    std::vector<pcl::Vertices> indicesOfVertexes;

    // hullVertices.vertices is the list of vertices...
    // by taking each point and the consequent in the list
    // (i.e. vertices[1]-vertices[0] it is possible to compute
    // bounding segments for the hull
    pcl::ConvexHull<pcl::PointXYZ> huller;
    huller.setInputCloud (_projectedPointCloud);
    huller.reconstruct(pointsInConvexHull, indicesOfVertexes);
    if(indicesOfVertexes.size() != 1) {
        std::cout<<"Error: more than one polygon found!"<<std::endl;
    }
    pcl::Vertices hullVertices = indicesOfVertexes[0];

    //printIndexAndPointsInfo(pointsInConvexHull, indicesOfVertexes);

    const pcl::Vertices& vs = indicesOfVertexes[0];
    for(unsigned int j = 0; j < vs.vertices.size(); ++j)
    {
        pcl::PointXYZ pointXYZ = pointsInConvexHull.at(vs.vertices[j]);
        convex_hull.push_back(fromPCLPointXYZ2KDLVector(pointXYZ));
    }

    _pointCloud->clear();
    _projectedPointCloud->clear();

    return true;
}

The projection is not super relevant and there are no real difference at the end. From my point of view, it's enough to take the contact point's transform.