template<size_t N>
class hebi::robot_model::CustomObjective< N >
Allows you to add a custom objective function.
To use, you must implement and pass in a std::function object that takes a vector of positions and an array of error values that you should fill in:
std::function<void(const std::vector<double>&, std::array<double, N>&)>;
This function is called at each step of the optimization.
Note that the template parameter N is the number of independent errors that your objective function sets.
Example usage, using a lambda function for the callback. Note that this toy example optimizes for joint angles that sum to 2:
Eigen::VectorXd initial_joint_angles(group->size());
Eigen::VectorXd ik_result_joint_angles(group->size());
model->solveIK(
initial_joint_angles,
ik_result_joint_angles,
robot_model::CustomObjective<1>(
[](const std::vector<double> positions, std::array<double, 1>& errors)
{
double sum = 0;
for (auto p : positions)
sum += p;
errors[0] = 2 - sum;
}
)
);