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

#include <chainiksolver_vel_pinv_mimic.hpp>

Inheritance diagram for KDL::ChainIkSolverVel_pinv_mimic:
Inheritance graph
[legend]

Public Member Functions

virtual int CartToJnt (const JntArray &q_in, const Twist &v_in, JntArray &qdot_out)
 
virtual int CartToJnt (const JntArray &q_init, const FrameVel &v_in, JntArrayVel &q_out)
 
virtual int CartToJnt (const JntArray &q_in, const Twist &v_in, JntArray &qdot_out)
 
virtual int CartToJnt (const JntArray &q_init, const FrameVel &v_in, JntArrayVel &q_out)
 
virtual int CartToJntRedundant (const JntArray &q_in, const Twist &v_in, JntArray &qdot_out)
 
virtual int CartToJntRedundant (const JntArray &q_in, const Twist &v_in, JntArray &qdot_out)
 
 ChainIkSolverVel_pinv_mimic (const Chain &chain, int num_mimic_joints=0, int num_redundant_joints=0, bool position_ik=false, double eps=0.00001, int maxiter=150)
 
 ChainIkSolverVel_pinv_mimic (const Chain &chain, int num_mimic_joints=0, int num_redundant_joints=0, bool position_ik=false, double eps=0.00001, int maxiter=150)
 
void lockRedundantJoints ()
 
void lockRedundantJoints ()
 
bool setMimicJoints (const std::vector< kdl_kinematics_plugin::JointMimic > &_mimic_joints)
 Set a vector of indices that map each (and every) joint in the chain onto the corresponding joint in a reduced set of joints that do not include the mimic joints. This vector must be of size chain.getNrOfJoints(). E.g. if an arm has 7 joints: j0 to j6. Say j2 mimics (follows) j0. Then, mimic_joints should be: [0 1 0 3 4 5 6]. More...
 
bool setMimicJoints (const std::vector< lma_kinematics_plugin::JointMimic > &_mimic_joints)
 Set a vector of indices that map each (and every) joint in the chain onto the corresponding joint in a reduced set of joints that do not include the mimic joints. This vector must be of size chain.getNrOfJoints(). E.g. if an arm has 7 joints: j0 to j6. Say j2 mimics (follows) j0. Then, mimic_joints should be: [0 1 0 3 4 5 6]. More...
 
bool setRedundantJointsMapIndex (const std::vector< unsigned int > &redundant_joints_map_index)
 Set a mapping between a reduced set of joints (numbering either 6 or 3) and the full set of active (i.e excluding the mimic joints) DOFs in the robot. As an example, consider an arm with 7 joints: j0 to j6. If j2 represents the redundancy, then redundant_joints_map_index will be a 6 dimensional vector - [0 1 3 4 5 6], i.e. joint_value_full(redundant_joints_map_index[i]) = joint_value_reduced(i), for i=0,...5. More...
 
bool setRedundantJointsMapIndex (const std::vector< unsigned int > &redundant_joints_map_index)
 Set a mapping between a reduced set of joints (numbering either 6 or 3) and the full set of active (i.e excluding the mimic joints) DOFs in the robot. As an example, consider an arm with 7 joints: j0 to j6. If j2 represents the redundancy, then redundant_joints_map_index will be a 6 dimensional vector - [0 1 3 4 5 6], i.e. joint_value_full(redundant_joints_map_index[i]) = joint_value_reduced(i), for i=0,...5. More...
 
void unlockRedundantJoints ()
 
void unlockRedundantJoints ()
 
 ~ChainIkSolverVel_pinv_mimic ()
 
 ~ChainIkSolverVel_pinv_mimic ()
 
- Public Member Functions inherited from KDL::ChainIkSolverVel
virtual void updateInternalDataStructures ()=0
 
virtual ~ChainIkSolverVel ()
 
- Public Member Functions inherited from KDL::SolverI
virtual int getError () const
 
 SolverI ()
 
virtual const char * strError (const int error) const
 
virtual ~SolverI ()
 

Private Member Functions

bool jacToJacLocked (const Jacobian &jac, Jacobian &jac_locked)
 
bool jacToJacLocked (const Jacobian &jac, Jacobian &jac_locked)
 
