00001 00019 #ifndef __INC_OPTIMIZER_GOAL_GLOBAL_ORIENTATION 00020 #define __INC_OPTIMIZER_GOAL_GLOBAL_ORIENTATION 00021 00022 #include <vector> 00023 #include <optimizer_goal_global.h> 00024 00025 namespace robotLibPbD { 00026 00027 class OptimizerGoalGlobalOrientation : public OptimizerGoalGlobal 00028 { 00029 protected: 00030 double angle; 00031 CVec axis; 00032 CMatrix pose; 00033 public: 00034 virtual void add() 00035 { 00036 item.resize(3); 00037 pose = first->getRelativeToBase(); 00038 CMathLib::getRotationFromMatrix(pose, axis, angle); 00039 axis *= 57.295779513082325 * angle; 00040 LOG_MSG(6, "Orientation: %s \n", axis.toString().c_str()); 00041 00042 item[0] = axis.x; 00043 item[1] = axis.y; 00044 item[2] = axis.z; 00045 data.push_back(item); 00046 }; 00047 }; 00048 00049 }; 00050 00051 #endif