/* Copyright 2017-2021 The MathWorks, Inc. */
/**
* @file
* Main interfaces for Reeds-Shepp motion primitive calculations.
* To fully support code generation, note that this file needs to be fully
* compliant with the C++98 standard.
*/
#ifndef AUTONOMOUSCODEGEN_REEDSSHEPP_H_
#define AUTONOMOUSCODEGEN_REEDSSHEPP_H_
#include <math.h> // for sqrt, trigonometric functions
#include <algorithm> // for max
#include <vector>
#ifdef BUILDING_LIBMWAUTONOMOUSCODEGEN
#include "autonomouscodegen/autonomouscodegen_reeds_shepp_primitives.hpp"
#else
// To deal with the fact that PackNGo has no include file hierarchy during test
#include "autonomouscodegen_reeds_shepp_primitives.hpp"
#endif
namespace autonomous {
namespace reedsshepp {
/*
* The code for ReedsShepp primitives and computation of the various
* segment paths is inspired from Open Motion Planning Library source.
*/
/*
* RSSegmentType - Enumeration for holding segment type
*/
enum RSSegmentType { Left = 0, Right, Straight, NOP };
const RSSegmentType pathToSegment[18][5] = {
{autonomous::reedsshepp::Left, autonomous::reedsshepp::Right, autonomous::reedsshepp::Left,
autonomous::reedsshepp::NOP, autonomous::reedsshepp::NOP},
{autonomous::reedsshepp::Right, autonomous::reedsshepp::Left, autonomous::reedsshepp::Right,
autonomous::reedsshepp::NOP, autonomous::reedsshepp::NOP},
{autonomous::reedsshepp::Left, autonomous::reedsshepp::Right, autonomous::reedsshepp::Left,
autonomous::reedsshepp::Right, autonomous::reedsshepp::NOP},
{autonomous::reedsshepp::Right, autonomous::reedsshepp::Left, autonomous::reedsshepp::Right,
autonomous::reedsshepp::Left, autonomous::reedsshepp::NOP},
{autonomous::reedsshepp::Left, autonomous::reedsshepp::Right, autonomous::reedsshepp::Straight,
autonomous::reedsshepp::Left, autonomous::reedsshepp::NOP},
{autonomous::reedsshepp::Right, autonomous::reedsshepp::Left, autonomous::reedsshepp::Straight,
autonomous::reedsshepp::Right, autonomous::reedsshepp::NOP},
{autonomous::reedsshepp::Left, autonomous::reedsshepp::Straight, autonomous::reedsshepp::Right,
autonomous::reedsshepp::Left, autonomous::reedsshepp::NOP},
{autonomous::reedsshepp::Right, autonomous::reedsshepp::Straight, autonomous::reedsshepp::Left,
autonomous::reedsshepp::Right, autonomous::reedsshepp::NOP},
{autonomous::reedsshepp::Left, autonomous::reedsshepp::Right, autonomous::reedsshepp::Straight,
autonomous::reedsshepp::Right, autonomous::reedsshepp::NOP},
{autonomous::reedsshepp::Right, autonomous::reedsshepp::Left, autonomous::reedsshepp::Straight,
autonomous::reedsshepp::Left, autonomous::reedsshepp::NOP},
{autonomous::reedsshepp::Right, autonomous::reedsshepp::Straight, autonomous::reedsshepp::Right,
autonomous::reedsshepp::Left, autonomous::reedsshepp::NOP},
{autonomous::reedsshepp::Left, autonomous::reedsshepp::Straight, autonomous::reedsshepp::Left,
autonomous::reedsshepp::Right, autonomous::reedsshepp::NOP},
{autonomous::reedsshepp::Left, autonomous::reedsshepp::Straight, autonomous::reedsshepp::Right,
autonomous::reedsshepp::NOP, autonomous::reedsshepp::NOP},
{autonomous::reedsshepp::Right, autonomous::reedsshepp::Straight, autonomous::reedsshepp::Left,
autonomous::reedsshepp::NOP, autonomous::reedsshepp::NOP},
{autonomous::reedsshepp::Left, autonomous::reedsshepp::Straight, autonomous::reedsshepp::Left,
autonomous::reedsshepp::NOP, autonomous::reedsshepp::NOP},
{autonomous::reedsshepp::Right, autonomous::reedsshepp::Straight, autonomous::reedsshepp::Right,
autonomous::reedsshepp::NOP, autonomous::reedsshepp::NOP},
{autonomous::reedsshepp::Left, autonomous::reedsshepp::Right, autonomous::reedsshepp::Straight,
autonomous::reedsshepp::Left, autonomous::reedsshepp::Right},
{autonomous::reedsshepp::Right, autonomous::reedsshepp::Left, autonomous::reedsshepp::Straight,
autonomous::reedsshepp::Right, autonomous::reedsshepp::Left}};
/*
* shortestReedsSheppPath returns shortest normalized Reeds-Shepp path.
*/
inline autonomous::reedsshepp::ReedsSheppPath shortestReedsSheppPath(real64_T x,
real64_T y,
real64_T phi,
real64_T reverseCost) {
autonomous::reedsshepp::ReedsSheppPath minPath, tempPath;
real64_T minLength, tempLength;
autonomous::reedsshepp::computeCSCPath(x, y, phi, reverseCost, minPath);
minLength = minPath.length();
autonomous::reedsshepp::computeCCCPath(x, y, phi, reverseCost, tempPath);
tempLength = tempPath.length();
if (tempLength < minLength) {
minLength = tempLength;
minPath = tempPath;
}
autonomous::reedsshepp::computeCCCCPath(x, y, phi, reverseCost, tempPath);
tempLength = tempPath.length();
if (tempLength < minLength) {
minLength = tempLength;
minPath = tempPath;
}
autonomous::reedsshepp::computeCCSCPath(x, y, phi, reverseCost, tempPath);
tempLength = tempPath.length();
if (tempLength < minLength) {
minLength = tempLength;
minPath = tempPath;
}
autonomous::reedsshepp::computeCCSCCPath(x, y, phi, reverseCost, tempPath);
tempLength = tempPath.length();
if (tempLength < minLength) {
minLength = tempLength;
minPath = tempPath;
}
return minPath;
}
/*
* computeReedsSheppPath - compute Reeds-Shepp shortest/all path.
*/
inline autonomous::reedsshepp::ReedsSheppPath computeReedsSheppPath(
const real64_T initialConfiguration[3],
const real64_T finalConfiguration[3],
const real64_T turningRadius,
const real64_T reverseCost) {
real64_T x1 = initialConfiguration[0];
real64_T y1 = initialConfiguration[1];
real64_T theta1 = initialConfiguration[2];
real64_T x2 = finalConfiguration[0];
real64_T y2 = finalConfiguration[1];
real64_T theta2 = finalConfiguration[2];
real64_T dx = x2 - x1;
real64_T dy = y2 - y1;
real64_T c = cos(theta1);
real64_T s = sin(theta1);
real64_T x = c * dx + s * dy;
real64_T y = -s * dx + c * dy;
real64_T phi = theta2 - theta1;
return shortestReedsSheppPath(x / turningRadius, y / turningRadius, phi, reverseCost);
}
/*
* AllReedsSheppPath returns all normalized Reeds-Shepp path.
*/
inline std::vector<autonomous::reedsshepp::ReedsSheppPath> AllReedsSheppPath(
real64_T x,
real64_T y,
real64_T phi,
real64_T forwardCost,
real64_T reverseCost,
const boolean_T isOptimal,
const boolean_T allPathTypes[autonomous::reedsshepp::TotalNumPaths],
const uint32_T numTotalPathTypes) {
std::vector<autonomous::reedsshepp::ReedsSheppPath> allPaths;
allPaths.reserve(numTotalPathTypes);
autonomous::reedsshepp::computeCSCPathAll(x, y, phi, forwardCost, reverseCost, &allPathTypes[0],
&allPaths);
autonomous::reedsshepp::computeCCCPathAll(x, y, phi, forwardCost, reverseCost, &allPathTypes[0],
&allPaths);
autonomous::reedsshepp::computeCCCCPathAll(x, y, phi, forwardCost, reverseCost,
&allPathTypes[0], &allPaths);
autonomous::reedsshepp::computeCCSCPathAll(x, y, phi, forwardCost, reverseCost,
&allPathTypes[0], &allPaths);
autonomous::reedsshepp::computeCCSCCPathAll(x, y, phi, forwardCost, reverseCost,
&allPathTypes[0], &allPaths);
if (isOptimal) {
real64_T optimalPath = autonomous::inf;
uint32_T optimalInd = 0;
// Round up to 15 decimal point to get same optimal path in
// all system (maci, linux, windows). (Geck --> g1770360)
real64_T round_val = pow(10.0, 15);
for (uint32_T ind = 0; ind < numTotalPathTypes; ++ind) {
// Round up to 15 decimal point to get same optimal path in
// all system (maci, linux, windows . (Geck --> g1770360)
real64_T tempcost = round(allPaths[ind].cost() * round_val) / round_val;
if (optimalPath > tempcost) {
optimalPath = tempcost;
optimalInd = ind;
}
}
std::vector<autonomous::reedsshepp::ReedsSheppPath> path(1);
path[0] = allPaths[optimalInd];
return path;
}
return allPaths;
}
/*
* computeAllReedsSheppPaths - compute Reeds-Shepp shortest/all path.
*/
inline std::vector<autonomous::reedsshepp::ReedsSheppPath> computeAllReedsSheppPaths(
const real64_T initialConfiguration[3],
const real64_T finalConfiguration[3],
const real64_T turningRadius,
const real64_T forwardCost,
const real64_T reverseCost,
const boolean_T isOptimal,
const boolean_T allPathTypes[autonomous::reedsshepp::TotalNumPaths],
const uint32_T numTotalPathTypes) {
real64_T x1 = initialConfiguration[0];
real64_T y1 = initialConfiguration[1];
real64_T theta1 = initialConfiguration[2];
real64_T x2 = finalConfiguration[0];
real64_T y2 = finalConfiguration[1];
real64_T theta2 = finalConfiguration[2];
real64_T dx = x2 - x1;
real64_T dy = y2 - y1;
real64_T c = cos(theta1);
real64_T s = sin(theta1);
real64_T x = c * dx + s * dy;
real64_T y = -s * dx + c * dy;
real64_T phi = theta2 - theta1;
return AllReedsSheppPath(x / turningRadius, y / turningRadius, phi, forwardCost, reverseCost,
isOptimal, &allPathTypes[0], numTotalPathTypes);
}
/*
* Compute direction value for start/goal pose.
*
* 1) For segmentLengths input {0, 9, 2, -10, 0},
*
* computeDirectionAtEdges({0, 9, 2, -10, 0}, true) or
* computeDirectionAtEdges({0, 9, 2, -10, 0}) will return 1, sign of first
* non-zero segment (sign(9)) i.e. direction value of start pose.
*
* computeDirectionAtEdges({0, 9, 2, -10, 0}, false) will return -1, sign
* of last non-zero segment (sign(-10)) i.e. direction value of goal pose.
*
* 2) For segmentLengths input {-8, -1, 2, 10, 0},
*
* computeDirectionAtEdges({-8, -1, 2, 10, 0}, true) or
* computeDirectionAtEdges({-8, -1, 2, 10, 0}) will return -1, sign of first
* non-zero segment (sign(-8)) i.e. direction value of start pose.
*
* computeDirectionAtEdges({-8, -1, 2, 10, 0}, false) will return 1, sign
* of last non-zero segment (sign(10)) i.e. direction value of goal pose.
*
*/
inline real64_T computeDirectionAtEdges(const real64_T* segmentLengths, boolean_T startFlag = true)
{
real64_T direction = 1;
for (uint32_T i = 0; i < 5; i++){
if(segmentLengths[i] != 0){
direction = ((segmentLengths[i]<0) ? -1 : 1);
//Exit from loop for start pose direction computation
if (startFlag){
break;
}
}
}
return direction;
}
/*
* interpolateAlongInitializedRSPath - interpolate points along a
* pre-computed ReedsShepp path.
*/
inline real64_T interpolateAlongInitializedRSPath(const real64_T pathLength,
const RSSegmentType* segments,
const real64_T* segmentLengths,
const real64_T* from,
const real64_T* towards,
const real64_T t,
const real64_T turningRadius,
real64_T* state) {
real64_T direction = 1;
if (t <= 0.0) {
state[0] = from[0];
state[1] = from[1];
state[2] = from[2];
// To ensure first pose direction is correct
direction = computeDirectionAtEdges(segmentLengths);
} else if (t >= 1.0) {
state[0] = towards[0];
state[1] = towards[1];
state[2] = towards[2];
// To ensure last pose direction is correct
direction = computeDirectionAtEdges(segmentLengths, false);
} else {
// Initialize at [0, 0, theta]
state[0] = 0;
state[1] = 0;
state[2] = from[2];
real64_T seg = t * pathLength;
real64_T v, phi;
// Compute normalized update.
for (uint32_T i = 0; i < 5 && seg > 0; ++i) {
if (segmentLengths[i] < 0) {
v = std::max(-seg, segmentLengths[i]);
seg += v;
direction = -1;
} else {
v = std::min(seg, segmentLengths[i]);
seg -= v;
direction = 1;
}
phi = state[2];
switch (segments[i]) {
case autonomous::reedsshepp::Left:
state[0] += sin(phi + v) - sin(phi);
state[1] += -cos(phi + v) + cos(phi);
state[2] = autonomous::wrapToTwoPi(phi + v);
break;
case autonomous::reedsshepp::Right:
state[0] += -sin(phi - v) + sin(phi);
state[1] += cos(phi - v) - cos(phi);
state[2] = autonomous::wrapToTwoPi(phi - v);
break;
case autonomous::reedsshepp::Straight:
state[0] += v * cos(phi);
state[1] += v * sin(phi);
break;
case autonomous::reedsshepp::NOP:
// no update needed
break;
}
}
// Denormalize and update.
state[0] = from[0] + state[0] * turningRadius;
state[1] = from[1] + state[1] * turningRadius;
}
return direction;
}
/*
* interpolateRS - interpolate along ReedsShepp curve between states.
*/
inline void interpolateRS(const real64_T* from,
const real64_T* towards,
const real64_T maxDistance,
const uint32_T numSteps,
const real64_T turningRadius,
const real64_T reverseCost,
real64_T* state) {
// Compute ReedsShepp path.
autonomous::reedsshepp::ReedsSheppPath path =
computeReedsSheppPath(from, towards, turningRadius, reverseCost);
const RSSegmentType* segments = pathToSegment[path.getPathType()];
// Compute the fraction of path to be traversed based on maximum
// connection distance (maxDistance).
// * If the distance between the states is less than maxDistance, we
// interpolate all the through to the destination state (towards).
// * If the distance between the states is more than maxDistance, we
// only interpolate a fraction of the path between the two states.
// Find the distance between the states.
const real64_T pathLength = path.length();
const real64_T dist = pathLength * turningRadius;
// Find the fraction of the path to interpolate.
const real64_T t = std::min(maxDistance / dist, 1.0);
// Find interpolation step based on number of steps.
const real64_T step = t / static_cast<real64_T>(numSteps);
std::vector<real64_T> temp(3, 0);
real64_T fraction = 0;
// Interpolate along transition points
const uint32_T numTransitions = 4;
const uint32_T arrLength = numSteps + numTransitions;
const real64_T* segLengths = path.getSegmentLengths();
for (uint32_T n = 0; n < numTransitions; ++n) {
// Compute fraction along path corresponding to n-th transition
// point
fraction += (autonomous::abs(segLengths[n]) / pathLength);
// Saturate at maxDistance. This has the effect of returning
// the ending pose for all transition poses that come after the
// maxDistance has been reached.
fraction = std::min(fraction, t);
interpolateAlongInitializedRSPath(pathLength, segments, segLengths, from, towards, fraction,
turningRadius, &temp[0]);
state[n] = temp[0];
state[n + arrLength] = temp[1];
state[n + 2 * arrLength] = temp[2];
}
// Interpolate along path at equidistant intervals
fraction = 0;
for (uint32_T n = numTransitions; n < arrLength; ++n) {
fraction += step;
interpolateAlongInitializedRSPath(pathLength, segments, segLengths, from, towards, fraction,
turningRadius, &temp[0]);
state[n] = temp[0];
state[n + arrLength] = temp[1];
state[n + 2 * arrLength] = temp[2];
}
}
/*
* interpolateReedsSheppSegments - interpolate along given states and Reeds-Shepp curve.
*/
inline void interpolateReedsSheppSegments(const real64_T* from,
const real64_T* towards,
const real64_T* samples,
const uint32_T numSamples,
const real64_T turningRadius,
const real64_T* segmentLengths,
const int32_T* segmentDirections,
const uint32_T* segmentTypes,
real64_T* state,
real64_T* directions) {
RSSegmentType segments[5] = {static_cast<RSSegmentType>(segmentTypes[0]),
static_cast<RSSegmentType>(segmentTypes[1]),
static_cast<RSSegmentType>(segmentTypes[2]),
static_cast<RSSegmentType>(segmentTypes[3]),
static_cast<RSSegmentType>(segmentTypes[4])};
real64_T temp[3] = {0, 0, 0};
const real64_T pathLength = segmentLengths[0] + segmentLengths[1] +
segmentLengths[2] + segmentLengths[3] + segmentLengths[4];
real64_T normalizedSegmentLengths[5] = {segmentLengths[0] / turningRadius,
segmentLengths[1] / turningRadius,
segmentLengths[2] / turningRadius,
segmentLengths[3] / turningRadius,
segmentLengths[4] / turningRadius};
const real64_T normalizedPathLength =
normalizedSegmentLengths[0] + normalizedSegmentLengths[1] +
normalizedSegmentLengths[2] + normalizedSegmentLengths[3] +
normalizedSegmentLengths[4];
real64_T round_val = pow(10.0, 15);
normalizedSegmentLengths[0] = round(normalizedSegmentLengths[0] * round_val) / round_val;
normalizedSegmentLengths[1] = round(normalizedSegmentLengths[1] * round_val) / round_val;
normalizedSegmentLengths[2] = round(normalizedSegmentLengths[2] * round_val) / round_val;
normalizedSegmentLengths[3] = round(normalizedSegmentLengths[3] * round_val) / round_val;
normalizedSegmentLengths[4] = round(normalizedSegmentLengths[4] * round_val) / round_val;
// Round up to 15 decimal point to get same optimal path in all system
// (maci, linux, windows). (Geck --> g1770360)
normalizedSegmentLengths[0] = normalizedSegmentLengths[0]*static_cast<real64_T>(segmentDirections[0]);
normalizedSegmentLengths[1] = normalizedSegmentLengths[1]*static_cast<real64_T>(segmentDirections[1]);
normalizedSegmentLengths[2] = normalizedSegmentLengths[2]*static_cast<real64_T>(segmentDirections[2]);
normalizedSegmentLengths[3] = normalizedSegmentLengths[3]*static_cast<real64_T>(segmentDirections[3]);
normalizedSegmentLengths[4] = normalizedSegmentLengths[4]*static_cast<real64_T>(segmentDirections[4]);
for (uint32_T n = 0; n < numSamples; ++n) {
// Compute fraction along path corresponding to n-th transition
// point
// Round up to 15 decimal point to get same optimal path in all
// system (maci, linux, windows). (Geck --> g1770360)
real64_T roundedSample = round(samples[n] * round_val) / round_val;
roundedSample = roundedSample / pathLength;
if (directions){
directions[n] = interpolateAlongInitializedRSPath(normalizedPathLength, segments, normalizedSegmentLengths,
from, towards, roundedSample, turningRadius, &temp[0]);
}else{
interpolateAlongInitializedRSPath(normalizedPathLength, segments, normalizedSegmentLengths,
from, towards, roundedSample, turningRadius, &temp[0]);
}
state[n] = round(temp[0] * round_val) / round_val;
state[n + numSamples] = round(temp[1] * round_val) / round_val;
state[n + 2 * numSamples] = round(temp[2] * round_val) / round_val;
}
}
} // namespace reedsshepp
} // namespace autonomous
#endif /* AUTONOMOUSCODEGEN_REEDSSHEPP_H_ */