nlbucki / rappids Goto Github PK

Hi ! Thank you for your great work. I notice that in the trajectory generation part. you set the initial position, velocity and accelerate to 0;

    int GetNextCandidateTrajectory(
        RapidQuadrocopterTrajectoryGenerator::RapidTrajectoryGenerator& nextTraj) {
      CommonMath::Vec3 posf;
      _planner->DeprojectPixelToPoint(_pixelX(_gen), _pixelY(_gen),
                                      _depth(_gen), posf);
      nextTraj.Reset();
      nextTraj.SetGoalPosition(posf);
      nextTraj.SetGoalVelocity(CommonMath::Vec3(0, 0, 0));
      nextTraj.SetGoalAcceleration(CommonMath::Vec3(0, 0, 0));
      nextTraj.Generate(_time(_gen));
      return 0;
    }

to my understanding the sampled reference point from trajectory is sent to controller in the world frame. So I have 3 questions as below:

1. Is the initial reference point sampled from trajectory being the same with the drone's current state(p, v, a) or it is (0,0,0)?
2. if the first sampled point of the trajectory being the drone's current state, and the drone is replanning fast(frequently), we notice that the drone is going to diverge, could you help how to solve this?

we solved the above question by init the new trajectory with the current sampled point like below

    RapidTrajectoryGenerator rappid_traj_temp = RapidTrajectoryGenerator(
        Vec3(last_sample_p_in_camera_(0), last_sample_p_in_camera_(1), last_sample_p_in_camera_(2)),
        Vec3(last_sample_v_in_camera_(0), last_sample_v_in_camera_(1), last_sample_v_in_camera_(2)),
        Vec3(last_sample_a_in_camera_(0), last_sample_a_in_camera_(1), last_sample_a_in_camera_(2)),
        gravity_in_camera_);

however, i noticed that when replanning happens, if the first sampled point of new trajectory is the same with the current sampled point(like the code above). the Closed form condition in RAPPIDS(traj(t) = ct^5 + ct^4 + ct^3 + ct^2+ ct + c intersect with a plane knowing one intersection point has a closed form of other intersection point) does not meet, how to solve this problem?

thank you again!