bool jacToJacReduced (const Jacobian &jac, Jacobian &jac_mimic)
 
bool jacToJacReduced (const Jacobian &jac, Jacobian &jac_mimic)
 

Private Attributes

const Chain chain
 
double eps
 
Jacobian jac
 
Jacobian jac_locked
 
Jacobian jac_reduced
 
ChainJntToJacSolver jnt2jac
 
std::vector< unsigned int > locked_joints_map_index
 
int maxiter
 
std::vector< kdl_kinematics_plugin::JointMimicmimic_joints_
 
std::vector< lma_kinematics_plugin::JointMimicmimic_joints_
 
int num_mimic_joints
 
unsigned int num_redundant_joints
 
bool position_ik
 
JntArray qdot_out_locked
 
JntArray qdot_out_reduced
 
JntArray qdot_out_reduced_locked
 
bool redundant_joints_locked
 
JntArray S
 
Eigen::VectorXd S_locked
 
Eigen::VectorXd S_translate
 
Eigen::VectorXd S_translate_locked
 
SVD_HH svd
 
JntArray tmp
 
Eigen::VectorXd tmp_locked
 
Eigen::VectorXd tmp_translate
 
Eigen::VectorXd tmp_translate_locked
 
std::vector< JntArrayU
 
Eigen::MatrixXd U_locked
 
Eigen::MatrixXd U_translate
 
Eigen::MatrixXd U_translate_locked
 
std::vector< JntArrayV
 
Eigen::MatrixXd V_locked
 
Eigen::MatrixXd V_translate
 
Eigen::MatrixXd V_translate_locked
 

Additional Inherited Members

- Public Attributes inherited from KDL::SolverI
 E_DEGRADED
 
 E_MAX_ITERATIONS_EXCEEDED
 
 E_NO_CONVERGE
 
 E_NOERROR
 
 E_NOT_IMPLEMENTED
 
 E_NOT_UP_TO_DATE
 
 E_OUT_OF_RANGE
 
 E_SIZE_MISMATCH
 
 E_SVD_FAILED
 
 E_UNDEFINED
 
- Protected Attributes inherited from KDL::SolverI
int error
 

Detailed Description

Implementation of a inverse velocity kinematics algorithm based on the generalize pseudo inverse to calculate the velocity transformation from Cartesian to joint space of a general KDL::Chain. It uses a svd-calculation based on householders rotations.

Definition at line 47 of file chainiksolver_vel_pinv_mimic.hpp.

Constructor & Destructor Documentation

KDL::ChainIkSolverVel_pinv_mimic::ChainIkSolverVel_pinv_mimic ( const Chain chain,
int  num_mimic_joints = 0,
int  num_redundant_joints = 0,
bool  position_ik = false,
double  eps = 0.00001,
int  maxiter = 150 
)
explicit

Constructor of the solver

Parameters
chainthe chain to calculate the inverse velocity kinematics for
num_mimic_jointsThe number of joints that are setup to follow other joints
num_redundant_jointsThe number of redundant dofs
position_ikfalse if you want to solve for the full 6 dof end-effector pose, true if you want to solve only for the 3 dof end-effector position.
epsif a singular value is below this value, its inverse is set to zero, default: 0.00001
maxitermaximum iterations for the svd calculation, default: 150

Definition at line 31 of file kdl_kinematics_plugin/src/chainiksolver_vel_pinv_mimic.cpp.

KDL::ChainIkSolverVel_pinv_mimic::~ChainIkSolverVel_pinv_mimic ( )
KDL::ChainIkSolverVel_pinv_mimic::ChainIkSolverVel_pinv_mimic ( const Chain chain,
int  num_mimic_joints = 0,
int  num_redundant_joints = 0,
bool  position_ik = false,
double  eps = 0.00001,
int  maxiter = 150 
)
explicit

Constructor of the solver

