trajectory_optimization::TrajectoryOptimizationRWSNode Class Reference

#include <trajectory_optimization_rws.hpp>

Inheritance diagram for trajectory_optimization::TrajectoryOptimizationRWSNode:

Public Member Functions
	TrajectoryOptimizationRWSNode (const rclcpp::NodeOptions &options)
	Initializes the optimization node for a kinematic bicycle model with rear wheel steering.
Public Member Functions inherited from trajectory_optimization::TrajectoryOptimizationNode
	TrajectoryOptimizationNode (const std::string node_name, const rclcpp::NodeOptions &options)
	Initializes the base node and loads the common optimizer configuration.
	~TrajectoryOptimizationNode () override
	Releases solver resources owned by the node.
	TrajectoryOptimizationNode (const TrajectoryOptimizationNode &)=delete
	Copy construction is disabled because the node owns non-copyable runtime resources.
TrajectoryOptimizationNode &	operator= (const TrajectoryOptimizationNode &)=delete
	Copy assignment is disabled because the node owns non-copyable runtime resources.
	TrajectoryOptimizationNode (TrajectoryOptimizationNode &&)=delete
	Move construction is disabled to keep solver and ROS handles bound to a single instance.
TrajectoryOptimizationNode &	operator= (TrajectoryOptimizationNode &&)=delete
	Move assignment is disabled to keep solver and ROS handles bound to a single instance.

Private Member Functions
void	initializeTrajectory (trajectory_planning_msgs::msg::Trajectory &trajectory) override
	Initializes a drivable trajectory message for a kinematic bicycle model with rear wheel steering.
std::vector< double >	getBiLevelX0 (const perception_msgs::msg::EgoData &ego_data) override
	Computes the initial optimizer state using bi-level stabilizaion.
std::vector< double >	getHighLevelX0 (const perception_msgs::msg::EgoData &ego_data) override
	Computes the initial optimizer state using high-level stabilization.
void	convertToTrajectoryMsg (trajectory_planning_msgs::msg::Trajectory &trajectory) override
	Maps the optimized state trajectory into a trajectory message for a kinematic bicycle model with rear wheel steering.
double	computeVehicleSlipAngle (const double &delta_front, const double &delta_rear) const
	Computes the kinematic vehicle slip angle from front and rear steering angles.

Static Private Member Functions
static double	projectVectorAonV (const geometry_msgs::msg::Vector3 &a, const geometry_msgs::msg::Vector3 &v)
	Projects the acceleration vector onto the current direction of motion.

Private Attributes
double	distance_front_axle_ = 1.7
double	distance_rear_axle_ = 1.7
double	bi_level_dDelta_front_ = 5.0
double	bi_level_dDelta_rear_ = 5.0

Additional Inherited Members
Protected Types inherited from trajectory_optimization::TrajectoryOptimizationNode
enum	CONSIDER_BOUNDARIES { NO_BOUNDS = 0 , SUGGESTED_LANE = 1 , INCLUDING_ADJACENT = 2 , DRIVABLE_SPACE = 3 }
enum	CONSIDER_OBJECTS { NO_OBJECTS = 0 , STATIC_OBJECTS = 1 , PREDICTED_OBJECTS = 2 }
Protected Member Functions inherited from trajectory_optimization::TrajectoryOptimizationNode
template<typename T >
void	declareAndLoadParameter (const std::string &name, T &param, const std::string &description, const bool add_to_auto_reconfigurable_params=true, const bool is_required=false, const bool read_only=false, const std::optional< double > &from_value=std::nullopt, const std::optional< double > &to_value=std::nullopt, const std::optional< double > &step_value=std::nullopt, const std::string &additional_constraints="")
	Declares a ROS parameter, loads its value and optionally registers it for runtime updates.
rcl_interfaces::msg::SetParametersResult	parametersCallback (const std::vector< rclcpp::Parameter > &parameters)
	Applies parameter updates that can be reconfigured while the node is running.
void	setup ()
	Creates ROS interfaces, initializes cached messages and prepares the solver.
void	resetSolver ()
	Resets the optimizer memory while retaining the generated solver instance.
bool	setInitialGuess (const std::vector< double > &x_init, const rclcpp::Time &stamp)
	Builds and sets a dynamically consistent NLP initial guess from the current state and cached controls.
void	setupSolver ()
	Creates the acados solver instance and initializes its state buffers.
