

		     ARC: A Robotics oriented Calculator
		     ____ _ ________ ________ __________


				 John Lloyd
			      McGill University
		  Research Center for Intelligent Machines
				 Summer 1987


	ARC is a calculator program that evaluates infix expressions
	and allows the assigning of variables. Integer, floating point, and 
	matrix data types are supported, along with a set of operations
	specific to robotics applications. The program contains elements
	of an infix calculator, MATLAB, and RCCL.

	This manual is divided into the following sections:


			1. Overview
			2. Matrices
			3. Operators
			4. Basic scalar and math functions
			5. Matrix and vector functions
			6. Functions related to robotics
			7. Robot kinematic routines
			8. System utility functions		
			9. Built in constants


    1. 	OVERVIEW

    1.1 Statements

	Statements are typed into ARC on a line by line basis. 

	A statement consists of an expression which is evaluated. Expressions
	are created with the standard operators '+', '-', '*', '/', '%' 
	(modulo), and '^' (exponent), and are associated with the usual 
	precedence and syntax rules (similar to the language C). 
	Expressions may be nested using parentheses '(' ')'. 

	The result of the statement is printed after the line has been 
	entered.

	Example:

		ARC-0-> 4 + (5.0* 2^3)

		         44

	Functions may be be invoked using an expression of the form

		function '(' arglist ')'

	where `arglist' is a list of expressions separated by commas.
	The available functions are described later.

    1.2 Help

	Help may be obtained with the commands "help" or "?".

	Help on a specific function may be obtained with the command
	"help <function>". 

	The command "help key <keyword>" searches the documentation for
	descriptions containing the indicated keyword and prints them.
	This is equivalent to the UNIX command "man -k <keyword>".

    1.3 Variables and assignments

	The result of any expression may be assigned to a variable. Variable
	identifiers follow the usual convention of being strings of 
	alpha-numerics and underscores that do not begin with a digit.
	Names are case sensitive.

	As in the language C, an assignment action is itself an expression, 
	returning the value that was assigned.

	Example:

		ARC-0-> b = (a = 4.5 - 5.6) * 2

			-2.2000
	
    1.4 Statement variables

	The variable "$" is special: it always contains the value of the
	last statement. 

	Variables "$<n>", where 'n' is an integer, are also special; they 
	contain the value returned by the statement number 'n' that appears 
	in the prompt ("ARC-<n>->").

	Example:
	
		ARC-10-> 4.5 + 2
	
			6.5

		ARC-11-> $ + 2

			8.5

		ARC-12-> $10 + $11

		       15.0

	The statement numbers wrap around after they reach a certain limit
	(currently 100).

    1.5 Integer vs. Floating Point

	ARC uses both integer and floating point types. Floating point 
	literals are distinguished from integers by the usual convention of
	including an exponent and/or a decimal point. 

	Integers are converted to float whenever other floats are encountered
	or required in an expression, and vice versa.

	The notion of keeping integers and floats separate is a bit of a
	kludge since the types are converted so often and so easily. The
	reason it is done is to make it possible to maintain certain
	integer specific operations such as modulo and bit operations
	(although bit operations have not yet been implemented).	

    1.6 Other data types.

	The other principle data type that ARC supports is matrices, which
	are described in a separate section.

	There is some (marginal) support for strings. String literals are
	described in the usual way by encasing them within quotes ("),
	with the quote character itself being described by the two character
	sequence \". About the only things you can do with strings at the 
	moment is define and assign them. They are used mainly to specify
	input/ouput formats and file names.

	Example:
		
		ARC-0-> a = "string ..."

			 string ...

    1.7 Input and output formats.

	Integers may be read or printed in any base from 2 through 16.
	The bases are associated with the following format strings:

		FORMAT CODE		DESCRIPTION

		    "<n>"	base n, where 2 <= n <= 16.
		    "b"		binary (base 2)
		    "o"		octal (base 8)
		    "d"		decimal (base 10)
		    "x"		hex (base 16)

	The default input base for integers is, predicatably, 10. An integer
	may be entered in a different base using the following syntax:

				<format>#<integer>

	The format string should not be enclosed in quotes.

	Example:

		ARC-0-> 2#10101

			 21

		ARC-1-> x#ffff
	
	 		   65535

	The default input base may be changed using the function input(),
	which takes a format string as an argument (it IS necessary to
	enclose this format string in quotes).

	Example:

		ARC-0-> input ("8")

			 1
		
		ARC-1-> 17767

		      8183

	The output base with which and integer is printed may be changed
	by preceding a statement with a format specifier according to
	the following syntax:

			<format>#  statement ....

	The format string should not be enclosed in quotes. The default
	output base for integers can be changed by calling the function
	output() with the desired format string (enclosed in quotes).

	Example:

		ARC-0-> 2#  21

		     10101

		ARC-1-> output (8)

			 1

		ARC-2-> 4567

			10727

	If the output base is a power of 2, then the result is printed
	unsigned. Otherwise, the result is printed signed.

	The default input and output bases may be set at the start of the
	program by calling arc with the options -i <base> or -o <base>, 
	respectively, where <base> is an integer 2 to 16. All default integer
	input and/or output will then be done in that base. 
	Both input and output bases can be changed separately from within 
	arc by using the 'input()' and 'output()' functions, described below.

	Floating data types are always read in base 10.
	
	Floating data types are always printed base 10, although there
	are several different printing formats as specified by the following
	format strings:

		FORMAT CODE		DESCRIPTION

		    "f"		floating format, 4 decimal places.
		    "lf"	floating format, 14 decimal places.
		    "e"		exp format, 4 decimal places.
		    "le"	exp format, 15 decimal places.
		    "z"		hexadecimal

	The default floating output code is "f". As with integer output, 
	the output for a float may be changed on a per statement basis
	by preceding the statement with a format specification, or on
	a permanent basis by calling the function output() with the desired
	format.

	Example:

		ARC-0-> 4.5
		
			4.5000

		ARC-1-> lf# 4.5

			4.500000000000000e+00

		ARC-2-> output ("lf")

			1

		ARC-3-> 4.321

			4.32100000000000
	
	If the result of an expression is an integer (or float), then the 
	floating (or integer) output code is ignored.

	The default integer input base 

    1.8 Robotics functions.

	ARC supports a number of operations specific to robotics 
	applications. This primarily includes support for creating
	4X4 homogeneous transforms, and mapping between them and 
	Euler angles, <roll,pitch,yaw> angles, Euler parameters,
	and quaternions. Support is also included for creating 
	transformation matrices which map force and velocity 6-vectors
	between coordinate frames. Detailed descriptions of the robotics
	functions are given in a separate section.

	If arc is compiled with support from the RCI (Robot Control Interface)
	system, then it also supports functions to do robot kinematics
	for robots known to the RCI system. It can also retrieve joint
	positions from the RCI robot position files.

    1.9 System utility functions.

	In addition to the functions input() and output() which modify
	the default IO formats, there are the functions save() and load()
	which save and restore variables in files, and log(), which creates
	a log file of the user interaction. These are detailed in a 
	separate section.

    2.  MATRICES

	ARC supports vectors and matrices (the former are treated simply
	as special cases of the latter). 

	Most of the matrix notation, display conventions, and operations
	have been taken directly from MATLAB, so users of that system should
	be quite at home (MATLAB is a interactive interface to LINPACK
	and EISPACK developed by Cleve Moler at the University of New
	Mexico. It is available at McGill McCRCIM on the VAXes, at the
	moment, by typing MATLAB).

    2.1 Entering Matrices
	
	Matrices are input on a row by row basis. Each row is described
	by a list of expressions separated by commas. Rows are separated
	by either semi-colons or new lines. The entire matrix is enclosed
	with angle brackets '<' '>'.

	Example:

		ARC-0-> a = <1, 2; 3, 4>

			 1     2 
			 3     4

		ARC-1-> a = <1; 2; 3>

			 1
			 2
			 3

	The expressions used in building a matrix may themselves be
	matrices, as long as row and column constraints are observed.

	Example:

		ARC-0-> a = <1, 2; 3, 4>

			 1     2 
			 3     4

		ARC-1-> b = < a, 10*a ; 10*a, a >

			 1     2    10    20
			 3     4    30    40
			10    20     1     2
			30    40     3     4

	When entering matrices, use may be may of the range operator ':'.

	The notation e1 : e2 returns a list of numbers starting with e1
	and proceeding up to (or down to) e2 in increments of 1.0.

	The notation e1 : e2 : e3 returns a list of numbers starting with e1
	and proceeding up to (or down to) e2 in increments of e3.

	Example:

		ARC-0-> < 1:4 >

			 1     2     3     4

		ARC-1-> < 1:7:2 ; 9:15:2 >

			 1     3     5     7
			 9    11    13    15

	The range operator may only be used within a matrix declaration
	(angle brackets), or a subscript (as described below).
	
    2.2	Subscripting Matrices

	Components of matrices may be subscripted with square brackets,
	as in a[1,1].

	In general, each matrix is indexed by two subscripts, one for rows
	and one for columns. Each subscript may be either a scalar or
	a vector, and the subscripting operation is defined to return
	a submatrix whose rows are given by the row subscript and whose
	columns are given by the column subscript.

	Range notation may be used in subscripts, and is particularly 
	useful in specifying submatrices. 

	In a subscript, the range operator ':' by itself denotes all
	possible rows (or columns) associated with the expression.

	Examples:

		ARC-0-> a = <1, 2, 3, 4; 5, 6, 7, 8; 9, 10, 11, 12>

			 1     2     3     4
			 5     6     7     8     
			 9    10    11    12

		ARC-1-> a[3,3]

			11

		ARC-2-> a[1:2,2:4]

			 2     3     4
			 6     7     8

		ARC-3-> a[:, 3]

			 3
			 7
			11

		ARC-4-> a[<1,3>,<2,4>]
		
			 2     4
			10    12

        If a matrix is a vector (i.e., at least one dimension has size 1),
	then a single subscript may be used. The wild card notation ':'
	would denote the entire vector.

	Example:

		ARC-0-> a = <1,2,3>

			 1     2     3

		ARC-1-> a[3]
		
			 3

    2.4	Matrix Operations.

	Most of the ARC operators have interpretations when applied to 
	matrices.

	'*' denotes matrix multiplication when the expression on
	both sides is a matrix, or scalar multiplication if one
	of the sides is a scalar.

	'+' and '-' denote element by element matrix addition and
	subtraction.

	'/' denotes multiplication by the inverse of the right
	matrix, according to m1 / m2 :== m1 * inv(m2).

	'\' denotes multiplication by the inverse of the left
	matrix, according to m1 / m2 :== inv(m1) * m2.

	The operator ' is used to denote the transpose of a matrix.

	Matrices may be assigned, of course. If the target of the assignment
	is subscripted, then only the portion of the target described by
	the subscript is assigned, and the dimension of the target must
	match the dimension of the assigned value. The expression result
	of such an assignment is equal to the assigned value. If the
	dimension of the target is not large enough to contain the assigned
	value, then the target is enlarged and the unpsecified entires are
	initialized to 0.0.

	Example:

		ARC-0-> a = <1,2;3,4>

			 1     2
			 3     4

		ARC-1-> a[2,3] = 5

			 5

		ARC-2-> a

			 1     2     0
		 	 3     4     5

		ARC-3-> a[<2,3>,:] = <10,20,30; 40,50,60>

			10    20    30
			40    50    60

		ARC-4-> a
	
			 1     2     0
			10    20    30
			40    50    60

    2.5 Matrix Functions

	The matrix functions are described in a separate section. The
	most important are probably inv(), which inverts a matrix, and
	the transpose operator '.

    2.6 Matrix Output.

	Matrices are printed using the same format codes as for integers
	floats. If all of the elements of the matrix can be classed as
	integers (i.e., they contain no fractional part and fit within
	an integer data type), then the matrix is printed according to
	the integer format; otherwise, the floating format is used.

    3.  OPERATORS

	ARC supports several binary and monadic operators on various
	data types.

	+ (usage: e1 + e2) --- addition.

		Performs addition of scalars or matrices. Matrices must
		have matching dimensions.

	- (usage: e1 - e2) --- subtraction.

		Performs subtraction of scalars or matrices. Matrices must
		have matching dimensions.
	
	- (usage: - e1) --- negation.

		Returns the negative of a scalar or a matrix.

	* (usage: e1 * e2) --- multiplication operator.

		If 'e1' and 'e2' are both scalars, the usual scalar
		product is computed.

		If 'e1' and 'e2' are both matrices, then matrix multiplication
		is performed.

	/ (usage: e1 / e2) --- division.

		If 'e1' and 'e2' are both scalars, the usual scalar quotient
		is computed.

		If 'e1' or 'e2' is a matrix and the other argument is
		a scalar, then every element in the matrix is divided
		by the scalar value.

		If 'e1' and 'e2' are both matrices, then the matrix product
		e1 * inv (e2) is computed.

	\ (usage: e1 \ e2) --- special matrix division.

		If 'e1' and 'e2' are both matrices, then the matrix product
		inv(e1) * e2 is computed.

	% (usage: e1 % e2) --- modulo.

		Computes the mod() function for the scalars 'e1' and 'e2'.

	^ (usage: e1 ^ e2) --- power.

		Raises 'e1' to the 'e2' power, where 'e1' and 'e2' are 
		scalars.

	' (usage: e1 ') --- transpose.

		Returns the transpose of the matrix 'e1'.

	: (usage: e1 : e2  or  e1 : e2 : e3) -- range operator.

		The range operator is used to create lists of numbers.
		
		When used with two expressions, the list consists of the
		numbers e1 through e2, with a difference of 1.0 between
		each number.

 			1 : 4  is equivalent to  1, 2, 3, 4

			1 : -2 is equivalent to  1, 0, -1, -2
		
		When used with three expressions, the list consists of
		the numbers e1 through e2, with a difference of e3 
		between them. 

			1 : 6 : 2  is equivalent to 1, 3, 5

		The lists always contain the number e1 as their first
		element, and the last element is never exceeds the value
		of e2.
		
		The range operator works on scalar expressions only. It can
		also be used only in matrix or subscript definitions.

    4.  BASIC SCALAR MATH FUNCTIONS

	sin (d) --- trigonometric sine.

		Returns the sine of the scalar 'd'.

	cos (d) --- trigonometric cosine.

		Returns the cosine of the scalar 'd'.

	tan (d) --- trigonometric tangent.

		Returns the tangent of the scalar 'd'.

	log (d) --- natural logarithm.

		Returns the natural log of the scalar 'd'.

	log10 (d) --- logarithm base 10.

		Returns the log base 10 of the scalar 'd'.

	exp (d)	--- exponential.

		Returns the e^d, where 'd' is a scalar.

	sinh (d) --- hyperbolic sine.

		Returns the hyperbolic sine of the scalar 'd'.

	cosh (d) --- hyperbolic cosine.

		Returns the hyperbolic cosine of the scalar 'd'.

	tanh (d) --- hyperbolic tangent.

		Returns the hyperbolic tangent of the scalar 'd'.

	atan (d) --- inverse trigonometric tangent.

		Returns the inverse tangent of the scalar 'd'.

	asin (d) --- inverse trigonometric sine.

		Returns the inverse sine of the scalar 'd'.

	acos (d) --- inverse trigonometric cosine.

		Returns the inverse cosine of the scalar 'd'.

	sqrt (d) --- square root.

		Returns the square root of the scalar 'd'.

	pow (d1,d2) --- power function.

		Returns the d1^d2, where 'd1' and 'd2' are scalars.

	atan2 (d1,d2) --- double argument inverse trigonometric tangent.

		Returns the angle whose sine and cosine are proportional
		to the scalars 'd1' and 'd2', respectively. The returned 
		angle is in radians and lies in the range -PI < theta <= PI.

	hypot (d1, d2) --- Euclidean distance.

		Returns sqrt(d1^2 + d2^2) for the scalars 'd1' and 'd2'.

    5.  MATRIX AND VECTOR FUNCTIONS

	ident (i) --- produce identity matrix.

		returns an identity matrix of size 'i'.

	zero (i1,i2) --- produce zero matrix.

		returns a zero matrix of size 'i1' X 'i2'.

	inv (m) --- matrix inverse.

		returns the inverse of the matrix 'm'.

	enorm (m) --- Euclidean norm.

		returns the Euclidean norm of the matrix 'm'.

	diag (v) --- produce a diagonal matrix.

		returns a diagonal matrix whose elements are specified
		by the vector 'v'.

	cross (m1,m2) --- compute cross product.

		If 'm1' and 'm2' are both 3 vectors, then the cross product
		m1 X m2 is computed.

		If 'm1' is a three vector and 'm2' is a matrix whose columns
		are all 3 vectors, then for each column j in 'm2' the cross
		product m1 X m2[:,j] is computed and the result is returned
		as a matrix.

		Likewise, if 'm2' is a three vector and 'm1' is a matrix 
		whose columns are all 3 vectors, then for each column j in 
		'm1' the cross product m1[:,j] X m2 is computed and the 
		result is returned as a matrix.

    6.  FUNCTIONS RELATED TO ROBOTICS

	trsl (dx, dy, dz) --- translation parameters to matrix.

		Returns a 4X4 transform describing a translation of
		'dx', 'dy', and 'dz' along the X, Y, and Z axes.
	
	rot (v) OR rot (d, v) --- Euler parameters to rotation matrix.

		Returns a 4X4 transform describing a rotation produced
		by a set of Euler parameters. Euler parameters consist
		of an axis and rotation (in degrees) about that axis. 

		If one 'v' is provided as an argument, it should be
		a 4-vector containing the rotation and the three axis
		components (not necessarily normalized), in that order.

		If 'd' and 'v' are provided as arguments, then 'd' should
		be the rotation and 'v' should be a 3-vector describing the
		axis.

	rpy (v) --- roll, pitch and yaw angles to rotation matrix.

		Returns a 4X4 transform describing the rotation produced by
		the <roll, pitch, yaw> angles stored in the vector 'v'.
		Angles are described in degrees.

	eul (v) --- euler angles to rotation matrix.

		Returns a 4X4 transform describing the rotation produced by
		the euler angles <phi, theta, psi> stored in the vector 'v'.
		Angles are described in degrees.

	q (v) --- quaternion to rotation matrix.

		Returns a 4X4 transform describing the rotation produced by
		the quaternion stored in the 4-vector 'v'. The quaternion
		does not need to be a unit quaternion.

	vao (va, vo) --- 'o' and 'a' vectors to rotation matrix.

		Generates a rotation matrix given the 'o' and 'a' vectors
		(columns 2 and 3). The vectors do not need to be normalized
		OR orthogonal ... the actual column vectors 'n', 'o', 'a'
		are computed according to the following algorithm:

	   			a = a / |a|
	   			n = o X a		
	   			n = n / |n|		
	   			o = a X n		

	getrot (m) --- rotation matrix to Euler parameters.

		Returns a 4-vector describing the Euler parameters for
		the rotational part of the 4X4 transform 'm'. The Euler
		parameters consist of, in order, the rotation in degrees
		about the Euler axis, and the 3 components of the unit
		Euler axis.

	getrpy (m) --- rotation matrix to roll, pitch and yaw angles.

		Returns a vector containing the roll, pitch, and yaw
		angles (in degrees) derived from the rotation part
		of the 4X4 transform 'm'.

	geteul (m) --- rotation matrix to Euler angles.

		Returns a vector containing the Euler angles phi, theta,
		and psi (in degrees) derived from the rotation part
		of the 4X4 transform 'm'.

	getq (m) --- rotation matrix to quaternion.

		Computes a a unit quaternion (represented as a 4-vector) from
		the rotation component of the 4X4 transform 'm'.

	ftrans (m) --- produce force transformation.

		Returns a 6X6 transformation matrix which maps forces from a 
		base coordinate	frame to a frame defined by the 4X4 
		transform 'm'.

	vtrans (m) --- produce velocity transformation.

		Returns a 6X6 transformation matrix which maps velocities
		from a base coordinate	frame to a frame defined by the 4X4 
		transform 'm'.

	trsfToDrive (m) --- produces drive parameters from a transform.

		Returns a 6-vector describing the Paul drive parameters
		associated with the given 4X4 transformation matrix.

	driveToTrsf (m) --- produces transform from drive parameters.

		Returns a 4X4 transformation matrix derived from a 6-vector
	        describing a set of Paul drive parameters.

    7.  ROBOT KINEMATIC ROUTINES

	These routines require that arc be configured with RCI support.

	getpos(r, n) --- get robot joint position by name.
	
		This function checks the RCI position data file for the
		robot named by the string 'r' for a position named 'n',
		and returns it if found. Angles are given in degrees.

	(t6, c) = fwdK (r, a) --- do forward robot kinematics

		This function computes the forward kinematics for the
		robot named by the string 'r' with the set of joint values
		'a'. The function returns the T6 matrix and a string 
		describing the robot configuration. Angles are specified in
		degrees.  

	a = invK (r, t6, c, olda) --- do inverse kinematics.

		This function computes the inverse robot kinematics for the
		robot named by the string 'r', given the T6 matrix and a
		string describing the robot configuration. The argument 'olda'
		is an optional set of joint angles that is used as a reference
		to resolve ambiguitues in the case of singularities or joints
		that are capable of multiple revolutions. If 'olda' is not
		specified, the joint mid-range values are used instead.
		Angles values are given in degrees.

	j = Jacobian (r, a) --- compute robot jacobian.

		This function computes the manipulator Jacobian for the robot
		named by the string 'r', given a set of joint angles 'a'.
		Angles values are given in degrees.

    8.  SYSTEM UTILITY FUNCTIONS

	Functions are provided for creating log files, saving and loading
	variables, and changing input and output formats.

	log (s) --- open a log file.

		Opens a log file whose name is given by 's'. A copy of all
		subsequent output is kept in the log file, until either
		another log file is opened, or the file is closed by calling
		log() with no argument.

	save (s, <arglist> ...) --- write variables into a data file.

		Save the variables indicated in the argument list in the
		file 's'. The created file is one that can be read back 
		with the load() function. The file itself is an ascii file,
		which gives us some measure of portability. 

		Each datum is stored surrounded by brackets '(' ')'.
		The formats are as follows:

			INTEGER:	'(' <variableName> 'integer' 
					    <value> 
					')'

			DOUBLE:		'(' <variableName> 'double' 
					    <value> 
					')'

			MATRIX:		'(' <variableName> 'matrix' 
					    <num-rows> <num-columns>
					    <values> ... 
					')'
		
			STRING:		'(' <variableName> 'string' <length>
					    "<string value>"
					')'

		White space is necessary only to separate fields; otherwise,
		it is ignored (white space includes tabs, blanks, and new 
		lines). Strings are stored between quotes ("), with the
		quote character itself denoted by \".

		If no variables are named, then all symbols are saved in
		the file.
	
	load (s) --- load data file.
		
		Loads the files 's' into ARC. The file must have the same 
		format as one created using the save() function.
		
	input (s) --- change default input format.

		This function takes a string describing an alternate input
		format. Currently, the only input format that can be changed
		is the base for reading integers; the base for reading
		floats is unaffected. Input bases of 2 through 16 are 
		supported. Valid strings include the following:

			"b"	-- base 2
			"o"	-- base 8
			"d"	-- base 10
			"x"	-- base 16
			"<n>"   -- base n, where 2 <= n <= 16.

	output (s) --- change default output format.

		This function takes a string describing an alternate output
		format. The system maintains two types of output formats:
		one for integers, and one for floats. The specifications for
		each are exclusive.

		The integer output format is simply the base in which the
		integer is printed, and the corresponding format strings are
		the same as for the input() function (above).

		The available floating output formats are described by the 
		following strings:

			"f"	-- 4 decimal places of precision.
			"lf"	-- 14 decimal places of precision.
			"e"	-- exponential format, 4 decimal places.
			"le"	-- exponential format, 15 decimal places.
			"z"	-- hexadecimal (machine dependent)

    9.  BUILT IN CONSTANTS

	ARC maintains a few built in constants. These are write protected;
	assigning to these will not affect their contents and will return
	their default value.

	PI --- the famous 3.1415 ...

	E  --- the base of the natural logs.

	RTOD --- the multiplier which converts from radians to degrees.

	DTOR --- the multiplier which converts from degrees to radians.

	XU --- the unit 3-vector along the X axis.

	YU --- the unit 3-vector along the Y axis.

	ZU --- the unit 3-vector along the Z axis.
	
	UNITR --- the unit 4 X 4 transform (equivalent to ident(4)).

