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. More...

int	enc2rad (std::vector< int > encoders, std::vector< double > &angles)
	Converts encoders to angles. More...

int	getAngMax (std::vector< double > &angleMax)
	Get angle max. More...

int	getAngMin (std::vector< double > &angleMin)
	Get angle min. More...

int	getAngOff (std::vector< double > &angleOffset)
	Get the angle offsets. More...

int	getAngRan (std::vector< double > &angleRange)
	Get the angle range. More...

int	getAngStop (std::vector< double > &angleStop)
	Get angle stop. More...

int	getDOF ()
	Get the degree of freedom. More...

int	getDOM ()
	Get the degree of mobility. More...

int	getEncOff (std::vector< int > &encOffset)
	Get the encoder offsets. More...

int	getEPC (std::vector< int > &epc)
	Get the encoders per cycle. More...

int	getImmob ()
	Get the immobile flag of the last joint. More...

int	getMaxDOF ()
	Get the maximum degree of freedom. More...

int	getMDH (std::vector< double > &theta, std::vector< double > &d, std::vector< double > &a, std::vector< double > &alpha)
	Get the modified Denavit-Hartenberg parameters. More...

int	getRotDir (std::vector< int > &rotDir)
	Get the rotation direction. More...

int	getTcpOff (std::vector< double > &tcpOffset)
	Get the tcp offset. More...

int	getType ()
	Get the robot type. More...

int	getVersion (std::vector< int > &version)
	Get the version number of the library. More...

int	init ()
	This initializes the kinematics. More...

int	inverseKinematics (std::vector< double > pose, std::vector< double > prev, std::vector< double > &angles, int maxBisection=0)
	Calculates the inverse kinematics. More...

int	K4D2mDHAng (std::vector< double > angleK4D, std::vector< double > &angleMDH)
	This converts angles from Katana4D to mDH convention. More...

	KinematicsLib ()
	Constructor. More...

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

int	mDH2K4DAng (std::vector< double > angleMDH, std::vector< double > &angleK4D)
	This converts angles from mDH to Katana4D convention. More...

int	rad2enc (std::vector< double > angles, std::vector< int > &encoders)
	Converts angles to encoders. More...

int	setAngOff (std::vector< double > angleOffset)
	This sets the angle offsets. More...

int	setAngRan (std::vector< double > angleRange)
	This sets the angle range. More...

int	setEncOff (std::vector< int > encOffset)
	This sets the encoder offsets. More...

int	setEPC (std::vector< int > epc)
	This sets the encoders per cycle. More...

int	setImmob (int immobile)
	This sets the immobile flag of the last joint. More...

int	setLinkLen (std::vector< double > links)
	This sets the link length parameters (part of mDH-parameters) More...

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. More...

int	setRotDir (std::vector< int > rotDir)
	This sets the rotation direction. More...

int	setTcpOff (std::vector< double > tcpOffset)
	This sets the tcp offset. More...

int	setType (int type)
	This sets the robot type. More...

	~KinematicsLib ()
	Destructor. More...

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. More...

double	_angleMax [MaxDof]
	Angle max. More...

double	_angleMin [MaxDof]
	Angle min. More...

double	_angleOffset [MaxDof]

double	_angleRange [MaxDof]

bool	_angOffInit
	Angle offset. More...

bool	_angRanInit
	Angle range. More...

Matrix	_data

int	_dof
	Degree of freedom. More...

int	_dom
	Degree of mobility. More...

int	_encoderOffset [MaxDof]
	Encoder offset. More...

int	_epc [MaxDof]
	Encoder per cycle. More...

int	_immobile
	Last joint is immobile (eg. K_6M90A_G, K_6M90B_G), _dom = _dof - 1. More...

bool	_initialized
	Kinematic initialized. More...

double	_linkLength [4]
	Link length (for defined types) More...

bool	_matrixInit
	Matrices for mDH, angle range and immobile data. More...

mRobot	_robot
	Roboop robot class. More...

int	_rotDir [MaxDof]
	Rotation direction. More...

