This class provides the utilities for wrist operation including kinematics and calibration. More...

#include <WristMechanism.h>

Classes
struct	Partials
Public Member Functions
void	applyLimits (double &pitch, double &yaw)
	applies limits to a given pitch and yaw
void	calWrist (Eigen::Vector2d encoder, Eigen::Vector2d calSliderPos, Eigen::Vector2d angle)
	the routine used to calibrate the wrist encoders
Eigen::Vector2d	getAngleFromSlider (Eigen::Vector2d slider)
	gets the current angle (pitch and yaw) given the current slider pos (in mm) using fwd kinematics
void	getLimits (double pitch, double yaw, double &upperPitch, double &lowerPitch, double &upperYaw, double &lowerYaw)
	gets the limits on pitch and yaw based on current pitch and yaw using the shortest distance to within limits
Eigen::Vector2d	getSliderFromAngle (Eigen::Vector2d ang)
	gets the slider position from the current angle using inv kinematics
Eigen::Vector2d	getSliderFromWristEncoder (Eigen::Vector2d encoder)
	gets the slider value from the current encoder value
Eigen::Vector2d	getWristEncoderFromSlider (Eigen::Vector2d slider)
	gets the encoder value from the current slider position
void	loadDesignParams (Eigen::Vector3d A, Eigen::Vector3d A0, Eigen::Vector3d B, Eigen::Vector3d C, Eigen::Vector3d D, Eigen::Vector3d D0, Eigen::Vector3d Pitch, Eigen::Vector3d Yaw, Eigen::Vector3d M, Eigen::Vector3d N, double linkLength)
	loads the physical design parameters of the wrist
Eigen::Vector2d	NewtonsMethod (Eigen::Vector2d ang, Eigen::Vector2d pos)
	performs Euler predition and Newton correction by solving a local linearization of the problem
void	setAngleOffsetGain (Eigen::Vector2d angleOffset, Eigen::Vector2d angleGain)
	sets the angle offset and gain when calculating getAngleFromWristEncoder
void	setLimits (const double &upperPitchMin, const double &upperPitchMax, const double &lowerPitchMin, const double &lowerPitchMax, const double &upperYawMin, const double &upperYawMax, const double &lowerYawMin, const double &lowerYawMax)
	sets the overall limits of the task space
void	setSliderOffsetGain (Eigen::Vector2d sliderOffset, Eigen::Vector2d sliderGain)
	sets the slider offset and gain for calculating encoder-slider relationship
void	tareWristEncoders (Eigen::Vector2d encoder)
	records the current encoder value as tared value
	WristMechanism ()
virtual	~WristMechanism ()
Public Attributes
double	eps
	the maximum difference between NewtonsMethod solutions before stopping
bool	isCalibrated
	true if calWrist has been called and successfully completed
int	maxIt
	the maximum number of iterations to perform NewtonsMethod
Protected Types
enum	RotationType { ONE, SIN, COS }
Protected Member Functions
Eigen::Matrix3d	AA2Rot (Eigen::Vector3d axis, RotationType index)
Eigen::Matrix3d	AA2Rot (Eigen::Vector3d axis, double theta)
int	doFwdKin (Eigen::Vector2d &ang, Eigen::Vector2d pos, Eigen::Vector2d lastang)
	calculates the forward kinematics
int	doInvKin (Eigen::Vector2d &pos, Eigen::Vector2d ang)
	calculates the inverse kinematics
Eigen::Matrix3d	findFwdKinMatrix (Eigen::Vector3d a, Eigen::Vector3d b, Eigen::Vector3d u, double q)
Partials	findPartialDerivatives (Eigen::Vector3d a, Eigen::Vector3d b, Eigen::Vector3d u, double q, Eigen::Vector2d ang)
void	getLineFromPoints (double x1, double x2, double y1, double y2, double &m, double &b)
	calculates a line equation from given points (x1, y1) (x2, y2)
void	getNearest (double m, double b, double x_in, double y_in, double &x_out, double &y_out)
	gets the closest point on a line from a given point
