/** Query the function approximator to make a prediction, and also to predict its variance
* \param[in] inputs Input values of the query (n_samples X n_dims_in)
* \param[out] outputs Predicted output values (n_samples X n_dims_out)
* \param[out] variances Predicted variances for the output values (n_samples X n_dims_out). Note that if the output has a dimensionality>1, these variances should actuall be covariance matrices (use function predict(const Eigen::MatrixXd& inputs, Eigen::MatrixXd& outputs, std::vector<Eigen::MatrixXd>& variances) to get the full covariance matrices). So for an output dimensionality of 1 this function works fine. For dimensionality>1 we return only the diagional of the covariance matrix, which may not always be what you want.
*
* \remark This method should be const. But third party functions which is called in this function
* have not always been implemented as const (Examples: LWPRObject::predict or IRFRLS::predict ).
* Therefore, this function cannot be const.
*/
virtual void predict(const Eigen::MatrixXd& inputs, Eigen::MatrixXd& outputs, Eigen::MatrixXd& variances)
{
predict(inputs, outputs);
variances.fill(0);
}
/** Query the function approximator to get the variance of a prediction
* This function is not implemented by all function approximators. Therefore, the default
* implementation fills outputs with 0s.
* \param[in] inputs Input values of the query (n_samples X n_dims_in)
* \param[out] variances Predicted variances for the output values (n_samples X n_dims_out). Note that if the output has a dimensionality>1, these variances should actuall be covariance matrices (use function predict(const Eigen::MatrixXd& inputs, Eigen::MatrixXd& outputs, std::vector<Eigen::MatrixXd>& variances) to get the full covariance matrices). So for an output dimensionality of 1 this function works fine. For dimensionality>1 we return only the diagional of the covariance matrix, which may not always be what you want.
*
* \remark This method should be const. But third party functions which is called in this function
* have not always been implemented as const (Examples: LWPRObject::predict or IRFRLS::predict ).
* Therefore, this function cannot be const.
*/
virtual void predictVariance(const Eigen::MatrixXd& inputs, Eigen::MatrixXd& variances)
{
variances.fill(0);
}
void Dmp::analyticalSolution(const VectorXd& ts, MatrixXd& xs, MatrixXd& xds, Eigen::MatrixXd& forcing_terms, Eigen::MatrixXd& fa_outputs) const
{
int n_time_steps = ts.size();
// 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);
MatrixXd xs_gating_rep = xs_gating.replicate(1,fa_outputs.cols());
MatrixXd g_minus_y0_rep = (attractor_state()-initial_state()).transpose().replicate(n_time_steps,1);
forcing_terms = fa_outputs.array()*xs_gating_rep.array(); // zzz *g_minus_y0_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); // zzz 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();
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;
// 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();
// Compute y component from z
xds.row(tt).SPRING_Y = xs.row(tt).SPRING_Z/tau();
}
}
