bool DMPPolicy::computeControlCosts(const std::vector<Eigen::MatrixXd>& control_cost_matrices,
const std::vector<std::vector<Eigen::VectorXd> >& parameters,
const double weight,
std::vector<Eigen::VectorXd>& control_costs)
{
int num_dimensions = control_cost_matrices.size();
int num_time_steps = parameters[0].size();
std::vector<int> num_parameters(num_dimensions);
getNumParameters(num_parameters);
// initialize output vector if necessary:
if (int(control_costs.size()) != num_dimensions)
{
control_costs.clear();
for (int d = 0; d < num_dimensions; ++d)
{
control_costs.push_back(VectorXd::Zero(num_time_steps));
}
}
// compute the costs
for (int d = 0; d < num_dimensions; ++d)
{
for (int t = 0; t < num_time_steps; ++t)
{
control_costs[d](t) = weight * parameters[d][t].dot(control_cost_matrices[d] * parameters[d][t]);
}
}
return true;
}
std::vector<double> exponential_function::df( double x ) const {
std::vector<double> value( num_parameters() );
double exponential( std::exp( get_parameter_value( 1 ) * x ) );
value.at( 0 ) = exponential;
value.at( 1 ) = get_parameter_value( 0 ) * x * exponential;
value.at( 2 ) = 1;
return value;
}
bool find_symbols(expression const e, std::function<bool(symbol)> const selector,
std::unordered_set<symbol,symbol::hash>& output)
{
if (selector(get_symbol(e)))
output.insert(get_symbol(e));
for (uint64_t i = 0ULL; i < num_parameters(e); ++i)
find_symbols(argument(e,i),selector,output);
return !output.empty();
}
std::vector<double> logarithmic_function::df( double x ) const {
std::vector<double> value( num_parameters() );
double logarg( x + get_parameter_value( 1 ) );
if( logarg == 0 ) {
logarg = std::numeric_limits<double>::epsilon();
}
value.at( 0 ) = std::log( logarg );
value.at( 1 ) = get_parameter_value( 0 ) / logarg;
value.at( 2 ) = 1;
return value;
}
std::vector<double> hyperbola_function::df( double x ) const {
std::vector<double> value( num_parameters() );
double denominator( get_parameter_value( 1 ) + x );
if( denominator == 0 ) {
denominator = std::numeric_limits<double>::epsilon();
}
value.at( 0 ) = 1 / denominator;
value.at( 1 ) = - get_parameter_value( 0 ) / (denominator * denominator);
value.at( 2 ) = 1;
return value;
}
