#include <stack_of_tasks_solver.h>
Public Member Functions | |
void | processState (std::set< ConstraintBase_t >::iterator &it, const Eigen::MatrixXd &projector, const Eigen::MatrixXd &particular_solution, double inv_sum_of_prios, Eigen::VectorXd &sum_of_gradient) |
virtual Eigen::MatrixXd | solve (const Vector6d_t &in_cart_velocities, const JointStates &joint_states) |
StackOfTasksSolver (const TwistControllerParams ¶ms, const LimiterParams &limiter_params, TaskStackController_t &task_stack_controller) | |
virtual | ~StackOfTasksSolver () |
Protected Attributes | |
EN_ConstraintStates | global_constraint_state_ |
double | in_cart_vel_damping_ |
ros::Time | last_time_ |
Definition at line 32 of file stack_of_tasks_solver.h.
StackOfTasksSolver::StackOfTasksSolver | ( | const TwistControllerParams & | params, |
const LimiterParams & | limiter_params, | ||
TaskStackController_t & | task_stack_controller | ||
) | [inline] |
Definition at line 35 of file stack_of_tasks_solver.h.
virtual StackOfTasksSolver::~StackOfTasksSolver | ( | ) | [inline, virtual] |
Definition at line 45 of file stack_of_tasks_solver.h.
void StackOfTasksSolver::processState | ( | std::set< ConstraintBase_t >::iterator & | it, |
const Eigen::MatrixXd & | projector, | ||
const Eigen::MatrixXd & | particular_solution, | ||
double | inv_sum_of_prios, | ||
Eigen::VectorXd & | sum_of_gradient | ||
) |
Process the state of the constraint and update the sum_of_gradient.
Definition at line 105 of file stack_of_tasks_solver.cpp.
Eigen::MatrixXd StackOfTasksSolver::solve | ( | const Vector6d_t & | in_cart_velocities, |
const JointStates & | joint_states | ||
) | [virtual] |
Specific implementation of solve-method to solve IK problem with constraints by using the GPM. See base class ConstraintSolver for more details on params and returns.
Implements ConstraintSolver<>.
Definition at line 23 of file stack_of_tasks_solver.cpp.
Definition at line 66 of file stack_of_tasks_solver.h.
double StackOfTasksSolver::in_cart_vel_damping_ [protected] |
Definition at line 67 of file stack_of_tasks_solver.h.
ros::Time StackOfTasksSolver::last_time_ [protected] |
Definition at line 65 of file stack_of_tasks_solver.h.