void	freeSolver ()
	Frees the acados solver and clears cached optimization results.
void	printSolution (const PerformanceMetrics &metrics)
	Logs solver status and optional debug statistics for the last optimization run.
void	logPerformance (const PerformanceMetrics &metrics)
	Emits one machine-readable performance record when performance logging is enabled.
bool	trajectory2outputFrame (trajectory_planning_msgs::msg::Trajectory &trajectory)
	Transforms the planned trajectory into the configured output frame (trajectory_frame_id_) if required.
bool	linearInterpolation (const std::vector< double > &X, const std::vector< double > &Y, const double &desired_x, double &output_y, const bool wrap_angle=false)
	Interpolates a value from sampled data and optionally handles angle wrap-around.
void	egoDataCallback (const perception_msgs::msg::EgoData::ConstSharedPtr msg)
	Stores the latest ego state used by the optimizer.
void	objectListCallback (const perception_msgs::msg::ObjectList::ConstSharedPtr msg)
	Stores the current object list when object handling is enabled.
void	referenceTrajectoryCallback (const trajectory_planning_msgs::msg::Trajectory::ConstSharedPtr msg)
	Updates the reference trajectory and optionally triggers optimization immediately.
void	routeCallback (const route_planning_msgs::msg::Route::ConstSharedPtr msg)
	Stores the current route used for boundary constraints when enabled.
void	planningCycle ()
	Runs one full planning cycle from input preparation, over solver execution, to trajectory publication.
bool	updateOcpInputs (const perception_msgs::msg::EgoData &ego_data, const perception_msgs::msg::ObjectList &object_list, const route_planning_msgs::msg::Route &route, const trajectory_planning_msgs::msg::Trajectory &reference_trajectory, const std::vector< double > &x_init)
	Transforms external inputs into the optimizer frame and writes them into the OCP.
void	setOcpGlobalParameters (const std::vector< double > &cost_weights, const trajectory_planning_msgs::msg::Trajectory &reference_trajectory, const route_planning_msgs::msg::Route &route)
	Writes stage-independent data into the OCP.
void	setOcpParameters (const perception_msgs::msg::EgoData &ego_data, const perception_msgs::msg::ObjectList &object_list)
	Writes stage-wise obstacle and dynamic weighting parameters into the OCP.
std::vector< std::pair< double, double > >	normalBoundaryDistance (const trajectory_planning_msgs::msg::Trajectory &reference_trajectory, const route_planning_msgs::msg::Route &route)
	Computes minimum normal distances from the reference path to the active route boundaries.
std::vector< double >	discretizeBB2Circles (const double x, const double y, const double yaw, const double length, const double width)
	Approximates an oriented bounding box with a set of obstacle circles.
void	vizCircles (const std::vector< double > &obstacles)
	Publishes visualization markers for the obstacle circles currently used by the optimizer.
void	vizEgoCircles (const std::vector< double > &x_trajectory, const std::string &model_name)
	Publishes the ego-vehicle circle approximation used by the selected OCP model.
void	vizBoundaryPoints (const std::vector< Eigen::Vector2d > &left_boundary_points, const std::vector< Eigen::Vector2d > &right_boundary_points)
	Publishes the boundary intersections corresponding to the distances passed to the OCP.
Static Protected Member Functions inherited from trajectory_optimization::TrajectoryOptimizationNode
static double	wrap_angle_rad (double angle_rad, double min_val=-M_PI, double max_val=M_PI)
	Wraps an angle into a configured interval.
static void	keepNClosestObjects (perception_msgs::msg::ObjectList &object_list, const int n_objects)
	Keeps the nearest forward objects and discards the remaining entries.
