Implementation of Contextual Dynamical Movement Primitives. More...

#include <DmpContextualTwoStep.hpp>

Inheritance diagram for DmpContextualTwoStep:

[legend]

Collaboration diagram for DmpContextualTwoStep:

[legend]

Public Member Functions
	DmpContextualTwoStep (int n_dims_dmp, std::vector< FunctionApproximator * > function_approximators, FunctionApproximator *policy_parameter_function, DmpType dmp_type=KULVICIUS_2012_JOINING)
	Initialization constructor for Contextual DMPs of known dimensionality, but with unknown initial and attractor states. More...

void	computeFunctionApproximatorOutput (const Eigen::Ref< const Eigen::MatrixXd > &phase_state, Eigen::MatrixXd &fa_output) const
	Compute the outputs of the function approximators. More...

void	train (const std::vector< Trajectory > &trajectories, const std::vector< Eigen::MatrixXd > &task_parameters, std::string save_directory="", bool overwrite=true)
	Train a contextual Dmp with a set of trajectories (and save results to file) This function is useful for debugging, i.e. More...

bool	isTrained (void) const
	Return whether the DMP is trained or not. More...

Public Member Functions inherited from DmpContextual
	DmpContextual (int n_dims_dmp, std::vector< FunctionApproximator * > function_approximators, DmpType dmp_type)
	Initialization constructor for Contextual DMPs of known dimensionality, but with unknown initial and attractor states. More...

void	set_task_parameters (const Eigen::MatrixXd &task_parameters)
	Set the current task parameters. More...

void	set_policy_parameter_function_goal (FunctionApproximator *function_approximator)
	Set a function approximator to predict the goal from the task parameters. More...

void	set_policy_parameter_function_duration (FunctionApproximator *function_approximator)
	Set a function approximator to predict the duration from the task parameters. More...

void	train (const std::vector< Trajectory > &trajectories, const std::vector< Eigen::MatrixXd > &task_parameters, std::string save_directory)
	Train a contextual DMP with multiple trajectories. More...

void	train (const std::vector< Trajectory > &trajectories, const std::vector< Eigen::MatrixXd > &task_parameters)
	Train a contextual DMP with multiple trajectories. More...

void	train (const std::vector< Trajectory > &trajectories, std::string save_directory, bool overwrite)
	Train a contextual DMP with multiple trajectories. More...

void	train (const std::vector< Trajectory > &trajectories, std::string save_directory)
	Train a contextual DMP with multiple trajectories. More...

void	train (const std::vector< Trajectory > &trajectories)
	Train a contextual DMP with multiple trajectories. More...

Public Member Functions inherited from Dmp
	Dmp (double tau, Eigen::VectorXd y_init, Eigen::VectorXd y_attr, std::vector< FunctionApproximator * > function_approximators, double alpha_spring_damper, DynamicalSystem goal_system, DynamicalSystem phase_system, DynamicalSystem *gating_system, ForcingTermScaling scaling=NO_SCALING)
	Initialization constructor. More...

	Dmp (int n_dims_dmp, std::vector< FunctionApproximator * > function_approximators, double alpha_spring_damper, DynamicalSystem goal_system, DynamicalSystem phase_system, DynamicalSystem *gating_system, ForcingTermScaling scaling=NO_SCALING)
	Initialization constructor for Dmps of known dimensionality, but with unknown initial and attractor states. More...

	Dmp (double tau, Eigen::VectorXd y_init, Eigen::VectorXd y_attr, std::vector< FunctionApproximator * > function_approximators, DmpType dmp_type=KULVICIUS_2012_JOINING, ForcingTermScaling scaling=NO_SCALING)
	Constructor that initializes the DMP with default dynamical systems. More...

	Dmp (int n_dims_dmp, std::vector< FunctionApproximator * > function_approximators, DmpType dmp_type=KULVICIUS_2012_JOINING, ForcingTermScaling scaling=NO_SCALING)
	Initialization constructor for Dmps of known dimensionality, but with unknown initial and attractor states. More...

	Dmp (double tau, Eigen::VectorXd y_init, Eigen::VectorXd y_attr, double alpha_spring_damper, DynamicalSystem *goal_system)
	Initialization constructor for Dmps without a forcing term. More...

	~Dmp (void)
	Destructor. More...

