#include <chainiksolver_vel_mimic_svd.hpp>

Inheritance diagram for KDL::ChainIkSolverVelMimicSVD:

Public Member Functions
int	CartToJnt (const JntArray &, const FrameVel &, JntArrayVel &) override
	not implemented. More...

int	CartToJnt (const JntArray &q_in, const Twist &v_in, JntArray &qdot_out) override

int	CartToJnt (const JntArray &q_in, const Twist &v_in, JntArray &qdot_out, const Eigen::VectorXd &joint_weights, const Eigen::Matrix< double, 6, 1 > &cartesian_weights)

	ChainIkSolverVelMimicSVD (const Chain &chain_, const std::vector< kdl_kinematics_plugin::JointMimic > &mimic_joints, bool position_ik=false, double threshold=0.001)

bool	isPositionOnly () const
	Return true iff we ignore orientation but only consider position for inverse kinematics. More...

void	updateInternalDataStructures ()

	~ChainIkSolverVelMimicSVD () override

Private Member Functions
bool	jacToJacReduced (const Jacobian &jac, Jacobian &jac_reduced)

Private Attributes
const Chain &	chain_

Jacobian	jac_

Jacobian	jac_reduced_

ChainJntToJacSolver	jnt2jac_

const std::vector< kdl_kinematics_plugin::JointMimic > &	mimic_joints_

int	num_mimic_joints_

Eigen::VectorXd	qdot_out_reduced_

Eigen::JacobiSVD< Eigen::MatrixXd >	svd_

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 46 of file chainiksolver_vel_mimic_svd.hpp.

Constructor & Destructor Documentation

◆ ChainIkSolverVelMimicSVD()

KDL::ChainIkSolverVelMimicSVD::ChainIkSolverVelMimicSVD	(	const Chain &	chain_,
		const std::vector< kdl_kinematics_plugin::JointMimic > &	mimic_joints,
		bool	position_ik = `false`,
		double	threshold = `0.001`
	)

explicit

Constructor of the solver

Parameters

chain	the chain to calculate the inverse velocity kinematics for
mimic_joints	A vector of indices that map each (and every) joint 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]
num_mimic_joints	The number of joints that are setup to follow other joints
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.
threshold	if a singular value is below this value, its inverse is set to zero, default: 0.001

Definition at line 44 of file chainiksolver_vel_mimic_svd.cpp.

◆ ~ChainIkSolverVelMimicSVD()

KDL::ChainIkSolverVelMimicSVD::~ChainIkSolverVelMimicSVD ( )

overridedefault

Member Function Documentation

◆ CartToJnt() [1/3]

int KDL::ChainIkSolverVelMimicSVD::CartToJnt	(	const JntArray &	,
		const FrameVel &	,
		JntArrayVel &
	)

inlineoverride

not implemented.

Definition at line 91 of file chainiksolver_vel_mimic_svd.hpp.

◆ CartToJnt() [2/3]

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

inlineoverride

Definition at line 76 of file chainiksolver_vel_mimic_svd.hpp.

◆ CartToJnt() [3/3]

int KDL::ChainIkSolverVelMimicSVD::CartToJnt	(	const JntArray &	q_in,
		const Twist &	v_in,
		JntArray &	qdot_out,
		const Eigen::VectorXd &	joint_weights,
		const Eigen::Matrix< double, 6, 1 > &	cartesian_weights
	)

Compute qdot_out = W_q * (W_x * J * W_q)^# * W_x * v_in

where W_q and W_x are joint- and Cartesian weights respectively. A smaller joint weight (< 1.0) will reduce the contribution of this joint to the solution.

Definition at line 87 of file chainiksolver_vel_mimic_svd.cpp.