optimizer_goal_global_orientation.h

Go to the documentation of this file.

#ifndef __INC_OPTIMIZER_GOAL_GLOBAL_ORIENTATION
#define __INC_OPTIMIZER_GOAL_GLOBAL_ORIENTATION

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

namespace robotLibPbD {
 
class OptimizerGoalGlobalOrientation : public OptimizerGoalGlobal
{ 
protected: 
    double angle;
    CVec axis;
    CMatrix pose;
public: 
  virtual void add()
  {  
    item.resize(3); 
    pose = first->getRelativeToBase(); 
    CMathLib::getRotationFromMatrix(pose, axis, angle);
    axis *= 57.295779513082325 * angle;  
    LOG_MSG(6, "Orientation: %s \n", axis.toString().c_str()); 
     
    item[0] = axis.x;
    item[1] = axis.y;
    item[2] = axis.z;
    data.push_back(item);
  }; 
};
 
};

#endif