stomp.cpp File Reference

This contains the stomp core algorithm. More...

#include <ros/console.h>
#include <limits.h>
#include <Eigen/LU>
#include <Eigen/Cholesky>
#include <math.h>
#include <stomp_core/utils.h>
#include <numeric>
#include "stomp_core/stomp.h"

Include dependency graph for stomp.cpp:

Go to the source code of this file.

Functions
static void	computeCubicInterpolation (const std::vector< double > &first, const std::vector< double > &last, int num_points, double dt, Eigen::MatrixXd &trajectory_joints)
	Compute a cubic interpolated trajectory given a start and end state. More...
static void	computeLinearInterpolation (const std::vector< double > &first, const std::vector< double > &last, int num_timesteps, Eigen::MatrixXd &trajectory_joints)
	Compute a linear interpolated trajectory given a start and end state. More...
bool	computeMinCostTrajectory (const std::vector< double > &first, const std::vector< double > &last, const Eigen::MatrixXd &control_cost_matrix_R_padded, const Eigen::MatrixXd &inv_control_cost_matrix_R, Eigen::MatrixXd &trajectory_joints)
	Compute a minimum cost trajectory given a start and end state. More...
void	computeParametersControlCosts (const Eigen::MatrixXd &parameters, double dt, double control_cost_weight, const Eigen::MatrixXd &control_cost_matrix_R, Eigen::MatrixXd &control_costs)
	Compute the parameters control costs. More...

Variables
static const double	DEFAULT_NOISY_COST_IMPORTANCE_WEIGHT = 1.0
static const double	MIN_CONTROL_COST_WEIGHT = 1e-8
static const double	MIN_COST_DIFFERENCE = 1e-8

Detailed Description

This contains the stomp core algorithm.

Author

Jorge Nicho

Date

March 7, 2016

Version

TODO

Bug:

No known bugs

Copyright

Copyright (c) 2016, Southwest Research Institute

License

Software License Agreement (Apache License)

Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. You may obtain a copy of the License at http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License.

Definition in file stomp.cpp.

Function Documentation

static void computeCubicInterpolation ( const std::vector< double > &  first, const std::vector< double > &  last, int  num_points, double  dt, Eigen::MatrixXd &  trajectory_joints  )

static

Compute a cubic interpolated trajectory given a start and end state.

Parameters

first	The start position
last	The final position
num_points	The number of points in the trajectory
dt	The timestep in seconds
trajectory_joints	The returned cubic interpolated trajectory

Definition at line 70 of file stomp.cpp.

static void computeLinearInterpolation ( const std::vector< double > &  first, const std::vector< double > &  last, int  num_timesteps, Eigen::MatrixXd &  trajectory_joints  )

static

Compute a linear interpolated trajectory given a start and end state.

Parameters

first	The start position
last	The final position
num_timesteps	The number of timesteps
trajectory_joints	The returned linear interpolated trajectory

Definition at line 47 of file stomp.cpp.

bool computeMinCostTrajectory	(	const std::vector< double > &	first,
		const std::vector< double > &	last,
		const Eigen::MatrixXd &	control_cost_matrix_R_padded,
		const Eigen::MatrixXd &	inv_control_cost_matrix_R,
		Eigen::MatrixXd &	trajectory_joints
	)

Compute a minimum cost trajectory given a start and end state.

Parameters

first	The start position
last	The final position
control_cost_matrix_R_padded	The control cost matrix with padding
inv_control_cost_matrix_R	The inverse constrol cost matrix
trajectory_joints	The returned minimum cost trajectory

Returns

True if successful, otherwise false

Definition at line 100 of file stomp.cpp.

void computeParametersControlCosts	(	const Eigen::MatrixXd &	parameters,
		double	dt,
		double	control_cost_weight,
		const Eigen::MatrixXd &	control_cost_matrix_R,
		Eigen::MatrixXd &	control_costs
	)

Compute the parameters control costs.

Parameters

parameters	The parameters used to compute the control cost
dt	The timestep in seconds
control_cost_weight	The control cost weight
control_cost_matrix_R	The control cost matrix
control_costs	returns The parameters control costs

Definition at line 149 of file stomp.cpp.

Variable Documentation

const double DEFAULT_NOISY_COST_IMPORTANCE_WEIGHT = 1.0

static

Default noisy cost importance weight

Definition at line 36 of file stomp.cpp.

const double MIN_CONTROL_COST_WEIGHT = 1e-8

static

Minimum control cost weight allowed

Definition at line 38 of file stomp.cpp.

const double MIN_COST_DIFFERENCE = 1e-8

static

Minimum cost difference allowed during probability calculation

Definition at line 37 of file stomp.cpp.