Kinematics class using the kinematics lib. More...

#include <kinematics.h>

Public Member Functions
int	directKinematics (std::vector< double > angles, std::vector< double > &pose)
	Calculates the direct kinematics.
int	enc2rad (std::vector< int > encoders, std::vector< double > &angles)
	Converts encoders to angles.
int	getAngMax (std::vector< double > &angleMax)
	Get angle max.
int	getAngMin (std::vector< double > &angleMin)
	Get angle min.
int	getAngOff (std::vector< double > &angleOffset)
	Get the angle offsets.
int	getAngRan (std::vector< double > &angleRange)
	Get the angle range.
int	getAngStop (std::vector< double > &angleStop)
	Get angle stop.
int	getDOF ()
	Get the degree of freedom.
int	getDOM ()
	Get the degree of mobility.
int	getEncOff (std::vector< int > &encOffset)
	Get the encoder offsets.
int	getEPC (std::vector< int > &epc)
	Get the encoders per cycle.
int	getImmob ()
	Get the immobile flag of the last joint.
int	getMaxDOF ()
	Get the maximum degree of freedom.
int	getMDH (std::vector< double > &theta, std::vector< double > &d, std::vector< double > &a, std::vector< double > &alpha)
	Get the modified Denavit-Hartenberg parameters.
int	getRotDir (std::vector< int > &rotDir)
	Get the rotation direction.
int	getTcpOff (std::vector< double > &tcpOffset)
	Get the tcp offset.
int	getType ()
	Get the robot type.
int	getVersion (std::vector< int > &version)
	Get the version number of the library.
int	init ()
	This initializes the kinematics.
int	inverseKinematics (std::vector< double > pose, std::vector< double > prev, std::vector< double > &angles, int maxBisection=0)
	Calculates the inverse kinematics.
int	K4D2mDHAng (std::vector< double > angleK4D, std::vector< double > &angleMDH)
	This converts angles from Katana4D to mDH convention.
	KinematicsLib ()
	Constructor.
	KinematicsLib (int type)
	Constructor with robot type 0 (6M90A_F), 1 (6M90A_G), 2 (K_6M180),.
int	mDH2K4DAng (std::vector< double > angleMDH, std::vector< double > &angleK4D)
	This converts angles from mDH to Katana4D convention.
int	rad2enc (std::vector< double > angles, std::vector< int > &encoders)
	Converts angles to encoders.
int	setAngOff (std::vector< double > angleOffset)
	This sets the angle offsets.
int	setAngRan (std::vector< double > angleRange)
	This sets the angle range.
int	setEncOff (std::vector< int > encOffset)
	This sets the encoder offsets.
int	setEPC (std::vector< int > epc)
	This sets the encoders per cycle.
int	setImmob (int immobile)
	This sets the immobile flag of the last joint.
int	setLinkLen (std::vector< double > links)
	This sets the link length parameters (part of mDH-parameters)
int	setMDH (std::vector< double > theta, std::vector< double > d, std::vector< double > a, std::vector< double > alpha, int typeNr=-1)
	This sets the modified Denavit-Hartenberg parameters.
int	setRotDir (std::vector< int > rotDir)
	This sets the rotation direction.
int	setTcpOff (std::vector< double > tcpOffset)
	This sets the tcp offset.
int	setType (int type)
	This sets the robot type.
	~KinematicsLib ()
	Destructor.
Private Member Functions
int	anaGuess (std::vector< double > pose, std::vector< double > prev, std::vector< double > &angle)
int	angleArrMDH2vecK4D (const double arr[], std::vector< double > *vec)
bool	checkConfig (std::vector< double > config, std::vector< double > pose, double tol=0.00001)
int	initDofMat (int dof)
int	initializeMembers ()
int	invKin (std::vector< double > pose, std::vector< double > prev, std::vector< double > &angle)
int	invKin_bisec (std::vector< double > pose, std::vector< double > prev, std::vector< double > &conf, int maxBisection)
int	setAngleMinMax ()
int	sign (int value)
int	sign (double value)
Private Attributes
AnaGuess::Kinematics *	_anaGuess
	Analytical guess.
double	_angleMax [MaxDof]
	Angle max.
double	_angleMin [MaxDof]
	Angle min.
double	_angleOffset [MaxDof]
double	_angleRange [MaxDof]
bool	_angOffInit
	Angle offset.
bool	_angRanInit
	Angle range.
Matrix	_data
int	_dof
	Degree of freedom.
int	_dom
	Degree of mobility.
