optimizer_goal_orientation.h

Go to the documentation of this file.

#ifndef __INC_OPTIMIZER_GOAL_ORIENTATION
#define __INC_OPTIMIZER_GOAL_ORIENTATION

#include <vector>  
#include <optimizer_goal.h>

namespace robotLibPbD {



class OptimizerGoalOrientation : public OptimizerGoal
{ 
public: 
  virtual double getDistance() 
  {
    double angle;
    CVec axis;
    CMatrix pose;
    LOG_MSG(10, "First:\n%s", first->getRelativeToBase().toString().c_str());
    LOG_MSG(10, "Second:\n%s", second->getRelativeToBase().toString().c_str());


    pose = first->getRelativeToBase();
    pose.invert();
    pose.mul(pose, second->getRelativeToBase());

    //LOG_MSG(5, "Difference:\n%s", pose.toString().c_str());

    CMathLib::getRotationFromMatrix(pose, axis, angle);
    axis *= angle;  
    
    return 57.295779513082325 * axis.length();
  };
}; 

};

#endif