Use angle between quaternions in OrientationGoal?

[AndyZe]

Currently the orientation goal looks like this:

    virtual double evaluate(const GoalContext& context) const
    {
        // return getOrientation().distance2(context.getLinkFrame().getOrientation());
        // return (getOrientation() - getOrientation().nearest(context.getLinkFrame().getOrientation())).length2();
        return fmin((getOrientation() - context.getLinkFrame().getOrientation()).length2(), (getOrientation() + context.getLinkFrame().getOrientation()).length2());
        /*return
            (getOrientation() - context.getLinkFrame().getOrientation()).length2() *
            (getOrientation() + context.getLinkFrame().getOrientation()).length2() * 0.5;*/
    }

I don't know a ton about quaternions but this seems like a nonlinear function so it might behave kind of funky: (getOrientation() - context.getLinkFrame().getOrientation()).length2()

Would it be better to use the angle between the quaternions? That would increase in a nice, linear fashion from 0 to pi.

https://math.stackexchange.com/a/90098/448782

[AndyZe]

Let me suggest an alternative. This takes a tolerance which defaults to 0.1 rad.

class OrientationWithToleranceGoal : public LinkGoalBase
{
  tf2::Quaternion orientation_;
  double radian_tolerance_;

public:
  OrientationWithToleranceGoal() : orientation_(0, 0, 0, 1), radian_tolerance_(0.1)
  {
  }
  OrientationWithToleranceGoal(const std::string& link_name, const tf2::Quaternion& orientation,
                               double radian_tolerance, double weight = 1.0)
    : LinkGoalBase(link_name, weight), orientation_(orientation.normalized()), radian_tolerance_(radian_tolerance)
  {
  }
  inline const tf2::Quaternion& getOrientation() const
  {
    return orientation_;
  }
  inline void setOrientation(const tf2::Quaternion& orientation)
  {
    orientation_ = orientation.normalized();
  }
  virtual double evaluate(const GoalContext& context) const
  {
    // Get angle between the quaternions. This is in the range [0, pi]
    // https://math.stackexchange.com/a/90098/448782
    double theta = 2 * acos(fabs(getOrientation().dot(context.getLinkFrame().getOrientation())));

    // Cost ramps up linearly for an angle between [tolerance, pi]
    // Cost is 0 between [0,tolerance)
    // Cost is undefined elsewhere (should never happen)
    if (theta >= radian_tolerance_ && theta <= M_PI)
    {
      // (cost) = (slope) * theta + intercept
      // slope = 1 / (pi - radian_tolerance_)
      // intercept = -slope * radian_tolerance_
      double slope = 1. / (M_PI - radian_tolerance_);
      return slope * theta - slope * radian_tolerance_;
    }
    else
      return 0;
  }
};

[v4hn]

It might make sense in some cases to have the additional tolerance in rad though you would need some other objective to get a gradient in this area if you want to avoid the additional noise from the plateau. Philipp investigated the different formulations for the OrientationGoal in depth when he benchmarked for his thesis and worked quite a bit to avoid trigonometic methods and taking roots. In his benchmarks using the squared angle instead of the squared distance (with a 5ms timeout) yielded almost 3% less success rate for random UR5 targets because of the additional computational complexity. From the [associated thesis](https://tams.informatik.uni-hamburg.de/publications/2017/MSc_Philipp_Ruppel.pdf):

Rotational distances between rotation quaternions can also be computed via the dot product. The angle between two normalized rotation quaternions is a = acos(Q1·Q2). A distance measure which does not require trigonometric operations can be defined as d = 1−(Q1·Q2). However, these methods can be less stable if the quaternions are not exactly normalized. For example, the dot product could become slightly larger than one, in which case the arcus cosine in the first formula would be undefined, and the second formula would compute a negative distance. Also, if denormalization of the quaternions is proportional to their magnitudes, the results could be inverted. For two zero-rotation quaternions, the result would be 0, but for slightly larger (proportinally denormalized) quaternions, the dot product could become larger than 1, and the result could become less than zero. This can be the case even for very small errors, since cos′(0) = 0 and limα→0acos(α)′=−∞. These issues are usually solved by explicitly normalizing the quaternions before computing the dot product. However, this would involve calculating computationally expensive square roots. Also, if the dot product is used as a distance measure, the gradient becomes zero if the angle between both quaternions is 180◦. If the minimum square distance is used instead, the gradient is always large if both quaternions are pointing away from each other, ensuring fast divergence away from incorrect solutions. This would also be the case if the square angle would be used, but computing the exact angle via trigonometry would be computationally more expensive.

That being said, if you deem the exact angular tolerance worth the lower performance you can still use it of course, but you might want to benchmark.