ErrorTrajectoryDuration error

[Linjackffy]

Dear @pantor ,
I got ErrorTrajectoryDuration err with the following Code. With the debug mode, I found that result of brake phase calculattion was so weird. Maybe it was a numerical issue.
Looking foreward for your reply!

    Ruckig<6> otg{ 0.004 };
    InputParameter<6> input;
    OutputParameter<6> output;

    input.control_interface = ControlInterface::Position;
    input.synchronization = Synchronization::Phase;
    input.duration_discretization = DurationDiscretization::Discrete;

    // Set input parameters
    input.max_velocity = { 0.002,0.002,0.002,0.003141592629253865,0.003141592629253865,0.003141592629253865 };
    input.max_acceleration = { 0.01,0.01,0.01,0.01570796314626932,0.01570796314626932,0.01570796314626932 };
    input.max_jerk = { 0.18,0.18,0.18,1.570796314626932,1.570796314626932,1.570796314626932 };

    input.current_position = { 0.1174161453909641,-0.4222080077114712,0.3003863125814782,2.960444926889819,-0.005791262234199017,-2.364847659719411 };
    input.current_velocity = { -3.662950284795513e-09,-5.585273134233322e-09,0.01325125623820627,1.800912308381978e-08,-1.307199431356758e-09,-1.908875705031016e-08 };
    input.current_acceleration = { 1.350015487635199e-07,2.058506025917496e-07,-0.4883877683641483,-6.637435179724953e-07,4.81780898060446e-08,7.035344641338199e-07 };

    input.target_position = { 0.117416145324707,-0.4222080078125,0.3006260070800781,2.960444927215576,-0.00579126225784421,-2.364847660064697 };
    input.target_velocity = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
    input.target_acceleration = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };

[Linjackffy]

I got anthor numerical issue caused by brake phase calculattion.
The lines caused the expression are:

ruckig/src/brake.cpp

Lines 45 to 55 in 4af5a39

	if (t_to_a_min < t_min_to_v_max) {
	const double v_at_a_min = v_at_t(v0, a0, -jMax, t_to_a_min);
	const double t_to_v_max_with_constant = -(v_at_a_min - vMax)/aMin;
	const double t_to_v_min_with_constant = aMin/(2*jMax) - (v_at_a_min - vMin)/aMin;
	t_brake[0] = t_to_a_min - eps;
	t_brake[1] = std::max(std::min(t_to_v_max_with_constant, t_to_v_min_with_constant), 0.0);
	} else {
	t_brake[0] = t_min_to_v_max - eps;
	}

t_brake[0] is a verry small value, -1e-17. Since t_brake[0] is smaller than 0, then following lines do not work and casue an error.

ruckig/include/ruckig/calculator.hpp

Lines 231 to 237 in 4af5a39

	// Integrate brake pre-trajectory
	for (size_t i = 0; i < 2 && p.brake.t[i] > 0; ++i) {
	p.brake.p[i] = p0s[dof];
	p.brake.v[i] = v0s[dof];
	p.brake.a[i] = a0s[dof];
	std::tie(p0s[dof], v0s[dof], a0s[dof]) = Profile::integrate(p.brake.t[i], p0s[dof], v0s[dof], a0s[dof], p.brake.j[i]);
	}

Below is the test case I used:

    Ruckig<6> otg{ 0.004 };
    InputParameter<6> input;
    OutputParameter<6> output;

    input.control_interface = ControlInterface::Position;
    input.synchronization = Synchronization::Phase;
    input.duration_discretization = DurationDiscretization::Discrete;

    input.current_position = { 0.1174172134399427, -0.4222288818359352, 0.2941486551093023, 2.960364580154412, -0.005511142313479924, -2.364976882934563 };
    input.current_velocity = { -3.846200918841657e-16, -6.734598745646603e-16, -0.01679703039934764, 1.93366473144585e-15, -1.378116843984702e-16, -2.054518768558403e-15 };
    input.current_acceleration = { -5.37694151146362e-16, -9.442280976427118e-16, 0.009999999999996042, 2.727751039870435e-15, -1.920695711984072e-16, -2.898235479862337e-15 };
    input.target_position = { 0.1174172134399414, -0.4222288818359375, 0.2299049987792969, 2.960364580154419, -0.005511142313480377, -2.36497688293457 };
    input.target_velocity = { 0, 0, 0, 0, 0, 0 };
    input.target_acceleration = { 0, 0, 0, 0, 0, 0 };
    input.max_velocity = { 0.002, 0.002, 0.002, 0.003141592629253865, 0.003141592629253865, 0.003141592629253865 };
    input.max_acceleration = { 0.01, 0.01, 0.01, 0.01570796314626932, 0.01570796314626932, 0.01570796314626932 };
    input.max_jerk = { 0.18, 0.18, 0.18, 1.570796314626932, 1.570796314626932, 1.570796314626932 };

[pantor]

Hey, and thanks for your patience! This should be fixed now with the latest commit on the master branch.
Can you confirm this?

[Linjackffy]

Sorry for the delay in responding. My second issue was fixed with the latest commit on the master branch, but first issue still got ErrorTrajectoryDuration error.

    Ruckig<6> otg{ 0.004 };
    InputParameter<6> input;
    OutputParameter<6> output;

    input.control_interface = ControlInterface::Position;
    input.synchronization = Synchronization::Phase;
    input.duration_discretization = DurationDiscretization::Discrete;

    // Set input parameters
    input.max_velocity = { 0.002,0.002,0.002,0.003141592629253865,0.003141592629253865,0.003141592629253865 };
    input.max_acceleration = { 0.01,0.01,0.01,0.01570796314626932,0.01570796314626932,0.01570796314626932 };
    input.max_jerk = { 0.18,0.18,0.18,1.570796314626932,1.570796314626932,1.570796314626932 };

    input.current_position = { 0.1174161453909641,-0.4222080077114712,0.3003863125814782,2.960444926889819,-0.005791262234199017,-2.364847659719411 };
    input.current_velocity = { -3.662950284795513e-09,-5.585273134233322e-09,0.01325125623820627,1.800912308381978e-08,-1.307199431356758e-09,-1.908875705031016e-08 };
    input.current_acceleration = { 1.350015487635199e-07,2.058506025917496e-07,-0.4883877683641483,-6.637435179724953e-07,4.81780898060446e-08,7.035344641338199e-07 };

    input.target_position = { 0.117416145324707,-0.4222080078125,0.3006260070800781,2.960444927215576,-0.00579126225784421,-2.364847660064697 };
    input.target_velocity = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
    input.target_acceleration = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };

[pantor]

When you use RuckigThrow<6> instead of Ruckig<6>, what's the exact error message?

[Linjackffy]

Nothing change after using RuckigThrow<6>. I still got ErrorTrajectoryDuration error without much more message.
The result of brake phase duration was 11198.4s.