Parameters
chainthe chain to calculate the inverse velocity kinematics for
num_mimic_jointsThe number of joints that are setup to follow other joints
num_redundant_jointsThe number of redundant dofs
position_ikfalse if you want to solve for the full 6 dof end-effector pose, true if you want to solve only for the 3 dof end-effector position.
epsif a singular value is below this value, its inverse is set to zero, default: 0.00001
maxitermaximum iterations for the svd calculation, default: 150
KDL::ChainIkSolverVel_pinv_mimic::~ChainIkSolverVel_pinv_mimic ( )

Member Function Documentation

int KDL::ChainIkSolverVel_pinv_mimic::CartToJnt ( const JntArray q_in,
const Twist v_in,
JntArray qdot_out 
)
virtual
virtual int KDL::ChainIkSolverVel_pinv_mimic::CartToJnt ( const JntArray q_init,
const FrameVel v_in,
JntArrayVel q_out 
)
inlinevirtual

not (yet) implemented.

Implements KDL::ChainIkSolverVel.

Definition at line 79 of file chainiksolver_vel_pinv_mimic.hpp.

virtual int KDL::ChainIkSolverVel_pinv_mimic::CartToJnt ( const JntArray q_in,
const Twist v_in,
JntArray qdot_out 
)
virtual

Implements KDL::ChainIkSolverVel.

virtual int KDL::ChainIkSolverVel_pinv_mimic::CartToJnt ( const JntArray q_init,
const FrameVel v_in,
JntArrayVel q_out 
)
inlinevirtual

not (yet) implemented.

Implements KDL::ChainIkSolverVel.

Definition at line 90 of file chainiksolver_vel_pinv_mimic.h.

int KDL::ChainIkSolverVel_pinv_mimic::CartToJntRedundant ( const JntArray q_in,
const Twist v_in,
JntArray qdot_out 
)
virtual
virtual int KDL::ChainIkSolverVel_pinv_mimic::CartToJntRedundant ( const JntArray q_in,
const Twist v_in,
JntArray qdot_out 
)
virtual
bool KDL::ChainIkSolverVel_pinv_mimic::jacToJacLocked ( const Jacobian jac,
Jacobian jac_locked 
)
private
bool KDL::ChainIkSolverVel_pinv_mimic::jacToJacLocked ( const Jacobian jac,
Jacobian jac_locked 
)
private
bool KDL::ChainIkSolverVel_pinv_mimic::jacToJacReduced ( const Jacobian jac,
Jacobian jac_mimic 
)
private
bool KDL::ChainIkSolverVel_pinv_mimic::jacToJacReduced ( const Jacobian jac,
Jacobian jac_mimic 
)
private
void KDL::ChainIkSolverVel_pinv_mimic::lockRedundantJoints ( )
inline

Definition at line 104 of file chainiksolver_vel_pinv_mimic.hpp.

void KDL::ChainIkSolverVel_pinv_mimic::lockRedundantJoints ( )
inline

Definition at line 115 of file chainiksolver_vel_pinv_mimic.h.

bool KDL::ChainIkSolverVel_pinv_mimic::setMimicJoints ( const std::vector< kdl_kinematics_plugin::JointMimic > &  _mimic_joints)

Set a vector of indices that map each (and every) joint in the chain onto the corresponding joint in a reduced set of joints that do not include the mimic joints. This vector must be of size chain.getNrOfJoints(). E.g. if an arm has 7 joints: j0 to j6. Say j2 mimics (follows) j0. Then, mimic_joints should be: [0 1 0 3 4 5 6].

Parameters
mimic_jointsVector of size chain.getNrOfJoints() that maps every joint in the chain onto (a) itself if its not a mimic joint or (b) onto the active dof that it is mimicking

Definition at line 77 of file kdl_kinematics_plugin/src/chainiksolver_vel_pinv_mimic.cpp.

bool KDL::ChainIkSolverVel_pinv_mimic::setMimicJoints ( const std::vector< lma_kinematics_plugin::JointMimic > &  _mimic_joints)

Set a vector of indices that map each (and every) joint in the chain onto the corresponding joint in a reduced set of joints that do not include the mimic joints. This vector must be of size chain.getNrOfJoints(). E.g. if an arm has 7 joints: j0 to j6. Say j2 mimics (follows) j0. Then, mimic_joints should be: [0 1 0 3 4 5 6].

