Class for control of the threedom robot (located in QS33, lab1) with use of grasp endpoint device (2D) More...

#include <ManipulandumRed2D.h>

Public Member Functions
	ManipulandumRed2D (void)
	Constructor.
	~ManipulandumRed2D ()
	Destructor.
void	init (string paramfile)
	initialize the manipulandum with parameter file
void	update ()
	Updates the sensory state of the robot.
void	update (double dt)
	Updates the sensory state of the robot for time dt.
void	recenter ()
	Put (0,0) of world at current position.
void	recenter (const Vector2D &)
	Put (0,0) of global coordinates at the specified position.
void	getCounter (long &sh, long &el, long &wr)
	Get the raw counter values.
void	getPosition (Vector2D &thePos)
	Get Positions.
Vector2D	getPosition ()
	Get Position.
void	getVelocity (Vector2D &theVel)
	Get Velocities.
Vector2D	getVelocity ()
	Get Velocity.
void	getAngles (Vector3D &theAngles)
	Get angles of the Shoulder and Elbow.
Vector3D	getAngles ()
void	setForce (Vector2D theForce)
	Get the Shoulder and Elbow angle in radians.
void	setTorque (Vector3D theTorque)
	Sets motor torques.
void	setVolts (double v0, double v1, double v2)
	sets voltages to shoulder and elbow directly
void	getLocalFTForce (Vector3D &theFTForce)
	Getforce measured by ft in sensor frame.
Vector3D	getLocalFTForce ()
	Getforce measured by ft in sensor frame.
void	getGlobalFTForce (Vector3D &theFTForce)
Vector3D	getGlobalFTForce ()
	Get force measured by ft in global frame.
void	enableWristControl ()
	Activates PID contol of wrist motor to align ft sensor (Will cause brief drop in force output when called for safety)
void	disableWristControl ()
	Deactivates PID contol of wrist motor to align ft sensor (Will cause brief drop in force output when called for safety)
void	enableOutput ()
	Master enable for Robot force output (Will cause brief drop in force output when called for safety)
void	disableOutput ()
	Master disable for Robot force output (Will cause brief drop in force output when called for safety)
void	printState (int col=0)
	prints the state of a robot on the text display
Public Attributes
Vector2D	position
	robot values that can be read
Vector2D	velocity
	Current velocity, unfiltered in m/s.
Vector2D	positionFilt
	Position after Kalmanfiltering.
Vector2D	velocityFilt
	Velocity estimate after Kalman filtering.
Vector2D	force
	Current Force Goal in global cooridinates.
Vector2D	forceProd
	Current estimate of force produced in global coordinates.
double	currentRate
	Current servo cycle rate.
Vector2D	motorPos [2]
	Position of motors (local coordinates)
Vector2D	elbowPos [2]
	Position of elbows (local coordinates)
Vector2D	elbow_angles [2]
	Angle of elbows (local coordinates)
double	safeVoltGainOut
	public copy of safety gain ramp on final output voltage
Vector3D	torqueDemandedOut
	public copy of current output torque
int	outputEnableOut
	public copy of Master output enable for all motors (1=enabled, 0= disabled)
int	hardEnableOut
	public copy of status of hard enable flag for motors (1=enabled, 0= disabled)
int	hardLimitOut
	public copy of status of hard limti switches flag for motors (1=at limit, 0= in range)
double	wristASetAngle
	Wrist angle set point.

Detailed Description

Class for control of the threedom robot (located in QS33, lab1) with use of grasp endpoint device (2D)

Function init is called with a parameter file that contains the data from the Calibration. Then you only have to call update() in regular intervals in the updateHaptics() loop, and you have access to estimates of position and velocity. Manipulandum uses a KalmanFilter for smoothing.

Author:: Nick Roach, Joern Diedrichsen, Julius Klein 2011

Constructor & Destructor Documentation

ManipulandumRed2D::ManipulandumRed2D ( void )

Constructor.

The Constructor really doesn't do anything

ManipulandumRed2D::~ManipulandumRed2D ( )

Destructor.

Destructor does nothing Should probably free the channels allocated on the s626 board

Member Function Documentation

void ManipulandumRed2D::init ( string paramfile )

initialize the manipulandum with parameter file

Reads a configuration file for the robot and initializes io channels

Parameters:

paramfile Parameter files The file contains

Board for Shoulder Encoder
Channel for Shoulder Encoder
Direction for Shoulder Encoder
Board for Elbow Encoder
Channel for Elbow Encoder
Direction for Elbow Encoder
Board for Wrist Encoder
Channel for Wrist Encoder
Direction for Wrist Encoder
Board for DA-piston forces
Offset for Shoulder encoder [deg]
Offset for Elbow Enconder [deg]
Offset for Wrist Enconder [deg]
Upper arm left [cm]
Upper arm Right [cm]
Lower arm Left [cm]
Lower arm Right [cm]
Robot distance [cm]
Length of wrist[cm]
Tranformation Nm to V at shoulder
Tranformation Nm to V at elbow
Tranformation Nm to V at wrist
Angle of ATI transducer to arm
Shoulder safe volt limit
Wrist safe volt limit
Right Shoulder -ive Limit Angle [Deg]
Right Shoulder +ive Limit Angle [Deg]
Left Shoulder -ive Limit Angle [Deg]
Left Shoulder +ive Limit Angle [Deg]
Wrist -ive Limit Angle [Deg]
Wrist +ive Limit Angle [Deg]
End point -x envelope limit [cm]
End point +x envelope limit [cm]
End point -y envelope limit [cm]
End point +y envelope limit [cm]

