#include <chainiksolver_vel_pinv_mimic.hpp>
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 | 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) | |
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]. | |
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. | |
void | unlockRedundantJoints () |
~ChainIkSolverVel_pinv_mimic () | |
Private Member Functions | |
bool | jacToJacLocked (const Jacobian &jac, Jacobian &jac_locked) |
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::JointMimic > | mimic_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< JntArray > | U |
Eigen::MatrixXd | U_locked |
Eigen::MatrixXd | U_translate |
Eigen::MatrixXd | U_translate_locked |
std::vector< JntArray > | V |
Eigen::MatrixXd | V_locked |
Eigen::MatrixXd | V_translate |
Eigen::MatrixXd | V_translate_locked |
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.
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
chain | the chain to calculate the inverse velocity kinematics for |
num_mimic_joints | The number of joints that are setup to follow other joints |
num_redundant_joints | The number of redundant dofs |
position_ik | false 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. |
eps | if a singular value is below this value, its inverse is set to zero, default: 0.00001 |
maxiter | maximum iterations for the svd calculation, default: 150 |
Definition at line 31 of file chainiksolver_vel_pinv_mimic.cpp.
Definition at line 69 of file chainiksolver_vel_pinv_mimic.cpp.
int KDL::ChainIkSolverVel_pinv_mimic::CartToJnt | ( | const JntArray & | q_in, |
const Twist & | v_in, | ||
JntArray & | qdot_out | ||
) | [virtual] |
Implements KDL::ChainIkSolverVel.
Definition at line 229 of file chainiksolver_vel_pinv_mimic.cpp.
virtual int KDL::ChainIkSolverVel_pinv_mimic::CartToJnt | ( | const JntArray & | q_init, |
const FrameVel & | v_in, | ||
JntArrayVel & | q_out | ||
) | [inline, virtual] |
not (yet) implemented.
Implements KDL::ChainIkSolverVel.
Definition at line 78 of file chainiksolver_vel_pinv_mimic.hpp.
int KDL::ChainIkSolverVel_pinv_mimic::CartToJntRedundant | ( | const JntArray & | q_in, |
const Twist & | v_in, | ||
JntArray & | qdot_out | ||
) | [virtual] |
Definition at line 131 of file chainiksolver_vel_pinv_mimic.cpp.
bool KDL::ChainIkSolverVel_pinv_mimic::jacToJacLocked | ( | const Jacobian & | jac, |
Jacobian & | jac_locked | ||
) | [private] |
Definition at line 121 of file chainiksolver_vel_pinv_mimic.cpp.
bool KDL::ChainIkSolverVel_pinv_mimic::jacToJacReduced | ( | const Jacobian & | jac, |
Jacobian & | jac_mimic | ||
) | [private] |
Definition at line 108 of file chainiksolver_vel_pinv_mimic.cpp.
void KDL::ChainIkSolverVel_pinv_mimic::lockRedundantJoints | ( | ) | [inline] |
Definition at line 99 of file chainiksolver_vel_pinv_mimic.hpp.
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].
mimic_joints | Vector 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 73 of file 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 87 of file chainiksolver_vel_pinv_mimic.cpp.
void KDL::ChainIkSolverVel_pinv_mimic::unlockRedundantJoints | ( | ) | [inline] |
Definition at line 104 of file chainiksolver_vel_pinv_mimic.hpp.
const Chain KDL::ChainIkSolverVel_pinv_mimic::chain [private] |
Definition at line 114 of file chainiksolver_vel_pinv_mimic.hpp.
double KDL::ChainIkSolverVel_pinv_mimic::eps [private] |
Definition at line 141 of file chainiksolver_vel_pinv_mimic.hpp.
Definition at line 119 of file chainiksolver_vel_pinv_mimic.hpp.
Definition at line 137 of file chainiksolver_vel_pinv_mimic.hpp.
Definition at line 127 of file chainiksolver_vel_pinv_mimic.hpp.
Definition at line 115 of file chainiksolver_vel_pinv_mimic.hpp.
std::vector<unsigned int> KDL::ChainIkSolverVel_pinv_mimic::locked_joints_map_index [private] |
Definition at line 165 of file chainiksolver_vel_pinv_mimic.hpp.
int KDL::ChainIkSolverVel_pinv_mimic::maxiter [private] |
Definition at line 142 of file chainiksolver_vel_pinv_mimic.hpp.
std::vector<kdl_kinematics_plugin::JointMimic> KDL::ChainIkSolverVel_pinv_mimic::mimic_joints_ [private] |
Definition at line 145 of file chainiksolver_vel_pinv_mimic.hpp.
int KDL::ChainIkSolverVel_pinv_mimic::num_mimic_joints [private] |
Definition at line 146 of file chainiksolver_vel_pinv_mimic.hpp.
unsigned int KDL::ChainIkSolverVel_pinv_mimic::num_redundant_joints [private] |
Definition at line 166 of file chainiksolver_vel_pinv_mimic.hpp.
bool KDL::ChainIkSolverVel_pinv_mimic::position_ik [private] |
Definition at line 148 of file chainiksolver_vel_pinv_mimic.hpp.
Definition at line 138 of file chainiksolver_vel_pinv_mimic.hpp.
Definition at line 128 of file chainiksolver_vel_pinv_mimic.hpp.
Definition at line 138 of file chainiksolver_vel_pinv_mimic.hpp.
bool KDL::ChainIkSolverVel_pinv_mimic::redundant_joints_locked [private] |
Definition at line 167 of file chainiksolver_vel_pinv_mimic.hpp.
JntArray KDL::ChainIkSolverVel_pinv_mimic::S [private] |
Definition at line 121 of file chainiksolver_vel_pinv_mimic.hpp.
Eigen::VectorXd KDL::ChainIkSolverVel_pinv_mimic::S_locked [private] |
Definition at line 153 of file chainiksolver_vel_pinv_mimic.hpp.
Eigen::VectorXd KDL::ChainIkSolverVel_pinv_mimic::S_translate [private] |
Definition at line 132 of file chainiksolver_vel_pinv_mimic.hpp.
Eigen::VectorXd KDL::ChainIkSolverVel_pinv_mimic::S_translate_locked [private] |
Definition at line 160 of file chainiksolver_vel_pinv_mimic.hpp.
SVD_HH KDL::ChainIkSolverVel_pinv_mimic::svd [private] |
Definition at line 140 of file chainiksolver_vel_pinv_mimic.hpp.
Definition at line 123 of file chainiksolver_vel_pinv_mimic.hpp.
Eigen::VectorXd KDL::ChainIkSolverVel_pinv_mimic::tmp_locked [private] |
Definition at line 155 of file chainiksolver_vel_pinv_mimic.hpp.
Eigen::VectorXd KDL::ChainIkSolverVel_pinv_mimic::tmp_translate [private] |
Definition at line 134 of file chainiksolver_vel_pinv_mimic.hpp.
Eigen::VectorXd KDL::ChainIkSolverVel_pinv_mimic::tmp_translate_locked [private] |
Definition at line 162 of file chainiksolver_vel_pinv_mimic.hpp.
std::vector<JntArray> KDL::ChainIkSolverVel_pinv_mimic::U [private] |
Definition at line 120 of file chainiksolver_vel_pinv_mimic.hpp.
Eigen::MatrixXd KDL::ChainIkSolverVel_pinv_mimic::U_locked [private] |
Definition at line 152 of file chainiksolver_vel_pinv_mimic.hpp.
Eigen::MatrixXd KDL::ChainIkSolverVel_pinv_mimic::U_translate [private] |
Definition at line 131 of file chainiksolver_vel_pinv_mimic.hpp.
Eigen::MatrixXd KDL::ChainIkSolverVel_pinv_mimic::U_translate_locked [private] |
Definition at line 159 of file chainiksolver_vel_pinv_mimic.hpp.
std::vector<JntArray> KDL::ChainIkSolverVel_pinv_mimic::V [private] |
Definition at line 122 of file chainiksolver_vel_pinv_mimic.hpp.
Eigen::MatrixXd KDL::ChainIkSolverVel_pinv_mimic::V_locked [private] |
Definition at line 154 of file chainiksolver_vel_pinv_mimic.hpp.
Eigen::MatrixXd KDL::ChainIkSolverVel_pinv_mimic::V_translate [private] |
Definition at line 133 of file chainiksolver_vel_pinv_mimic.hpp.
Eigen::MatrixXd KDL::ChainIkSolverVel_pinv_mimic::V_translate_locked [private] |
Definition at line 161 of file chainiksolver_vel_pinv_mimic.hpp.