int	_encoderOffset [MaxDof]
	Encoder offset.
int	_epc [MaxDof]
	Encoder per cycle.
int	_immobile
	Last joint is immobile (eg. K_6M90A_G, K_6M90B_G), _dom = _dof - 1.
bool	_initialized
	Kinematic initialized.
double	_linkLength [4]
	Link length (for defined types)
bool	_matrixInit
	Matrices for mDH, angle range and immobile data.
mRobot	_robot
	Roboop robot class.
int	_rotDir [MaxDof]
	Rotation direction.
double	_tcpOffset [4]
	TCP offset.
double	_thetaimmobile
int	_type
	Robot Type.

Detailed Description

Kinematics class using the kinematics lib.

Definition at line 178 of file kinematics.h.

Constructor & Destructor Documentation

KinematicsLib::KinematicsLib ( )

Constructor.

Definition at line 11 of file kinematics.cpp.

KinematicsLib::KinematicsLib ( int type )

Constructor with robot type 0 (6M90A_F), 1 (6M90A_G), 2 (K_6M180),.

Definition at line 15 of file kinematics.cpp.

KinematicsLib::~KinematicsLib ( )

Destructor.

Definition at line 21 of file kinematics.cpp.

Member Function Documentation

int KinematicsLib::anaGuess	(	std::vector< double >	pose,
		std::vector< double >	prev,
		std::vector< double > &	angle
	)		`[private]`

Definition at line 975 of file kinematics.cpp.

int KinematicsLib::angleArrMDH2vecK4D	(	const double	arr[],
		std::vector< double > *	vec
	)		`[private]`

Definition at line 104 of file kinematics.cpp.

bool KinematicsLib::checkConfig	(	std::vector< double >	config,
		std::vector< double >	pose,
		double	tol = `0.00001`
	)		`[private]`

Definition at line 1024 of file kinematics.cpp.

int KinematicsLib::directKinematics	(	std::vector< double >	angles,
		std::vector< double > &	pose
	)

Calculates the direct kinematics.

Parameters:

angles	robot configuration (joint angles)
pose	resulting pose of the TCP [x, y, z, phi, theta, psi]

Returns:: 1 if successful, -1 if failed

Definition at line 840 of file kinematics.cpp.

int KinematicsLib::enc2rad	(	std::vector< int >	encoders,
		std::vector< double > &	angles
	)

Converts encoders to angles.

Parameters:

encoders	encoder positions of joints
angles	resulting joint angles

Returns:: 1 if successful, -1 if failed

Definition at line 812 of file kinematics.cpp.

int KinematicsLib::getAngMax ( std::vector< double > & angleMax )

Get angle max.

Parameters:

angleMax vector to write angle max

Returns:: 1 if successful, -1 if failed

Definition at line 697 of file kinematics.cpp.

int KinematicsLib::getAngMin ( std::vector< double > & angleMin )

Get angle min.

Parameters:

angleMin vector to write angle min

Returns:: 1 if successful, -1 if failed

Definition at line 684 of file kinematics.cpp.

int KinematicsLib::getAngOff ( std::vector< double > & angleOffset )

Get the angle offsets.

Parameters:

angleOffset vector to write in angle values at calibration stops

Returns:: 1 if successful, -1 if failed

Definition at line 644 of file kinematics.cpp.

int KinematicsLib::getAngRan ( std::vector< double > & angleRange )

Get the angle range.

Parameters:

angleRange vector to write in angle range

Returns:: 1 if successful, -1 if failed

Definition at line 655 of file kinematics.cpp.

int KinematicsLib::getAngStop ( std::vector< double > & angleStop )

Get angle stop.

Parameters:

angleStop vector to write angle stop

Returns:: 1 if successful, -1 if failed

Definition at line 666 of file kinematics.cpp.

int KinematicsLib::getDOF ( )

Get the degree of freedom.

Get the degree of freedom

Returns:: DOF (-1 if not set)

Definition at line 581 of file kinematics.cpp.

int KinematicsLib::getDOM ( )

Get the degree of mobility.

Get the degree of mobility

Returns:: DOM (-1 if not set)

Definition at line 585 of file kinematics.cpp.

int KinematicsLib::getEncOff ( std::vector< int > & encOffset )

Get the encoder offsets.

Parameters:

encoderOffset vector to write in encoder values at calibration stops

Returns:: 1 if successful, -1 if failed

Definition at line 622 of file kinematics.cpp.

int KinematicsLib::getEPC ( std::vector< int > & epc )