Parameters
mimic_jointsVector of size chain.getNrOfJoints() that maps every joint in the chain onto (a) itself if its not a mimic joint or (b) onto the active dof that it is mimicking

Definition at line 88 of file lma_kinematics_plugin/src/chainiksolver_vel_pinv_mimic.cpp.

bool KDL::ChainIkSolverVel_pinv_mimic::setRedundantJointsMapIndex ( const std::vector< unsigned int > &  redundant_joints_map_index)

Set a mapping between a reduced set of joints (numbering either 6 or 3) and the full set of active (i.e excluding the mimic joints) DOFs in the robot. As an example, consider an arm with 7 joints: j0 to j6. If j2 represents the redundancy, then redundant_joints_map_index will be a 6 dimensional vector - [0 1 3 4 5 6], i.e. joint_value_full(redundant_joints_map_index[i]) = joint_value_reduced(i), for i=0,...5.

Definition at line 91 of file kdl_kinematics_plugin/src/chainiksolver_vel_pinv_mimic.cpp.

bool KDL::ChainIkSolverVel_pinv_mimic::setRedundantJointsMapIndex ( const std::vector< unsigned int > &  redundant_joints_map_index)

Set a mapping between a reduced set of joints (numbering either 6 or 3) and the full set of active (i.e excluding the mimic joints) DOFs in the robot. As an example, consider an arm with 7 joints: j0 to j6. If j2 represents the redundancy, then redundant_joints_map_index will be a 6 dimensional vector - [0 1 3 4 5 6], i.e. joint_value_full(redundant_joints_map_index[i]) = joint_value_reduced(i), for i=0,...5.

void KDL::ChainIkSolverVel_pinv_mimic::unlockRedundantJoints ( )
inline

Definition at line 109 of file chainiksolver_vel_pinv_mimic.hpp.

void KDL::ChainIkSolverVel_pinv_mimic::unlockRedundantJoints ( )
inline

Definition at line 120 of file chainiksolver_vel_pinv_mimic.h.

Member Data Documentation

const Chain KDL::ChainIkSolverVel_pinv_mimic::chain
private

Definition at line 118 of file chainiksolver_vel_pinv_mimic.hpp.

double KDL::ChainIkSolverVel_pinv_mimic::eps
private

Definition at line 145 of file chainiksolver_vel_pinv_mimic.hpp.

Jacobian KDL::ChainIkSolverVel_pinv_mimic::jac
private

Definition at line 123 of file chainiksolver_vel_pinv_mimic.hpp.

Jacobian KDL::ChainIkSolverVel_pinv_mimic::jac_locked
private

Definition at line 141 of file chainiksolver_vel_pinv_mimic.hpp.

Jacobian KDL::ChainIkSolverVel_pinv_mimic::jac_reduced
private

Definition at line 131 of file chainiksolver_vel_pinv_mimic.hpp.

ChainJntToJacSolver KDL::ChainIkSolverVel_pinv_mimic::jnt2jac
private

Definition at line 119 of file chainiksolver_vel_pinv_mimic.hpp.

std::vector< unsigned int > KDL::ChainIkSolverVel_pinv_mimic::locked_joints_map_index
private

Definition at line 169 of file chainiksolver_vel_pinv_mimic.hpp.

int KDL::ChainIkSolverVel_pinv_mimic::maxiter
private

Definition at line 146 of file chainiksolver_vel_pinv_mimic.hpp.

std::vector<kdl_kinematics_plugin::JointMimic> KDL::ChainIkSolverVel_pinv_mimic::mimic_joints_
private

Definition at line 149 of file chainiksolver_vel_pinv_mimic.hpp.

std::vector<lma_kinematics_plugin::JointMimic> KDL::ChainIkSolverVel_pinv_mimic::mimic_joints_
private

Definition at line 160 of file chainiksolver_vel_pinv_mimic.h.

int KDL::ChainIkSolverVel_pinv_mimic::num_mimic_joints
private

Definition at line 150 of file chainiksolver_vel_pinv_mimic.hpp.

unsigned int KDL::ChainIkSolverVel_pinv_mimic::num_redundant_joints
private

Definition at line 170 of file chainiksolver_vel_pinv_mimic.hpp.

