00001 00019 #ifndef __INC_OPTIMIZER_GOAL_ORIENTATION 00020 #define __INC_OPTIMIZER_GOAL_ORIENTATION 00021 00022 #include <vector> 00023 #include <optimizer_goal.h> 00024 00025 namespace robotLibPbD { 00026 00027 00028 00029 class OptimizerGoalOrientation : public OptimizerGoal 00030 { 00031 public: 00032 virtual double getDistance() 00033 { 00034 double angle; 00035 CVec axis; 00036 CMatrix pose; 00037 LOG_MSG(10, "First:\n%s", first->getRelativeToBase().toString().c_str()); 00038 LOG_MSG(10, "Second:\n%s", second->getRelativeToBase().toString().c_str()); 00039 00040 00041 pose = first->getRelativeToBase(); 00042 pose.invert(); 00043 pose.mul(pose, second->getRelativeToBase()); 00044 00045 //LOG_MSG(5, "Difference:\n%s", pose.toString().c_str()); 00046 00047 CMathLib::getRotationFromMatrix(pose, axis, angle); 00048 axis *= angle; 00049 00050 return 57.295779513082325 * axis.length(); 00051 }; 00052 }; 00053 00054 }; 00055 00056 #endif