Implementation of Dynamical Movement Primitives. More...

#include <Dmp.hpp>

Inheritance diagram for Dmp:

[legend]

Collaboration diagram for Dmp:

Public Types
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...

Public Member Functions
	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...

virtual void	computeFunctionApproximatorOutput (const Eigen::Ref< const Eigen::MatrixXd > &phase_state, Eigen::MatrixXd &fa_output) const
	Compute the outputs of 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
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...

Detailed Description

Implementation of Dynamical Movement Primitives.

Definition at line 56 of file Dmp.hpp.

Member Enumeration Documentation

enum DmpType

Different types of DMPs that can be initialized.

Definition at line 61 of file Dmp.hpp.

61 { IJSPEERT_2002_MOVEMENT, KULVICIUS_2012_JOINING, COUNTDOWN_2013 };

enum ForcingTermScaling

Different ways to scale the forcing term.

Definition at line 64 of file Dmp.hpp.

64 { NO_SCALING, G_MINUS_Y0_SCALING, AMPLITUDE_SCALING };

Constructor & Destructor Documentation

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.

Parameters

tau	Time constant
y_init	Initial state
y_attr	Attractor state
alpha_spring_damper	$\alpha$ in the spring-damper system of the dmp
goal_system	Dynamical system to compute delayed goal
phase_system	Dynamical system to compute the phase
gating_system	Dynamical system to compute the gating term
function_approximators	Function approximators for the forcing term
scaling	Which method to use for scaling the forcing term (see Dmp::ForcingTermScaling)

Definition at line 85 of file Dmp.cpp.

   : DynamicalSystem(1, tau, y_init, y_attr, "name"),
   goal_system_(goal_system),
   phase_system_(phase_system), gating_system_(gating_system), 
   forcing_term_scaling_(scaling)
 {
   initSubSystems(alpha_spring_damper, goal_system, phase_system, gating_system);
   initFunctionApproximators(function_approximators);
 }

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.

Parameters

n_dims_dmp	Dimensionality of the DMP
alpha_spring_damper	$\alpha$ in the spring-damper system of the dmp
goal_system	Dynamical system to compute delayed goal
phase_system	Dynamical system to compute the phase
gating_system	Dynamical system to compute the gating term
function_approximators	Function approximators for the forcing term
scaling	Which method to use for scaling the forcing term (see Dmp::ForcingTermScaling)

Definition at line 102 of file Dmp.cpp.

   : DynamicalSystem(1, 1.0, VectorXd::Zero(n_dims_dmp), VectorXd::Ones(n_dims_dmp), "name"),
   goal_system_(goal_system),
   phase_system_(phase_system), gating_system_(gating_system),
   forcing_term_scaling_(scaling)
 {
   initSubSystems(alpha_spring_damper, goal_system, phase_system, gating_system);
   initFunctionApproximators(function_approximators);
 }

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.

Parameters

tau	Time constant
y_init	Initial state
y_attr	Attractor state
function_approximators	Function approximators for the forcing term
dmp_type	The type of DMP, see Dmp::DmpType
scaling	Which method to use for scaling the forcing term (see Dmp::ForcingTermScaling)

Definition at line 115 of file Dmp.cpp.

   : DynamicalSystem(1, tau, y_init, y_attr, "name"),
     forcing_term_scaling_(scaling)
 {  
   initSubSystems(dmp_type);
   initFunctionApproximators(function_approximators);
 }

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.

Initializes the DMP with default dynamical systems.

Parameters

n_dims_dmp	Dimensionality of the DMP
function_approximators	Function approximators for the forcing term
dmp_type	The type of DMP, see Dmp::DmpType
scaling	Which method to use for scaling the forcing term (see Dmp::ForcingTermScaling)

Definition at line 126 of file Dmp.cpp.

   : DynamicalSystem(1, 1.0, VectorXd::Zero(n_dims_dmp), VectorXd::Ones(n_dims_dmp), "name"),
     forcing_term_scaling_(scaling)
 {
   initSubSystems(dmp_type);
   initFunctionApproximators(function_approximators);
 }

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.

Parameters

tau	Time constant
y_init	Initial state
y_attr	Attractor state
alpha_spring_damper	$\alpha$ in the spring-damper system of the dmp
goal_system	Dynamical system to compute delayed goal

Definition at line 136 of file Dmp.cpp.

   : DynamicalSystem(1, tau, y_init, y_attr, "name"), forcing_term_scaling_(NO_SCALING)
 {
   
   VectorXd one_1 = VectorXd::Ones(1);
   VectorXd one_0 = VectorXd::Zero(1);
   DynamicalSystem* phase_system  = new ExponentialSystem(tau,one_1,one_0,4);
   DynamicalSystem* gating_system = new ExponentialSystem(tau,one_1,one_0,4); 
   initSubSystems(alpha_spring_damper, goal_system, phase_system, gating_system);
 
   vector<FunctionApproximator*> function_approximators(y_init.size());    
   for (int dd=0; dd<y_init.size(); dd++)
     function_approximators[dd] = NULL;
   initFunctionApproximators(function_approximators);  
 }

Here is the call graph for this function:

~Dmp ( void )

Destructor.

Definition at line 242 of file Dmp.cpp.

 {
   delete goal_system_;   
   delete spring_system_;
   delete phase_system_;
   delete gating_system_;
   for (unsigned int ff=0; ff<function_approximators_.size(); ff++)
     delete (function_approximators_[ff]);
 }