Dmp *	clone (void) const
	Return a deep copy of this object. More...

virtual void	integrateStart (Eigen::Ref< Eigen::VectorXd > x, Eigen::Ref< Eigen::VectorXd > xd) const
	Start integrating the system. More...

void	differentialEquation (const Eigen::Ref< const Eigen::VectorXd > &x, Eigen::Ref< Eigen::VectorXd > xd) const
	The differential equation which defines the system. More...

void	analyticalSolution (const Eigen::VectorXd &ts, Eigen::MatrixXd &xs, Eigen::MatrixXd &xds, Eigen::MatrixXd &forcing_terms, Eigen::MatrixXd &fa_output) const
	Return analytical solution of the system at certain times (and return forcing terms) More...

void	analyticalSolution (const Eigen::VectorXd &ts, Eigen::MatrixXd &xs, Eigen::MatrixXd &xds, Eigen::MatrixXd &forcing_terms) const
	Return analytical solution of the system at certain times (and return forcing terms) More...

void	analyticalSolution (const Eigen::VectorXd &ts, Eigen::MatrixXd &xs, Eigen::MatrixXd &xds) const
	Return analytical solution of the system at certain times. More...

virtual void	analyticalSolution (const Eigen::VectorXd &ts, Trajectory &trajectory) const
	Return analytical solution of the system at certain times. More...

void	analyticalSolution (const Eigen::VectorXd &ts, Trajectory &trajectory, Eigen::MatrixXd &forcing_terms) const
	Return analytical solution of the system at certain times. More...

virtual void	statesAsTrajectory (const Eigen::MatrixXd &x_in, const Eigen::MatrixXd &xd_in, Eigen::MatrixXd &y_out, Eigen::MatrixXd &yd_out, Eigen::MatrixXd &ydd_out) const
	Get the output of a DMP dynamical system as a trajectory. More...

virtual void	statesAsTrajectory (const Eigen::VectorXd &ts, const Eigen::MatrixXd &x_in, const Eigen::MatrixXd &xd_in, Trajectory &trajectory) const
	Get the output of a DMP dynamical system as a trajectory. More...

virtual void	train (const Trajectory &trajectory)
	Train a DMP with a trajectory. More...

virtual void	train (const Trajectory &trajectory, std::string save_directory, bool overwrite=false)
	Train a DMP with a trajectory, and write results to file. More...

virtual void	set_tau (double tau)
	Accessor function for the time constant. More...

virtual void	set_initial_state (const Eigen::VectorXd &y_init)
	Accessor function for the initial state of the system. More...

virtual void	set_attractor_state (const Eigen::VectorXd &y_attr)
	Accessor function for the attractor state of the system. More...

void	set_damping_coefficient (double damping_coefficient)
	Accessor function for damping coefficient of spring-damper system. More...

void	set_spring_constant (double spring_constant)
	Accessor function for spring constant of spring-damper system. More...

std::string	toString (void) const
	Returns a string representation of the object. More...

void	getSelectableParameters (std::set< std::string > &selectable_values_labels) const
	Return all the names of the parameter types that can be selected. More...

void	setSelectedParameters (const std::set< std::string > &selected_values_labels)
	Determine which subset of parameters is represented in the vector returned by Parameterizable::getParameterVectorSelected. More...

int	getParameterVectorAllSize (void) const
	Get the size of the parameter values vector when it contains all available parameter values. More...

void	getParameterVectorAll (Eigen::VectorXd &values) const
	Return a vector that returns all available parameter values. More...

void	setParameterVectorAll (const Eigen::VectorXd &values)
	Set all available parameter values with one vector. More...

void	getParameterVectorMask (const std::set< std::string > selected_values_labels, Eigen::VectorXi &selected_mask) const
	Get a mask for selecting parameters. More...

void	computeFunctionApproximatorInputsAndTargets (const Trajectory &trajectory, Eigen::VectorXd &fa_inputs_phase, Eigen::MatrixXd &fa_targets) const
	Given a trajectory, compute the inputs and targets for the function approximators. More...

