optimizer_goal_global_position.h

Go to the documentation of this file.

#ifndef __INC_OPTIMIZER_GOAL_GLOBAL_POSITION
#define __INC_OPTIMIZER_GOAL_GLOBAL_POSITION

#include <vector>  
#include <optimizer_goal_global.h>

namespace robotLibPbD {

class OptimizerGoalGlobalPosition : public OptimizerGoalGlobal
{ 
protected:
    CMatrix pose;
public: 
  virtual void add()
  {  
    item.resize(3);
    LOG_MSG(6, "Position: %s \n", first->getRelativeToBase()[3].toString().c_str()); 


    pose = first->getRelativeToBase(); 
    item[0] = pose.a[12];
    item[1] = pose.a[13];
    item[2] = pose.a[14];
    data.push_back(item);
  }; 
};
 
};

#endif