#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 () |
Public Member Functions inherited from ConstraintSolver<> | |
void | clearConstraints () |
ConstraintSolver (const TwistControllerParams ¶ms, const LimiterParams &limiter_params, TaskStackController_t &task_stack_controller) | |
void | setConstraints (std::set< ConstraintBase_t > &constraints) |
void | setDamping (boost::shared_ptr< DampingBase > &damping) |
virtual void | setJacobianData (const Matrix6Xd_t &jacobian_data) |
virtual | ~ConstraintSolver () |
Protected Attributes | |
EN_ConstraintStates | global_constraint_state_ |
double | in_cart_vel_damping_ |
ros::Time | last_time_ |
Protected Attributes inherited from ConstraintSolver<> | |
std::set< ConstraintBase_t > | constraints_ |
set inserts sorted (default less operator); if element has already been added it returns an iterator on it. More... | |
boost::shared_ptr< DampingBase > | damping_ |
References the current Jacobian (matrix data only). More... | |
Matrix6Xd_t | jacobian_data_ |
References the limiter parameters (up-to-date according to KinematicExtension). More... | |
const LimiterParams & | limiter_params_ |
References the inv. diff. kin. solver parameters. More... | |
const TwistControllerParams & | params_ |
Set of constraints. More... | |
PInvBySVD | pinv_calc_ |
The currently set damping method. More... | |
TaskStackController_t & | task_stack_controller_ |
An instance that helps solving the inverse of the Jacobian. More... | |
Definition at line 32 of file stack_of_tasks_solver.h.
|
inline |
Definition at line 35 of file stack_of_tasks_solver.h.
|
inlinevirtual |
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.
|
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.
|
protected |
Definition at line 66 of file stack_of_tasks_solver.h.
|
protected |
Definition at line 67 of file stack_of_tasks_solver.h.
|
protected |
Definition at line 65 of file stack_of_tasks_solver.h.