IRI ROS Specific Driver Class. More...

#include <uncalibvs_sim_alg.h>

Public Types
typedef iri_uncalibvs_sim::Uncalibvs_simConfig	Config
	define config type
Public Member Functions
void	config_update (Config &new_cfg, uint32_t level=0)
	config update
void	lock (void)
	Lock Algorithm.
bool	try_enter (void)
	Tries Access to Algorithm.
void	uncalib_vs_sim (const bool &traditional, const bool &random_points, const bool &quadrotor, const bool &arm_unina, const bool &arm_catec, const bool &output_files, const std::string &path, const double &lambda, const Eigen::MatrixXf &lambda_robot, const double &final_z, const Eigen::Matrix4f &cTo, const double &secs, const Eigen::MatrixXf &V2, KDL::Chain &chain, Eigen::MatrixXf &QQ, const Eigen::Matrix3f &Rquad_inertial, Eigen::MatrixXf &Vel_quad)
	Main algorithm.
	Uncalibvs_simAlgorithm (void)
	constructor
void	unlock (void)
	Unlock Algorithm.
	~Uncalibvs_simAlgorithm (void)
	Destructor.
Public Attributes
Config	config_
	config variable
Protected Member Functions
void	base_rtw ()
	Base points.
Eigen::MatrixXf	calcPinv (const Eigen::MatrixXf &a)
void	compute_jacobian_t ()
	Compute Traditional Jacobian.
void	compute_jacobian_u ()
	Compute Uncalibrated Jacobian.
void	features_vector ()
	Features vector.
Eigen::Matrix4f	GetTransform (const KDL::Frame &f)
void	image_projection (const float &fu, const float &fv)
	Image projection.
void	initZeros ()
	Initialitzation with zeros.
void	K_control ()
	Compute 6DOF Control.
void	pseudo_inverse (const Eigen::MatrixXf &M, const Eigen::MatrixXf &Y, Eigen::MatrixXf &X)
void	q_control ()
	Compute the 4DOF control law.
void	qa_control ()
	Compute the 9DOF control law.
void	qa_kinematics ()
	Compute Quadrotor and Arm 9DOF Kinematics.
void	v_null_space_catec ()
	Compute Desired pose velocities at the jacobian null space (over-determined system)
void	v_null_space_unina ()
	Compute Desired pose velocities at the jacobian null space (over-determined system)
Protected Attributes
bool	activate
Eigen::MatrixXf	alfas_
CMutex	alg_mutex_
	define config type
bool	arm_catec_
bool	arm_unina_
Eigen::MatrixXf	ccP_
KDL::Chain	chain_
Eigen::MatrixXf	cj_0_
Eigen::MatrixXf	cP_
Eigen::MatrixXf	cP_x_
Eigen::Matrix4f	cTo_
Eigen::MatrixXf	diff_
Eigen::Matrix4f	diTj_int_
float	ff_
float	ff_old_
double	final_z_
Eigen::MatrixXf	J_
KDL::Jacobian	jacobian_
KDL::JntArray	jnt_pos_
boost::scoped_ptr < KDL::ChainJntToJacSolver >	jnt_to_jac_solver_
boost::shared_ptr < KDL::ChainFkSolverPos_recursive >	jnt_to_pose_solver_
Eigen::MatrixXf	Jqa_
double	lambda_
Eigen::MatrixXf	lambda_robot_
int	n_
unsigned int	nj_
int	noise_
bool	output_files_
Eigen::MatrixXf	P0_
std::string	path_
double	phi_old_
Eigen::MatrixXf	pj_
bool	planar_
Eigen::MatrixXf	pn_
Eigen::MatrixXf	QQ_
bool	quadrotor_
bool	random_points_
Eigen::Matrix3f	Rquad_inertial_
Eigen::MatrixXf	s_
Eigen::MatrixXf	S_
Eigen::MatrixXf	S_x_
double	secs_
Eigen::Matrix4f	T0c_0_
Eigen::Matrix4f	T0c_new_
Eigen::Matrix4f	T0c_x_
Eigen::Matrix4f	Tc0_
bool	traditional_
float	u0_
Eigen::MatrixXf	uv_
float	v0_
Eigen::MatrixXf	V2_
Eigen::Matrix4f	V_mod_
Eigen::MatrixXf	Vel_
Eigen::MatrixXf	Velqa_
Eigen::MatrixXf	Vqa0_

Detailed Description

IRI ROS Specific Driver Class.

Definition at line 72 of file uncalibvs_sim_alg.h.

Member Typedef Documentation

typedef iri_uncalibvs_sim::Uncalibvs_simConfig Uncalibvs_simAlgorithm::Config

define config type

Define a Config type with the Uncalibvs_simConfig. All driver implementations will then use the same variable type Config.

Definition at line 234 of file uncalibvs_sim_alg.h.

Constructor & Destructor Documentation

Uncalibvs_simAlgorithm::Uncalibvs_simAlgorithm ( void )

constructor

In this constructor parameters related to the specific driver can be initalized. Those parameters can be also set in the openDriver() function. Attributes from the main node driver class IriBaseDriver such as loop_rate, may be also overload here.

Definition at line 4 of file uncalibvs_sim_alg.cpp.

