Closed qsl123 closed 4 years ago
Hey, unfortunately not. What parts are you interested in specifically?
Hi Karl: Thank you for your reply!I have some questions. When will DUBINS SHOT be used? How are “const float Node3D::dy[] = { 0, -0.0415893, 0.0415893}; const float Node3D::dx[] = { 0.7068582, 0.705224, 0.705224};const float Node3D::dt[] = { 0, 0.1178097, -0.1178097};” calculated from the minimum turning radius?
Here is the line for the dubins shot: https://github.com/karlkurzer/path_planner/blob/b102a3252e0c949d7106bd85d6a0dda3b130b7ef/src/algorithm.cpp#L166
so basically if it is activated and it is in range.
The motion primitives are constant curvature curves (so, partial circles) with the maximum curvature of the vehicle with a length of more than \sqrt(2)*a with a being the side length of a cell. The three arrays hold the values for delta x, y and theta respectively.
Hi Karl: Sorry to bother you again. // NODE POINTER Node3D nPred; Node3D nSucc; What type of pointer is this? // define list pointers and initialize lists Node3D nodes3D = new Node3D[length](); Node2D nodes2D = new Node2D[width * height](); What is the relationship between this item and the above?
Is this still of interest, sorry for the delayed reply?
Is this still of interest, sorry for the delayed reply?
sure,thank you for your reply
how to show ’ALL EXPANDED 3D and 2D NODES‘ by function Visualize::publishNode3DPoses AND publishNode2DPoses ?
Here is the line for the dubins shot:
so basically if it is activated and it is in range.
The motion primitives are constant curvature curves (so, partial circles) with the maximum curvature of the vehicle with a length of more than \sqrt(2)*a with a being the side length of a cell. The three arrays hold the values for delta x, y and theta respectively.
Hi, Karl. Thanks for your reply, But I'm still confused about how to get these 'dx,dy,dt' if I have a different turning radius with different maximum degree restrict. Looking forward your answer, Thanks.
I'm working through that math as we speak, so I figured it would be good for posterity to explain this a little more. This is another way of looking at it with inline comments.
// square root of two arc length used to ensure leaving current cell
const float sqrt_2 = sqrt(2.0);
// angle must meet 3 requirements:
// 1) be increment of quantized bin size
// 2) chord length must be greater than sqrt(2) to leave current cell
// 3) maximum curvature must be respected, represented by minimum turning angle
// Thusly:
// On circle of radius minimum turning angle, we need select motion primatives
// with chord length > sqrt(2) and be an increment of our bin size
//
// chord >= sqrt(2) >= 2 * R * sin (angle / 2); where angle / N = quantized bin size
// Thusly: angle <= asin(sqrt(2) / (2 * R))
float angle = asin(sqrt_2 / (2 * min_turning_angle));
// Now make sure angle is an increment of the quantized bin size
// And since its based on the minimum chord, we need to make sure its always larger
const float bin_size = 2.0 * M_PI / num_angle_quantization;
if (angle < bin_size) {
angle = bin_size;
} else if (angle > bin_size) {
int increments = ceil(angle / bin_size);
angle = bin_size * increments;
}
// find deflections
// If we make a right triangle out of the chord in circle of radius
// min turning angle, we can see that delta X = R * sin (angle)
float delta_x = min_turning_angle * sin(angle);
// Using that same right triangle, we can see that the complement
// to delta Y is R * cos (angle). If we subtract R from it, we get
// the actual value
float delta_y = (R * cos(angle)) - R;
projections.clear();
projections.reserve(3);
projections.emplace_back(sqrt_2, 0.0, 0.0); // Forward
projections.emplace_back(delta_x, delta_y, angle); // Left
projections.emplace_back(delta_x, -delta_y, -angle); // Right
@SteveMacenski do you wanna make a PR for documentation you added, might be helpful for others?
I'm working through that math as we speak, so I figured it would be good for posterity to explain this a little more. This is another way of looking at it with inline comments.
// square root of two arc length used to ensure leaving current cell const float sqrt_2 = sqrt(2.0); // angle must meet 3 requirements: // 1) be increment of quantized bin size // 2) chord length must be greater than sqrt(2) to leave current cell // 3) maximum curvature must be respected, represented by minimum turning angle // Thusly: // On circle of radius minimum turning angle, we need select motion primatives // with chord length > sqrt(2) and be an increment of our bin size // // chord >= sqrt(2) >= 2 * R * sin (angle / 2); where angle / N = quantized bin size // Thusly: angle <= asin(sqrt(2) / (2 * R)) float angle = asin(sqrt_2 / (2 * min_turning_angle)); // Now make sure angle is an increment of the quantized bin size // And since its based on the minimum chord, we need to make sure its always larger const float bin_size = 2.0 * M_PI / num_angle_quantization; if (angle < bin_size) { angle = bin_size; } else if (angle > bin_size) { int increments = ceil(angle / bin_size); angle = bin_size * increments; } // find deflections // If we make a right triangle out of the chord in circle of radius // min turning angle, we can see that delta X = R * sin (angle) float delta_x = min_turning_angle * sin(angle); // Using that same right triangle, we can see that the complement // to delta Y is R * cos (angle). If we subtract R from it, we get // the actual value float delta_y = (R * cos(angle)) - R; projections.clear(); projections.reserve(3); projections.emplace_back(sqrt_2, 0.0, 0.0); // Forward projections.emplace_back(delta_x, delta_y, angle); // Left projections.emplace_back(delta_x, -delta_y, -angle); // Right
when i read original paper, one section said same cell expansion, if we consider that, chord length doesn`t need to be largeer than sqrt(2)
Hi karl: Do you have more detailed code comments? I don't understand some code. thank you very much。