00001 00019 #ifndef __INC_OPTIMIZER_GOAL_POSITION 00020 #define __INC_OPTIMIZER_GOAL_POSITION 00021 00022 #include <vector> 00023 #include <optimizer_goal.h> 00024 00025 namespace robotLibPbD { 00026 00027 class OptimizerGoalPosition : public OptimizerGoal 00028 { 00029 public: 00030 virtual double getDistance() 00031 { 00032 CMatrix pose; 00033 LOG_MSG(10, "First:\n%s", first->getRelativeToBase().toString().c_str()); 00034 LOG_MSG(10, "Second:\n%s", second->getRelativeToBase().toString().c_str()); 00035 00036 00037 pose = first->getRelativeToBase(); 00038 pose.invert(); 00039 pose.mul(pose, second->getRelativeToBase()); 00040 00041 return pose[3].length(); 00042 }; 00043 }; 00044 00045 }; 00046 00047 #endif