Esempio n. 1
0
 bool ParameterSE2Offset::read(std::istream& is) {
   Vector3 off;
   for (int i=0; i<3; i++) {
     is >> off[i];
     std::cerr << off[i] << " " ;
   }
   std::cerr <<  std::endl;
   setOffset(SE2(off));
   return is.good() || is.eof();
 }
Esempio n. 2
0
 bool EdgeSE2::read(std::istream& is)
 {
   Vector3 p;
   is >> p[0] >> p[1] >> p[2];
   setMeasurement(SE2(p));
   _inverseMeasurement = measurement().inverse();
   for (int i = 0; i < 3; ++i)
     for (int j = i; j < 3; ++j) {
       is >> information()(i, j);
       if (i != j)
         information()(j, i) = information()(i, j);
     }
   return true;
 }
Esempio n. 3
0
File: se2.hpp Progetto: catree/LATL
 SE2 inverse() const {
     return SE2(-(R.inverse()*t), R.inverse());
 }
Esempio n. 4
0
File: se2.hpp Progetto: catree/LATL
 SE2 operator*(const SE2& other) const {
     return SE2(t + R*other.t, R * other.R);
 }
int main(int, char**)
{
    // plan for hybrid car in SE(2) with discrete gears
    ob::StateSpacePtr SE2(new ob::SE2StateSpace());
    ob::StateSpacePtr velocity(new ob::RealVectorStateSpace(1));
    // set the range for gears: [-1,3] inclusive
    ob::StateSpacePtr gear(new ob::DiscreteStateSpace(-1,3));
    ob::StateSpacePtr stateSpace = SE2 + velocity + gear;

    // set the bounds for the R^2 part of SE(2)
    ob::RealVectorBounds bounds(2);
    bounds.setLow(-100);
    bounds.setHigh(100);
    SE2->as<ob::SE2StateSpace>()->setBounds(bounds);

    // set the bounds for the velocity
    ob::RealVectorBounds velocityBound(1);
    velocityBound.setLow(0);
    velocityBound.setHigh(60);
    velocity->as<ob::RealVectorStateSpace>()->setBounds(velocityBound);

    // create start and goal states
    ob::ScopedState<> start(stateSpace);
    ob::ScopedState<> goal(stateSpace);

    // Both start and goal are states with high velocity with the car in third gear.
    // However, to make the turn, the car cannot stay in third gear and will have to
    // shift to first gear.
    start[0] = start[1] = -90.; // position
    start[2] = boost::math::constants::pi<double>()/2; // orientation
    start[3] = 40.; // velocity
    start->as<ob::CompoundState>()->as<ob::DiscreteStateSpace::StateType>(2)->value = 3; // gear

    goal[0] = goal[1] = 90.; // position
    goal[2] = 0.; // orientation
    goal[3] = 40.; // velocity
    goal->as<ob::CompoundState>()->as<ob::DiscreteStateSpace::StateType>(2)->value = 3; // gear

    oc::ControlSpacePtr cmanifold(new oc::RealVectorControlSpace(stateSpace, 2));

    // set the bounds for the control manifold
    ob::RealVectorBounds cbounds(2);
    // bounds for steering input
    cbounds.setLow(0, -1.);
    cbounds.setHigh(0, 1.);
    // bounds for brake/gas input
    cbounds.setLow(1, -20.);
    cbounds.setHigh(1, 20.);
    cmanifold->as<oc::RealVectorControlSpace>()->setBounds(cbounds);

    oc::SimpleSetup setup(cmanifold);
    setup.setStartAndGoalStates(start, goal, 5.);
    setup.setStateValidityChecker(boost::bind(
        &isStateValid, setup.getSpaceInformation().get(), _1));
    setup.setStatePropagator(boost::bind(
        &propagate, setup.getSpaceInformation().get(), _1, _2, _3, _4));
    setup.getSpaceInformation()->setPropagationStepSize(.1);
    setup.getSpaceInformation()->setMinMaxControlDuration(2, 3);

    // try to solve the problem
    if (setup.solve(30))
    {
        // print the (approximate) solution path: print states along the path
        // and controls required to get from one state to the next
        oc::PathControl& path(setup.getSolutionPath());

        // print out full state on solution path
        // (format: x, y, theta, v, u0, u1, dt)
        for(unsigned int i=0; i<path.getStateCount(); ++i)
        {
            const ob::State* state = path.getState(i);
            const ob::SE2StateSpace::StateType *se2 =
                state->as<ob::CompoundState>()->as<ob::SE2StateSpace::StateType>(0);
            const ob::RealVectorStateSpace::StateType *velocity =
                state->as<ob::CompoundState>()->as<ob::RealVectorStateSpace::StateType>(1);
            const ob::DiscreteStateSpace::StateType *gear =
                state->as<ob::CompoundState>()->as<ob::DiscreteStateSpace::StateType>(2);
            std::cout << se2->getX() << ' ' << se2->getY() << ' ' << se2->getYaw()
                << ' ' << velocity->values[0] << ' ' << gear->value << ' ';
            if (i==0)
                // null controls applied for zero seconds to get to start state
                std::cout << "0 0 0";
            else
            {
                // print controls and control duration needed to get from state i-1 to state i
                const double* u =
                    path.getControl(i-1)->as<oc::RealVectorControlSpace::ControlType>()->values;
                std::cout << u[0] << ' ' << u[1] << ' ' << path.getControlDuration(i-1);
            }
            std::cout << std::endl;
        }
        if (!setup.haveExactSolutionPath())
        {
            std::cout << "Solution is approximate. Distance to actual goal is " <<
                setup.getProblemDefinition()->getSolutionDifference() << std::endl;
        }
    }

    return 0;
}
Esempio n. 6
0
bool ClosedFormCalibration::calibrate(const MotionInformationVector& measurements, SE2& laserOffset, Eigen::Vector3d& odomParams)
{
  std::vector<VelocityMeasurement, Eigen::aligned_allocator<VelocityMeasurement> > velMeasurements;
  for (size_t i = 0; i < measurements.size(); ++i) {
    const SE2& odomMotion = measurements[i].odomMotion;
    const double& timeInterval = measurements[i].timeInterval;
    MotionMeasurement mm(odomMotion.translation().x(), odomMotion.translation().y(), odomMotion.rotation().angle(), timeInterval);
    VelocityMeasurement velMeas = OdomConvert::convertToVelocity(mm);
    velMeasurements.push_back(velMeas);
  }

  double J_21, J_22;
  {
    Eigen::MatrixXd A(measurements.size(), 2);
    Eigen::VectorXd x(measurements.size());
    for (size_t i = 0; i < measurements.size(); ++i) {
      const SE2& laserMotion = measurements[i].laserMotion;
      const double& timeInterval = measurements[i].timeInterval;
      const VelocityMeasurement& velMeas = velMeasurements[i];
      A(i, 0) = velMeas.vl() * timeInterval;
      A(i, 1) = velMeas.vr() * timeInterval;
      x(i) = laserMotion.rotation().angle();
    }
    // (J_21, J_22) = (-r_l / b, r_r / b)
    Eigen::Vector2d linearSolution = (A.transpose() * A).inverse() * A.transpose() * x;
    //std::cout << linearSolution.transpose() << std::endl;
    J_21 = linearSolution(0);
    J_22 = linearSolution(1);
  }

  // construct M
  Eigen::Matrix<double, 5, 5> M;
  M.setZero();
  for (size_t i = 0; i < measurements.size(); ++i) {
    const SE2& laserMotion = measurements[i].laserMotion;
    const double& timeInterval = measurements[i].timeInterval;
    const VelocityMeasurement& velMeas = velMeasurements[i];
    Eigen::Matrix<double, 2, 5> L;
    double omega_o_k = J_21 * velMeas.vl() + J_22 * velMeas.vr();
    double o_theta_k = omega_o_k * timeInterval;
    double sx = 1.;
    double sy = 0.;
    if (fabs(o_theta_k) > std::numeric_limits<double>::epsilon()) {
      sx = sin(o_theta_k)       / o_theta_k;
      sy = (1 - cos(o_theta_k)) / o_theta_k;
    }
    double c_x = 0.5 * timeInterval * (-J_21 * velMeas.vl() + J_22 * velMeas.vr()) * sx;
    double c_y = 0.5 * timeInterval * (-J_21 * velMeas.vl() + J_22 * velMeas.vr()) * sy;
    L(0, 0) = -c_x;
    L(0, 1) = 1 - cos(o_theta_k);
    L(0, 2) = sin(o_theta_k);
    L(0, 3) = laserMotion.translation().x(); 
    L(0, 4) = -laserMotion.translation().y();
    L(1, 0) = -c_y;
    L(1, 1) = - sin(o_theta_k);
    L(1, 2) = 1 - cos(o_theta_k);
    L(1, 3) = laserMotion.translation().y(); 
    L(1, 4) = laserMotion.translation().x();
    M.noalias() += L.transpose() * L;
  }
  //std::cout << M << std::endl;

  // compute lagrange multiplier
  // and solve the constrained least squares problem
  double m11 = M(0,0);
  double m13 = M(0,2);
  double m14 = M(0,3);
  double m15 = M(0,4);
  double m22 = M(1,1);
  double m34 = M(2,3);
  double m35 = M(2,4);
  double m44 = M(3,3); 

  double a = m11 * SQR(m22) - m22 * SQR(m13);
  double b = 2*m11 * SQR(m22) * m44 - SQR(m22) * SQR(m14) - 2*m22 * SQR(m13) * m44 - 2*m11 * m22 * SQR(m34) 
    - 2*m11 * m22 * SQR(m35) - SQR(m22) * SQR(m15) + 2*m13 * m22 * m34 * m14 + SQR(m13) * SQR(m34) 
    + 2*m13 * m22 * m35 * m15 + SQR(m13) * SQR(m35);
  double c = - 2*m13 * CUBE(m35) * m15 - m22 * SQR(m13) * SQR(m44) + m11 * SQR(m22) * SQR(m44) + SQR(m13) * SQR(m35) * m44
    + 2*m13 * m22 * m34 * m14 * m44 + SQR(m13) * SQR(m34) * m44 - 2*m11 * m22 * SQR(m34) * m44 - 2 * m13 * CUBE(m34) * m14
    - 2*m11 * m22 * SQR(m35) * m44 + 2*m11 * SQR(m35) * SQR(m34) + m22 * SQR(m14) * SQR(m35) - 2*m13 * SQR(m35) * m34 * m14
    - 2*m13 * SQR(m34) * m35 * m15 + m11 * std::pow(m34, 4) + m22 * SQR(m15) * SQR(m34) + m22 * SQR(m35) * SQR(m15)
    + m11 * std::pow(m35, 4) - SQR(m22) * SQR(m14) * m44 + 2*m13 * m22 * m35 * m15 * m44 + m22 * SQR(m34) * SQR(m14)
    - SQR(m22) * SQR(m15) * m44;

  // solve the quadratic equation
  double lambda1, lambda2;
  if(a < std::numeric_limits<double>::epsilon()) {
    if(b <= std::numeric_limits<double>::epsilon())
      return false;
    lambda1 = lambda2 = -c/b;
  } else {
    double delta = b*b - 4*a*c;
    if (delta < 0)
      return false;
    lambda1 = 0.5 * (-b-sqrt(delta)) / a;
    lambda2 = 0.5 * (-b+sqrt(delta)) / a;
  }

  Eigen::VectorXd x1 = solveLagrange(M, lambda1);
  Eigen::VectorXd x2 = solveLagrange(M, lambda2);
  double err1 = x1.dot(M * x1);
  double err2 = x2.dot(M * x2);

  const Eigen::VectorXd& calibrationResult = err1 < err2 ? x1 : x2;
  odomParams(0) = - calibrationResult(0) * J_21;
  odomParams(1) = calibrationResult(0) * J_22;
  odomParams(2) = calibrationResult(0);

  laserOffset = SE2(calibrationResult(1), calibrationResult(2), atan2(calibrationResult(4), calibrationResult(3)));

  return true;
}