Uncalibvs_simAlgorithm::~Uncalibvs_simAlgorithm ( void )

Destructor.

This destructor is called when the object is about to be destroyed.

Definition at line 10 of file uncalibvs_sim_alg.cpp.

Member Function Documentation

void Uncalibvs_simAlgorithm::base_rtw ( ) [protected]

Base points.

Obtain the base points of the object

Definition at line 171 of file uncalibvs_sim_alg.cpp.

Eigen::MatrixXf Uncalibvs_simAlgorithm::calcPinv ( const Eigen::MatrixXf & a ) [protected]

Matrix pseudo-inverse

Compute the matrix pseudo-inverse using SVD

Definition at line 1361 of file uncalibvs_sim_alg.cpp.

void Uncalibvs_simAlgorithm::compute_jacobian_t ( ) [protected]

Compute Traditional Jacobian.

Traditional Jacobian that translates image features velocities to camera velocities

Definition at line 443 of file uncalibvs_sim_alg.cpp.

void Uncalibvs_simAlgorithm::compute_jacobian_u ( ) [protected]

Compute Uncalibrated Jacobian.

Uncalibrated Jacobian that translates image features velocities to camera velocities

Definition at line 489 of file uncalibvs_sim_alg.cpp.

void Uncalibvs_simAlgorithm::config_update	(	Config &	new_cfg,
		uint32_t	level = `0`
	)

config update

In this function the driver parameters must be updated with the input config variable. Then the new configuration state will be stored in the Config attribute.

Parameters:

new_cfg	the new driver configuration state
level	level in which the update is taken place

Definition at line 14 of file uncalibvs_sim_alg.cpp.

void Uncalibvs_simAlgorithm::features_vector ( ) [protected]

Features vector.

Vector of features (x,y) in the camera frame

Definition at line 409 of file uncalibvs_sim_alg.cpp.

Eigen::Matrix4f Uncalibvs_simAlgorithm::GetTransform ( const KDL::Frame & f ) [protected]

KDL Frame to Homogenous transform

Compute the convertion from KDL Frame to Homogenous transform (Eigen Matrix 4f)

Definition at line 1414 of file uncalibvs_sim_alg.cpp.

void Uncalibvs_simAlgorithm::image_projection	(	const float &	fu,
		const float &	fv
	)		`[protected]`

Image projection.

Projection of 3D points (x,y,z) to image plane (u,v)

Definition at line 401 of file uncalibvs_sim_alg.cpp.

void Uncalibvs_simAlgorithm::initZeros ( ) [protected]

Initialitzation with zeros.

Matrix structures initialization.

Definition at line 342 of file uncalibvs_sim_alg.cpp.

void Uncalibvs_simAlgorithm::K_control ( ) [protected]

Compute 6DOF Control.

Compute the Lambda proportional 6DOF control of the visual servo

Definition at line 691 of file uncalibvs_sim_alg.cpp.

void Uncalibvs_simAlgorithm::lock ( void ) [inline]

Lock Algorithm.

Locks access to the Algorithm class

Definition at line 261 of file uncalibvs_sim_alg.h.

void Uncalibvs_simAlgorithm::pseudo_inverse	(	const Eigen::MatrixXf &	M,
		const Eigen::MatrixXf &	Y,
		Eigen::MatrixXf &	X
	)		`[protected]`

Matrix pseudo-inverse

Compute the QR matrix pseudo-inverse such as M*X=Y -> X=M^-1*Y

Definition at line 1406 of file uncalibvs_sim_alg.cpp.

void Uncalibvs_simAlgorithm::q_control ( ) [protected]

Compute the 4DOF control law.

Compute the motions of the quadrotor

Definition at line 749 of file uncalibvs_sim_alg.cpp.

void Uncalibvs_simAlgorithm::qa_control ( ) [protected]

Compute the 9DOF control law.

Compute the motions of the quadrotor and arm joints

Definition at line 878 of file uncalibvs_sim_alg.cpp.

void Uncalibvs_simAlgorithm::qa_kinematics ( ) [protected]

Compute Quadrotor and Arm 9DOF Kinematics.

Compute the quadrotor and arm jacobian and the position of the quadrotor using forward kinematics

Definition at line 781 of file uncalibvs_sim_alg.cpp.

bool Uncalibvs_simAlgorithm::try_enter ( void ) [inline]

Tries Access to Algorithm.

Tries access to Algorithm

Returns:: true if the lock was adquired, false otherwise

Definition at line 277 of file uncalibvs_sim_alg.h.

void Uncalibvs_simAlgorithm::uncalib_vs_sim	(	const bool &	traditional,
		const bool &	random_points,
		const bool &	quadrotor,
		const bool &	arm_unina,
		const bool &	arm_catec,
		const bool &	output_files,
		const std::string &	path,
		const double &	lambda,
		const Eigen::MatrixXf &	lambda_robot,
		const double &	final_z,
		const Eigen::Matrix4f &	cTo,
		const double &	secs,
		const Eigen::MatrixXf &	V2,
		KDL::Chain &	chain,
		Eigen::MatrixXf &	QQ,
		const Eigen::Matrix3f &	Rquad_inertial,
		Eigen::MatrixXf &	Vel_quad
	)