optimizer_goal_position.h

Go to the documentation of this file.

#ifndef __INC_OPTIMIZER_GOAL_POSITION
#define __INC_OPTIMIZER_GOAL_POSITION

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

namespace robotLibPbD {
 
class OptimizerGoalPosition : public OptimizerGoal
{ 
public: 
  virtual double getDistance() 
  { 
    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());
 
    return pose[3].length();
  };
};

};

#endif