Protected Attributes
Eigen::Vector3d	alpha
Eigen::Vector2d	angleGain
Eigen::Vector2d	angleOffset
Eigen::Vector3d	beta
double	bLowerPitchLowerYaw
double	bLowerPitchUpperYaw
double	bUpperPitchLowerYaw
double	bUpperPitchUpperYaw
Eigen::Vector2d	calSliderPos
Eigen::Vector3d	delta
Eigen::Vector2d	encoderTarePos
Eigen::Vector3d	gamma
Eigen::Vector2d	lastAng
double	lengthSq
double	linkLength
std::string	logCategory
double	lowerPitchMax
double	lowerPitchMin
double	lowerYawMax
double	lowerYawMin
Eigen::Vector3d	m
double	mLowerPitchLowerYaw
double	mLowerPitchUpperYaw
double	mUpperPitchLowerYaw
double	mUpperPitchUpperYaw
Eigen::Vector3d	n
Eigen::Vector3d	p
Eigen::Matrix3d	R1m
Eigen::Matrix3d	R1n
Eigen::Matrix3d	Rcm
Eigen::Matrix3d	Rcn
Eigen::Matrix3d	Rsm
Eigen::Matrix3d	Rsn
Eigen::Vector2d	sliderGain
Eigen::Vector2d	sliderOffset
Eigen::Vector3d	u
double	upperPitchMax
double	upperPitchMin
double	upperYawMax
double	upperYawMin
Eigen::Vector3d	v

Detailed Description

This class provides the utilities for wrist operation including kinematics and calibration.

Definition at line 23 of file WristMechanism.h.

Member Enumeration Documentation

enum WristMechanism::RotationType [protected]

Enumerator:

ONE
SIN
COS

Definition at line 100 of file WristMechanism.h.

Constructor & Destructor Documentation

WristMechanism::WristMechanism ( )

Definition at line 19 of file WristMechanism.cpp.

WristMechanism::~WristMechanism ( ) [virtual]

Definition at line 27 of file WristMechanism.cpp.

Member Function Documentation

Eigen::Matrix3d WristMechanism::AA2Rot	(	Eigen::Vector3d	axis,
		RotationType	index
	)		`[protected]`

Eigen::Matrix3d WristMechanism::AA2Rot	(	Eigen::Vector3d	axis,
		double	theta
	)		`[protected]`

void WristMechanism::applyLimits	(	double &	pitch,
		double &	yaw
	)

applies limits to a given pitch and yaw

Parameters:

pitch	limited pitch
yaw	limited yaw

Definition at line 262 of file WristMechanism.cpp.

void WristMechanism::calWrist	(	Eigen::Vector2d	encoder,
		Eigen::Vector2d	calSliderPos,
		Eigen::Vector2d	angle
	)

the routine used to calibrate the wrist encoders

Parameters:

encoder	current relative position encoder reading for the wrist actuators
sliderPos	output from the inverse kinematics (getSliderFromAngle). This signal is updated when the wrist is calibrated and gives the position of the sliders (in encoder counts) based on halls
angle	the angle input into the inverse kinematics to get the slider pos (or known value)

Note:: encoderTared relative position of the encoder from where it was last tared; taredWristEncoderPos current absolute position of the sliders in counts; calSliderPos current absolute position of the sliders in millimeters, where zero is all the way in; angle current wrist angle in rad

Definition at line 817 of file WristMechanism.cpp.

int WristMechanism::doFwdKin	(	Eigen::Vector2d &	ang,
		Eigen::Vector2d	pos,
		Eigen::Vector2d	lastang
	)		`[protected]`

calculates the forward kinematics

Parameters:

ang	the output angle (pitch yaw) of wrist
pos	the input slider position
lastang	the previous angle (final step on reducing solution)

Returns:: 0 for success

Definition at line 326 of file WristMechanism.cpp.

int WristMechanism::doInvKin	(	Eigen::Vector2d &	pos,
		Eigen::Vector2d	ang
	)		`[protected]`

calculates the inverse kinematics

Parameters:

ang	the input angle (pitch yaw) of wrist
pos	the output slider position

Returns:: 0 for success

Definition at line 612 of file WristMechanism.cpp.

Matrix3d WristMechanism::findFwdKinMatrix	(	Eigen::Vector3d	a,
		Eigen::Vector3d	b,
		Eigen::Vector3d	u,
		double	q
	)		`[protected]`

Definition at line 568 of file WristMechanism.cpp.

WristMechanism::Partials WristMechanism::findPartialDerivatives	(	Eigen::Vector3d	a,
		Eigen::Vector3d	b,
		Eigen::Vector3d	u,
		double	q,
		Eigen::Vector2d	ang
	)		`[protected]`

Definition at line 989 of file WristMechanism.cpp.

Vector2d WristMechanism::getAngleFromSlider ( Eigen::Vector2d slider )

gets the current angle (pitch and yaw) given the current slider pos (in mm) using fwd kinematics

Parameters:

slider the current slider value

Returns:: the current angle (pitch and yaw)

Exceptions:

runtime_error if fwd kinematic solution does not converge

Definition at line 671 of file WristMechanism.cpp.

void WristMechanism::getLimits	(	double	pitch,
		double	yaw,
		double &	upperPitch,
		double &	lowerPitch,
		double &	upperYaw,
		double &	lowerYaw
	)

gets the limits on pitch and yaw based on current pitch and yaw using the shortest distance to within limits

Parameters:

