tool_pt orientation_tolerance seems not working

[jacknlliu]

We set the orientation_tolerance as 30 degree

tool_pt.orientation_tolerance.z_lower = -30.0/180.0 * M_PI; // Search -30 degree to 30 degree
tool_pt.orientation_tolerance.z_upper = 30.0/180.0 * M_PI;

But the output trajectory, the z axis tolerance is larger than 30 degree, actually max 90 degree!

I set the cartesian trajectory point like

    boost::shared_ptr<CartTrajectoryPt> pt(
          new CartTrajectoryPt(wobj_base, wobj_pt, tool_base, tool_pt, // Here we specify our frames
                               0, // Don't search in cartesian space. We want the points to be exact.
                              M_PI/90.0,
                               descartes_core::TimingConstraint(timing_constraint)));

[jrgnicho]

The tolerance is applied relative to the tool point orientation, so it can still exceed that tolerance relative to a global reference frame's orientation

[jacknlliu]

Is it relative to the tool_base, not the given tool_pt z-axis?

[jacknlliu]

What about importing a new construction method of the TolerancedFrame like

  TolerancedFrame(const Eigen::Affine3d &a, const std::vector<double> &pos_tol, const std::vector<double> &orient_tol)
    : Frame(a)
  {
    Eigen::Vector3d t = a.translation();
    Eigen::Matrix3d m = a.rotation();
    Eigen::Vector3d rxyz = m.eulerAngles(0, 1, 2);
    position_tolerance = ToleranceBase::createSymmetric<PositionTolerance>(t(0), t(1), t(2), pos_tol.at(0), pos_tol.at(1), pos_tol.at(2));
    orientation_tolerance = ToleranceBase::createSymmetric<OrientationTolerance>(rxyz(0), rxyz(1), rxyz(2), orient_tol.at(0), orient_tol.at(1), orient_tol.at(2));
  }

this will enable user create a tolerance frame with a tolerance relative to the given axis of the tool_pt

std::vector<double> pos_tol(3, 0.0);
std::vector<double> orient_tol(3,0.0);
orient_tol.at(2) = M_PI/2.0;
TolerancedFrame tool_pt(point, pos_tol, orient_tol);

[jrgnicho]

You can look into the sampling method for the cartesian point here

this will enable user create a tolerance frame with a tolerance relative to the given axis of the tool_pt

I believe that what you are requesting is already possible with the current implementation unless there's a bug

[jacknlliu]

@jrgnicho yeah, it's possible from the current implementation, but the default implementation allow the user to create the absolute range for an axis relative to the tool_base easily, like

tool_pt.orientation_tolerance.z_lower = -30.0/180.0 * M_PI; // Search -30 degree to 30 degree
tool_pt.orientation_tolerance.z_upper = 30.0/180.0 * M_PI;

the planner will search between -30 degree to 30 degree which relatvie to the tool_base, not the tool_pt.

But if the user wants to the planner searching between -30 degrees to 30 degrees which relative the tool_pt, not the tool_base, it seems not direct.