stomp_3dof.cpp File Reference

This contains gtest code for stomp. More...

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

Include dependency graph for stomp_3dof.cpp:

Go to the source code of this file.

Classes
class	DummyTask
	A dummy task for testing STOMP. More...

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...
StompConfiguration	create3DOFConfiguration ()
	Create the STOMP configuration for the 3 DOF problem. 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...
	TEST (Stomp3DOF, construction)
	This tests the Stomp constructor.
	TEST (Stomp3DOF, solve_default)
	This tests the Stomp solve method for a default case.
	TEST (Stomp3DOF, solve_interpolated_initial)
	This tests the Stomp solve method using a linear interpolated inital trajectory.
	TEST (Stomp3DOF, solve_cubic_polynomial_initial)
	This tests the Stomp solve method using a cubic polynomial interpolated inital trajectory.
	TEST (Stomp3DOF, solve_min_control_cost_initial)
	This tests the Stomp solve method using a minimum cost inital trajectory.
	TEST (Stomp3DOF, solve_40_timesteps)
	This tests Stomp solve method given 40 timesteps.
	TEST (Stomp3DOF, solve_60_timesteps)
	This tests Stomp solve method given 60 timesteps.

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

Detailed Description

This contains gtest code for stomp.

Author

Jorge Nicho

Date

March 25, 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_3dof.cpp.

Typedef Documentation

using Trajectory = Eigen::MatrixXd

Assign Type Trajectory to Eigen::MatrixXd Type

Definition at line 32 of file stomp_3dof.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 188 of file stomp_3dof.cpp.

StompConfiguration create3DOFConfiguration

(

)

Create the STOMP configuration for the 3 DOF problem.

Returns

StompConfiguration

Definition at line 210 of file stomp_3dof.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 233 of file stomp_3dof.cpp.

Variable Documentation

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

Threshold to determine whether two trajectories are equal

Definition at line 39 of file stomp_3dof.cpp.

const double DELTA_T = 0.1

Timestep in seconds

Definition at line 36 of file stomp_3dof.cpp.

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

Trajectory ending posiiton

Definition at line 38 of file stomp_3dof.cpp.

const std::size_t NUM_DIMENSIONS = 3

Number of parameters to optimize

Definition at line 34 of file stomp_3dof.cpp.

const std::size_t NUM_TIMESTEPS = 20

Number of timesteps

Definition at line 35 of file stomp_3dof.cpp.

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

Trajectory starting position

Definition at line 37 of file stomp_3dof.cpp.

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

Standard deviation used for generating noisy parameters

Definition at line 40 of file stomp_3dof.cpp.