Protected Attributes inherited from trajectory_optimization::TrajectoryOptimizationNode
OnSetParametersCallbackHandle::SharedPtr	parameters_callback_
rclcpp::Subscription< perception_msgs::msg::EgoData >::SharedPtr	ego_data_sub_
rclcpp::Subscription< perception_msgs::msg::ObjectList >::SharedPtr	object_list_sub_
rclcpp::Subscription< route_planning_msgs::msg::Route >::SharedPtr	route_sub_
rclcpp::Subscription< trajectory_planning_msgs::msg::Trajectory >::SharedPtr	reference_trajectory_sub_
rclcpp::Publisher< trajectory_planning_msgs::msg::Trajectory >::SharedPtr	trajectory_pub_
rclcpp::Publisher< visualization_msgs::msg::MarkerArray >::SharedPtr	circles_pub_
rclcpp::Publisher< visualization_msgs::msg::MarkerArray >::SharedPtr	ego_circles_pub_
rclcpp::Publisher< visualization_msgs::msg::MarkerArray >::SharedPtr	boundary_pub_
rclcpp::TimerBase::SharedPtr	planning_timer_
std::unique_ptr< tf2_ros::Buffer >	tf2_buffer_
std::shared_ptr< tf2_ros::TransformListener >	tf2_listener_
perception_msgs::msg::EgoData	ego_data_
perception_msgs::msg::ObjectList	object_list_
route_planning_msgs::msg::Route	route_
trajectory_planning_msgs::msg::Trajectory	reference_trajectory_
std::vector< std::tuple< std::string, std::function< void(const rclcpp::Parameter &)> > >	auto_reconfigurable_params_
std::string	vehicle_frame_id_ = "base_link"
std::string	trajectory_frame_id_ = "base_link"
std::string	fixed_over_time_frame_id_ = "map"
std::string	model_name_ = "karl"
double	ego_data_timeout_ = 1.0
double	optimization_freq_ = 10.0
int	n_shots_ = 50
double	optimization_horizon_ = 1.0
bool	verbose_ = false
bool	performance_logging_ = false
bool	debug_viz_ = false
double	standstill_threshold_ = 0.45
bool	high_level_stabilization_ = false
bool	add_x_init_to_ref_ = false
uint8_t	consider_objects_ = CONSIDER_OBJECTS::PREDICTED_OBJECTS
uint8_t	consider_boundaries_ = CONSIDER_BOUNDARIES::SUGGESTED_LANE
bool	run_as_callback_ = false
double	bi_level_dV_ = 5.0
double	bi_level_dA_ = 2.0
double	bi_level_dY_ = 0.1
double	bi_level_dYaw_ = 5.0
trajectory_planning_msgs::msg::Trajectory	latest_valid_trajectory_
std::vector< double >	control_guess_
rclcpp::Time	control_guess_stamp_ {0, 0, RCL_ROS_TIME}
std::vector< double >	viz_circles_
std::vector< double >	cost_weights_ = std::vector<double>(12, 1.0)
double	dynamic_weight_ = 1.0
double	thw_ = 2.0
double	d_min_obstacle_long_ = 5.0
double	d_min_obstacle_lat_ = 0.5
double	d_min_boundary_lat_ = 0.0
double	min_prediction_probability_ = 0.0
std::vector< int64_t >	p_cost_weights_shape_ = {12, 1}
std::vector< int64_t >	p_ref_path_shape_ = {51, 6}
std::vector< int64_t >	p_obstacle_circles_shape_ = {30, 3}
ocp_model_capsule_t	ocp_capsule_
ocp_nlp_config *	nlp_config_
ocp_nlp_dims *	nlp_dims_
ocp_nlp_in *	nlp_in_
ocp_nlp_out *	nlp_out_
ocp_nlp_solver *	nlp_solver_
void *	nlp_opts_
std::vector< double >	xtraj_
std::vector< double >	utraj_
uint64_t	logging_cycle_ = 0
std::unique_ptr< PerformanceLogger >	performance_logger_

Detailed Description

Definition at line 10 of file trajectory_optimization_rws.hpp.

Constructor & Destructor Documentation

TrajectoryOptimizationRWSNode()

trajectory_optimization::TrajectoryOptimizationRWSNode::TrajectoryOptimizationRWSNode ( const rclcpp::NodeOptions & options )

explicit

Initializes the optimization node for a kinematic bicycle model with rear wheel steering.

Parameters

[in]

options

ROS node options used for construction.

Definition at line 13 of file trajectory_optimization_rws.cpp.

    : TrajectoryOptimizationNode("TrajectoryOptimizationRWSNode", options) {
  this->declareAndLoadParameter("distance_front_axle", distance_front_axle_, "Distance from center of gravity to front axle [m]");
  this->declareAndLoadParameter("distance_rear_axle", distance_rear_axle_, "Distance from center of gravity to rear axle [m]");
  this->declareAndLoadParameter("bi_level_dDelta_front", bi_level_dDelta_front_,
                                "Threshold for bi-level stabilization: maximum front steering angle difference [degree]");
  this->declareAndLoadParameter("bi_level_dDelta_rear", bi_level_dDelta_rear_,
                                "Threshold for bi-level stabilization: maximum rear steering angle difference [degree]");
}

