IRI ROS Specific Driver Class. More...

#include <uncalibvs_alg.h>

Public Types
typedef iri_uncalibvs::UncalibvsConfig	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 (const bool &traditional, const bool &random_points, const bool &quadrotor, const bool &arm, const bool &output_files, const std::string &path, const double &lambda, 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_Algorithm (void)
	constructor
void	unlock (void)
	Unlock Algorithm.
	~Uncalibvs_Algorithm (void)
	Destructor.
Public Attributes
Config	config_
	config variable
Protected Member Functions
void	base_rtw ()
	Base points.
void	compute_jacobian_t ()
	Compute Traditional Jacobian.
void	compute_jacobian_u ()
	Compute Uncalibrated Jacobian.
void	features_vector ()
	Features vector.
void	image_projection (const float &fu, const float &fv)
	Image projection.
void	initZeros ()
	Initialitzation with zeros.
void	K_control ()
	Compute 6DOF Control.
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 ()
	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_
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_
int	n_
int	noise_
bool	output_files_
Eigen::MatrixXf	P0_
std::string	path_
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 66 of file uncalibvs_alg.h.

Member Typedef Documentation

typedef iri_uncalibvs::UncalibvsConfig Uncalibvs_Algorithm::Config

define config type

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

Definition at line 197 of file uncalibvs_alg.h.

Constructor & Destructor Documentation

Uncalibvs_Algorithm::Uncalibvs_Algorithm ( 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_alg.cpp.

Uncalibvs_Algorithm::~Uncalibvs_Algorithm ( void )

Destructor.

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

Definition at line 10 of file uncalibvs_alg.cpp.

Member Function Documentation

void Uncalibvs_Algorithm::base_rtw ( ) [protected]

Base points.

Obtain the base points of the object

Definition at line 203 of file uncalibvs_alg.cpp.

void Uncalibvs_Algorithm::compute_jacobian_t ( ) [protected]

Compute Traditional Jacobian.

Traditional Jacobian that translates image features velocities to camera velocities

Definition at line 471 of file uncalibvs_alg.cpp.

void Uncalibvs_Algorithm::compute_jacobian_u ( ) [protected]

Compute Uncalibrated Jacobian.

Uncalibrated Jacobian that translates image features velocities to camera velocities

Definition at line 518 of file uncalibvs_alg.cpp.

void Uncalibvs_Algorithm::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_alg.cpp.

void Uncalibvs_Algorithm::features_vector ( ) [protected]

Features vector.

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

Definition at line 437 of file uncalibvs_alg.cpp.

void Uncalibvs_Algorithm::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 429 of file uncalibvs_alg.cpp.

void Uncalibvs_Algorithm::initZeros ( ) [protected]

Initialitzation with zeros.

Matrix structures initialization.

Definition at line 368 of file uncalibvs_alg.cpp.

void Uncalibvs_Algorithm::K_control ( ) [protected]

Compute 6DOF Control.

Compute the Lambda proportional 6DOF control of the visual servo

Definition at line 720 of file uncalibvs_alg.cpp.

void Uncalibvs_Algorithm::lock ( void ) [inline]

Lock Algorithm.

Locks access to the Algorithm class

Definition at line 224 of file uncalibvs_alg.h.

void Uncalibvs_Algorithm::q_control ( ) [protected]

Compute the 4DOF control law.

Compute the motions of the quadrotor

Definition at line 770 of file uncalibvs_alg.cpp.

void Uncalibvs_Algorithm::qa_control ( ) [protected]

Compute the 9DOF control law.

Compute the motions of the quadrotor and arm joints

Definition at line 811 of file uncalibvs_alg.cpp.

void Uncalibvs_Algorithm::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 922 of file uncalibvs_alg.cpp.

bool Uncalibvs_Algorithm::try_enter ( void ) [inline]

Tries Access to Algorithm.

Tries access to Algorithm

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

Definition at line 240 of file uncalibvs_alg.h.

void Uncalibvs_Algorithm::uncalib_vs	(	const bool &	traditional,
		const bool &	random_points,
		const bool &	quadrotor,
		const bool &	arm,
		const bool &	output_files,
		const std::string &	path,
		const double &	lambda,
		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
	)