void	set_perturbation_analytical_solution (double perturbation_standard_deviation)
	Add a perturbation to the forcing term when computing the analytical solution. More...

double	get_perturbation_analytical_solution () const
	Get the perturbation to the forcing term when computing the analytical solution. More...

Public Member Functions inherited from DynamicalSystem
	DynamicalSystem (int order, double tau, Eigen::VectorXd initial_state, Eigen::VectorXd attractor_state, std::string name)
	Initialization constructor. More...

virtual	~DynamicalSystem (void)
	Destructor.

void	integrateStart (const Eigen::VectorXd &x_init, Eigen::Ref< Eigen::VectorXd > x, Eigen::Ref< Eigen::VectorXd > xd)
	Start integrating the system with a new initial state. More...

virtual void	integrateStep (double dt, const Eigen::Ref< const Eigen::VectorXd > x, Eigen::Ref< Eigen::VectorXd > x_updated, Eigen::Ref< Eigen::VectorXd > xd_updated) const
	Integrate the system one time step. More...

void	set_integration_method (IntegrationMethod integration_method)
	Choose the integration method. More...

int	dim (void) const
	Get the dimensionality of the dynamical system, i.e. More...

int	dim_orig (void) const
	Get the dimensionality of the dynamical system, i.e. More...

double	tau (void) const
	Accessor function for the time constant. More...

Eigen::VectorXd	initial_state (void) const
	Accessor function for the initial state of the dynamical system. More...

void	initial_state (Eigen::VectorXd &initial_state) const
	Accessor function for the initial state of the dynamical system. More...

Eigen::VectorXd	attractor_state (void) const
	Accessor function for the attractor state of the dynamical system. More...

void	attractor_state (Eigen::VectorXd &attractor_state) const
	Accessor function for the attractor state of the dynamical system. More...

virtual void	set_attractor_state (const Eigen::Ref< const Eigen::VectorXd > &attractor_state)
	Mutator function for the attractor state of the dynamical system. More...

std::string	name (void) const
	Accessor function for the name of the dynamical system. More...

virtual void	set_name (std::string name)
	Mutator function for the name of the dynamical system. More...

Public Member Functions inherited from Parameterizable
virtual	~Parameterizable (void)
	Destructor.