Get the encoders per cycle.

Parameters:

epc	vector to write in encoders pec cycle

Returns:: 1 if successful, -1 if failed

Definition at line 611 of file kinematics.cpp.

int KinematicsLib::getImmob ( )

Get the immobile flag of the last joint.

Returns:: immobile flag: 0 if mobile, 1 if immobile (-1 if not set)

Definition at line 607 of file kinematics.cpp.

int KinematicsLib::getMaxDOF ( )

Get the maximum degree of freedom.

Get the maximum degree of freedom (maximum size of the vectors)

Returns:: maximum DOF

Definition at line 577 of file kinematics.cpp.

int KinematicsLib::getMDH	(	std::vector< double > &	theta,
		std::vector< double > &	d,
		std::vector< double > &	a,
		std::vector< double > &	alpha
	)

Get the modified Denavit-Hartenberg parameters.

Parameters:

theta	vector to write in angle about z-axis
d	vector to write in distance along x-axis
a	vector to write in distance previous to current along previous x-axis
alpha	vector to write in angle about previous x-axis

Returns:: 1 if successful, -1 if failed

Definition at line 589 of file kinematics.cpp.

int KinematicsLib::getRotDir ( std::vector< int > & rotDir )

Get the rotation direction.

Parameters:

rotationDirection vector to write in rotation direction of the joints

Returns:: 1 if successful, -1 if failed

Definition at line 633 of file kinematics.cpp.

int KinematicsLib::getTcpOff ( std::vector< double > & tcpOffset )

Get the tcp offset.

Parameters:

tcpOffset vector to write in (x, y, z, psi) offset

Returns:: 1 if successful, -1 if failed

Definition at line 710 of file kinematics.cpp.

int KinematicsLib::getType ( void )

Get the robot type.

Get the robot type

Returns:: type: 0 for K_6M90A_F, 1 for K_6M90A_G, 2 for K_6M180, 3 for K_6M90B_F, 4 for K_6M90B_G, -1 for other or not set yet

Definition at line 573 of file kinematics.cpp.

int KinematicsLib::getVersion ( std::vector< int > & version )

Get the version number of the library.

Parameters:

version vector to write in version (major, minor, revision)

Returns:: 1 if successful, -1 if failed

Definition at line 721 of file kinematics.cpp.

int KinematicsLib::init ( )

This initializes the kinematics.

Initialize the kinematics. Parameters mDH, dof, angle offset and angle range (SetType or SetMDH and SetAngleOffset and SetAngleRange) have to be set prior to initialization of the kinematics!

Returns:: 1 if successful, -1 if failed

Definition at line 730 of file kinematics.cpp.

int KinematicsLib::initDofMat ( int dof ) [private]

Definition at line 95 of file kinematics.cpp.

int KinematicsLib::initializeMembers ( ) [private]

Definition at line 63 of file kinematics.cpp.

int KinematicsLib::inverseKinematics	(	std::vector< double >	pose,
		std::vector< double >	prev,
		std::vector< double > &	angles,
		int	maxBisection = `0`
	)

Calculates the inverse kinematics.

Parameters:

pose	pose of the TCP [x, y, z, phi, theta, psi]
prev	previous angles (starting configuration)
angles	resulting robot configuration (joint angles)

Returns:: 1 if successful, -1 if failed

Definition at line 1043 of file kinematics.cpp.

int KinematicsLib::invKin	(	std::vector< double >	pose,
		std::vector< double >	prev,
		std::vector< double > &	angle
	)		`[private]`

Definition at line 897 of file kinematics.cpp.

int KinematicsLib::invKin_bisec	(	std::vector< double >	pose,
		std::vector< double >	prev,
		std::vector< double > &	conf,
		int	maxBisection
	)		`[private]`

Definition at line 940 of file kinematics.cpp.

int KinematicsLib::K4D2mDHAng	(	std::vector< double >	angleK4D,
		std::vector< double > &	angleMDH
	)

This converts angles from Katana4D to mDH convention.

Length of angleK4D needs to be equal to DOM set with Type or mDH parameters.

Parameters:

angleK4D	angle in K4D convention
angleMDH	angle in mDH convention

Returns:: 1 if successful, -1 if failed

Definition at line 740 of file kinematics.cpp.

int KinematicsLib::mDH2K4DAng	(	std::vector< double >	angleMDH,
		std::vector< double > &	angleK4D
	)

This converts angles from mDH to Katana4D convention.

Length of angleMDH needs to be equal to DOM set with Type or mDH parameters.

