Go to the documentation of this file.00001
00019 #ifndef __INC_OPTIMIZER_CONTAINER
00020 #define __INC_OPTIMIZER_CONTAINER
00021
00022
00023 namespace robotLibPbD {
00024
00025 class OptimizerContainer
00026 {
00027 public:
00028 CFrame *frame;
00029 std::vector<double> dofs_max, dofs_min;
00030 std::vector<unsigned int> dofs;
00031 void setValue(double *x, unsigned int index)
00032 {
00033 CMatrix pose;
00034 CVec axis, position;
00035
00036 for (unsigned int i=0; i<dofs.size(); i++)
00037 {
00038 LOG_MSG(11, "DOF %d -> %d: %f ( %f %f )\n", i, dofs[i], x[index + i], dofs_min[dofs[i]], dofs_max[dofs[i]]);
00039 switch (dofs[i])
00040 {
00041 case 0:
00042 position.x = dofs_min[dofs[i]] + x[index + i] * (dofs_max[dofs[i]] - dofs_min[dofs[i]]);
00043 break;
00044 case 1:
00045 position.y = dofs_min[dofs[i]] + x[index + i] * (dofs_max[dofs[i]] - dofs_min[dofs[i]]);
00046 break;
00047 case 2:
00048 position.z = dofs_min[dofs[i]] + x[index + i] * (dofs_max[dofs[i]] - dofs_min[dofs[i]]);
00049 break;
00050 case 3:
00051 axis.x = dofs_min[dofs[i]] + x[index + i] * (dofs_max[dofs[i]] - dofs_min[dofs[i]]);
00052 break;
00053 case 4:
00054 axis.y = dofs_min[dofs[i]] + x[index + i] * (dofs_max[dofs[i]] - dofs_min[dofs[i]]);
00055 break;
00056 case 5:
00057 axis.z = dofs_min[dofs[i]] + x[index + i] * (dofs_max[dofs[i]] - dofs_min[dofs[i]]);
00058 break;
00059 };
00060 }
00061
00062 double angle = axis.length();
00063 axis.normalize();
00064 CMathLib::getMatrixFromRotation(pose, axis, angle);
00065 pose.a[12] = position.x;
00066 pose.a[13] = position.y;
00067 pose.a[14] = position.z;
00068
00069
00070 frame->setPose(pose);
00071
00072 LOG_MSG(11, "%s:\n%s", frame->getName(), pose.toString().c_str());
00073 frame->invalidate();
00074 };
00075 };
00076
00077 };
00078
00079
00080 #endif