Member Function Documentation

Dmp * clone ( void ) const

virtual

Return a deep copy of this object.

Returns: A deep copy of this object

Implements DynamicalSystem.

Reimplemented in DmpWithGainSchedules.

Definition at line 252 of file Dmp.cpp.

                           {
   vector<FunctionApproximator*> function_approximators;
   for (unsigned int ff=0; ff<function_approximators_.size(); ff++)
     function_approximators.push_back(function_approximators_[ff]->clone());
   
   return new Dmp(tau(), initial_state(), attractor_state(), function_approximators,
    spring_system_->damping_coefficient(), goal_system_->clone(),
    phase_system_->clone(), gating_system_->clone());
 }

Here is the call graph for this function:

void integrateStart	(	Eigen::Ref< Eigen::VectorXd >	x,
		Eigen::Ref< Eigen::VectorXd >	xd
	)		const

virtual

Start integrating the system.

Parameters

[out]	x	- The first vector of state variables
[out]	xd	- The first vector of rates of change of the state variables

Remarks: x and xd should be of size dim() X 1. This forces you to pre-allocate memory, which speeds things up (and also makes Eigen's Ref functionality easier to deal with).

Reimplemented from DynamicalSystem.

Definition at line 263 of file Dmp.cpp.

 {
   assert(x.size()==dim());
   assert(xd.size()==dim());
   
   x.fill(0);  
   xd.fill(0);  
   
   // Start integrating goal system if it exists
   if (goal_system_==NULL)
   {
     // No goal system, simply set goal state to attractor state
     x.GOAL = attractor_state();
     xd.GOAL.fill(0);
   }
   else
   {
     // Goal system exists. Start integrating it.
     goal_system_->integrateStart(x.GOAL,xd.GOAL);
   }
   
     
   // Set the attractor state of the spring system
   spring_system_->set_attractor_state(x.GOAL);
   
   // Start integrating all futher subsystems
   spring_system_->integrateStart(x.SPRING, xd.SPRING);
   phase_system_->integrateStart(  x.PHASE,  xd.PHASE);
   gating_system_->integrateStart(x.GATING, xd.GATING);
 
   // Add rates of change
   differentialEquation(x,xd);
   
 }

Here is the call graph for this function:

void differentialEquation	(	const Eigen::Ref< const Eigen::VectorXd > &	x,
		Eigen::Ref< Eigen::VectorXd >	xd
	)		const

virtual

The differential equation which defines the system.

It relates state values to rates of change of those state values

Parameters

[in]	x	current state (column vector of size dim() X 1)
[out]	xd	rate of change in state (column vector of size dim() X 1)

Remarks: x and xd should be of size dim() X 1. This forces you to pre-allocate memory, which speeds things up (and also makes Eigen's Ref functionality easier to deal with).

Implements DynamicalSystem.

Definition at line 344 of file Dmp.cpp.

 {
   
   ENTERING_REAL_TIME_CRITICAL_CODE
   
   attractor_state(attractor_state_prealloc_);  
   if (goal_system_==NULL)
   {
     // If there is no dynamical system for the delayed goal, the goal is
     // simply the attractor state
     spring_system_->set_attractor_state(attractor_state_prealloc_);
     // with zero change
     xd.GOAL.fill(0);
   } 
   else
   {
     // Integrate goal system and get current goal state
     goal_system_->set_attractor_state(attractor_state_prealloc_);
     goal_system_->differentialEquation(x.GOAL, xd.GOAL);
     // The goal state is the attractor state of the spring-damper system
     spring_system_->set_attractor_state(x.GOAL);
     
   }
 
   
   // Integrate spring damper system
   // Forcing term is added to spring_state later
   spring_system_->differentialEquation(x.SPRING, xd.SPRING);
 
   
   // Non-linear forcing term
   phase_system_->differentialEquation(x.PHASE, xd.PHASE);
   gating_system_->differentialEquation(x.GATING, xd.GATING);
 
 
   //MatrixXd phase_state(1,1);
   //phase_state = x.PHASE;
   computeFunctionApproximatorOutput(x.PHASE, fa_output_prealloc_); 
 
   // Gate the output of the function approximators
   int t0 = 0; 
   double gating = (x.GATING)[0];
   forcing_term_prealloc_ = gating*fa_output_prealloc_.row(t0);
   
   
   // Scale the forcing term, if necessary
   if (forcing_term_scaling_==G_MINUS_Y0_SCALING)
   {
     initial_state(initial_state_prealloc_);  
     g_minus_y0_prealloc_ = (attractor_state_prealloc_-initial_state_prealloc_).transpose();
     forcing_term_prealloc_ = forcing_term_prealloc_.array()*g_minus_y0_prealloc_.array();
   }
   else if (forcing_term_scaling_==AMPLITUDE_SCALING)
   {
     forcing_term_prealloc_ = forcing_term_prealloc_.array()*trajectory_amplitudes_.array();
   }
 
   // Add forcing term to the ZD component of the spring state
   xd.SPRING_Z = xd.SPRING_Z + forcing_term_prealloc_/tau();
 
   EXITING_REAL_TIME_CRITICAL_CODE
 
 }

Here is the call graph for this function:

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)

Parameters

[in]	ts	A vector of times for which to compute the analytical solutions
[out]	xs	Sequence of state vectors. T x D or D x T matrix, where T is the number of times (the length of 'ts'), and D the size of the state (i.e. dim())
[out]	xds	Sequence of state vectors (rates of change). T x D or D x T matrix, where T is the number of times (the length of 'ts'), and D the size of the state (i.e. dim())
[out]	forcing_terms	The forcing terms for each dimension, for debugging purposes only.
[out]	fa_output	The output of the function approximators, for debugging purposes only.

Remarks: The output xs and xds will be of size D x T only if the matrix x you pass as an argument of size D x T. In all other cases (i.e. including passing an empty matrix) the size of x will be T x D. This feature has been added so that you may pass matrices of either size.

Definition at line 449 of file Dmp.cpp.

 {
   int n_time_steps = ts.size();
   assert(n_time_steps>0);
   
   // Usually, we expect xs and xds to be of size T X dim(), so we resize to that. However, if the
   // input matrices were of size dim() X T, we return the matrices of that size by doing a 
   // transposeInPlace at the end. That way, the user can also request dim() X T sized matrices.
   bool caller_expects_transposed = (xs.rows()==dim() && xs.cols()==n_time_steps);
   
   // INTEGRATE SYSTEMS ANALYTICALLY AS MUCH AS POSSIBLE
 
   // Integrate phase
   MatrixXd xs_phase;
   MatrixXd xds_phase;
   phase_system_->analyticalSolution(ts,xs_phase,xds_phase);
   
   // Compute gating term
   MatrixXd xs_gating;
   MatrixXd xds_gating;
   gating_system_->analyticalSolution(ts,xs_gating,xds_gating);
 
   // Compute the output of the function approximator
   fa_outputs.resize(ts.size(),dim_orig());
   fa_outputs.fill(0.0);
   //if (isTrained())
   computeFunctionApproximatorOutput(xs_phase, fa_outputs);
 
   // Gate the output to get the forcing term
   MatrixXd xs_gating_rep = xs_gating.replicate(1,fa_outputs.cols());
   forcing_terms = fa_outputs.array()*xs_gating_rep.array();
   
   // Scale the forcing term, if necessary
   if (forcing_term_scaling_==G_MINUS_Y0_SCALING)
   {
     MatrixXd g_minus_y0_rep = (attractor_state()-initial_state()).transpose().replicate(n_time_steps,1);
     forcing_terms = forcing_terms.array()*g_minus_y0_rep.array();
   }
   else if (forcing_term_scaling_==AMPLITUDE_SCALING)
   {
     MatrixXd trajectory_amplitudes_rep = trajectory_amplitudes_.transpose().replicate(n_time_steps,1);
     forcing_terms = forcing_terms.array()*trajectory_amplitudes_rep.array();
   }
   
   
   MatrixXd xs_goal, xds_goal;
   // Get current delayed goal
   if (goal_system_==NULL)
   {
     // If there is no dynamical system for the delayed goal, the goal is
     // simply the attractor state               
     xs_goal  = attractor_state().transpose().replicate(n_time_steps,1);
     // with zero change
     xds_goal = MatrixXd::Zero(n_time_steps,dim_orig());
   } 
   else
   {
     // Integrate goal system and get current goal state
     goal_system_->analyticalSolution(ts,xs_goal,xds_goal);
   }
 
   xs.resize(n_time_steps,dim());
   xds.resize(n_time_steps,dim());
 
   int T = n_time_steps;
     
   xs.GOALM(T) = xs_goal;     xds.GOALM(T) = xds_goal;
   xs.PHASEM(T) = xs_phase;   xds.PHASEM(T) = xds_phase;
   xs.GATINGM(T) = xs_gating; xds.GATINGM(T) = xds_gating;
 
   
   // THE REST CANNOT BE DONE ANALYTICALLY
   
   // Reset the dynamical system, and get the first state
   double damping = spring_system_->damping_coefficient();
   SpringDamperSystem localspring_system_(tau(),initial_state(),attractor_state(),damping);
   
   // Set first attractor state
   localspring_system_.set_attractor_state(xs_goal.row(0));
   
   // Start integrating spring damper system
   int dim_spring = localspring_system_.dim();
   VectorXd x_spring(dim_spring), xd_spring(dim_spring); // todo Why are these needed?
   int t0 = 0;
   localspring_system_.integrateStart(x_spring, xd_spring);
   xs.row(t0).SPRING  = x_spring;
   xds.row(t0).SPRING = xd_spring;
 
   // Add forcing term to the acceleration of the spring state  
   xds.SPRINGM_Z(1) = xds.SPRINGM_Z(1) + forcing_terms.row(t0)/tau();
 
   // Initialize perturber, if necessary
   if (analytical_solution_perturber_==NULL && perturbation_standard_deviation_>0.0)
   {
     boost::normal_distribution<> normal(0, perturbation_standard_deviation_);
     analytical_solution_perturber_ = new boost::variate_generator<boost::mt19937&, boost::normal_distribution<> >(rng, normal);
   }
  
   
   for (int tt=1; tt<n_time_steps; tt++)
   {
     double dt = ts[tt]-ts[tt-1];
     
     // Euler integration
     xs.row(tt).SPRING  = xs.row(tt-1).SPRING + dt*xds.row(tt-1).SPRING;
   
     // Set the attractor state of the spring system
     localspring_system_.set_attractor_state(xs.row(tt).GOAL);
 
     // Integrate spring damper system
     localspring_system_.differentialEquation(xs.row(tt).SPRING, xd_spring);
     xds.row(tt).SPRING = xd_spring;
     
     // If necessary add a perturbation. May be useful for some off-line tests.
     RowVectorXd perturbation = RowVectorXd::Constant(dim_orig(),0.0);
     if (analytical_solution_perturber_!=NULL)
       for (int i_dim=0; i_dim<dim_orig(); i_dim++)
         // Sample perturbation from a normal Gaussian distribution
         perturbation(i_dim) = (*analytical_solution_perturber_)();
       
     // Add forcing term to the acceleration of the spring state
     xds.row(tt).SPRING_Z = xds.row(tt).SPRING_Z + forcing_terms.row(tt)/tau() + perturbation;
     // Compute y component from z
     xds.row(tt).SPRING_Y = xs.row(tt).SPRING_Z/tau();
     
   } 
   
   if (caller_expects_transposed)
   {
     xs.transposeInPlace();
     xds.transposeInPlace();
   }
 }

Here is the call graph for this function:

void analyticalSolution	(	const Eigen::VectorXd &	ts,
		Eigen::MatrixXd &	xs,
		Eigen::MatrixXd &	xds,
		Eigen::MatrixXd &	forcing_terms
	)		const

inline

Return analytical solution of the system at certain times (and return forcing terms)

Parameters

[in]	ts	A vector of times for which to compute the analytical solutions
[out]	xs	Sequence of state vectors. T x D or D x T matrix, where T is the number of times (the length of 'ts'), and D the size of the state (i.e. dim())
[out]	xds	Sequence of state vectors (rates of change). T x D or D x T matrix, where T is the number of times (the length of 'ts'), and D the size of the state (i.e. dim())
[out]	forcing_terms	The forcing terms for each dimension, for debugging purposes only.

Remarks: The output xs and xds will be of size D x T only if the matrix x you pass as an argument of size D x T. In all other cases (i.e. including passing an empty matrix) the size of x will be T x D. This feature has been added so that you may pass matrices of either size.

Definition at line 174 of file Dmp.hpp.

   {
     Eigen::MatrixXd fa_output;
     analyticalSolution(ts, xs, xds, forcing_terms, fa_output);
   }

Here is the call graph for this function:

void analyticalSolution	(	const Eigen::VectorXd &	ts,
		Eigen::MatrixXd &	xs,
		Eigen::MatrixXd &	xds
	)		const

inlinevirtual

Return analytical solution of the system at certain times.

Parameters

[in]	ts	A vector of times for which to compute the analytical solutions
[out]	xs	Sequence of state vectors. T x D or D x T matrix, where T is the number of times (the length of 'ts'), and D the size of the state (i.e. dim())
[out]	xds	Sequence of state vectors (rates of change). T x D or D x T matrix, where T is the number of times (the length of 'ts'), and D the size of the state (i.e. dim())

Remarks: The output xs and xds will be of size D x T only if the matrix x you pass as an argument of size D x T. In all other cases (i.e. including passing an empty matrix) the size of x will be T x D. This feature has been added so that you may pass matrices of either size.

Implements DynamicalSystem.

Definition at line 180 of file Dmp.hpp.

   {
     Eigen::MatrixXd forcing_terms, fa_output;
     analyticalSolution(ts, xs, xds, forcing_terms, fa_output);
   }

Here is the call graph for this function:

virtual void analyticalSolution	(	const Eigen::VectorXd &	ts,
		Trajectory &	trajectory
	)		const

inlinevirtual

Return analytical solution of the system at certain times.

Parameters

[in]	ts	A vector of times for which to compute the analytical solutions
[out]	trajectory	The computed states as a trajectory.

Reimplemented in DmpWithGainSchedules.

Definition at line 192 of file Dmp.hpp.

   {
     Eigen::MatrixXd xs,  xds;
     analyticalSolution(ts, xs, xds);
     statesAsTrajectory(ts, xs, xds, trajectory);
   }

Here is the call graph for this function:

void analyticalSolution	(	const Eigen::VectorXd &	ts,
		Trajectory &	trajectory,
		Eigen::MatrixXd &	forcing_terms
	)		const

inline

Return analytical solution of the system at certain times.

Parameters

[in]	ts	A vector of times for which to compute the analytical solutions
[out]	trajectory	The computed states as a trajectory.
[out]	forcing_terms	The forcing terms

Definition at line 206 of file Dmp.hpp.

   {
     Eigen::MatrixXd xs,  xds;
     analyticalSolution(ts, xs, xds, forcing_terms);
     statesAsTrajectory(ts, xs, xds, trajectory);
   }

Here is the call graph for this function:

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

virtual

Get the output of a DMP dynamical system as a trajectory.

As a dynamical system, the state vector of a DMP contains the output of the goal, spring, phase and gating system. What we are most interested in is the output of the spring system. This function extracts that information, and also computes the accelerations of the spring system, which are only stored implicitely in xd_in because second order systems are converted to first order systems with expanded state.

Parameters

[in]	x_in	State vector over time (size n_time_steps X dim())
[in]	xd_in	State vector over time (rates of change)
[out]	y_out	State vector over time (size n_time_steps X dim_orig())
[out]	yd_out	State vector over time (rates of change)
[out]	ydd_out	State vector over time (rates of change of rates of change)

Definition at line 410 of file Dmp.cpp.

 {
   int n_time_steps = x_in.rows(); 
   y_out  = x_in.SPRINGM_Y(n_time_steps);
   yd_out = xd_in.SPRINGM_Y(n_time_steps);
   ydd_out = xd_in.SPRINGM_Z(n_time_steps)/tau();
   // MatrixXd z_out, zd_out;
   // z_out  = x_in.SPRINGM_Z(n_time_steps);
   // zd_out = xd_in.SPRINGM_Z(n_time_steps);
   // Divide by tau to go from z to y space
   // yd = z_out/obj.tau;
   // ydd_out = zd_out/tau();
 }

Here is the call graph for this function:

void statesAsTrajectory	(	const Eigen::VectorXd &	ts,
		const Eigen::MatrixXd &	x_in,
		const Eigen::MatrixXd &	xd_in,
		Trajectory &	trajectory
	)		const

virtual

Get the output of a DMP dynamical system as a trajectory.

As a dynamical system, the state vector of a DMP contains the output of the goal, spring, phase and gating system. What we are most interested in is the output of the spring system. This function extracts that information, and also computes the accelerations of the spring system, which are only stored implicitely in xd_in because second order systems are converted to first order systems with expanded state.

Parameters

[in]	ts	A vector of times
[in]	x_in	State vector over time
[in]	xd_in	State vector over time (rates of change)
[out]	trajectory	Trajectory representation of the DMP state vector output.

Definition at line 425 of file Dmp.cpp.

                                                                                                                                          {
   int n_time_steps = ts.rows();
 #ifndef NDEBUG // Variables below are only required for asserts; check for NDEBUG to avoid warnings.
   int n_dims       = x_in.cols();
 #endif
   assert(n_time_steps==x_in.rows());
   assert(n_time_steps==xd_in.rows());
   assert(n_dims==xd_in.cols());
 
   // Left column is time
   Trajectory new_trajectory(
     ts,
     // y_out (see function above)
     x_in.SPRINGM_Y(n_time_steps),
     // yd_out (see function above)
     xd_in.SPRINGM_Y(n_time_steps),
     // ydd_out (see function above)
     xd_in.SPRINGM_Z(n_time_steps)/tau()
   );
   
   trajectory = new_trajectory;
   
 }

Here is the call graph for this function:

void train ( const Trajectory & trajectory )

virtual

Train a DMP with a trajectory.

Parameters

[in] trajectory The trajectory with which to train the DMP.

Reimplemented in DmpWithGainSchedules.

Definition at line 643 of file Dmp.cpp.

 {
   train(trajectory,"");
 }

void train	(	const Trajectory &	trajectory,
		std::string	save_directory,
		bool	overwrite = `false`
	)

virtual

Train a DMP with a trajectory, and write results to file.

Parameters

[in]	trajectory	The trajectory with which to train the DMP.
[in]	save_directory	The directory to which to save the results.
[in]	overwrite	Overwrite existing files in the directory above (default: false)

Reimplemented in DmpWithGainSchedules.

Definition at line 648 of file Dmp.cpp.

 {
   
   // Set tau, initial_state and attractor_state from the trajectory 
   set_tau(trajectory.duration());
   set_initial_state(trajectory.initial_y());
   set_attractor_state(trajectory.final_y());
 
   // This needs to be computed for (optional) scaling of the forcing term.
   // Needs to be done BEFORE computeFunctionApproximatorInputsAndTargets
   trajectory_amplitudes_ = trajectory.getRangePerDim();
   
   VectorXd fa_input_phase;
   MatrixXd f_target;
   computeFunctionApproximatorInputsAndTargets(trajectory, fa_input_phase, f_target);
   
   // Some checks before training function approximators
   assert(!function_approximators_.empty());
   
   for (unsigned int dd=0; dd<function_approximators_.size(); dd++)
   {
     // This is just boring stuff to figure out if and where to store the results of training
     string save_directory_dim;
     if (!save_directory.empty())
     {
       if (function_approximators_.size()==1)
         save_directory_dim = save_directory;
       else
         save_directory_dim = save_directory + "/dim" + to_string(dd);
     }
     
     // Actual training is happening here.
     VectorXd fa_target = f_target.col(dd);
     if (function_approximators_[dd]==NULL)
     {
       cerr << __FILE__ << ":" << __LINE__ << ":";
       cerr << "WARNING: function approximator cannot be trained because it is NULL." << endl;
     }
     else
     {
       if (function_approximators_[dd]->isTrained())
         function_approximators_[dd]->reTrain(fa_input_phase,fa_target,save_directory_dim,overwrite);
       else
         function_approximators_[dd]->train(fa_input_phase,fa_target,save_directory_dim,overwrite);
     }
   }
   
   if (!save_directory.empty())
   {
     int n_time_steps = 101;
     VectorXd ts = VectorXd::LinSpaced(n_time_steps,0,tau());
     Trajectory traj_reproduced;
     analyticalSolution(ts,traj_reproduced);
     
     trajectory.saveToFile(save_directory,"traj_demonstration.txt",overwrite);
     traj_reproduced.saveToFile(save_directory,"traj_reproduced.txt",overwrite);
   }
   
 }

Here is the call graph for this function:

void set_tau ( double tau )

virtual

Accessor function for the time constant.

Parameters

[in] tau Time constant We need to override DynamicalSystem::set_tau, because the DMP must also change the time constant of all of its subsystems.

Reimplemented from DynamicalSystem.

Definition at line 853 of file Dmp.cpp.

                             {
   DynamicalSystem::set_tau(tau);
 
   // Set value in all relevant subsystems also  
   spring_system_->set_tau(tau);
   if (goal_system_!=NULL)
     goal_system_->set_tau(tau);
   phase_system_ ->set_tau(tau);
   gating_system_->set_tau(tau);
 }

Here is the call graph for this function:

void set_initial_state ( const Eigen::VectorXd & y_init )

virtual

Accessor function for the initial state of the system.

Parameters

[in] y_init Initial state of the system. We need to override DynamicalSystem::set_initial_state, because the DMP must also change the initial state of the goal system as well.

Reimplemented from DynamicalSystem.

Definition at line 864 of file Dmp.cpp.

                                                   {
   DynamicalSystem::set_initial_state(y_init);
   
   // Set value in all relevant subsystems also  
   spring_system_->set_initial_state(y_init);
   if (goal_system_!=NULL)
     goal_system_->set_initial_state(y_init);
 }    

Here is the call graph for this function:

void set_attractor_state ( const Eigen::VectorXd & y_attr )

virtual

Accessor function for the attractor state of the system.

Parameters

[in] y_attr Attractor state of the system.

Definition at line 873 of file Dmp.cpp.

                                                     {
   DynamicalSystem::set_attractor_state(y_attr);
   
   // Set value in all relevant subsystems also  
   if (goal_system_!=NULL)
     goal_system_->set_attractor_state(y_attr);
 
   // Do NOT do the following. The attractor state of the spring system is determined by the goal 
   // system
   // spring_system_->set_attractor_state(y_attr);
 
 }    

Here is the call graph for this function:

void set_damping_coefficient ( double damping_coefficient )

Accessor function for damping coefficient of spring-damper system.

Parameters

[in] damping_coefficient Damping coefficient

Definition at line 215 of file Dmp.cpp.

 {
   spring_system_->set_damping_coefficient(damping_coefficient); 
 }

Here is the call graph for this function:

void set_spring_constant ( double spring_constant )

Accessor function for spring constant of spring-damper system.

Parameters

[in] spring_constant Spring constant

Definition at line 219 of file Dmp.cpp.

                                                     {
   spring_system_->set_spring_constant(spring_constant); 
 }

Here is the call graph for this function:

string toString ( void ) const

virtual

Returns a string representation of the object.

Returns: A string representation of the object.

Implements DynamicalSystem.

Definition at line 893 of file Dmp.cpp.

 {
   RETURN_STRING_FROM_BOOST_SERIALIZATION_XML("Dmp");
 }

void getSelectableParameters ( std::set< std::string > & selected_values_labels ) const

virtual

Return all the names of the parameter types that can be selected.

Parameters

[out] selected_values_labels Names of the parameter types that can be selected

Implements Parameterizable.

Definition at line 708 of file Dmp.cpp.

                                                                              {
   assert(function_approximators_.size()>0);
   for (int dd=0; dd<dim_orig(); dd++)
   {
     if (function_approximators_[dd]!=NULL)
     {
       if (function_approximators_[dd]->isTrained())
       {
         set<string> cur_labels;
         function_approximators_[dd]->getSelectableParameters(cur_labels);
         selectable_values_labels.insert(cur_labels.begin(), cur_labels.end());
       }
     }
   }
   selectable_values_labels.insert("goal");
   
   //cout << "selected_values_labels=["; 
   //for (string label : selected_values_labels) 
   //  cout << label << " ";
   //cout << "]" << endl;
   
 }

Here is the call graph for this function:

void setSelectedParameters ( const std::set< std::string > & selected_values_labels )

virtual

Determine which subset of parameters is represented in the vector returned by Parameterizable::getParameterVectorSelected.

Different function approximators have different types of model parameters. For instance, LWR has the centers and widths of basis functions, along with the slopes of each line segment. Parameterizable::setSelectedParameters provides a means to determine which parameters should be returned by Parameterizable::getParameterVectorSelected, i.e. by calling: std::set<std::string> selected; selected.insert("slopes"); model_parameters.setSelectedParameters(selected)

Parameters

[in] selected_values_labels The names of the parameters that are selected

Reimplemented from Parameterizable.

Definition at line 731 of file Dmp.cpp.

 {
   assert(function_approximators_.size()>0);
   for (int dd=0; dd<dim_orig(); dd++)
     if (function_approximators_[dd]!=NULL)
       if (function_approximators_[dd]->isTrained())
         function_approximators_[dd]->setSelectedParameters(selected_values_labels);
 
   // Call superclass for initializations
   Parameterizable::setSelectedParameters(selected_values_labels);
 
   VectorXi lengths_per_dimension = VectorXi::Zero(dim_orig());
   for (int dd=0; dd<dim_orig(); dd++)
   {
     if (function_approximators_[dd]!=NULL)
       if (function_approximators_[dd]->isTrained())
         lengths_per_dimension[dd] = function_approximators_[dd]->getParameterVectorSelectedSize();
     
     if (selected_values_labels.find("goal")!=selected_values_labels.end())
       lengths_per_dimension[dd]++;
   }
   
   setVectorLengthsPerDimension(lengths_per_dimension);
       
 }

Here is the call graph for this function:

int getParameterVectorAllSize ( void ) const

virtual

Get the size of the parameter values vector when it contains all available parameter values.

Returns: The size of the parameter vector

For instance, if the parameters consists of centers, widths and slopes the parameter values vector will be something like

    centers     widths    slopes
[ 100 110 120 10 10 10 0.4 0.7 0.4 ]

then getParameterVectorAllSize() will return 9

Implements Parameterizable.

Definition at line 801 of file Dmp.cpp.

 {
   int total_size = 0;
   for (unsigned int dd=0; dd<function_approximators_.size(); dd++)
     total_size += function_approximators_[dd]->getParameterVectorAllSize();
   
   // For the goal
   total_size += dim_orig();
   return total_size;
 }

Here is the call graph for this function:

void getParameterVectorAll ( Eigen::VectorXd & values ) const

virtual

Return a vector that returns all available parameter values.

Parameters

[out] values All available parameter values in one vector.

Remarks: Contrast this with Parameterizable::getParameterVectorSelected, which return only the SELECTED parameter values. Selecting parameters is done with Parameterizable::setSelectedParameters

Implements Parameterizable.

Definition at line 813 of file Dmp.cpp.

 {
   values.resize(getParameterVectorAllSize());
   int offset = 0;
   VectorXd cur_values;
   VectorXd attractor = attractor_state();
   for (int dd=0; dd<dim_orig(); dd++)
   {
     function_approximators_[dd]->getParameterVectorAll(cur_values);
     values.segment(offset,cur_values.size()) = cur_values;
     offset += cur_values.size();
 
     values(offset) = attractor(dd);
     offset++;
   }
 }

Here is the call graph for this function:

void setParameterVectorAll ( const Eigen::VectorXd & values )

virtual

Set all available parameter values with one vector.

Parameters

[in] values All available parameter values in one vector.

Remarks: Contrast this with Parameterizable::setParameterVectorSelected, which sets only the SELECTED parameter values. Selecting parameters is done with Parameterizable::setSelectedParameters

Implements Parameterizable.

Definition at line 830 of file Dmp.cpp.

 {
   assert(values.size()==getParameterVectorAllSize());
   int offset = 0;
   VectorXd cur_values;
   VectorXd attractor(dim_orig());
   for (int dd=0; dd<dim_orig(); dd++)
   {
     int n_parameters_required = function_approximators_[dd]->getParameterVectorAllSize();
     cur_values = values.segment(offset,n_parameters_required);
     function_approximators_[dd]->setParameterVectorAll(cur_values);
     offset += n_parameters_required;
     
     attractor(dd) = values(offset);
     offset += 1;
   }
   
   // Set the goal
   set_attractor_state(attractor); 
 }

Here is the call graph for this function:

void getParameterVectorMask	(	const std::set< std::string >	selected_values_labels,
		Eigen::VectorXi &	selected_mask
	)		const

virtual

Get a mask for selecting parameters.

Parameters

[in]	selected_values_labels	Labels of the selected parameter values
[out]	selected_mask	A mask indicating indices of selected parameters. 0 indicates not selected, >0 indicates selected.

For instance, if the parameters consists of centers, widths and slopes the parameter values vector will be something like

    centers     widths    slopes
[ 100 110 120 10 10 10 0.4 0.7 0.4 ]

In this case, if selected_values_labels contains "centers" and "slopes", the mask will be:

    centers     widths    slopes
[   1   1   1  0  0  0   3   3   3 ]

The '0' indicates that these parameters are not selected. The other ones have different numbers so that they may be discerned from one another (as required in Parameterizable::getParameterVectorSelectedMinMax for instance.

Implements Parameterizable.

Definition at line 757 of file Dmp.cpp.

 {
   assert(function_approximators_.size()>0);
   for (int dd=0; dd<dim_orig(); dd++)
   {
     assert(function_approximators_[dd]!=NULL);
     assert(function_approximators_[dd]->isTrained());
   }
 
   selected_mask.resize(getParameterVectorAllSize());
   selected_mask.fill(0);
   
   const int TMP_GOAL_NUMBER = -1;
   int offset = 0;
   VectorXi cur_mask;
   for (int dd=0; dd<dim_orig(); dd++)
   {
     function_approximators_[dd]->getParameterVectorMask(selected_values_labels,cur_mask);
 
     // This makes sure that the indices for each function approximator are different    
     int mask_offset = selected_mask.maxCoeff(); 
     for (int ii=0; ii<cur_mask.size(); ii++)
       if (cur_mask[ii]!=0)
         cur_mask[ii] += mask_offset;
         
     selected_mask.segment(offset,cur_mask.size()) = cur_mask;
     offset += cur_mask.size();
     
     // Goal
     if (selected_values_labels.find("goal")!=selected_values_labels.end())
       selected_mask(offset) = TMP_GOAL_NUMBER;
     offset++;
     
   }
   assert(offset == getParameterVectorAllSize());
   
   // Replace TMP_GOAL_NUMBER with current max value
   int goal_number = selected_mask.maxCoeff() + 1; 
   for (int ii=0; ii<selected_mask.size(); ii++)
     if (selected_mask[ii]==TMP_GOAL_NUMBER)
       selected_mask[ii] = goal_number;
     
 }

Here is the call graph for this function:

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.

For a standard Dmp (such as the one in this class) the inputs will be the phase over time, and the targets will be the forcing term (with the gating function factored out).

Parameters

[in]	trajectory	Trajectory, e.g. a demonstration.
[out]	fa_inputs_phase	The inputs for the function approximators (phase signal)
[out]	fa_targets	The targets for the function approximators (forcing term)

Definition at line 583 of file Dmp.cpp.

 {
   int n_time_steps = trajectory.length();
   double dim_data = trajectory.dim();
   
   if (dim_orig()!=dim_data)
   {
     cout << "WARNING: Cannot train " << dim_orig() << "-D DMP with " << dim_data << "-D data. Doing nothing." << endl;
     return;
   }
   
   // Integrate analytically to get goal, gating and phase states
   MatrixXd xs_ana;
   MatrixXd xds_ana;
   
   // Before, we would make clone of the dmp, and integrate it with the tau, and initial/attractor
   // state of the trajectory. However, Thibaut needed to call this from outside the Dmp as well,
   // with the tau/states of the this object. Therefore, we no longer clone. 
   // Dmp* dmp_clone = static_cast<Dmp*>(this->clone());
   // dmp_clone->set_tau(trajectory.duration());
   // dmp_clone->set_initial_state(trajectory.initial_y());
   // dmp_clone->set_attractor_state(trajectory.final_y());
   // dmp_clone->analyticalSolution(trajectory.ts(),xs_ana,xds_ana);
   analyticalSolution(trajectory.ts(),xs_ana,xds_ana);
   MatrixXd xs_goal   = xs_ana.GOALM(n_time_steps);
   MatrixXd xs_gating = xs_ana.GATINGM(n_time_steps);
   MatrixXd xs_phase  = xs_ana.PHASEM(n_time_steps);
   
   fa_inputs_phase = xs_phase;
   
   // Get parameters from the spring-dampers system to compute inverse
   double damping_coefficient = spring_system_->damping_coefficient();
   double spring_constant     = spring_system_->spring_constant();
   double mass                = spring_system_->mass();
   if (mass!=1.0)
   {
     cout << "WARNING: Usually, spring-damper system of the DMP should have mass==1, but it is " << mass << endl;
   }
 
   // Compute inverse
   f_target = tau()*tau()*trajectory.ydds() + (spring_constant*(trajectory.ys()-xs_goal) + damping_coefficient*tau()*trajectory.yds())/mass;
   
   //Factor out gating term
   for (unsigned int dd=0; dd<function_approximators_.size(); dd++)
     f_target.col(dd) = f_target.col(dd).array()/xs_gating.array();
   
   // Factor out scaling
   if (forcing_term_scaling_==G_MINUS_Y0_SCALING)
   {
     MatrixXd g_minus_y0_rep = (attractor_state()-initial_state()).transpose().replicate(n_time_steps,1);
     f_target = f_target.array()/g_minus_y0_rep.array();
   }
   else if (forcing_term_scaling_==AMPLITUDE_SCALING)
   {
     MatrixXd trajectory_amplitudes_rep = trajectory_amplitudes_.transpose().replicate(n_time_steps,1);
     f_target = f_target.array()/trajectory_amplitudes_rep.array();
   }
  
 }

Here is the call graph for this function:

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.

Reimplemented in DmpContextual, DmpContextualTwoStep, and DmpContextualOneStep.

Definition at line 313 of file Dmp.cpp.

 {
   int T = phase_state.rows();
   fa_output.resize(T,dim_orig());
   fa_output.fill(0.0);
   
   if (T>1) {
     fa_outputs_prealloc_.resize(T,dim_orig());
   }
   
   for (int i_dim=0; i_dim<dim_orig(); i_dim++)
   {
     if (function_approximators_[i_dim]!=NULL)
     {
       if (function_approximators_[i_dim]->isTrained()) 
       {
         if (T==1)
         {
           function_approximators_[i_dim]->predict(phase_state,fa_outputs_one_prealloc_);
           fa_output.col(i_dim) = fa_outputs_one_prealloc_;
         }
         else
         {
           function_approximators_[i_dim]->predict(phase_state,fa_outputs_prealloc_);
           fa_output.col(i_dim) = fa_outputs_prealloc_;
         }
       }
     }
   }
 }

Here is the call graph for this function:

void set_perturbation_analytical_solution ( double perturbation_standard_deviation )

Add a perturbation to the forcing term when computing the analytical solution.

This is only relevant for off-line experiments, i.e. not on a robot, for testing how the system responds to perturbations. Does not affect the output of Dmp::differentialEquation(), only of Dmp::analyticalSolution().

Parameters

[in] perturbation_standard_deviation Standard deviation of the normal distribution from which perturbations will be sampled.

Definition at line 886 of file Dmp.cpp.

 {
   perturbation_standard_deviation_ = perturbation_standard_deviation;
   analytical_solution_perturber_ = NULL;
 }

double get_perturbation_analytical_solution ( ) const

inline

Get the perturbation to the forcing term when computing the analytical solution.

Returns: Standard deviation of the normal distribution from which perturbations will be sampled.

Definition at line 334 of file Dmp.hpp.

   {
     return perturbation_standard_deviation_;
   }

FunctionApproximator* function_approximator ( int i_dim ) const

inlineprotected

Get a pointer to the function approximator for a certain dimension.

Parameters

[in] i_dim Dimension for which to get the function approximator

Returns: Pointer to the function approximator.

Definition at line 345 of file Dmp.hpp.

   {
     assert(i_dim<(int)function_approximators_.size());
     return function_approximators_[i_dim];
   }

Friends And Related Function Documentation

friend class boost::serialization::access

friend

Give boost serialization access to private members.

Definition at line 425 of file Dmp.hpp.

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

Public Types

Public Member Functions

Protected Member Functions

Friends

Detailed Description

Member Enumeration Documentation

Constructor & Destructor Documentation

Member Function Documentation

Friends And Related Function Documentation