00001 00019 #ifndef __INC_OPTIMIZER_GOAL_GLOBAL_POSITION 00020 #define __INC_OPTIMIZER_GOAL_GLOBAL_POSITION 00021 00022 #include <vector> 00023 #include <optimizer_goal_global.h> 00024 00025 namespace robotLibPbD { 00026 00027 class OptimizerGoalGlobalPosition : public OptimizerGoalGlobal 00028 { 00029 protected: 00030 CMatrix pose; 00031 public: 00032 virtual void add() 00033 { 00034 item.resize(3); 00035 LOG_MSG(6, "Position: %s \n", first->getRelativeToBase()[3].toString().c_str()); 00036 00037 00038 pose = first->getRelativeToBase(); 00039 item[0] = pose.a[12]; 00040 item[1] = pose.a[13]; 00041 item[2] = pose.a[14]; 00042 data.push_back(item); 00043 }; 00044 }; 00045 00046 }; 00047 00048 #endif