Member Function Documentation

computeVehicleSlipAngle()

double trajectory_optimization::TrajectoryOptimizationRWSNode::computeVehicleSlipAngle ( const double & delta_front, const double & delta_rear ) const

private

Computes the kinematic vehicle slip angle from front and rear steering angles.

Parameters

[in]	delta_front	Front steering angle.
[in]	delta_rear	Rear steering angle.

Returns

Vehicle slip angle in radians.

Definition at line 134 of file trajectory_optimization_rws.cpp.

                                                                                                                       {
  double wheel_base = distance_front_axle_ + distance_rear_axle_;
  double slip_angle =
      atan(distance_rear_axle_ / wheel_base * tan(delta_front) + distance_front_axle_ / wheel_base * tan(delta_rear));
  return slip_angle;
}

convertToTrajectoryMsg()

void trajectory_optimization::TrajectoryOptimizationRWSNode::convertToTrajectoryMsg ( trajectory_planning_msgs::msg::Trajectory & trajectory )

overrideprivatevirtual

Maps the optimized state trajectory into a trajectory message for a kinematic bicycle model with rear wheel steering.

Parameters

[in,out]

trajectory

Trajectory message to populate.

Implements trajectory_optimization::TrajectoryOptimizationNode.

Definition at line 119 of file trajectory_optimization_rws.cpp.

                                                                                                            {
  for (int i = 0; i <= n_shots_; ++i) {
    trajectory_planning_msgs::trajectory_access::setX(trajectory, xtraj_[i * *nlp_dims_->nx + 0], i);
    trajectory_planning_msgs::trajectory_access::setY(trajectory, xtraj_[i * *nlp_dims_->nx + 1], i);
    trajectory_planning_msgs::trajectory_access::setS(trajectory, xtraj_[i * *nlp_dims_->nx + 2], i);
    trajectory_planning_msgs::trajectory_access::setV(trajectory, xtraj_[i * *nlp_dims_->nx + 3], i);
    trajectory_planning_msgs::trajectory_access::setA(trajectory, xtraj_[i * *nlp_dims_->nx + 4], i);
    trajectory_planning_msgs::trajectory_access::setTheta(trajectory, xtraj_[i * *nlp_dims_->nx + 5], i);
    trajectory_planning_msgs::trajectory_access::setDeltaFront(trajectory, xtraj_[i * *nlp_dims_->nx + 6], i);
    trajectory_planning_msgs::trajectory_access::setDeltaRear(trajectory, xtraj_[i * *nlp_dims_->nx + 7], i);
    trajectory_planning_msgs::trajectory_access::setBeta(
        trajectory, xtraj_[i * *nlp_dims_->nx + 6], xtraj_[i * *nlp_dims_->nx + 7], distance_front_axle_, distance_rear_axle_, i);
  }
}

getBiLevelX0()

std::vector< double > trajectory_optimization::TrajectoryOptimizationRWSNode::getBiLevelX0 ( const perception_msgs::msg::EgoData & ego_data )

overrideprivatevirtual

Computes the initial optimizer state using bi-level stabilizaion.

Parameters

[in]

ego_data

Current EgoData based on the kinematic bicycle model with rear wheel steering.

Returns

Initial state vector for the RWS model.

Implements trajectory_optimization::TrajectoryOptimizationNode.

