search

tesseract-robotics / tesseract_ros

ROS Interface for the Tesseract Planning Environment.
http://tesseract-docs.rtfd.io
61 stars 24 forks source link

Planning failed when an obstacle was created on the path using Command::Ptr addPointCloud()' #246

Closed sm3304love closed 3 weeks ago

sm3304love commented 4 weeks ago

Hello. To perform motion planning that avoids obstacles, I modified the basic_cartesian_example.cpp code to create a 0.1 x 0.1 x 0.1 cubic obstacle along the end effector’s path.

The function I used is as follows:

Command::Ptr addPointCloud()
{
  // Create octomap and add it to the local environment
  pcl::PointCloud<pcl::PointXYZ> full_cloud;
  double delta = 0.01;
  auto length = static_cast<int>(0.1 / delta);

  // Adjust point cloud generation to center around the specified size
  for (int x = 0; x < length; ++x)
    for (int y = 0; y < length; ++y)
      for (int z = 0; z < length; ++z)
        full_cloud.push_back(pcl::PointXYZ(-0.05F + static_cast<float>(x * delta),
                                           -0.05F + static_cast<float>(y * delta),
                                           -0.05F + static_cast<float>(z * delta)));

  // Add octomap to environment
  Link link_octomap("octomap_attached");

  Visual::Ptr visual = std::make_shared<Visual>();
  visual->origin = Eigen::Isometry3d::Identity();
  visual->origin.translation() = Eigen::Vector3d(0.3, 0, 0.8);
  auto ot = tesseract_geometry::createOctree(full_cloud, 2 * delta, true, true);
  visual->geometry =
      std::make_shared<tesseract_geometry::Octree>(std::move(ot), tesseract_geometry::OctreeSubType::BOX, true, true);
  link_octomap.visual.push_back(visual);

  Collision::Ptr collision = std::make_shared<Collision>();
  collision->origin = visual->origin;
  collision->geometry = visual->geometry;
  link_octomap.collision.push_back(collision);

  Joint joint_octomap("joint_octomap_attached");
  joint_octomap.parent_link_name = "base_link";
  joint_octomap.child_link_name = link_octomap.getName();
  joint_octomap.type = JointType::FIXED;

  return std::make_shared<tesseract_environment::AddLinkCommand>(link_octomap, joint_octomap);
}

However, I encountered the following error in the terminal and motion plan failed.


The only changes made from the example code were the size and position of the obstacle. Is there anything additional that I need to adjust for obstacle avoidance?
Levi-Armstrong commented 4 weeks ago

It looks like the Cartesian path goes through the collision object and if that is the case then I would expect it to fail. Since the Cartesian path is a constraint this would make the problem infeasible.

sm3304love commented 4 weeks ago

I understand. So, if I want to plan a trajectory that avoids obstacles, would it be better to refer to pick and place example?

Levi-Armstrong commented 4 weeks ago

I would start with the freepspace example.

sm3304love commented 3 weeks ago

I have confirmed through an example that to avoid obstacles, StateWaypointPoly wp1{StateWaypoint(joint_names, q)}; should be used.

Levi-Armstrong commented 3 weeks ago

The waypoint is not critical here because you should be able to use JointWaypointPoly, StateWaypointPoly, or CartesianWaypointPoly but you must configure your profile. The profile dictates what constraints and costs are added to the optimizer based on the waypoint type. A good example to review is the freespace glass upright example with uses CartesianWaypointPoly where it disable the translation constraint and only enables the rotational constraint.