[Question] buildConstraintMatrix / Multiplication with (t_next - t_now)^(- derivative)

[TiFu]

Hi,

Thanks for sharing this implementation of minimum snap trajectories. I am currently trying to understand your code to determine if adding corridor constraints can be easily added in your implementation.

I think I am finally starting to understand how the constraint matrix in TrajectoryGenerator.cpp is built but still have one more question:

In TrajectoryGenerator.cpp (see code section below) all constraints are multiplied with 1 / std::pow(t_next - t_now, der) i.e. (t_next - t_now)^-derivative - my understanding is that this is related to equation (12) on page 6 of your report:

(t_next - t_now)^(n - derivative) = (t_next - t_now)^n * (t_next - t_now)^-derivative

Now my question is: Why is (t_next - t_now)^n not included here? Is it included somewhere else instead?

Thanks a lot! Tino

Relevant Code section:

                    // Intermediate waypoint, add both departure and arrival constraints
                    VectorXd a1 = VectorXd::Zero(constraint_size);
                    VectorXd a2 = a1;
                    double t_next = keyframes_[wp+1].getTime();
                    double t_now = keyframes_[wp].getTime();
                    double t_prev = keyframes_[wp-1].getTime();

                    // Arrival constraint
                    double int_t_prev = 1 / std::pow(t_now - t_prev, der);
                    VectorXd polynomial = coeffs.row(der) * int_t_prev;
                    unsigned int idx = (wp-1) * n_coeffs_;
                    a1.segment(idx, n_coeffs_) << polynomial;

                    // Departure constraint
                    double int_t = 1 / std::pow(t_next - t_now, der);
                    polynomial = coeffs.row(der).cwiseProduct(I.row(der)) * int_t_prev;
                    idx = wp * n_coeffs_;
                    a2.segment(idx, n_coeffs_) << polynomial;

                    A_eq.push_back(a1);
                    A_eq.push_back(a2);
                    double b = keyframes_[wp].getConstraint(der)(dim);
                    b_eq.push_back(b);  // !!Both a1 and a2 are equal to the same thing
                    b_eq.push_back(b);

[andre-nguyen]

Hi there,

I did this work more than two years ago now so my memory is a bit fuzzy. I think it would be useful if you read Adam Bry's thesis, specifically on pages 37-39 where he sets up the problem. I think you might have misread something (sorry for the report being in french). I believe double int_t_prev = 1 / std::pow(t_now - t_prev, der); is basically the coefficient that arises out of integrating the polynomial.

So the arrival constraint ("arriving" to the current waypoint) uses t_now and t_prev whereas the departure constraint ("departing" from the current waypoint) uses t_next and t_now. What's interesting is that I just noticed there is a bug in the c++ version and the variable int_t is never actually used, must be a copy-paste error...

Hope this clears things up? I might be able to look into this during the weekend.