#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 | 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::JointMimic > | mimic_joints_ |
std::vector< lma_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 |
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 |
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.
|
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 kdl_kinematics_plugin/src/chainiksolver_vel_pinv_mimic.cpp.
KDL::ChainIkSolverVel_pinv_mimic::~ChainIkSolverVel_pinv_mimic | ( | ) |
Definition at line 73 of file kdl_kinematics_plugin/src/chainiksolver_vel_pinv_mimic.cpp.
|
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 |
KDL::ChainIkSolverVel_pinv_mimic::~ChainIkSolverVel_pinv_mimic | ( | ) |
|
virtual |
Implements KDL::ChainIkSolverVel.
Definition at line 239 of file kdl_kinematics_plugin/src/chainiksolver_vel_pinv_mimic.cpp.
|
inlinevirtual |
not (yet) implemented.
Implements KDL::ChainIkSolverVel.
Definition at line 79 of file chainiksolver_vel_pinv_mimic.hpp.
|
virtual |
Implements KDL::ChainIkSolverVel.
|
inlinevirtual |
not (yet) implemented.
Implements KDL::ChainIkSolverVel.
Definition at line 90 of file chainiksolver_vel_pinv_mimic.h.
|
virtual |
Definition at line 135 of file kdl_kinematics_plugin/src/chainiksolver_vel_pinv_mimic.cpp.
|
virtual |
|
private |
Definition at line 125 of file kdl_kinematics_plugin/src/chainiksolver_vel_pinv_mimic.cpp.
|
private |
|
private |
Definition at line 112 of file kdl_kinematics_plugin/src/chainiksolver_vel_pinv_mimic.cpp.
|
private |
|
inline |
Definition at line 104 of file chainiksolver_vel_pinv_mimic.hpp.
|
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].
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 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].
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 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.
|
inline |
Definition at line 109 of file chainiksolver_vel_pinv_mimic.hpp.
|
inline |
Definition at line 120 of file chainiksolver_vel_pinv_mimic.h.
|
private |
Definition at line 118 of file chainiksolver_vel_pinv_mimic.hpp.
|
private |
Definition at line 145 of file chainiksolver_vel_pinv_mimic.hpp.
|
private |
Definition at line 123 of file chainiksolver_vel_pinv_mimic.hpp.
|
private |
Definition at line 141 of file chainiksolver_vel_pinv_mimic.hpp.
|
private |
Definition at line 131 of file chainiksolver_vel_pinv_mimic.hpp.
|
private |
Definition at line 119 of file chainiksolver_vel_pinv_mimic.hpp.
|
private |
Definition at line 169 of file chainiksolver_vel_pinv_mimic.hpp.
|
private |
Definition at line 146 of file chainiksolver_vel_pinv_mimic.hpp.
|
private |
Definition at line 149 of file chainiksolver_vel_pinv_mimic.hpp.
|
private |
Definition at line 160 of file chainiksolver_vel_pinv_mimic.h.
|
private |
Definition at line 150 of file chainiksolver_vel_pinv_mimic.hpp.
|
private |
Definition at line 170 of file chainiksolver_vel_pinv_mimic.hpp.
|
private |
Definition at line 152 of file chainiksolver_vel_pinv_mimic.hpp.
|
private |
Definition at line 142 of file chainiksolver_vel_pinv_mimic.hpp.
|
private |
Definition at line 132 of file chainiksolver_vel_pinv_mimic.hpp.
|
private |
Definition at line 142 of file chainiksolver_vel_pinv_mimic.hpp.
|
private |
Definition at line 171 of file chainiksolver_vel_pinv_mimic.hpp.
|
private |
Definition at line 125 of file chainiksolver_vel_pinv_mimic.hpp.
|
private |
Definition at line 157 of file chainiksolver_vel_pinv_mimic.hpp.
|
private |
Definition at line 136 of file chainiksolver_vel_pinv_mimic.hpp.
|
private |
Definition at line 164 of file chainiksolver_vel_pinv_mimic.hpp.
|
private |
Definition at line 144 of file chainiksolver_vel_pinv_mimic.hpp.
|
private |
Definition at line 127 of file chainiksolver_vel_pinv_mimic.hpp.
|
private |
Definition at line 159 of file chainiksolver_vel_pinv_mimic.hpp.
|
private |
Definition at line 138 of file chainiksolver_vel_pinv_mimic.hpp.
|
private |
Definition at line 166 of file chainiksolver_vel_pinv_mimic.hpp.
|
private |
Definition at line 124 of file chainiksolver_vel_pinv_mimic.hpp.
|
private |
Definition at line 156 of file chainiksolver_vel_pinv_mimic.hpp.
|
private |
Definition at line 135 of file chainiksolver_vel_pinv_mimic.hpp.
|
private |
Definition at line 163 of file chainiksolver_vel_pinv_mimic.hpp.
|
private |
Definition at line 126 of file chainiksolver_vel_pinv_mimic.hpp.
|
private |
Definition at line 158 of file chainiksolver_vel_pinv_mimic.hpp.
|
private |
Definition at line 137 of file chainiksolver_vel_pinv_mimic.hpp.
|
private |
Definition at line 165 of file chainiksolver_vel_pinv_mimic.hpp.