pitch	limited pitch
yaw	limited yaw
upperPitch	upper pitch limit
lowerPitch	lower pitch limit
upperYaw	upper yaw limit
lowerYaw	lower yaw limit

limit pitch and yaw to outer box

Definition at line 205 of file WristMechanism.cpp.

void WristMechanism::getLineFromPoints	(	double	x1,
		double	x2,
		double	y1,
		double	y2,
		double &	m,
		double &	b
	)		`[protected]`

calculates a line equation from given points (x1, y1) (x2, y2)

Definition at line 308 of file WristMechanism.cpp.

void WristMechanism::getNearest	(	double	m,
		double	b,
		double	x_in,
		double	y_in,
		double &	x_out,
		double &	y_out
	)		`[protected]`

gets the closest point on a line from a given point

Parameters:

m	line equation gain
b	line equation offset
x_in	x point to get nearest location from
y_in	y point to get nearest location from
x_out	resulting closest point on line
y_out	resulting closest point on line

Definition at line 299 of file WristMechanism.cpp.

Vector2d WristMechanism::getSliderFromAngle ( Eigen::Vector2d ang )

gets the slider position from the current angle using inv kinematics

Parameters:

ang	the current angle of the wrist (either from halls or fwd kinematics)

Returns:: the current slider position (in mm)

Exceptions:

runtime_error if inv kinematic solution not solvable

Definition at line 759 of file WristMechanism.cpp.

Vector2d WristMechanism::getSliderFromWristEncoder ( Eigen::Vector2d encoder )

gets the slider value from the current encoder value

Parameters:

encoder the current encoder value

Returns:: the current slider position (in mm)

Definition at line 695 of file WristMechanism.cpp.

Vector2d WristMechanism::getWristEncoderFromSlider ( Eigen::Vector2d slider )

gets the encoder value from the current slider position

Parameters:

slider the current slider position (in mm)

Returns:: the current encoder value;

Definition at line 719 of file WristMechanism.cpp.

void WristMechanism::loadDesignParams	(	Eigen::Vector3d	A,
		Eigen::Vector3d	A0,
		Eigen::Vector3d	B,
		Eigen::Vector3d	C,
		Eigen::Vector3d	D,
		Eigen::Vector3d	D0,
		Eigen::Vector3d	Pitch,
		Eigen::Vector3d	Yaw,
		Eigen::Vector3d	M,
		Eigen::Vector3d	N,
		double	linkLength
	)

loads the physical design parameters of the wrist

Note:: here might be a good place to put in vectors that become negative because they are left vs right

Parameters:

A	outer arm forward position
A0	origin of A (or point along axis of A) -- known point for calibration
B	outer wrist position
C	inner wrist position
D	inner forward arm forward position
D0	origin of D (or point along axis of D) -- known point for calibration
Pitch	origin of pitch axis
Yaw	origin of yaw axis
PalmCenter	center point of palm
M	axis about which the pitch rotates (most likely y, unless frames have changed)
N	axis about which the yaw rotates (most likey x, unless frames have changed)
maxTravel	the amount of distance an actuator can travel (in mm)
linkLength	the length of a link (A-B) or (C-D)

Exceptions:

runtime_error	if A - A0 has no normal
runtime_error	if D - D0 has no normal

Definition at line 77 of file WristMechanism.cpp.

Vector2d WristMechanism::NewtonsMethod	(	Eigen::Vector2d	ang,
		Eigen::Vector2d	pos
	)

performs Euler predition and Newton correction by solving a local linearization of the problem

Parameters:

ang	the output joint angles
pos	the input slider values

Returns:: the correction to the joint angles

Definition at line 971 of file WristMechanism.cpp.

void WristMechanism::setAngleOffsetGain	(	Eigen::Vector2d	angleOffset,
		Eigen::Vector2d	angleGain
	)

sets the angle offset and gain when calculating getAngleFromWristEncoder

Parameters:

angleOffset	the offset of the angle
angelGain	the gain of the angle

Definition at line 49 of file WristMechanism.cpp.

void WristMechanism::setLimits	(	const double &	upperPitchMin,
		const double &	upperPitchMax,
		const double &	lowerPitchMin,
		const double &	lowerPitchMax,
		const double &	upperYawMin,
		const double &	upperYawMax,
		const double &	lowerYawMin,
		const double &	lowerYawMax
	)

sets the overall limits of the task space

Parameters:

upperPitchMin	min upper limit of pitch
upperPitchMax	max upper limit of pitch
lowerPitchMin	min lower limit of pitch
lowerPitchMax	max lower limit of pitch
upperYawMin	min upper limit of yaw
upperYawMax	max upper limit of yaw
lowerYawMin	min lower limit of yaw
lowerYawMax	max lower limit of yaw