bool KDL::ChainIkSolverVel_pinv_mimic::position_ik
private

Definition at line 152 of file chainiksolver_vel_pinv_mimic.hpp.

JntArray KDL::ChainIkSolverVel_pinv_mimic::qdot_out_locked
private

Definition at line 142 of file chainiksolver_vel_pinv_mimic.hpp.

JntArray KDL::ChainIkSolverVel_pinv_mimic::qdot_out_reduced
private

Definition at line 132 of file chainiksolver_vel_pinv_mimic.hpp.

JntArray KDL::ChainIkSolverVel_pinv_mimic::qdot_out_reduced_locked
private

Definition at line 142 of file chainiksolver_vel_pinv_mimic.hpp.

bool KDL::ChainIkSolverVel_pinv_mimic::redundant_joints_locked
private

Definition at line 171 of file chainiksolver_vel_pinv_mimic.hpp.

JntArray KDL::ChainIkSolverVel_pinv_mimic::S
private

Definition at line 125 of file chainiksolver_vel_pinv_mimic.hpp.

Eigen::VectorXd KDL::ChainIkSolverVel_pinv_mimic::S_locked
private

Definition at line 157 of file chainiksolver_vel_pinv_mimic.hpp.

Eigen::VectorXd KDL::ChainIkSolverVel_pinv_mimic::S_translate
private

Definition at line 136 of file chainiksolver_vel_pinv_mimic.hpp.

Eigen::VectorXd KDL::ChainIkSolverVel_pinv_mimic::S_translate_locked
private

Definition at line 164 of file chainiksolver_vel_pinv_mimic.hpp.

SVD_HH KDL::ChainIkSolverVel_pinv_mimic::svd
private

Definition at line 144 of file chainiksolver_vel_pinv_mimic.hpp.

JntArray KDL::ChainIkSolverVel_pinv_mimic::tmp
private

Definition at line 127 of file chainiksolver_vel_pinv_mimic.hpp.

Eigen::VectorXd KDL::ChainIkSolverVel_pinv_mimic::tmp_locked
private

Definition at line 159 of file chainiksolver_vel_pinv_mimic.hpp.

Eigen::VectorXd KDL::ChainIkSolverVel_pinv_mimic::tmp_translate
private

Definition at line 138 of file chainiksolver_vel_pinv_mimic.hpp.

Eigen::VectorXd KDL::ChainIkSolverVel_pinv_mimic::tmp_translate_locked
private

Definition at line 166 of file chainiksolver_vel_pinv_mimic.hpp.

std::vector< JntArray > KDL::ChainIkSolverVel_pinv_mimic::U
private

Definition at line 124 of file chainiksolver_vel_pinv_mimic.hpp.

Eigen::MatrixXd KDL::ChainIkSolverVel_pinv_mimic::U_locked
private

Definition at line 156 of file chainiksolver_vel_pinv_mimic.hpp.

Eigen::MatrixXd KDL::ChainIkSolverVel_pinv_mimic::U_translate
private

Definition at line 135 of file chainiksolver_vel_pinv_mimic.hpp.

Eigen::MatrixXd KDL::ChainIkSolverVel_pinv_mimic::U_translate_locked
private

Definition at line 163 of file chainiksolver_vel_pinv_mimic.hpp.

std::vector< JntArray > KDL::ChainIkSolverVel_pinv_mimic::V
private

Definition at line 126 of file chainiksolver_vel_pinv_mimic.hpp.

Eigen::MatrixXd KDL::ChainIkSolverVel_pinv_mimic::V_locked
private

Definition at line 158 of file chainiksolver_vel_pinv_mimic.hpp.

Eigen::MatrixXd KDL::ChainIkSolverVel_pinv_mimic::V_translate
private

Definition at line 137 of file chainiksolver_vel_pinv_mimic.hpp.

Eigen::MatrixXd KDL::ChainIkSolverVel_pinv_mimic::V_translate_locked
private

Definition at line 165 of file chainiksolver_vel_pinv_mimic.hpp.


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


moveit_kinematics
Author(s): Dave Coleman , Ioan Sucan , Sachin Chitta
autogenerated on Sun Oct 18 2020 13:17:53