Public Member Functions | List of all members
GradientProjectionMethodSolver Class Reference

#include <gradient_projection_method_solver.h>

Inheritance diagram for GradientProjectionMethodSolver:
Inheritance graph
[legend]

Public Member Functions

 GradientProjectionMethodSolver (const TwistControllerParams &params, const LimiterParams &limiter_params, TaskStackController_t &task_stack_controller)
 
virtual Eigen::MatrixXd solve (const Vector6d_t &in_cart_velocities, const JointStates &joint_states)
 
virtual ~GradientProjectionMethodSolver ()
 
- Public Member Functions inherited from ConstraintSolver<>
void clearConstraints ()
 
 ConstraintSolver (const TwistControllerParams &params, 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 ()
 

Additional Inherited Members

- Protected Attributes inherited from ConstraintSolver<>
std::set< ConstraintBase_tconstraints_
 set inserts sorted (default less operator); if element has already been added it returns an iterator on it. More...
 
boost::shared_ptr< DampingBasedamping_
 References the current Jacobian (matrix data only). More...
 
Matrix6Xd_t jacobian_data_
 References the limiter parameters (up-to-date according to KinematicExtension). More...
 
const LimiterParamslimiter_params_
 References the inv. diff. kin. solver parameters. More...
 
const TwistControllerParamsparams_
 Set of constraints. More...
 
PInvBySVD pinv_calc_
 The currently set damping method. More...
 
TaskStackController_ttask_stack_controller_
 An instance that helps solving the inverse of the Jacobian. More...
 

Detailed Description

Definition at line 30 of file gradient_projection_method_solver.h.

Constructor & Destructor Documentation

GradientProjectionMethodSolver::GradientProjectionMethodSolver ( const TwistControllerParams params,
const LimiterParams limiter_params,
TaskStackController_t task_stack_controller 
)
inline

Definition at line 33 of file gradient_projection_method_solver.h.

virtual GradientProjectionMethodSolver::~GradientProjectionMethodSolver ( )
inlinevirtual

Definition at line 39 of file gradient_projection_method_solver.h.

Member Function Documentation

Eigen::MatrixXd GradientProjectionMethodSolver::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.

Solve the inverse differential kinematics equation by using GPM. In addtion to the partial solution q_dot = J^+ * v the homogeneous solution (I - J^+ * J) q_dot_0 is calculated. The q_dot_0 results from the sum of the constraint cost function gradients. The terms of the sum are weighted with a factor k_H separately.

Implements ConstraintSolver<>.

Definition at line 28 of file gradient_projection_method_solver.cpp.


The documentation for this class was generated from the following files:


cob_twist_controller
Author(s): Felix Messmer , Marco Bezzon , Christoph Mark , Francisco Moreno
autogenerated on Thu Apr 8 2021 02:40:01