optimizer_container.h

Go to the documentation of this file.

#ifndef __INC_OPTIMIZER_CONTAINER
#define __INC_OPTIMIZER_CONTAINER


namespace robotLibPbD {

class OptimizerContainer
{
public:
  CFrame *frame;
  std::vector<double> dofs_max, dofs_min;
  std::vector<unsigned int> dofs; 
  void setValue(double *x, unsigned int index) 
  {
    CMatrix pose;
    CVec axis, position;
    
    for (unsigned int i=0; i<dofs.size(); i++)
      {
        LOG_MSG(11, "DOF %d -> %d: %f ( %f %f )\n", i, dofs[i], x[index + i], dofs_min[dofs[i]], dofs_max[dofs[i]]);
      switch (dofs[i])
        {
        case 0:
          position.x = dofs_min[dofs[i]] + x[index + i] * (dofs_max[dofs[i]] - dofs_min[dofs[i]]);
          break;
        case 1:
          position.y = dofs_min[dofs[i]] + x[index + i] * (dofs_max[dofs[i]] - dofs_min[dofs[i]]);
          break;
        case 2:
          position.z = dofs_min[dofs[i]] + x[index + i] * (dofs_max[dofs[i]] - dofs_min[dofs[i]]);
          break;
        case 3:
          axis.x = dofs_min[dofs[i]] + x[index + i] * (dofs_max[dofs[i]] - dofs_min[dofs[i]]);
          break;
        case 4:
          axis.y = dofs_min[dofs[i]] + x[index + i] * (dofs_max[dofs[i]] - dofs_min[dofs[i]]);
          break;
        case 5:
          axis.z = dofs_min[dofs[i]] + x[index + i] * (dofs_max[dofs[i]] - dofs_min[dofs[i]]);
          break;
        };
      }

    double angle = axis.length();
    axis.normalize();
    CMathLib::getMatrixFromRotation(pose, axis, angle);
    pose.a[12] = position.x;
    pose.a[13] = position.y;
    pose.a[14] = position.z;


    frame->setPose(pose);

    LOG_MSG(11, "%s:\n%s", frame->getName(), pose.toString().c_str());
    frame->invalidate();
  };
};

};


#endif