virtual int	getParameterVectorSelectedSize (void) const
	Get the size of the vector of selected parameters, as returned by getParameterVectorSelected(. More...

virtual void	getParameterVectorSelected (Eigen::VectorXd &values, bool normalized=false) const
	Get the values of the selected parameters in one vector. More...

virtual void	getParameterVectorSelectedNormalized (Eigen::VectorXd &values) const
	Get the normalized values of the selected parameters in one vector. More...

void	getParameterVectorSelectedMinMax (Eigen::VectorXd &min, Eigen::VectorXd &max) const
	Get the minimum and maximum of the selected parameters in one vector. More...

void	getParameterVectorAllMinMax (Eigen::VectorXd &min, Eigen::VectorXd &max) const
	Get the minimum and maximum values of the current parameter vector. More...

void	getParameterVectorSelectedRanges (Eigen::VectorXd &ranges) const
	Get the ranges of the selected parameters, i.e. More...

virtual void	setParameterVectorSelected (const Eigen::VectorXd &values, bool normalized=false)
	Set all the values of the selected parameters with one vector. More...

virtual void	setParameterVectorSelectedNormalized (const Eigen::VectorXd &values)
	Set all the values of the selected parameters with one vector of normalized values. More...

void	setSelectedParametersOne (std::string selected)
	Set the parameters that are currently selected. More...

void	setParameterVectorModifier (std::string modifier, bool new_value)
	Turn certain modifiers on or off. More...

void	setVectorLengthsPerDimension (const Eigen::VectorXi &lengths_per_dimension)
	The vector (VectorXd) with parameter values can be split into different parts (as vector<VectorXd>; this function specifices the length of each sub-vector. More...

Eigen::VectorXi	getVectorLengthsPerDimension (void) const
	Get the specified length of each vector in each dimension. More...

void	getParameterVectorSelected (std::vector< Eigen::VectorXd > &values, bool normalized=false) const
	Get the values of the selected parameters in one vector. More...

void	setParameterVectorSelected (const std::vector< Eigen::VectorXd > &values, bool normalized=false)
	Set all the values of the selected parameters with a vector of vectors. More...

Protected Member Functions
	DmpContextualTwoStep (void)
	Default constructor. More...

Protected Member Functions inherited from DmpContextual
void	trainLocal (const std::vector< Trajectory > &trajectories, const std::vector< Eigen::MatrixXd > &task_parameters, std::string save_directory, bool overwrite)
	Train a contextual DMP. More...

void	checkTrainTrajectories (const std::vector< Trajectory > &trajectories)
	Check if several trajectories have the same duration and initial/final states. More...

Protected Member Functions inherited from Dmp
FunctionApproximator *	function_approximator (int i_dim) const
	Get a pointer to the function approximator for a certain dimension. More...

Protected Member Functions inherited from DynamicalSystem
	DynamicalSystem (void)
	Default constructor. More...

void	set_dim (int dim)
	Set the dimensionality of the dynamical system, i.e. More...

Friends
class	boost::serialization::access
	Give boost serialization access to private members. More...

Additional Inherited Members
Public Types inherited from Dmp
enum	DmpType { IJSPEERT_2002_MOVEMENT, KULVICIUS_2012_JOINING, COUNTDOWN_2013 }
	Different types of DMPs that can be initialized. More...

enum	ForcingTermScaling { NO_SCALING, G_MINUS_Y0_SCALING, AMPLITUDE_SCALING }
	Different ways to scale the forcing term. More...

Public Types inherited from DynamicalSystem
enum	IntegrationMethod { EULER, RUNGE_KUTTA }
	The possible integration methods that can be used. More...

Protected Attributes inherited from DmpContextual
Eigen::MatrixXd	task_parameters_
	The current task parameters.

std::vector< FunctionApproximator * >	policy_parameter_function_goal_
	FunctionApproximators that relate task parameters the goal of the DMP.

FunctionApproximator *	policy_parameter_function_duration_
	FunctionApproximator that relates task parameters the duration of the DMP.

Detailed Description

Implementation of Contextual Dynamical Movement Primitives.

Definition at line 40 of file DmpContextualTwoStep.hpp.

Constructor & Destructor Documentation

DmpContextualTwoStep	(	int	n_dims_dmp,
		std::vector< FunctionApproximator * >	function_approximators,
		FunctionApproximator *	policy_parameter_function,
		DmpType	dmp_type = `KULVICIUS_2012_JOINING`
	)

Initialization constructor for Contextual DMPs of known dimensionality, but with unknown initial and attractor states.

Initializes the DMP with default dynamical systems.

Parameters

n_dims_dmp	Dimensionality of the DMP
function_approximators	Function approximators for the forcing term
policy_parameter_function	Function approximators for the policy parameter function
dmp_type	The type of DMP, see Dmp::DmpType

Definition at line 41 of file DmpContextualTwoStep.cpp.

 :  DmpContextual(n_dims_dmp, function_approximators, dmp_type)
 {
   policy_parameter_function_ = vector<vector<FunctionApproximator*> >(dim_orig());
   for (int dd=0; dd<dim_orig(); dd++)
   { 
     policy_parameter_function_[dd] = vector<FunctionApproximator*>(1);
     policy_parameter_function_[dd][0] = policy_parameter_function->clone();
   }
 }

Here is the call graph for this function:

DmpContextualTwoStep ( void )

inlineprotected

Default constructor.

Remarks: This default constuctor is required for boost::serialization to work. See Boost serialization issues

Definition at line 75 of file DmpContextualTwoStep.hpp.

75 {};

Here is the call graph for this function:

Member Function Documentation

void computeFunctionApproximatorOutput	(	const Eigen::Ref< const Eigen::MatrixXd > &	phase_state,
		Eigen::MatrixXd &	fa_output
	)		const

virtual

Compute the outputs of the function approximators.

Parameters

[in]	phase_state	The phase states for which the outputs are computed.
[out]	fa_output	The outputs of the function approximators.

Implements DmpContextual.

Definition at line 53 of file DmpContextualTwoStep.cpp.

 {
   int n_time_steps = phase_state.rows(); 
   fa_output.resize(n_time_steps,dim_orig());
   fa_output.fill(0.0);
   
   if (task_parameters_.rows()==0)
   {
     // When the task parameters are not set, we cannot compute the output of the function approximator.
     return;
   }
   
   MatrixXd task_parameters = task_parameters_;
   if (task_parameters.rows()==1)
   { 
     task_parameters = task_parameters.row(0).replicate(n_time_steps,1).eval();
   }
   else if (task_parameters.cols()==1)
   {
     task_parameters = task_parameters.col(0).transpose().replicate(n_time_steps,1).eval();
   }
 
   assert(n_time_steps==task_parameters.rows());
   
   //int n_task_parameters = task_parameters.cols();
   
   VectorXd model_parameters;
   MatrixXd output(1,1);
   for (int dd=0; dd<dim_orig(); dd++)
   { 
     int n_parameters = function_approximator(dd)->getParameterVectorSelectedSize();
     model_parameters.resize(n_parameters);
     for (int pp=0; pp<n_parameters; pp++)
     {
       policy_parameter_function_[dd][pp]->predict(task_parameters,output);
       model_parameters[pp] = output(0,0);
     }
     function_approximator(dd)->setParameterVectorSelected(model_parameters);
   }
 
   // The parameters of the function_approximators have been set, get their outputs now.  
   for (int dd=0; dd<dim_orig(); dd++)
   {
     function_approximator(dd)->predict(phase_state,output);
     if (output.size()>0)
     {
       fa_output.col(dd) = output;
     }
   }
 
 }

Here is the call graph for this function:

void train	(	const std::vector< Trajectory > &	trajectories,
		const std::vector< Eigen::MatrixXd > &	task_parameters,
		std::string	save_directory = `""`,
		bool	overwrite = `true`
	)

virtual

Train a contextual Dmp with a set of trajectories (and save results to file) This function is useful for debugging, i.e.

if you want to save intermediate results to a directory

Parameters

[in]	trajectories	The set of trajectories
[in]	task_parameters	The task parameters for each of the trajectories.
[in]	save_directory	Directory to which to save intermediate results.
[in]	overwrite	Overwrite existing files in the directory above Overloads Dmp::train

Implements DmpContextual.

Definition at line 116 of file DmpContextualTwoStep.cpp.

 {
   // Here's the basic structure of this function
   // 1) Do some checks
   // 2) Train a separate Dmp for each demonstration, and get the resulting model parameters
   // 3) Gather all task parameter values for all demonstrations
   // 4) Train the policy parameter function for each dimension and each model parameter
   
   
   //-----------------------------------------------------
   // 1) Do some checks
   
   // Check if inputs are of the right size.
   unsigned int n_demonstrations = trajectories.size();
   assert(n_demonstrations==task_parameters.size());  
   
   
   // Then check if the trajectories have the same duration and initial/final state
   // Later on, if they are not the same, they should be learned also.
   checkTrainTrajectories(trajectories);
 
   // Set tau, initial_state and attractor_state from the trajectories 
   set_tau(trajectories[0].duration());
   set_initial_state(trajectories[0].initial_y());
   set_attractor_state(trajectories[0].final_y());
 
   //-----------------------------------------------------
   // 2) Train a separate Dmp for each demonstration, and get the resulting model parameters
   std::set<std::string> selected;
   selected.insert("offsets");
   selected.insert("slopes");
   
   MatrixXd cur_task_parameters;
   VectorXd cur_model_parameters;// todo Remove redundant tmp variable
   vector<MatrixXd> all_model_parameters(n_demonstrations); 
   for (unsigned int i_demo=0; i_demo<n_demonstrations; i_demo++)
   {
     
     string save_directory_demo;
     if (!save_directory.empty())
       save_directory_demo = save_directory + "/demo" + to_string(i_demo);
     
     Dmp::train(trajectories[i_demo],save_directory_demo,overwrite);
     
     for (int i_dim=0; i_dim<dim_orig(); i_dim++)
     {
 
       // todo Should be argument of constructor
       function_approximator(i_dim)->setSelectedParameters(selected); 
   
       function_approximator(i_dim)->getParameterVectorSelected(cur_model_parameters);
       //cout << cur_model_parameters << endl;
       if (i_demo==0)
         all_model_parameters[i_dim].resize(n_demonstrations,cur_model_parameters.size());
       else
         assert(cur_model_parameters.size()==all_model_parameters[i_dim].cols());
 
       all_model_parameters[i_dim].row(i_demo) = cur_model_parameters;
       
     }
   }
 
    
   //-----------------------------------------------------
   // 3) Gather all task parameter values for all demonstrations
   
   // Gather task parameters in a matrix
   int n_task_parameters = task_parameters[0].cols();
   // This is the first time task_parameters_ is set, because this is the first time we know 
   // n_task_parameters.
   // We set it so that set_task_parameters can check if task_parameters_.cols()==n_task_parameters
   task_parameters_ = MatrixXd::Zero(1,n_task_parameters);
   VectorXd cur_task_parameters_t0;
   
   MatrixXd inputs(n_demonstrations,n_task_parameters);
   for (unsigned int i_demo=0; i_demo<n_demonstrations; i_demo++)
   {
     // These are the task parameters for the current demonstration at t=0
     cur_task_parameters_t0 = task_parameters[i_demo].row(0);
     
     // Task parameter may not change over time for 2-Step contextual DMP
     // Start comparison to i_time=0 at i_time=1
     for (int i_time=1; i_time<task_parameters[i_demo].rows(); i_time++)
     {
       if ( (cur_task_parameters_t0.array() != task_parameters[i_demo].row(i_time).array()).any())
       {
         cerr << __FILE__ << ":" << __LINE__ << ":";
         cerr << "WARNING. For DmpContextualTwoStep, task parameters may not vary over time during training. Using task parameters at t=0 only." << endl;
       }
     }
 
     // Take the first row, i.e. at time_i = 0. We checked above if they are constant over time.
     inputs.row(i_demo) = cur_task_parameters_t0;
   }
 
   //-----------------------------------------------------
   // 4) Train the policy parameter function for each dimension and each model parameter
   
   // Input to policy parameter functions: task_parameters
   // Target for each policy parameter function: all_model_parameters.col(param)
   
   for (int i_dim=0; i_dim<dim_orig(); i_dim++)
   {
     int n_pol_pars = all_model_parameters[i_dim].cols();
     for (int i_pol_par=1; i_pol_par<n_pol_pars; i_pol_par++)
     {
       policy_parameter_function_[i_dim].push_back(policy_parameter_function_[i_dim][0]->clone());
       //cout << *(policy_parameter_function_[i_dim][i_pol_par]) << endl;
     }
 
     for (int i_pol_par=0; i_pol_par<n_pol_pars; i_pol_par++)
     {
       MatrixXd targets = all_model_parameters[i_dim].col(i_pol_par);
       //cout << "_________________" << endl;
       //cout << inputs.transpose() << endl << endl;
       //cout << targets.transpose() << endl;
       
       string save_directory_cur;
       if (!save_directory.empty())
           save_directory_cur = save_directory + "/dim" + to_string(i_dim) + "_polpar" + to_string(i_pol_par);
       
       policy_parameter_function_[i_dim][i_pol_par]->train(inputs,targets,save_directory_cur,overwrite);
     
     }
   }
   
   
 }

Here is the call graph for this function:

bool isTrained ( void ) const

Return whether the DMP is trained or not.

Returns: true if the DMP is trained, false otherwise

Definition at line 106 of file DmpContextualTwoStep.cpp.

 {
   for (int dd=0; dd<dim_orig(); dd++)
     for (unsigned int pp=0; pp<policy_parameter_function_[dd].size(); pp++)
       if (!policy_parameter_function_[dd][pp]->isTrained())
         return false;
       
   return true;   
 }

Here is the call graph for this function:

Friends And Related Function Documentation

friend class boost::serialization::access

friend

Give boost serialization access to private members.

Definition at line 75 of file DmpContextualTwoStep.hpp.

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

Public Member Functions

Protected Member Functions

Friends

Additional Inherited Members

Detailed Description

Constructor & Destructor Documentation

Member Function Documentation

Friends And Related Function Documentation