#include <chainiksolver_vel_pinv_mimic.hpp>

Inheritance diagram for KDL::ChainIkSolverVel_pinv_mimic:

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

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

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.

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

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 ( )

Member Function Documentation

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

virtual

Implements KDL::ChainIkSolverVel.

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

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

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

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

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

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

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

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_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].

Parameters

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.

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.