KDL  1.4.0
Public Member Functions | Private Member Functions | Private Attributes | List of all members
KDL::TreeIkSolverPos_Online Class Reference

Implementation of a general inverse position kinematics algorithm to calculate the position transformation from Cartesian to joint space of a general KDL::Tree. More...

#include <src/treeiksolverpos_online.hpp>

Inheritance diagram for KDL::TreeIkSolverPos_Online:
Inheritance graph
[legend]
Collaboration diagram for KDL::TreeIkSolverPos_Online:
Collaboration graph
[legend]

Public Member Functions

 TreeIkSolverPos_Online (const double &nr_of_jnts, const std::vector< std::string > &endpoints, const JntArray &q_min, const JntArray &q_max, const JntArray &q_dot_max, const double x_dot_trans_max, const double x_dot_rot_max, TreeFkSolverPos &fksolver, TreeIkSolverVel &iksolver)
 Constructor of the solver, it needs the number of joints of the tree, a list of the endpoints you are interested in, the maximum and minimum values you want to enforce and a forward position kinematics solver as well as an inverse velocity kinematics solver for the calculations. More...
 
 ~TreeIkSolverPos_Online ()
 
virtual double CartToJnt (const JntArray &q_in, const Frames &p_in, JntArray &q_out)
 Calculate inverse position kinematics, from cartesian coordinates to joint coordinates. More...
 

Private Member Functions

void enforceJointVelLimits ()
 Scales the class member KDL::JntArray q_dot_, if one (or more) joint velocity exceeds the maximum value. More...
 
void enforceCartVelLimits ()
 Scales translational and rotational velocity vectors of the class member KDL::Twist twist_, if at least one of both exceeds the maximum value/length. More...
 

Private Attributes

JntArray q_min_
 
JntArray q_max_
 
JntArray q_dot_max_
 
double x_dot_trans_max_
 
double x_dot_rot_max_
 
TreeFkSolverPosfksolver_
 
TreeIkSolverVeliksolver_
 
JntArray q_dot_
 
Twist twist_
 
Frames frames_
 
Twists delta_twists_
 

Detailed Description

Implementation of a general inverse position kinematics algorithm to calculate the position transformation from Cartesian to joint space of a general KDL::Tree.

This class has been derived from the TreeIkSolverPos_NR_JL class, but was modified for online solving for use in realtime systems. Thus, the calculation is only done once, meaning that no iteration is done, because this solver is intended to run at a high frequency. It enforces velocity limits in task as well as in joint space. It also takes joint limits into account.

Constructor & Destructor Documentation

§ TreeIkSolverPos_Online()

KDL::TreeIkSolverPos_Online::TreeIkSolverPos_Online ( const double &  nr_of_jnts,
const std::vector< std::string > &  endpoints,
const JntArray q_min,
const JntArray q_max,
const JntArray q_dot_max,
const double  x_dot_trans_max,
const double  x_dot_rot_max,
TreeFkSolverPos fksolver,
TreeIkSolverVel iksolver 
)

Constructor of the solver, it needs the number of joints of the tree, a list of the endpoints you are interested in, the maximum and minimum values you want to enforce and a forward position kinematics solver as well as an inverse velocity kinematics solver for the calculations.

Parameters
nr_of_jntsnumber of joints of the tree to calculate the joint positions for
endpointsthe list of endpoints you are interested in
q_minthe minimum joint positions
q_maxthe maximum joint positions
q_dot_maxthe maximum joint velocities
x_dot_trans_maxthe maximum translational velocity of your endpoints
x_dot_rot_maxthe maximum rotational velocity of your endpoints
fksolvera forward position kinematics solver
iksolveran inverse velocity kinematics solver
Returns

References delta_twists_, frames_, KDL::Frame::Identity(), q_dot_max_, q_max_, q_min_, x_dot_rot_max_, x_dot_trans_max_, and KDL::Twist::Zero().

§ ~TreeIkSolverPos_Online()

KDL::TreeIkSolverPos_Online::~TreeIkSolverPos_Online ( )

Member Function Documentation

§ CartToJnt()

double KDL::TreeIkSolverPos_Online::CartToJnt ( const JntArray q_init,
const Frames p_in,
JntArray q_out 
)
virtual

Calculate inverse position kinematics, from cartesian coordinates to joint coordinates.

Parameters
q_initinitial guess of the joint coordinates
p_ininput cartesian coordinates
q_outoutput joint coordinates
Returns
if < 0 something went wrong otherwise (>=0) remaining (weighted) distance to target

Implements KDL::TreeIkSolverPos.

References KDL::Add(), KDL::TreeIkSolverVel::CartToJnt(), delta_twists_, KDL::diff(), enforceCartVelLimits(), enforceJointVelLimits(), fksolver_, frames_, iksolver_, KDL::TreeFkSolverPos::JntToCart(), q_dot_, q_dot_max_, q_max_, q_min_, KDL::JntArray::rows(), and twist_.

§ enforceCartVelLimits()

void KDL::TreeIkSolverPos_Online::enforceCartVelLimits ( )
private

Scales translational and rotational velocity vectors of the class member KDL::Twist twist_, if at least one of both exceeds the maximum value/length.

Scaling is done proportional to the biggest overshoot among both velocities.

References KDL::Twist::rot, twist_, KDL::Twist::vel, KDL::Vector::x(), x_dot_rot_max_, x_dot_trans_max_, KDL::Vector::y(), and KDL::Vector::z().

Referenced by CartToJnt().

§ enforceJointVelLimits()

void KDL::TreeIkSolverPos_Online::enforceJointVelLimits ( )
private

Scales the class member KDL::JntArray q_dot_, if one (or more) joint velocity exceeds the maximum value.

Scaling is done proportional to the biggest overshoot among all joint velocities.

References KDL::Multiply(), q_dot_, q_dot_max_, and KDL::JntArray::rows().

Referenced by CartToJnt().

Member Data Documentation

§ delta_twists_

Twists KDL::TreeIkSolverPos_Online::delta_twists_
private

§ fksolver_

TreeFkSolverPos& KDL::TreeIkSolverPos_Online::fksolver_
private

Referenced by CartToJnt().

§ frames_

Frames KDL::TreeIkSolverPos_Online::frames_
private

§ iksolver_

TreeIkSolverVel& KDL::TreeIkSolverPos_Online::iksolver_
private

Referenced by CartToJnt().

§ q_dot_

JntArray KDL::TreeIkSolverPos_Online::q_dot_
private

Referenced by CartToJnt(), and enforceJointVelLimits().

§ q_dot_max_

JntArray KDL::TreeIkSolverPos_Online::q_dot_max_
private

§ q_max_

JntArray KDL::TreeIkSolverPos_Online::q_max_
private

§ q_min_

JntArray KDL::TreeIkSolverPos_Online::q_min_
private

§ twist_

Twist KDL::TreeIkSolverPos_Online::twist_
private

Referenced by CartToJnt(), and enforceCartVelLimits().

§ x_dot_rot_max_

double KDL::TreeIkSolverPos_Online::x_dot_rot_max_
private

§ x_dot_trans_max_

double KDL::TreeIkSolverPos_Online::x_dot_trans_max_
private

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