optimizer_goal.h

Go to the documentation of this file.

#ifndef __INC_OPTIMIZER_GOAL
#define __INC_OPTIMIZER_GOAL

#include <vector>  
#include <frame.h>
#include <vecmath.h>
#include <log.h>
#include <utils.h>
#include <datapairs.h>

namespace robotLibPbD {

class OptimizerGoal
{
protected:
  DataPairs information;
  std::string function;
public:
  CFrame *first, *second;
  ~OptimizerGoal() {}
  void loadFromXml(CFrameContainer &frames, TiXmlElement* frameNode)
  {
    first = frames.getFrame(CConfiguration::getAttributeString(frameNode, "first", ""));
    second = frames.getFrame(CConfiguration::getAttributeString(frameNode, "second", ""));
    if (second == NULL)
      second = first;

    function = CConfiguration::getAttributeString(frameNode, "value", "");
  };

  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; 

    information.add("x", printToString("%f", pose.a[12]), true);
    information.add("y", printToString("%f", pose.a[13]), true);
    information.add("z", printToString("%f", pose.a[14]), true);
    information.add("rx", printToString("%f", axis.x), true);
    information.add("ry", printToString("%f", axis.y), true);
    information.add("rz", printToString("%f", axis.z), true);

    return atof(information.resolveString(function).c_str());
  };

  void getDistance(CVec &from, CVec &to)
  {
    from = first->getRelativeToBase()[3];
    to = second->getRelativeToBase()[3];
  };

  void getDistance(CVec &to)
  {
    CMatrix pose = first->getRelativeToBase();
    pose.invert();
    to = pose * second->getRelativeToBase()[3];
  };
};
};

#endif