void ManipulandumRed2D::update ( double dt )

Updates the sensory state of the robot for time dt.

Main routine that does all the heavy work with update frequency

Parameters:

dt	time in ms since last update

void ManipulandumRed2D::recenter ( )

Put (0,0) of world at current position.

Set the current position of the robot to be (0,0)

void ManipulandumRed2D::recenter ( const Vector2D & x )

Put (0,0) of global coordinates at the specified position.

Set the current position of the robot to be (0,0)

void ManipulandumRed2D::getCounter	(	long &	rSh,
		long &	lSh,
		long &	wr
	)

Get the raw counter values.

Gets the raw counter values by reference

Parameters:

sh	this is where the shoulder counter will be dumped
el	this is where the elbow counter will be dumped

void ManipulandumRed2D::getPosition ( Vector2D & thePos )

Get Positions.

Gets the x and y end point position in cm via forwardKinePos

Parameters:

thePos 2D Vector by which co-ordinates are passed

void ManipulandumRed2D::getVelocity ( Vector2D & theVel )

Get Velocities.

Gets the Kalman filter estimated x and y end effector velocity in cm/s

Parameters:

thePos 2D Vector by which velocity is passed

void ManipulandumRed2D::getAngles ( Vector3D & theAngles )

Get angles of the Shoulder and Elbow.

Gets the calibrated shoudler and wrist motor angles in radians

Parameters:

theAngles 3D Vector by which angles are passed

void ManipulandumRed2D::setForce ( Vector2D theForce )

Get the Shoulder and Elbow angle in radians.

Sets endpoint forces in Euclidean cooridinates #

Calculates the force output for the robot in Eucledian coordinates using the current state of the robot. This needs to be called again as position of the robot changes. see Jacobian for details of method. Output overidden by calls to setTorque

Parameters:

force Vector of desired forces in N

void ManipulandumRed2D::setTorque ( Vector3D torque )

Sets motor torques.

Sets the torque output for each motor in Nm Output overidden by calls to setForce

Parameters:

torque 3D Vector of desired motor torques

void ManipulandumRed2D::setVolts	(	double	v0,
		double	v1,
		double	v2
	)

sets voltages to shoulder and elbow directly

Sets the voltage output to the Motors directly Actual output is subject to voltage and postion limits imposed by software/hardware

Parameters:

v0	volts to the right shoulder motor
v1	volts to the left shoulder motor
v2	volts to the wrist motor

void ManipulandumRed2D::getLocalFTForce ( Vector3D & theFTForce )

Getforce measured by ft in sensor frame.

Current calibrated force measurements from ATI transducer in sensor frame (N)

Parameters:

theForce 3D Vector by which force data is passed

Vector3D ManipulandumRed2D::getLocalFTForce ( )

Getforce measured by ft in sensor frame.

Current calibrated force measurements from ATI transducer in sensor frame (N)

Returns:: 3D vector giving force in sensor coordinates

void ManipulandumRed2D::getGlobalFTForce ( Vector3D & theFTForce )

Current calibrated force measurements from ATI transducer in glocal robot frame (N) (sensor frame rotated by wrist angle)

Parameters:

theForce 3D Vector by which force data is passed

Vector3D ManipulandumRed2D::getGlobalFTForce ( )

Get force measured by ft in global frame.

Get force measured by ft in global frame

Current calibrated force measurements from ATI transducer in glocal robot frame (N) (sensor frame rotated by wrist angle)

Returns:: theForce 3D Vector giving force in robot coordinates

void ManipulandumRed2D::enableWristControl ( )

Activates PID contol of wrist motor to align ft sensor (Will cause brief drop in force output when called for safety)

Enable for wrist angle contoller, deramps output for safety when called

void ManipulandumRed2D::disableWristControl ( )

Deactivates PID contol of wrist motor to align ft sensor (Will cause brief drop in force output when called for safety)

Disable for wrist angle contoller, deramps output for safety when called

void ManipulandumRed2D::enableOutput ( )

Master enable for Robot force output (Will cause brief drop in force output when called for safety)

Master enable function for motors, deramps output for safety when called

void ManipulandumRed2D::disableOutput ( )

Master disable for Robot force output (Will cause brief drop in force output when called for safety)

Master disable function for motors, deramps output for safety when called Forces all DAC outputs to 0v

void ManipulandumRed2D::printState ( int col = 0 )

prints the state of a robot on the text display

Needs a textdisplay to be effective.

Parameters:

col	column in the text display that will be used.

Member Data Documentation

Vector2D ManipulandumRed2D::position

robot values that can be read

global position, follwing recentering etc.

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

ManipulandumRed2D.h
ManipulandumRed2D.cpp

Public Member Functions

Public Attributes

Detailed Description

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation