#include <wln_joint_limit_avoidance_solver.h>
Public Member Functions | |
WLN_JointLimitAvoidanceSolver (const TwistControllerParams ¶ms, const LimiterParams &limiter_params, TaskStackController_t &task_stack_controller) | |
virtual | ~WLN_JointLimitAvoidanceSolver () |
Private Member Functions | |
virtual Eigen::MatrixXd | calculateWeighting (const JointStates &joint_states) const |
Implementation of ConstraintSolver to solve inverse kinematics with joint limit avoidance Uses solve method of the WeightedLeastNormSolver
Definition at line 26 of file wln_joint_limit_avoidance_solver.h.
WLN_JointLimitAvoidanceSolver::WLN_JointLimitAvoidanceSolver | ( | const TwistControllerParams & | params, |
const LimiterParams & | limiter_params, | ||
TaskStackController_t & | task_stack_controller | ||
) | [inline] |
Definition at line 29 of file wln_joint_limit_avoidance_solver.h.
virtual WLN_JointLimitAvoidanceSolver::~WLN_JointLimitAvoidanceSolver | ( | ) | [inline, virtual] |
Definition at line 35 of file wln_joint_limit_avoidance_solver.h.
Eigen::MatrixXd WLN_JointLimitAvoidanceSolver::calculateWeighting | ( | const JointStates & | joint_states | ) | const [private, virtual] |
Helper method that calculates a weighting for the Jacobian to adapt the impact on joint velocities. Overridden from base class WLNSolver
q | The current joint positions. |
q_dot | The current joint velocities. |
This function calculates the weighting matrix used to penalize a joint when it is near and moving towards a limit. The last joint velocity is used to determine if it that happens or not
Reimplemented from WeightedLeastNormSolver.
Definition at line 27 of file wln_joint_limit_avoidance_solver.cpp.