Ejemplos de computeFK en C++ (Cpp)

Ejemplo n.º 1

Archivo: Optimizer.cpp Proyecto: haudren/TrajPlanning

void Optimizer::optimize(int nrIter, double learningRate, const Eigen::VectorXd& initPath)
{
  iters_.clear();
  iters_.reserve(nrIter);

  path_ = initPath;

  for(int iter = 0; iter < nrIter; ++iter)
  {
    computeFK();
    computeVel();
    computeJac();

    computeObsCost();
    computeSpeedCost();
    computeSmCost();

    computeObsGrad();
    computeSpeedGrad();

    path_ -= learningRate*AInv_.solve(obsGrad_ + speedGrad_);

    iters_.push_back({obsCost_, speedCost_, smCost_});
  }
}

Ejemplo n.º 2

Archivo: kdl_robot_model.cpp Proyecto: shivamvats/pr2_planning

bool KDLRobotModel::computeFK(const std::vector<double> &angles, std::string name, std::vector<double> &pose)
{
  KDL::Frame f;
  pose.resize(6,0);
  if(computeFK(angles, name, f))
  {
    pose[0] = f.p[0];
    pose[1] = f.p[1];
    pose[2] = f.p[2];
    f.M.GetRPY(pose[3], pose[4], pose[5]);
    return true;
  }
  return false;
}