Parameters:

angleMDH	angle in mDH convention
angleK4D	angle in K4D convention

Returns:: 1 if successful, -1 if failed

Definition at line 776 of file kinematics.cpp.

int KinematicsLib::rad2enc	(	std::vector< double >	angles,
		std::vector< int > &	encoders
	)

Converts angles to encoders.

Parameters:

angles	joint angles
encoders	resulting encoder positions of joints

Returns:: 1 if successful, -1 if failed

Definition at line 826 of file kinematics.cpp.

int KinematicsLib::setAngleMinMax ( ) [private]

Definition at line 78 of file kinematics.cpp.

int KinematicsLib::setAngOff ( std::vector< double > angleOffset )

This sets the angle offsets.

The angles at the calibration stops in mDH convention! Length of angleOffset needs to be equal to DOM set with Type or mDH parameters.

Parameters:

angleOffset angle values at calibration stops in mDH

Returns:: 1 if successful, -1 if failed

Definition at line 450 of file kinematics.cpp.

int KinematicsLib::setAngRan ( std::vector< double > angleRange )

This sets the angle range.

The angle range from angle offset to angle stop in mDH convention (negative if angleOffset > angleStop)! Length of angleRange needs to be equal to DOM set with Type or mDH parameters.

Parameters:

angleRange angle range from angle offset to angle stop

Returns:: 1 if successful, -1 if failed

Definition at line 510 of file kinematics.cpp.

int KinematicsLib::setEncOff ( std::vector< int > encOffset )

This sets the encoder offsets.

The encoder values at the calibration stops. Length of encOffset needs to be equal to DOM set with Type or mDH parameters.

Parameters:

encOffset encoder values at calibration stops

Returns:: 1 if successful, -1 if failed

Definition at line 422 of file kinematics.cpp.

int KinematicsLib::setEPC ( std::vector< int > epc )

This sets the encoders per cycle.

Number of encoders in a full cycle of each joint. Length of epc needs to be equal to DOM set with Type or mDH parameters.

Parameters:

epc	encoders pec cycle

Returns:: 1 if successful, -1 if failed

Definition at line 410 of file kinematics.cpp.

int KinematicsLib::setImmob ( int immobile )

This sets the immobile flag of the last joint.

Flag for the last joint if it is free (0) or locked (1). Setting the immobile flag includes adjusting dom. Type or mDH parameters have to be set.

Parameters:

immobile last joint immobile flag: 0 for free, 1 for locked (immobile)

Returns:: 1 if successful, -1 if failed

Definition at line 393 of file kinematics.cpp.

int KinematicsLib::setLinkLen ( std::vector< double > links )

This sets the link length parameters (part of mDH-parameters)

Setting link length parameters requires the type to be set to 0 (K_6M90A_F), 1 (K_6M90A_G), 2 (K_6M180), 3 (K_6M90B_F) or 4 (K_6M90B_G). This is done using the setType or setModifiedDH function that set the mDH parameters so they can be adjusted. The length of the vector has to be four.

Parameters:

links length of the links in m

Returns:: 1 if successful, -1 if failed

Definition at line 355 of file kinematics.cpp.

int KinematicsLib::setMDH	(	std::vector< double >	theta,
		std::vector< double >	d,
		std::vector< double >	a,
		std::vector< double >	alpha,
		int	typeNr = `-1`
	)

This sets the modified Denavit-Hartenberg parameters.

Transformation from previous to current link: move a along (previous) x-axis, rotate alpha about (previous) x-axis, move d along (current) z-axis, rotate theta about (current) z-axis. Setting the mDH parameters includes setting dof, adjusting immobile flag (all free) and dom (= dof).

The length of the vectors need to be the same.

Parameters:

theta	angle about z-axis
d	distance along x-axis
a	distance previous to current along previous x-axis
alpha	angle about previous x-axis
typeNr	0: K_6M90A_F, 1: K_6M90A_G, 2: K_6M180, 3: K_6M90B_F, 4: K_6M90B_G, -1 for other

Returns:: 1 if successful, -1 if failed

Definition at line 323 of file kinematics.cpp.

int KinematicsLib::setRotDir ( std::vector< int > rotDir )

This sets the rotation direction.

The rotation direction of the joints: +1 if encoders and angles (mDH convention) grow in same direction, -1 if encoders and angles grow in opposit direction. Length of rotDir needs to be equal to DOM set with Type or mDH parameters.

Parameters:

rotDir rotation direction of the joints