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; }