double	_tcpOffset [4]
	TCP offset. More...

double	_thetaimmobile

int	_type
	Robot Type. More...

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

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

Returns: 1 if successful, -1 if failed

Definition at line 434 of file kinematics.cpp.

int KinematicsLib::setTcpOff ( std::vector< double > tcpOffset )

This sets the tcp offset.

Offset from the flange to the effective tcp.

Parameters

tcpOffset (x, y, z, psi) offset in m and rad respectively where psi is rotation about x-axis of tool coordinate system

Returns: 1 if successful, -1 if failed

Definition at line 561 of file kinematics.cpp.

int KinematicsLib::setType ( int type )

This sets the robot type.

Setting robot type includes setting mDH parameters, immobile flag, degree of freedom (dof), degree of mobility (dom) and angles.

type dof dom K_6M90A_F 6 6 K_6M90A_G 6 5 K_6M180 5 5 K_6M90B_F 6 6 K_6M90B_G 6 5

Parameters

type	0: K_6M90A_F, 1: K_6M90A_G, 2: K_6M180, 3: K_6M90B_F, 4: K_6M90B_G

Returns: 1 if successful, -1 if failed

Definition at line 118 of file kinematics.cpp.

int KinematicsLib::sign ( int value )

private

Definition at line 27 of file kinematics.cpp.

int KinematicsLib::sign ( double value )

private

Definition at line 30 of file kinematics.cpp.

Member Data Documentation

AnaGuess::Kinematics* KinematicsLib::_anaGuess

private

Analytical guess.

Definition at line 216 of file kinematics.h.

double KinematicsLib::_angleMax[MaxDof]

private

Angle max.

Definition at line 207 of file kinematics.h.

double KinematicsLib::_angleMin[MaxDof]

private

Angle min.

Definition at line 205 of file kinematics.h.

double KinematicsLib::_angleOffset[MaxDof]

private

Definition at line 200 of file kinematics.h.

double KinematicsLib::_angleRange[MaxDof]

private

Definition at line 203 of file kinematics.h.

bool KinematicsLib::_angOffInit

private

Angle offset.

Definition at line 199 of file kinematics.h.

bool KinematicsLib::_angRanInit

private

Angle range.

Definition at line 202 of file kinematics.h.

Matrix KinematicsLib::_data

private

Definition at line 187 of file kinematics.h.

int KinematicsLib::_dof

private

Degree of freedom.

Definition at line 189 of file kinematics.h.

int KinematicsLib::_dom

private

Degree of mobility.

Definition at line 191 of file kinematics.h.

int KinematicsLib::_encoderOffset[MaxDof]

private

Encoder offset.

Definition at line 195 of file kinematics.h.

int KinematicsLib::_epc[MaxDof]

private

Encoder per cycle.

Definition at line 193 of file kinematics.h.

int KinematicsLib::_immobile

private

Last joint is immobile (eg. K_6M90A_G, K_6M90B_G), _dom = _dof - 1.

Definition at line 213 of file kinematics.h.

bool KinematicsLib::_initialized

private

Kinematic initialized.

Definition at line 218 of file kinematics.h.

double KinematicsLib::_linkLength[4]

private

Link length (for defined types)

Definition at line 209 of file kinematics.h.

bool KinematicsLib::_matrixInit

private

Matrices for mDH, angle range and immobile data.

Definition at line 186 of file kinematics.h.

mRobot KinematicsLib::_robot

private

Roboop robot class.

Definition at line 211 of file kinematics.h.

int KinematicsLib::_rotDir[MaxDof]

private

Rotation direction.

Definition at line 197 of file kinematics.h.

double KinematicsLib::_tcpOffset[4]

private

TCP offset.

Definition at line 220 of file kinematics.h.

double KinematicsLib::_thetaimmobile

private

Definition at line 214 of file kinematics.h.

int KinematicsLib::_type

private

Robot Type.

Definition at line 184 of file kinematics.h.

The documentation for this class was generated from the following files:

Public Member Functions

Private Member Functions

Private Attributes

Detailed Description

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation