Public Member Functions | Protected Types | Protected Member Functions | Protected Attributes | Private Member Functions | Private Attributes
KdlTreeTr Class Reference

#include <KdlTreeTr.h>

Inheritance diagram for KdlTreeTr:
Inheritance graph
[legend]

List of all members.

Public Member Functions

virtual void getJointPositions (const KDL::JntArray &joints_in, const std::vector< std::string > &nodeNames, const std::vector< KDL::Frame > &nodeFrames, KDL::JntArray &joints_out, const std::vector< NodePriority > &nodePriorities)
 getJoints
virtual void initialize ()
 initialize called by both of the load functions, allowing derived classes to perform additional initialization after the tree is loaded
 KdlTreeTr ()
bool limitJoints (const KDL::JntArray &joints_in, KDL::JntArray &joints_out, KDL::JntArray &delta_joints)
 limits joint angles to be within valid range
void setJointInertias (const std::map< std::string, double > &inertia_in)
void setKr (double Kr=0.00032)
void setMaxJointDecel (double maxDecel=1.0)
void setMaxTwist (double maxTwist=0.1)
void setMBar (double mbar=1e-7)
 ~KdlTreeTr ()

Protected Types

typedef std::map< std::string,
KDL::Frame >::iterator 		FrameMapItr
typedef std::multimap< int,
std::pair< std::string, int >
>::iterator 		PriorityTaskAxisItr
typedef std::pair
< Eigen::MatrixXd,
Eigen::VectorXd > 		Task
typedef std::map< std::string,
TwistPriority >::iterator 		TwistMapItr
typedef std::pair< KDL::Twist,
NodePriority > 		TwistPriority

Protected Member Functions

bool limitTaskTwist (Eigen::VectorXd &twist)
bool limitTaskTwist (KDL::Twist &twist)

Protected Attributes

double jointMaxDecel
double Kr
double maxTwist
double mbar

Private Member Functions

double boundedCubic (const double &q, const double &pLeft, const double &pRight, const double &ct1, const double &ct2)
void createFrameTwistMaps (const std::vector< KDL::Frame > &nodeFrames, const std::vector< NodePriority > &NodePriorities)
 creates empty maps for frames and twist
double cubic (const double &q, const double &pLeft, const double &pRight, const double &ct1, const double &ct2)
 shape function
bool findFrameTwist (const KDL::JntArray &joints_in)
 fills frame and twist maps
void findJointInertia (const KDL::JntArray &joints_in, KDL::RotationalInertia &inertia_out)
Eigen::VectorXd getBiasVector (const Eigen::VectorXd &joints_in, const Eigen::VectorXd &joint_deltas, const Eigen::MatrixXd &ijac)
Task getCenterJointsTask (const KDL::JntArray &joints_in)
void getOrderedTasksByFrame (const KDL::JntArray &joints_in)
 creates a sorted vector of Tasks (jacobian and dr) based on the order they are provided. This is much more efficient since the jacobian matrix and dr vector need to be resized just once.
void getOrderedTasksByPriority (const KDL::JntArray &joints_in)
 creates a sorted vector of Tasks (jacobian and dr) based on the priority value given. It combines tasks in different frames which all have the same priority number. They are then sorted by highest priority first.
void getTaskReconstruction (const KDL::JntArray &joints_in, KDL::JntArray &joint_deltas)
 performs a task based decomposition (tasks defined by twist_deltas and twist_priorities) and reconstructs the task to avoid algorithmic and kinematic singularities
bool isTaskAchieved (const KDL::JntArray &joints_in, std::stringstream &errMsg)
void modifyToAvoidSingularities (const Eigen::VectorXd &nm, const double mom, const Eigen::VectorXd &dr, Eigen::VectorXd &drp)
int pInv (const Eigen::MatrixXd &jac_in, Eigen::MatrixXd &ijac_out, double &mom)
 calculate the pseudoinverse of a matrix
double sign (double num)
void updateNodeFrameMap (const std::vector< KDL::Frame > &nodeFrames)
 updates the desired frames

Private Attributes

std::map< std::string, KDL::Frame > currentFrameMap
std::map< std::string, KDL::Frame > desiredFrameMap
Eigen::MatrixXd ijacJntWeights
Eigen::MatrixXd jacJntWeights
bool jointLim
bool momLim
std::vector< std::string > nodeNames
std::vector< NodePriority > nodePriorities
int numConstraints
int numJoints
std::multimap< int, std::pair
< std::string, int > > 		priorityTaskAxisMap
std::vector< Task > taskList
std::map< std::string,
TwistPriority > 		twistMap
bool velocityLim

Detailed Description

Definition at line 11 of file KdlTreeTr.h.

Member Typedef Documentation

typedef std::map<std::string, KDL::Frame>::iterator KdlTreeTr::FrameMapItr [protected]

Definition at line 31 of file KdlTreeTr.h.

typedef std::multimap<int, std::pair<std::string, int> >::iterator KdlTreeTr::PriorityTaskAxisItr [protected]

Definition at line 35 of file KdlTreeTr.h.

typedef std::pair<Eigen::MatrixXd, Eigen::VectorXd> KdlTreeTr::Task [protected]

Definition at line 34 of file KdlTreeTr.h.

typedef std::map<std::string, TwistPriority>::iterator KdlTreeTr::TwistMapItr [protected]

Definition at line 33 of file KdlTreeTr.h.

typedef std::pair<KDL::Twist, NodePriority> KdlTreeTr::TwistPriority [protected]

Definition at line 32 of file KdlTreeTr.h.

Constructor & Destructor Documentation

KdlTreeTr::KdlTreeTr ( )

Definition at line 8 of file KdlTreeTr.cpp.

KdlTreeTr::~KdlTreeTr ( )

Definition at line 24 of file KdlTreeTr.cpp.

Member Function Documentation

double KdlTreeTr::boundedCubic ( const double &  q,
const double &  pLeft,
const double &  pRight,
const double &  ct1,
const double &  ct2 
) [private]

Shape_04_bounded: Cubic Bounded. _ | pLeft x <= ct1 | Y(x) = -| a3*t^3 + a2*t^2 + a1*t + a0 ct1 < x < ct2 | |_ pRight ct2 <= x

D(Y)(ct1) = 0 D(Y)(ct2) = 0

Definition at line 1019 of file KdlTreeTr.cpp.

void KdlTreeTr::createFrameTwistMaps ( const std::vector< KDL::Frame > &  nodeFrames,
const std::vector< NodePriority > &  NodePriorities 
) [private]

creates empty maps for frames and twist

Definition at line 56 of file KdlTreeTr.cpp.

double KdlTreeTr::cubic ( const double &  q,
const double &  pLeft,
const double &  pRight,
const double &  ct1,
const double &  ct2 
) [private]

shape function

Shape_04: Cubic. Y(x) = a3*t^3 + a2*t^2 + a1*t + a0; Y(ct1) = pLeft Y(ct2) = pRight D(Y)(ct1) = 0 D(Y)(ct2) = 0

Definition at line 979 of file KdlTreeTr.cpp.

bool KdlTreeTr::findFrameTwist ( const KDL::JntArray &  joints_in) [private]

fills frame and twist maps

Parameters:
jointInthe input joint positions
Returns:
true if solution has converged, false otherwise;

find corresponding current frame for desired frame

find the difference between the desired frame and the current frame

save in twistMap

Definition at line 121 of file KdlTreeTr.cpp.

void KdlTreeTr::findJointInertia ( const KDL::JntArray &  joints_in,
KDL::RotationalInertia &  inertia_out 
) [private]
VectorXd KdlTreeTr::getBiasVector ( const Eigen::VectorXd &  joints_in,
const Eigen::VectorXd &  joint_deltas,
const Eigen::MatrixXd &  ijac 
) [private]

Definition at line 934 of file KdlTreeTr.cpp.

KdlTreeTr::Task KdlTreeTr::getCenterJointsTask ( const KDL::JntArray &  joints_in) [private]

find the midpoint of all joints

find the twist bewteen the current frame and the center frame

Todo:
hunt down this nan problem

solve for the jacobian

build big Jacobian and big Twist

Definition at line 679 of file KdlTreeTr.cpp.

void KdlTreeTr::getJointPositions ( const KDL::JntArray &  joints_in,
const std::vector< std::string > &  nodeNames,
const std::vector< KDL::Frame > &  nodeFrames,
KDL::JntArray &  joints_out,
const std::vector< NodePriority > &  nodePriorities 
) [virtual]

getJoints

Parameters:
jointIninput joints
nodeNamesinput frame names
nodeFramesinput frames
jointsOutoutput joints
nodePrioritiesinput node priorities (6 * nodeNames.size())

A move is neccesssary, find it

Todo:
do this in the loop, but that requires adjusting the desired setpoint by the same factor limitJoints
Todo:
output the maximum difference between desired pose and reconstructed pose and pass that back or add an additional "accuracy" input for each task which will cause this to throw if it is not achieved

Added in here since limiting is outside the loop

Reimplemented from KdlTreeIk.

Reimplemented in MobileTreeIk.

Definition at line 296 of file KdlTreeTr.cpp.

void KdlTreeTr::getOrderedTasksByFrame ( const KDL::JntArray &  joints_in) [private]

creates a sorted vector of Tasks (jacobian and dr) based on the order they are provided. This is much more efficient since the jacobian matrix and dr vector need to be resized just once.

Parameters:
jointInThe current joint positions
Returns:
a vector of sorted Tasks

Definition at line 605 of file KdlTreeTr.cpp.

void KdlTreeTr::getOrderedTasksByPriority ( const KDL::JntArray &  joints_in) [private]

creates a sorted vector of Tasks (jacobian and dr) based on the priority value given. It combines tasks in different frames which all have the same priority number. They are then sorted by highest priority first.

Todo:

This is not very efficient because the matrices and vectors get resized at every iteration, really, it only needs to be resized when we create the maps....

Remove items that have a priority of IGNORE

Parameters:
jointInthe current joint positions
Returns:
a sorted vector of Tasks

Definition at line 534 of file KdlTreeTr.cpp.

void KdlTreeTr::getTaskReconstruction ( const KDL::JntArray &  jointIn,
KDL::JntArray &  joint_deltas 
) [private]

performs a task based decomposition (tasks defined by twist_deltas and twist_priorities) and reconstructs the task to avoid algorithmic and kinematic singularities

Parameters:
jointIninput joint positions
twist_deltasthe desired displacement
joint_deltasthe output joint positions
twist_prioritiesinput twist priorities ( 0 - do not care)
Todo:
add optimization to push null space solution toward the middle of the joint's range

Definition at line 451 of file KdlTreeTr.cpp.

void KdlTreeTr::initialize ( ) [virtual]

initialize called by both of the load functions, allowing derived classes to perform additional initialization after the tree is loaded

Reimplemented from KdlTreeIk.

Reimplemented in MobileTreeIk.

Definition at line 29 of file KdlTreeTr.cpp.

bool KdlTreeTr::isTaskAchieved ( const KDL::JntArray &  joints_in,
std::stringstream &  errMsg 
) [private]

find corresponding current frame for desired frame

find the difference between the desired frame and the current frame

save in twistMap

Definition at line 357 of file KdlTreeTr.cpp.

bool KdlTreeTr::limitJoints ( const KDL::JntArray &  joints_in,
KDL::JntArray &  joints_out,
KDL::JntArray &  delta_joints 
)

limits joint angles to be within valid range

Parameters:
qThe input joint angles
dqThe desired joint changes
qpThe modified joint angles to avoid limits

limit joints - assumes we are outside the IK iterative loop

Todo:
Pull this value from class safety files (JointLimBuffer)

make max vel a distance

make it high to avoid limiting (rad/s)

about 5 degrees

Definition at line 829 of file KdlTreeTr.cpp.

bool KdlTreeTr::limitTaskTwist ( Eigen::VectorXd &  twist) [protected]
bool KdlTreeTr::limitTaskTwist ( KDL::Twist &  twist) [protected]

Definition at line 242 of file KdlTreeTr.cpp.

