C++ (Cpp) RobotWithGeometry::UpdateGeometry Examples

Example #1

//Produces a list of contacts as though the robot were standing on a plane.
//tol is the tolerance with which minimum-distance points are generated.
//All contacts are given zero friction and in the local frame of the robot.
void GetFlatContacts(RobotWithGeometry& robot,Real tol,ContactFormation& contacts)
{
  vector<AABB3D> bbs(robot.geometry.size());
  vector<pair<Real,int> > order;
  robot.UpdateGeometry();
  for(size_t i=0;i<robot.geometry.size();i++)
    if(robot.parents[i] >= 0 && !robot.IsGeometryEmpty(i)) {
      AABB3D aabb = robot.geometry[i]->GetAABB();
      bbs[i] = aabb;
      order.push_back(pair<Real,int>(aabb.bmin.z,(int)i));
    }
  sort(order.begin(),order.end());
  Real best = Inf;
  for(size_t i=0;i<order.size();i++) {
    if(order[i].first > best) break; //done
    int k=order[i].second;
    switch(robot.geometry[k]->type) {
    case AnyGeometry3D::Primitive:
      FatalError("Can't get flat contacts for primitives");
      break;
    case AnyGeometry3D::ImplicitSurface:
      FatalError("Can't get flat contacts for implicit surfaces");
      break;
    case AnyGeometry3D::Group:
      FatalError("Can't get flat contacts for geometry group");
      break;
    case AnyGeometry3D::TriangleMesh:
      {
	const TriMesh& m=robot.geometry[k]->AsTriangleMesh();
	for(size_t v=0;v<m.verts.size();v++) {
	  Vector3 pw = robot.links[k].T_World*m.verts[v];
	  if(pw.z < best)
	    best = pw.z;
	  assert(pw.z >= order[i].first);
	}
      }
      break;
    case AnyGeometry3D::PointCloud:
      {
	const PointCloud3D& pc=robot.geometry[k]->AsPointCloud();
	for(size_t v=0;v<pc.points.size();v++) {
	  Vector3 pw = robot.links[k].T_World*pc.points[v];
	  if(pw.z < best)
	    best = pw.z;
	  assert(pw.z >= order[i].first);
	}
      }
      break;
    }
  }
  
  //got the plane height, now output the vertices
  ContactPoint cp;
  cp.kFriction = 0.0;
  contacts.links.resize(0);
  contacts.contacts.resize(0);
  for(size_t i=0;i<order.size();i++) {
    if(order[i].first > best+tol) break; //done
    int k=order[i].second;
    contacts.links.resize(contacts.links.size()+1);
    contacts.links.back()=k;
    contacts.contacts.resize(contacts.contacts.size()+1);
    vector<Vector3> pts;
    switch(robot.geometry[k]->type) {
    case AnyGeometry3D::TriangleMesh:
      {
	const TriMesh& m=robot.geometry[k]->AsTriangleMesh();
	for(size_t v=0;v<m.verts.size();v++) {
	  Vector3 pw = robot.links[k].T_World*m.verts[v];
	  if(pw.z < best+tol) {
	    pts.push_back(pw);
	  }
	}
      }
      break;
    case AnyGeometry3D::PointCloud:
      {
	const PointCloud3D& pc=robot.geometry[k]->AsPointCloud();
	for(size_t v=0;v<pc.points.size();v++) {
	  Vector3 pw = robot.links[k].T_World*pc.points[v];
	  if(pw.z < best+tol) {
	    pts.push_back(pw);
	  }
	}
      }
      break;
    default:
      break;
    }
    //get the convex hull of those points
    vector<Vector2> pts2(pts.size());
    vector<Vector2> hull(pts.size());
    vector<int> hindex(pts.size());
    for(size_t v=0;v<pts.size();v++)
      pts2[v].set(pts[v].x,pts[v].y);
    int num = ConvexHull2D_Chain_Unsorted( &pts2[0], pts2.size(), &hull[0], &hindex[0]);
    contacts.contacts.back().resize(num);
    for(int v=0;v<num;v++) {
      //local estimate
      robot.links[k].T_World.mulInverse(pts[hindex[v]],cp.x);
      robot.links[k].T_World.R.mulTranspose(Vector3(0,0,1),cp.n);
      contacts.contacts.back()[v] = cp;
    }
    if(contacts.contacts.back().empty()) {
      //bound was below threshold, but no contacts below threshold
      contacts.links.resize(contacts.links.size()-1);
      contacts.contacts.resize(contacts.contacts.size()-1);
    }
  }
}