Implementation of ConstraintSolver to solve inverse kinematics by using a weighted least norm. More...
#include <unified_joint_limit_singularity_solver.h>
Public Member Functions | |
virtual Eigen::MatrixXd | solve (const Vector6d_t &in_cart_velocities, const JointStates &joint_states) |
UnifiedJointLimitSingularitySolver (const TwistControllerParams ¶ms, const LimiterParams &limiter_params, TaskStackController_t &task_stack_controller) | |
virtual | ~UnifiedJointLimitSingularitySolver () |
Private Member Functions | |
virtual Eigen::MatrixXd | calculateWeighting (const Vector6d_t &in_cart_velocities, const JointStates &joint_states) const |
Implementation of ConstraintSolver to solve inverse kinematics by using a weighted least norm.
Definition at line 25 of file unified_joint_limit_singularity_solver.h.
UnifiedJointLimitSingularitySolver::UnifiedJointLimitSingularitySolver | ( | const TwistControllerParams & | params, |
const LimiterParams & | limiter_params, | ||
TaskStackController_t & | task_stack_controller | ||
) | [inline] |
Definition at line 28 of file unified_joint_limit_singularity_solver.h.
virtual UnifiedJointLimitSingularitySolver::~UnifiedJointLimitSingularitySolver | ( | ) | [inline, virtual] |
Definition at line 34 of file unified_joint_limit_singularity_solver.h.
Eigen::MatrixXd UnifiedJointLimitSingularitySolver::calculateWeighting | ( | const Vector6d_t & | in_cart_velocities, |
const JointStates & | joint_states | ||
) | const [private, virtual] |
Virtual helper method that calculates a weighting for the Jacobian to adapt joint velocity calculation for given constraints.
q | The current joint positions. |
q_dot | The current joint velocities. |
This function returns the identity as weighting matrix for base functionality.
Definition at line 70 of file unified_joint_limit_singularity_solver.cpp.
Eigen::MatrixXd UnifiedJointLimitSingularitySolver::solve | ( | const Vector6d_t & | in_cart_velocities, |
const JointStates & | joint_states | ||
) | [virtual] |
Specific implementation of solve-method to solve IK problem with joint limit avoidance. See base class ConstraintSolver for more details on params and returns.
Specific implementation simultaneous singularity and joint limit avoidance solver. This work is based in the general weighted least norm and unified weighted least norm methods.
Implements ConstraintSolver<>.
Definition at line 24 of file unified_joint_limit_singularity_solver.cpp.