stomp_example.cpp File Reference

Demonstrates how to optimize a trajectory using STOMP. More...

#include <iostream>
#include <Eigen/Dense>
#include "stomp_core/stomp.h"
#include "simple_optimization_task.h"

Include dependency graph for stomp_example.cpp:

Go to the source code of this file.

Typedefs
using	Trajectory = Eigen::MatrixXd

Functions
bool	compareDiff (const Trajectory &optimized, const Trajectory &desired, const std::vector< double > &thresholds)
	Compares whether two trajectories are close to each other within a threshold. More...
stomp_core::StompConfiguration	create3DOFConfiguration ()
	Creates a STOMP configuration object with default parameters. More...
void	interpolate (const std::vector< double > &start, const std::vector< double > &end, std::size_t num_timesteps, Trajectory &traj)
	Compute a linear interpolated trajectory given a start and end state. More...
int	main (int argc, char **argv)

Variables
static const std::vector< double >	BIAS_THRESHOLD = {0.050,0.050,0.050}
static const double	DELTA_T = 0.1
static const std::vector< double >	END_POS = {-1.25, 1.0, -0.26}
static const std::size_t	NUM_DIMENSIONS = 3
static const std::size_t	NUM_TIMESTEPS = 20
static const std::vector< double >	START_POS = {1.4, 1.4, 0.5}
static const std::vector< double >	STD_DEV = {1.0, 1.0, 1.0}

Detailed Description

Demonstrates how to optimize a trajectory using STOMP.

Author

Jorge Nicho

Date

Dec 14, 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_example.cpp.

Typedef Documentation

using Trajectory = Eigen::MatrixXd

Declaring optimization variables

Definition at line 34 of file stomp_example.cpp.

Function Documentation

bool compareDiff	(	const Trajectory &	optimized,
		const Trajectory &	desired,
		const std::vector< double > &	thresholds
	)

Compares whether two trajectories are close to each other within a threshold.

Parameters

optimized	optimized trajectory
desired	desired trajectory
thresholds	used to determine if two values are equal

Returns

True if the difference between the two is less than the threshold, otherwise false

Definition at line 74 of file stomp_example.cpp.

stomp_core::StompConfiguration create3DOFConfiguration

(

)

Creates a STOMP configuration object with default parameters.

Returns

A STOMP configuration object

[Create Config]

[Create Config]

Definition at line 47 of file stomp_example.cpp.

void interpolate	(	const std::vector< double > &	start,
		const std::vector< double > &	end,
		std::size_t	num_timesteps,
		Trajectory &	traj
	)

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

Parameters

start	start position
end	last position
num_timesteps	number of timesteps
traj	returned linear interpolated trajectory

Definition at line 99 of file stomp_example.cpp.

int main	(	int	argc,
		char **	argv
	)

< Creating a Task with a trajectory bias

[Create Task Object]

[Create Task Object]

[Create STOMP]

< Creating STOMP to find a trajectory close enough to the bias

[Create STOMP]

[Solve]

< Optimizing a trajectory close enough to the bias is produced

[Solve]

< Further verifying the results

Definition at line 114 of file stomp_example.cpp.

Variable Documentation

const std::vector<double> BIAS_THRESHOLD = {0.050,0.050,0.050}

static

Threshold to determine whether two trajectories are equal

Definition at line 40 of file stomp_example.cpp.

const double DELTA_T = 0.1

static

Timestep in seconds

Definition at line 37 of file stomp_example.cpp.

const std::vector<double> END_POS = {-1.25, 1.0, -0.26}

static

Trajectory ending posiiton

Definition at line 39 of file stomp_example.cpp.

const std::size_t NUM_DIMENSIONS = 3

static

Number of parameters to optimize

Definition at line 35 of file stomp_example.cpp.

const std::size_t NUM_TIMESTEPS = 20

static

Number of timesteps

Definition at line 36 of file stomp_example.cpp.

const std::vector<double> START_POS = {1.4, 1.4, 0.5}

static

Trajectory starting position

Definition at line 38 of file stomp_example.cpp.

const std::vector<double> STD_DEV = {1.0, 1.0, 1.0}

static

Standard deviation used for generating noisy parameters

Definition at line 41 of file stomp_example.cpp.