#include <chainiksolvervel_pinv_nso.hpp>
Public Member Functions | |
virtual int | CartToJnt (const JntArray &q_in, const Twist &v_in, JntArray &qdot_out) |
virtual int | CartToJnt (const JntArray &, const FrameVel &, JntArrayVel &) |
ChainIkSolverVel_pinv_nso (const Chain &chain, const JntArray &opt_pos, const JntArray &weights, double eps=0.00001, int maxiter=150, double alpha=0.25) | |
ChainIkSolverVel_pinv_nso (const Chain &chain, double eps=0.00001, int maxiter=150, double alpha=0.25) | |
const double & | getAlpha () const |
const JntArray & | getOptPos () const |
int | getSVDResult () const |
const JntArray & | getWeights () const |
virtual int | setAlpha (const double alpha) |
virtual int | setOptPos (const JntArray &opt_pos) |
virtual int | setWeights (const JntArray &weights) |
virtual void | updateInternalDataStructures () |
~ChainIkSolverVel_pinv_nso () | |
Public Member Functions inherited from KDL::ChainIkSolverVel | |
virtual | ~ChainIkSolverVel () |
Public Member Functions inherited from KDL::SolverI | |
virtual int | getError () const |
Return the latest error. More... | |
SolverI () | |
Initialize latest error to E_NOERROR. More... | |
virtual const char * | strError (const int error) const |
virtual | ~SolverI () |
Private Attributes | |
double | alpha |
const Chain & | chain |
double | eps |
Jacobian | jac |
ChainJntToJacSolver | jnt2jac |
int | maxiter |
unsigned int | nj |
JntArray | opt_pos |
Eigen::VectorXd | S |
Eigen::VectorXd | Sinv |
int | svdResult |
Eigen::VectorXd | tmp |
Eigen::VectorXd | tmp2 |
Eigen::MatrixXd | U |
Eigen::MatrixXd | V |
JntArray | weights |
Additional Inherited Members | |
Public Types inherited from KDL::SolverI | |
enum | { E_DEGRADED = +1, E_NOERROR = 0, E_NO_CONVERGE = -1, E_UNDEFINED = -2, E_NOT_UP_TO_DATE = -3, E_SIZE_MISMATCH = -4, E_MAX_ITERATIONS_EXCEEDED = -5, E_OUT_OF_RANGE = -6, E_NOT_IMPLEMENTED = -7, E_SVD_FAILED = -8 } |
Protected Attributes inherited from KDL::SolverI | |
int | error |
Latest error, initialized to E_NOERROR in constructor. More... | |
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.
In case of a redundant robot this solver optimizes the following criterium: g=0.5*sum(weight*(Desired_joint_positions - actual_joint_positions))^2 as described in A. Liegeois. Automatic supervisory control of the configuration and behavior of multibody mechanisms. IEEE Transactions on Systems, Man, and Cybernetics, 7(12):868–871, 1977
Definition at line 46 of file chainiksolvervel_pinv_nso.hpp.
KDL::ChainIkSolverVel_pinv_nso::ChainIkSolverVel_pinv_nso | ( | const Chain & | chain, |
const JntArray & | opt_pos, | ||
const JntArray & | weights, | ||
double | eps = 0.00001 , |
||
int | maxiter = 150 , |
||
double | alpha = 0.25 |
||
) |
Constructor of the solver
chain | the chain to calculate the inverse velocity kinematics for |
opt_pos | the desired positions of the chain used by to resolve the redundancy |
weights | the weights applied in the joint space |
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 |
alpha | the null-space velocity gain |
Definition at line 27 of file chainiksolvervel_pinv_nso.cpp.
|
explicit |
Definition at line 47 of file chainiksolvervel_pinv_nso.cpp.
KDL::ChainIkSolverVel_pinv_nso::~ChainIkSolverVel_pinv_nso | ( | ) |
Definition at line 79 of file chainiksolvervel_pinv_nso.cpp.
|
virtual |
Calculate inverse velocity kinematics, from joint positions and cartesian velocity to joint velocities.
q_in | input joint positions |
v_in | input cartesian velocity |
qdot_out | output joint velocities |
Implements KDL::ChainIkSolverVel.
Definition at line 84 of file chainiksolvervel_pinv_nso.cpp.
|
inlinevirtual |
not (yet) implemented.
Implements KDL::ChainIkSolverVel.
Definition at line 72 of file chainiksolvervel_pinv_nso.hpp.
|
inline |
Request null space velocity gain
Definition at line 102 of file chainiksolvervel_pinv_nso.hpp.
|
inline |
Request the optimal joint positions
Definition at line 91 of file chainiksolvervel_pinv_nso.hpp.
|
inline |
Retrieve the latest return code from the SVD algorithm
Definition at line 135 of file chainiksolvervel_pinv_nso.hpp.
|
inline |
Request the joint weights for optimization criterion
Definition at line 80 of file chainiksolvervel_pinv_nso.hpp.
|
virtual |
Set null space velocity gain
alpha | NUllspace velocity cgain |
Definition at line 182 of file chainiksolvervel_pinv_nso.cpp.
|
virtual |
Set optimal joint positions
opt_pos | optimal joint positions |
Definition at line 174 of file chainiksolvervel_pinv_nso.cpp.
|
virtual |
Set joint weights for optimization criterion
weights | the joint weights |
Definition at line 166 of file chainiksolvervel_pinv_nso.cpp.
|
virtual |
Update the internal data structures. This is required if the number of segments or number of joints of a chain/tree have changed. This provides a single point of contact for solver memory allocations.
Implements KDL::ChainIkSolverVel.
Definition at line 65 of file chainiksolvervel_pinv_nso.cpp.
|
private |
Definition at line 154 of file chainiksolvervel_pinv_nso.hpp.
|
private |
Definition at line 141 of file chainiksolvervel_pinv_nso.hpp.
|
private |
Definition at line 151 of file chainiksolvervel_pinv_nso.hpp.
|
private |
Definition at line 144 of file chainiksolvervel_pinv_nso.hpp.
|
private |
Definition at line 142 of file chainiksolvervel_pinv_nso.hpp.
|
private |
Definition at line 152 of file chainiksolvervel_pinv_nso.hpp.
|
private |
Definition at line 143 of file chainiksolvervel_pinv_nso.hpp.
|
private |
Definition at line 156 of file chainiksolvervel_pinv_nso.hpp.
|
private |
Definition at line 146 of file chainiksolvervel_pinv_nso.hpp.
|
private |
Definition at line 147 of file chainiksolvervel_pinv_nso.hpp.
|
private |
Definition at line 153 of file chainiksolvervel_pinv_nso.hpp.
|
private |
Definition at line 149 of file chainiksolvervel_pinv_nso.hpp.
|
private |
Definition at line 150 of file chainiksolvervel_pinv_nso.hpp.
|
private |
Definition at line 145 of file chainiksolvervel_pinv_nso.hpp.
|
private |
Definition at line 148 of file chainiksolvervel_pinv_nso.hpp.
|
private |
Definition at line 155 of file chainiksolvervel_pinv_nso.hpp.