Definition at line 28 of file trajectory_optimization_rws.cpp.

                                                                                                       {
  // transform latest trajectory to current base_link frame
  trajectory_planning_msgs::msg::Trajectory tf_trajectory;
  try {
    tf_trajectory = tf2_buffer_->transform(latest_valid_trajectory_, vehicle_frame_id_, tf2_ros::fromMsg(ego_data.header.stamp),
                                           fixed_over_time_frame_id_, tf2::durationFromSec(0.01));
  } catch (tf2::TransformException& ex) {
    RCLCPP_WARN(this->get_logger(), "Transformation is not available. Init high-level instead. Ex: %s", ex.what());
    return getHighLevelX0(ego_data);
  }
 
  // fill vectors with state values from the transformed trajectory
  std::vector<double> TIME, V, Y, A, THETA, DELTA_FRONT, DELTA_REAR;
  for (int i = 0; i < trajectory_planning_msgs::trajectory_access::getSamplePointSize(tf_trajectory); i++) {
    TIME.push_back(trajectory_planning_msgs::trajectory_access::getT(tf_trajectory, i));
    Y.push_back(trajectory_planning_msgs::trajectory_access::getY(tf_trajectory, i));
    V.push_back(trajectory_planning_msgs::trajectory_access::getV(tf_trajectory, i));
    A.push_back(trajectory_planning_msgs::trajectory_access::getA(tf_trajectory, i));
    THETA.push_back(trajectory_planning_msgs::trajectory_access::getTheta(tf_trajectory, i));
    DELTA_FRONT.push_back(trajectory_planning_msgs::trajectory_access::getDeltaFront(tf_trajectory, i));
    DELTA_REAR.push_back(trajectory_planning_msgs::trajectory_access::getDeltaRear(tf_trajectory, i));
  }
 
  // interpolate target states by time from the extracted vectors; if not successful, set to ego state (high-level initialization)
  double v_tgt = 0.0, a_tgt = 0.0, y_tgt = 0.0, theta_tgt = 0.0, delta_front_tgt = 0.0, delta_rear_tgt = 0.0;
  double des_time = (rclcpp::Time(ego_data.header.stamp) - rclcpp::Time(tf_trajectory.header.stamp)).seconds();
  if (!linearInterpolation(TIME, Y, des_time, y_tgt)) y_tgt = 0.0;
  if (!linearInterpolation(TIME, THETA, des_time, theta_tgt, true)) theta_tgt = 0.0;
  if (!linearInterpolation(TIME, V, des_time, v_tgt)) v_tgt = perception_msgs::object_access::getVelocityMagnitude(ego_data);
  if (!linearInterpolation(TIME, A, des_time, a_tgt)) {
    a_tgt = projectVectorAonV(perception_msgs::object_access::getAcceleration(ego_data),
                              perception_msgs::object_access::getVelocity(ego_data));
  }
  if (!linearInterpolation(TIME, DELTA_FRONT, des_time, delta_front_tgt)) {
    delta_front_tgt = perception_msgs::object_access::getSteeringAngleFront(ego_data);
  }
  if (!linearInterpolation(TIME, DELTA_REAR, des_time, delta_rear_tgt)) {
    delta_rear_tgt = perception_msgs::object_access::getSteeringAngleRear(ego_data);
  }
 
  RCLCPP_DEBUG(this->get_logger(), "y_tgt: %f, v_tgt: %f, a_tgt: %f, theta_tgt: %f, delta_front_tgt: %f, delta_rear_tgt: %f",
               y_tgt, v_tgt, a_tgt, theta_tgt, delta_front_tgt, delta_rear_tgt);
 
  // handle thresholds for bi-level stabilization (which means, using ego state as initial state for the optimization)
  // longitudinal reinits
  double a_path = projectVectorAonV(perception_msgs::object_access::getAcceleration(ego_data),
                                    perception_msgs::object_access::getVelocity(ego_data));
  if (fabs(v_tgt - perception_msgs::object_access::getVelocityMagnitude(ego_data)) > bi_level_dV_ ||
      fabs(a_tgt - a_path) > bi_level_dA_) {
    v_tgt = perception_msgs::object_access::getVelocityMagnitude(ego_data);
    a_tgt = a_path;
  }
  // lateral reinits
  if (fabs(y_tgt) > bi_level_dY_ || fabs(theta_tgt) > bi_level_dYaw_ * M_PI / 180.0) {
    y_tgt = 0.0;
    theta_tgt = 0.0;
    delta_front_tgt = perception_msgs::object_access::getSteeringAngleFront(ego_data);
    delta_rear_tgt = perception_msgs::object_access::getSteeringAngleRear(ego_data);
  } else if (fabs(delta_front_tgt - perception_msgs::object_access::getSteeringAngleFront(ego_data)) >
             bi_level_dDelta_front_ * M_PI / 180.0) {
    delta_front_tgt = perception_msgs::object_access::getSteeringAngleFront(ego_data);
  } else if (fabs(delta_rear_tgt - perception_msgs::object_access::getSteeringAngleRear(ego_data)) >
             bi_level_dDelta_rear_ * M_PI / 180.0) {
    delta_rear_tgt = perception_msgs::object_access::getSteeringAngleRear(ego_data);
  }
 
  std::vector<double> x_init(*nlp_dims_->nx, 0.0);
  x_init[0] = 0.0;
  x_init[1] = y_tgt;
  x_init[2] = 0.0;
  x_init[3] = v_tgt;
  x_init[4] = a_tgt;
  x_init[5] = theta_tgt;
  x_init[6] = delta_front_tgt;
  x_init[7] = delta_rear_tgt;
 
  return x_init;
}