void KdlTreeTr::modifyToAvoidSingularities ( const Eigen::VectorXd &  nm,
const double  mom,
const Eigen::VectorXd &  dr,
Eigen::VectorXd &  drp 
) [private]
Parameters:
nm,:bias vector (vector perpendicular to the object to be avoided in task space)
mom,:the measure of manipulability for this task
dr,:the desired task deltas
drp,:the modified task deltas

Definition at line 771 of file KdlTreeTr.cpp.

int KdlTreeTr::pInv ( const Eigen::MatrixXd &  jac_in,
Eigen::MatrixXd &  ijac_out,
double &  mom 
) [private]

calculate the pseudoinverse of a matrix

Parameters:
jacInthe input matrix
ijacOutthe inverted matrix
momthe measure of manipulability of the matrix

Definition at line 1042 of file KdlTreeTr.cpp.

void KdlTreeTr::setJointInertias ( const std::map< std::string, double > &  inertia_in)

Reimplemented from KdlTreeIk.

Definition at line 39 of file KdlTreeTr.cpp.

void KdlTreeTr::setKr ( double  Kr = 0.00032) [inline]

Definition at line 22 of file KdlTreeTr.h.

void KdlTreeTr::setMaxJointDecel ( double  maxDecel = 1.0) [inline]

Definition at line 24 of file KdlTreeTr.h.

void KdlTreeTr::setMaxTwist ( double  maxTwist = 0.1) [inline]

Definition at line 23 of file KdlTreeTr.h.

void KdlTreeTr::setMBar ( double  mbar = 1e-7) [inline]

Definition at line 21 of file KdlTreeTr.h.

double KdlTreeTr::sign ( double  num) [inline, private]

Definition at line 70 of file KdlTreeTr.h.

void KdlTreeTr::updateNodeFrameMap ( const std::vector< KDL::Frame > &  nodeFrames) [private]

updates the desired frames

Definition at line 93 of file KdlTreeTr.cpp.

Member Data Documentation

std::map<std::string, KDL::Frame> KdlTreeTr::currentFrameMap [private]

Definition at line 50 of file KdlTreeTr.h.

std::map<std::string, KDL::Frame> KdlTreeTr::desiredFrameMap [private]

Definition at line 51 of file KdlTreeTr.h.

Eigen::MatrixXd KdlTreeTr::ijacJntWeights [private]

Definition at line 78 of file KdlTreeTr.h.

Eigen::MatrixXd KdlTreeTr::jacJntWeights [private]

Definition at line 77 of file KdlTreeTr.h.

bool KdlTreeTr::jointLim [private]

Definition at line 81 of file KdlTreeTr.h.

double KdlTreeTr::jointMaxDecel [protected]

Definition at line 40 of file KdlTreeTr.h.

double KdlTreeTr::Kr [protected]

Definition at line 38 of file KdlTreeTr.h.

double KdlTreeTr::maxTwist [protected]

Definition at line 39 of file KdlTreeTr.h.

double KdlTreeTr::mbar [protected]

Definition at line 37 of file KdlTreeTr.h.

bool KdlTreeTr::momLim [private]

Definition at line 82 of file KdlTreeTr.h.

std::vector<std::string> KdlTreeTr::nodeNames [private]

Definition at line 48 of file KdlTreeTr.h.

std::vector<NodePriority> KdlTreeTr::nodePriorities [private]

Definition at line 49 of file KdlTreeTr.h.

int KdlTreeTr::numConstraints [private]

Definition at line 47 of file KdlTreeTr.h.

int KdlTreeTr::numJoints [private]

Definition at line 46 of file KdlTreeTr.h.

std::multimap<int, std::pair<std::string, int> > KdlTreeTr::priorityTaskAxisMap [private]

Definition at line 54 of file KdlTreeTr.h.

std::vector<Task> KdlTreeTr::taskList [private]

Definition at line 53 of file KdlTreeTr.h.

std::map<std::string, TwistPriority > KdlTreeTr::twistMap [private]

Definition at line 52 of file KdlTreeTr.h.

bool KdlTreeTr::velocityLim [private]

Definition at line 80 of file KdlTreeTr.h.

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

robodyn_controllers
Author(s):
autogenerated on Sat Jun 8 2019 20:20:54