getHighLevelX0()

std::vector< double > trajectory_optimization::TrajectoryOptimizationRWSNode::getHighLevelX0 ( const perception_msgs::msg::EgoData & ego_data )

overrideprivatevirtual

Computes the initial optimizer state using high-level stabilization.

Parameters

[in]

ego_data

Current EgoData based on the kinematic bicycle model with rear wheel steering.

Returns

Initial state vector for the RWS model.

Implements trajectory_optimization::TrajectoryOptimizationNode.

Definition at line 107 of file trajectory_optimization_rws.cpp.

                                                                                                         {
  std::vector<double> x_init(*nlp_dims_->nx, 0.0);
 
  x_init[3] = perception_msgs::object_access::getVelocityMagnitude(ego_data);
  x_init[4] = projectVectorAonV(perception_msgs::object_access::getAcceleration(ego_data),
                                perception_msgs::object_access::getVelocity(ego_data));
  x_init[6] = perception_msgs::object_access::getSteeringAngleFront(ego_data);
  x_init[7] = perception_msgs::object_access::getSteeringAngleRear(ego_data);
 
  return x_init;
}

initializeTrajectory()

void trajectory_optimization::TrajectoryOptimizationRWSNode::initializeTrajectory ( trajectory_planning_msgs::msg::Trajectory & trajectory )

overrideprivatevirtual

Initializes a drivable trajectory message for a kinematic bicycle model with rear wheel steering.

Parameters

[out]

trajectory

Trajectory message to initialize.

Implements trajectory_optimization::TrajectoryOptimizationNode.

Definition at line 23 of file trajectory_optimization_rws.cpp.

                                                                                                          {
  trajectory_planning_msgs::trajectory_access::initializeTrajectory(
      trajectory, trajectory_planning_msgs::msg::DRIVABLERWS::TYPE_ID, n_shots_ + 1);
}

projectVectorAonV()

double trajectory_optimization::TrajectoryOptimizationRWSNode::projectVectorAonV ( const geometry_msgs::msg::Vector3 & a, const geometry_msgs::msg::Vector3 & v )

staticprivate

Projects the acceleration vector onto the current direction of motion.

Parameters

[in]	a	Acceleration vector.
[in]	v	Velocity vector.

Returns

Longitudinal acceleration along the velocity direction.

Definition at line 141 of file trajectory_optimization_rws.cpp.

                                                                                          {
  double v_magnitude_squared = v.x * v.x + v.y * v.y;
  if (v_magnitude_squared < std::numeric_limits<double>::epsilon()) {
    return 0.0;
  }
  double scale = (a.x * v.x + a.y * v.y) / v_magnitude_squared;
  return scale * std::sqrt(v_magnitude_squared);
}

Member Data Documentation

bi_level_dDelta_front_

double trajectory_optimization::TrajectoryOptimizationRWSNode::bi_level_dDelta_front_ = 5.0

private

Definition at line 73 of file trajectory_optimization_rws.hpp.

bi_level_dDelta_rear_

double trajectory_optimization::TrajectoryOptimizationRWSNode::bi_level_dDelta_rear_ = 5.0

private

Definition at line 74 of file trajectory_optimization_rws.hpp.

distance_front_axle_

double trajectory_optimization::TrajectoryOptimizationRWSNode::distance_front_axle_ = 1.7

private

Definition at line 69 of file trajectory_optimization_rws.hpp.

distance_rear_axle_

double trajectory_optimization::TrajectoryOptimizationRWSNode::distance_rear_axle_ = 1.7

private

Definition at line 70 of file trajectory_optimization_rws.hpp.

---

The documentation for this class was generated from the following files:

• trajectory_optimization/include/trajectory_optimization/trajectory_optimization_rws.hpp
• trajectory_optimization/src/trajectory_optimization_rws.cpp