You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
1350 lines
33 KiB
1350 lines
33 KiB
import java.io.*;
|
|
import java.util.*;
|
|
import java.lang.*;
|
|
|
|
/**
|
|
* A class to simplify symbolic transform expressions.
|
|
* @author Peter I. Corke peter.i.corke@gmail.com
|
|
*/
|
|
|
|
/**
|
|
* Element is a class that represents one element in a transform expression.
|
|
* The transform types represented include:
|
|
* TX, TY, TZ pure translation along X, Y and Z axes respectively
|
|
* RX, RY, RZ pure rotations about the X, Y and Z axes respectively
|
|
* DH Denavit-Hartenberg joint transformation
|
|
*
|
|
* public boolean istrans()
|
|
* public boolean isrot()
|
|
* public int axis()
|
|
* public boolean isjoint() {
|
|
* public boolean factorMatch(int dhWhich, int i, int verbose) {
|
|
* public void add(Element e) {
|
|
* public Element(int type, int constant) {
|
|
* public Element(int type) // new of specified type
|
|
*
|
|
* public static String toString(Element [] e) {
|
|
* public String argString() {
|
|
* public String toString() {
|
|
*
|
|
* Constructors:
|
|
* Element(Element e) // clone of argument
|
|
* Element(Element e, int type, int sign) // clone of argument with new type
|
|
* Element(Element e, int type) // clone of argument with new type
|
|
* Element(String s)
|
|
*/
|
|
class Element {
|
|
static final int TX = 0,
|
|
TY = 1,
|
|
TZ = 2,
|
|
RX = 3,
|
|
RY = 4,
|
|
RZ = 5,
|
|
DH_STANDARD = 6,
|
|
DH_MODIFIED = 7;
|
|
|
|
// one of TX, TY ... RZ, DH_STANDARD/MODIFIED
|
|
int type;
|
|
|
|
// transform parameters, only one of these is set
|
|
String var; // eg. q1, for joint var types
|
|
String symconst; // eg. L1, for lengths
|
|
int constant; // eg. 90, for angles
|
|
|
|
// DH parameters, only set if type is DH_STANDARD/MODIFIED
|
|
int theta,
|
|
alpha;
|
|
String A,
|
|
D;
|
|
int prismatic;
|
|
int offset;
|
|
|
|
// an array of counters for the application of each rule
|
|
// just for debugging.
|
|
static int[] rules = new int[20]; // class variable
|
|
|
|
// mapping from type to string
|
|
static final String[] typeName = {
|
|
"Tx", "Ty", "Tz", "Rx", "Ry", "Rz", "DH", "DHm"};
|
|
|
|
// order of elementary transform for each DH convention
|
|
// in each tuple, the first element is the transform type,
|
|
// the second is true if it can be a joint variable.
|
|
static final int dhStandard[][] = {
|
|
{RZ, 1}, {TX, 0}, {TZ, 1}, {RX, 0} };
|
|
static final int dhModified[][] = {
|
|
{RX, 0}, {TX, 0}, {RZ, 1}, {TZ, 1} };
|
|
|
|
/*
|
|
* Display the number of times each rule was used in the
|
|
* conversion.
|
|
*/
|
|
public static void showRuleUsage() {
|
|
for (int i=0; i<20; i++)
|
|
if (rules[i] > 0)
|
|
System.out.println("Rule " + i + ": " +
|
|
rules[i]);
|
|
}
|
|
|
|
// test if the Element is a translation, eg. TX, TY or TZ
|
|
public boolean istrans() {
|
|
return (type == TX) || (type == TY) || (type == TZ);
|
|
}
|
|
|
|
// test if the Element is a rotation, eg. RX, RY or RZ
|
|
public boolean isrot() {
|
|
return (type == RX) || (type == RY) || (type == RZ);
|
|
}
|
|
|
|
// true if this transform represents a joint coordinate, ie. not
|
|
// a constant
|
|
public boolean isjoint() {
|
|
return this.var != null;
|
|
}
|
|
|
|
// return the axis, 0 for X, 1 for Y, 2 for Z
|
|
public int axis() {
|
|
switch (type) {
|
|
case TX:
|
|
case RX:
|
|
return 0;
|
|
case TY:
|
|
case RY:
|
|
return 1;
|
|
case TZ:
|
|
case RZ:
|
|
return 2;
|
|
default:
|
|
throw new IllegalArgumentException("bad transform type");
|
|
}
|
|
}
|
|
|
|
// return the summation of two symbolic parameters as a string
|
|
private String symAdd(String s1, String s2)
|
|
{
|
|
if ( (s1 == null) && (s2 == null) )
|
|
return null;
|
|
else if ( (s1 != null) && (s2 == null) )
|
|
return new String(s1);
|
|
else if ( (s1 == null) && (s2 != null) )
|
|
return new String(s2);
|
|
else {
|
|
return s1 + "+" + s2;
|
|
}
|
|
}
|
|
|
|
/**
|
|
* Add the argument of another Element to this element.
|
|
* assumes that variable has not already been set
|
|
* used by factor() to build a DH element
|
|
*/
|
|
public void add(Element e) {
|
|
if ((this.type != DH_STANDARD) && (this.type != DH_MODIFIED))
|
|
throw new IllegalArgumentException("wrong element type " + this);
|
|
|
|
System.out.println(" adding: " + this + " += " + e);
|
|
switch (e.type) {
|
|
case RZ:
|
|
if (e.isjoint()) {
|
|
this.prismatic = 0;
|
|
this.var = e.var;
|
|
this.offset = e.constant;
|
|
this.theta = 0;
|
|
} else
|
|
this.theta = e.constant;
|
|
break;
|
|
case TX:
|
|
this.A = e.symconst; break;
|
|
case TZ:
|
|
if (e.isjoint()) {
|
|
this.prismatic = 1;
|
|
this.var = e.var;
|
|
this.D = null;
|
|
} else
|
|
this.D = e.symconst;
|
|
break;
|
|
case RX:
|
|
this.alpha = e.constant; break;
|
|
default:
|
|
throw new IllegalArgumentException("cant factorize " + e);
|
|
}
|
|
}
|
|
/*
|
|
public void add(Element e) {
|
|
if ((this.type != DH_STANDARD) && (this.type != DH_MODIFIED))
|
|
throw new IllegalArgumentException("wrong element type " + this);
|
|
|
|
System.out.println(" adding: " + this + " += " + e);
|
|
switch (e.type) {
|
|
case RZ:
|
|
this.theta = e.argString();
|
|
if (e.isjoint())
|
|
this.prismatic = 0;
|
|
break;
|
|
case TX:
|
|
this.A = e.argString(); break;
|
|
case TZ:
|
|
this.D = e.argString();
|
|
if (e.isjoint())
|
|
this.prismatic = 1;
|
|
break;
|
|
case RX:
|
|
this.alpha = e.argString(); break;
|
|
default:
|
|
throw new IllegalArgumentException("cant factorize " + e);
|
|
}
|
|
}
|
|
*/
|
|
|
|
|
|
// test if this particular element could be part of a DH term
|
|
// eg. Rz(q1) can be, Rx(q1) cannot.
|
|
public boolean factorMatch(int dhWhich, int i, int verbose) {
|
|
int dhFactors[][];
|
|
boolean match;
|
|
|
|
switch (dhWhich) {
|
|
case DH_STANDARD:
|
|
dhFactors = dhStandard;
|
|
break;
|
|
case DH_MODIFIED:
|
|
dhFactors = dhModified;
|
|
break;
|
|
default:
|
|
throw new IllegalArgumentException("bad DH type");
|
|
}
|
|
|
|
match = (this.type == dhFactors[i][0]) &&
|
|
!((dhFactors[i][1] == 0) && this.isjoint());
|
|
|
|
if (verbose > 0)
|
|
System.out.println(" matching " + this + " (i=" + i + ") " +
|
|
" to " + typeName[dhFactors[i][0]] + "<" +
|
|
dhFactors[i][1] + ">" + " -> " + match);
|
|
return match;
|
|
}
|
|
|
|
/**
|
|
* test if two transforms can be merged
|
|
* @param e the element to compare with this
|
|
* @return - this if no merge to be done
|
|
* - null if the result is a null transform
|
|
* - new transform resulting from a merge.
|
|
*/
|
|
Element merge(Element e) {
|
|
|
|
/*
|
|
* don't merge if dissimilar transform or
|
|
* both are joint variables
|
|
*/
|
|
if (
|
|
(e.type != this.type) ||
|
|
(e.isjoint() && this.isjoint())
|
|
)
|
|
return this;
|
|
|
|
|
|
Element sum = new Element(this);
|
|
|
|
sum.var = symAdd(this.var, e.var);
|
|
sum.symconst = symAdd(this.symconst, e.symconst);
|
|
sum.constant = this.constant + e.constant;
|
|
if (Math.abs(sum.constant) > 90)
|
|
throw new IllegalArgumentException("rotation angle > 90");
|
|
|
|
/*
|
|
* remove a null transform which can result from
|
|
* a merge operation
|
|
*/
|
|
if ( !sum.isjoint() && (sum.symconst == null) && (sum.constant == 0)) {
|
|
System.out.println("Eliminate: " + this + " " + e);
|
|
return null;
|
|
} else {
|
|
System.out.println("Merge: " + this + " " + e + " := " + sum);
|
|
return sum;
|
|
}
|
|
|
|
}
|
|
|
|
/**
|
|
* test if two transforms need to be swapped
|
|
* @param e the element to compare with this
|
|
* @return - true if swap is required
|
|
*/
|
|
boolean swap(Element next, int dhWhich) {
|
|
/*
|
|
* don't swap if both are joint variables
|
|
*/
|
|
if ( this.isjoint() && next.isjoint() )
|
|
return false;
|
|
|
|
switch (dhWhich) {
|
|
case Element.DH_STANDARD:
|
|
/*
|
|
* we want to sort terms into the order:
|
|
* RZ
|
|
* TX
|
|
* TZ
|
|
* RX
|
|
*/
|
|
/* TX TY TZ RX RY RZ */
|
|
int order[] = { 2, 0, 3, 4, 0, 1 };
|
|
if (
|
|
((this.type == TZ) && (next.type == TX)) ||
|
|
|
|
/*
|
|
* push constant translations through rotational joints
|
|
* of the same type
|
|
*/
|
|
((this.type == TX) && (next.type == RX) && next.isjoint()) ||
|
|
((this.type == TY) && (next.type == RY)) && next.isjoint() ||
|
|
((this.type == TZ) && (next.type == RZ)) && next.isjoint() ||
|
|
|
|
(!this.isjoint() && (this.type == RX) && (next.type == TX)) ||
|
|
(!this.isjoint() && (this.type == RY) && (next.type == TY)) ||
|
|
//(!this.isjoint() && (this.type == RZ) && (next.type == TZ)) ||
|
|
|
|
(!this.isjoint() && !next.isjoint() && (this.type == TZ) && (next.type == RZ)) ||
|
|
|
|
/*
|
|
* move Ty terms to the right
|
|
*/
|
|
((this.type == TY) && (next.type == TZ)) ||
|
|
((this.type == TY) && (next.type == TX))
|
|
) {
|
|
System.out.println("Swap: " + this + " <-> " + next);
|
|
return true;
|
|
}
|
|
break;
|
|
case Element.DH_MODIFIED:
|
|
if (
|
|
((this.type == RX) && (next.type == TX)) ||
|
|
((this.type == RY) && (next.type == TY)) ||
|
|
((this.type == RZ) && (next.type == TZ)) ||
|
|
((this.type == TZ) && (next.type == TX))
|
|
) {
|
|
System.out.println("Swap: " + this + " <-> " + next);
|
|
return true;
|
|
}
|
|
break;
|
|
default:
|
|
throw new IllegalArgumentException("bad DH type");
|
|
}
|
|
return false;
|
|
}
|
|
|
|
/**
|
|
* Substitute this transform for a triple of transforms
|
|
* that includes an RZ or TZ.
|
|
*
|
|
* @return - null if no substituion required
|
|
* - array of Elements to substitute
|
|
*/
|
|
Element[] substituteToZ() {
|
|
|
|
Element[] s = new Element[3];
|
|
|
|
switch (this.type) {
|
|
case RX:
|
|
s[0] = new Element(RY, 90);
|
|
s[1] = new Element(this, RZ);
|
|
s[2] = new Element(RY, -90);
|
|
return s;
|
|
case RY:
|
|
s[0] = new Element(RX, -90);
|
|
s[1] = new Element(this, RZ);
|
|
s[2] = new Element(RX, 90);
|
|
return s;
|
|
case TX:
|
|
s[0] = new Element(RY, 90);
|
|
s[1] = new Element(this, TZ);
|
|
s[2] = new Element(RY, -90);
|
|
return s;
|
|
case TY:
|
|
s[0] = new Element(RX, -90);
|
|
s[1] = new Element(this, TZ);
|
|
s[2] = new Element(RX, 90);
|
|
return s;
|
|
default:
|
|
return null;
|
|
}
|
|
}
|
|
|
|
Element[] substituteToZ(Element prev) {
|
|
|
|
Element[] s = new Element[3];
|
|
|
|
switch (this.type) {
|
|
case RY:
|
|
s[0] = new Element(RZ, 90);
|
|
s[1] = new Element(this, RX);
|
|
s[2] = new Element(RZ, -90);
|
|
rules[8]++;
|
|
return s;
|
|
case TY:
|
|
if (prev.type == RZ) {
|
|
s[0] = new Element(RZ, 90);
|
|
s[1] = new Element(this, TX);
|
|
s[2] = new Element(RZ, -90);
|
|
rules[6]++;
|
|
return s;
|
|
} else {
|
|
s[0] = new Element(RX, -90);
|
|
s[1] = new Element(this, TZ);
|
|
s[2] = new Element(RX, 90);
|
|
rules[7]++;
|
|
return s;
|
|
}
|
|
default:
|
|
return null;
|
|
}
|
|
}
|
|
|
|
/**
|
|
* Simple rewriting rule for adjacent transform pairs. Attempt to
|
|
* eliminate TY and RY.
|
|
* @param previous element in list
|
|
* @return - null if no substituion required
|
|
* - array of Elements to subsitute
|
|
*/
|
|
Element[] substituteY(Element prev, Element next) {
|
|
|
|
Element[] s = new Element[2];
|
|
|
|
if (prev.isjoint() || this.isjoint())
|
|
return null;
|
|
|
|
/* note that if rotation is -90 we must make the displacement -ve */
|
|
if ((prev.type == RX) && (this.type == TY)) {
|
|
// RX.TY -> TZ.RX
|
|
s[0] = new Element(this, TZ, prev.constant);
|
|
s[1] = new Element(prev);
|
|
rules[0]++;
|
|
return s;
|
|
} else if ((prev.type == RX) && (this.type == TZ)) {
|
|
// RX.TZ -> TY.RX
|
|
s[0] = new Element(this, TY, -prev.constant);
|
|
s[1] = new Element(prev);
|
|
rules[2]++;
|
|
return s;
|
|
} else if ((prev.type == RY) && (this.type == TX)) {
|
|
// RY.TX-> TZ.RY
|
|
s[0] = new Element(this, TZ, -prev.constant);
|
|
s[1] = new Element(prev);
|
|
rules[1]++;
|
|
return s;
|
|
} else if ((prev.type == RY) && (this.type == TZ)) {
|
|
// RY.TZ-> TX.RY
|
|
s[0] = new Element(this, TX, prev.constant);
|
|
s[1] = new Element(prev);
|
|
rules[11]++;
|
|
return s;
|
|
} else if ((prev.type == TY) && (this.type == RX)) {
|
|
// TY.RX -> RX.TZ
|
|
s[0] = new Element(this);
|
|
s[1] = new Element(prev, TZ, -this.constant);
|
|
rules[5]++;
|
|
//return s;
|
|
return null;
|
|
} else if ((prev.type == RY) && (this.type == RX)) {
|
|
// RY(Q).RX -> RX.RZ(-Q)
|
|
s[0] = new Element(this);
|
|
s[1] = new Element(prev, RZ, -1);
|
|
rules[3]++;
|
|
return s;
|
|
} else if ((prev.type == RX) && (this.type == RY)) {
|
|
// RX.RY -> RZ.RX
|
|
s[0] = new Element(this, RZ);
|
|
s[1] = new Element(prev);
|
|
rules[4]++;
|
|
return s;
|
|
} else if ((prev.type == RZ) && (this.type == RX)) {
|
|
// RZ.RX -> RX.RY
|
|
s[0] = new Element(this);
|
|
s[1] = new Element(prev, RY);
|
|
//rules[10]++;
|
|
//return s;
|
|
return null;
|
|
}
|
|
return null;
|
|
}
|
|
|
|
/*
|
|
* Element contructors. String is of the form:
|
|
*/
|
|
public Element(int type, int constant) {
|
|
this.type = type;
|
|
this.var = null;
|
|
this.symconst = null;
|
|
this.constant = constant;
|
|
}
|
|
|
|
public Element(int type) { // new of specified type
|
|
this.type = type;
|
|
}
|
|
|
|
public Element(Element e) { // clone of argument
|
|
this.type = e.type;
|
|
if (e.var != null)
|
|
this.var = new String(e.var);
|
|
if (e.symconst != null)
|
|
this.symconst = new String(e.symconst);
|
|
this.constant = e.constant;
|
|
}
|
|
|
|
/**
|
|
* Constructor for Element.
|
|
* @param e Template for new Element.
|
|
* @param type Replacement type for new Element.
|
|
* @param sign Sign of argument, either -1 or +1.
|
|
* @return a new Element with specified type and argument.
|
|
*/
|
|
public Element(Element e, int type, int sign) { // clone of argument with new type
|
|
this.type = type;
|
|
if (e.var != null)
|
|
this.var = new String(e.var);
|
|
this.constant = e.constant;
|
|
if (e.symconst != null)
|
|
this.symconst = new String(e.symconst);
|
|
|
|
if (sign < 0)
|
|
this.negate();
|
|
}
|
|
|
|
public Element(Element e, int type) { // clone of argument with new type
|
|
this(e, type, 1);
|
|
}
|
|
|
|
// negate the arguments of the element
|
|
public void negate() {
|
|
//System.out.println("negate: " + this.constant + " " + this.symconst);
|
|
|
|
// flip the numeric part, easy
|
|
this.constant = -this.constant;
|
|
|
|
|
|
if (this.symconst != null) {
|
|
StringBuffer s = new StringBuffer(this.symconst);
|
|
// if no leading sign character insert one (so we can flip it)
|
|
if ((s.charAt(0) != '+') &&
|
|
(s.charAt(0) != '-')
|
|
)
|
|
s.insert(0, '+');
|
|
|
|
// go through the string and flip all sign chars
|
|
for (int i=0; i<s.length(); i++)
|
|
switch (s.charAt(i)) {
|
|
case '+':
|
|
s.setCharAt(i, '-');
|
|
break;
|
|
case '-':
|
|
s.setCharAt(i, '+');
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
|
|
this.symconst = new String(s);
|
|
}
|
|
//System.out.println("negate: " + this.constant + " " + this.symconst);
|
|
}
|
|
|
|
/**
|
|
* Parsing constructor.
|
|
* @param transform string expression, eg. Tx(q1) Rx(90) Ty(L2)
|
|
*
|
|
* where q1 represents a joint variable, L2 is a dimension.
|
|
*/
|
|
public Element(String s)
|
|
throws IllegalArgumentException { // constructor
|
|
int i;
|
|
String sType = s.substring(0,2);
|
|
String sRest = s.substring(2);
|
|
|
|
if (!(sRest.endsWith(")") && sRest.startsWith("(")))
|
|
throw(new IllegalArgumentException("brackets"));
|
|
|
|
for (i=0; i<6; i++)
|
|
if (sType.equalsIgnoreCase(typeName[i]))
|
|
break;
|
|
if (i >= 6)
|
|
throw(new IllegalArgumentException("bad transform name" + sType));
|
|
type = i;
|
|
|
|
sRest = sRest.substring(1, sRest.length()-1);
|
|
switch (sRest.charAt(0)) {
|
|
case 'q':
|
|
var = sRest;
|
|
break;
|
|
case 'L':
|
|
symconst = sRest;
|
|
break;
|
|
default:
|
|
try {
|
|
constant = Integer.parseInt(sRest);
|
|
}
|
|
catch(NumberFormatException e) {
|
|
System.err.println(e.getMessage());
|
|
throw(new IllegalArgumentException("bracket contents"));
|
|
}
|
|
}
|
|
}
|
|
|
|
// class method to convert Element vector to string
|
|
/*
|
|
public static String toString(Element [] e) {
|
|
String s = "";
|
|
|
|
for (int i=0; i<e.length; i++)
|
|
s += e + " ";
|
|
return s;
|
|
}
|
|
*/
|
|
|
|
/*
|
|
* Return a string representation of the parameters (argument)
|
|
* of the element, which can be a number, symbolic constant,
|
|
* or a joint variable.
|
|
*/
|
|
public String argString() {
|
|
String s = "";
|
|
|
|
switch (type) {
|
|
case RX:
|
|
case RY:
|
|
case RZ:
|
|
case TX:
|
|
case TY:
|
|
case TZ:
|
|
if (var != null)
|
|
s += var;
|
|
if (symconst != null) {
|
|
if (var != null)
|
|
s += "+";
|
|
s += symconst;
|
|
}
|
|
if (constant != 0)
|
|
s += (constant < 0 ? "" : "+") + constant;
|
|
break;
|
|
case DH_STANDARD:
|
|
case DH_MODIFIED:
|
|
// theta, d, a, alpha
|
|
|
|
// theta
|
|
if (prismatic == 0) {
|
|
s += var;
|
|
if (offset > 0)
|
|
s += "+" + offset;
|
|
else if (offset < 0)
|
|
s += offset;
|
|
} else
|
|
s += theta;
|
|
s += ", ";
|
|
|
|
// d
|
|
if (prismatic > 0)
|
|
s += var;
|
|
else
|
|
s += (D == null) ? "0" : D;
|
|
s += ", ";
|
|
|
|
// a
|
|
s += (A == null) ? "0" : A;
|
|
s += ", ";
|
|
|
|
// alpha
|
|
s += alpha;
|
|
break;
|
|
default:
|
|
throw new IllegalArgumentException("bad Element type");
|
|
}
|
|
return s;
|
|
}
|
|
|
|
/*
|
|
* Return a string representation of the element.
|
|
* eg. Rz(q1), Tx(L1), Rx(90), DH(....)
|
|
*/
|
|
public String toString() {
|
|
|
|
String s = typeName[type] + "(";
|
|
s += argString();
|
|
s += ")";
|
|
return s;
|
|
}
|
|
|
|
/*
|
|
public String rotation() {
|
|
}
|
|
|
|
public String translation() {
|
|
}
|
|
*/
|
|
}
|
|
|
|
/**********************************************************************
|
|
/* A list of Elements. Subclass of Java's arrayList
|
|
*
|
|
* public int factorize(int dhWhich, int verbose)
|
|
*
|
|
* public int floatRight() {
|
|
* public int swap(int dhWhich) {
|
|
* public int substituteToZ() {
|
|
* public int substituteToZ2() {
|
|
* public int substituteY() {
|
|
* public int merge() {
|
|
* public void simplify() {
|
|
* public ElementList() { // constructor, use superclass
|
|
* public String toString() {
|
|
*/
|
|
class ElementList extends ArrayList {
|
|
|
|
/**
|
|
* Attempt to group this and subsequent elements into a DH term
|
|
* @return: the number of factors matched, zero means no DH term found
|
|
*
|
|
* Modifies the ElementList and compresses the terms.
|
|
*/
|
|
public int factorize(int dhWhich, int verbose) {
|
|
|
|
int match, jvars;
|
|
int i, j, f;
|
|
Element e;
|
|
int nfactors = 0;
|
|
|
|
for (i=0; i<this.size(); i++) {
|
|
j = i;
|
|
jvars = match = 0;
|
|
for (f=0; f<4; f++) {
|
|
if (j >= this.size())
|
|
break;
|
|
e = (Element) this.get(j);
|
|
if ((f == 0) && (verbose > 0))
|
|
System.out.println("Starting at " + e);
|
|
if (e.factorMatch(dhWhich, f, verbose)
|
|
) {
|
|
j++; // move on to next element
|
|
match++;
|
|
if (e.isjoint())
|
|
jvars++;
|
|
if (jvars > 1) // can only have 1 joint var per DH
|
|
break;
|
|
}
|
|
}
|
|
|
|
if ((match == 0) || (jvars == 0))
|
|
continue; // no DH subexpression found, keep looking
|
|
|
|
int start, end;
|
|
if (verbose > 0)
|
|
System.out.println(" found subexpression " + match + " " + jvars);
|
|
|
|
start = i;
|
|
end = j;
|
|
if (jvars > 1)
|
|
end--;
|
|
|
|
Element dh = new Element(dhWhich);
|
|
|
|
for (j=start; j<end; j++) {
|
|
dh.add( (Element) this.get(i) );
|
|
this.remove(i);
|
|
}
|
|
this.add(i, dh);
|
|
nfactors++;
|
|
if (verbose > 0)
|
|
System.out.println(" result: " + dh);
|
|
}
|
|
return nfactors;
|
|
}
|
|
|
|
/**
|
|
* Attempt to 'float' translational terms as far to the right as
|
|
* possible and across joint boundaries.
|
|
*/
|
|
public int floatRight() {
|
|
Element e, f = null;
|
|
int nchanges = 0;
|
|
int i, j;
|
|
boolean crossed;
|
|
|
|
for (i=0; i<(this.size()-1); i++) {
|
|
e = (Element) this.get(i);
|
|
if (e.isjoint())
|
|
continue;
|
|
if (!e.istrans())
|
|
continue;
|
|
f = null;
|
|
crossed = false;
|
|
for (j=i+1; j<(this.size()-1); j++) {
|
|
f = (Element) this.get(j);
|
|
if (f.istrans())
|
|
continue;
|
|
if (f.isrot() && (f.axis() == e.axis())) {
|
|
crossed = true;
|
|
continue;
|
|
}
|
|
break;
|
|
}
|
|
if (crossed && (f != null)) {
|
|
System.out.println("Float: " + e + " to " + f);
|
|
this.remove(i);
|
|
this.add(j-1, e);
|
|
nchanges++;
|
|
i--;
|
|
}
|
|
}
|
|
|
|
return nchanges;
|
|
}
|
|
|
|
/**
|
|
* Swap adjacent terms according to inbuilt rules so as to achieve
|
|
* desired term ordering.
|
|
*/
|
|
public int swap(int dhWhich) {
|
|
Element e;
|
|
int total_changes = 0;
|
|
int nchanges = 0;
|
|
|
|
do {
|
|
nchanges = 0;
|
|
|
|
for (int i=0; i<(this.size()-1); i++) {
|
|
e = (Element) this.get(i);
|
|
if (e.swap( (Element) this.get(i+1), dhWhich)) {
|
|
this.remove(i);
|
|
this.add(i+1, e);
|
|
nchanges++;
|
|
}
|
|
}
|
|
total_changes += nchanges;
|
|
} while (nchanges > 0);
|
|
|
|
return total_changes;
|
|
}
|
|
|
|
/**
|
|
* substitute all non Z joint transforms according to rules.
|
|
*/
|
|
public int substituteToZ() {
|
|
Element e;
|
|
Element[] replacement;
|
|
int nchanges = 0;
|
|
|
|
for (int i=0; i<this.size(); i++) {
|
|
e = (Element) this.get(i);
|
|
if (!e.isjoint())
|
|
continue;
|
|
replacement = e.substituteToZ();
|
|
if (replacement != null) {
|
|
// diagnostic string
|
|
System.out.print("ReplaceToZ: " + e + " := ");
|
|
for (int j=0; j<replacement.length; j++)
|
|
System.out.print(replacement[j]);
|
|
System.out.println();
|
|
|
|
this.remove(i);
|
|
for (int j=replacement.length-1; j>=0; j--)
|
|
this.add(i, replacement[j]);
|
|
i += replacement.length-1;
|
|
nchanges++;
|
|
}
|
|
}
|
|
return nchanges;
|
|
}
|
|
|
|
/**
|
|
* substitute all non Z joint transforms according to rules.
|
|
*/
|
|
public int substituteToZ2() {
|
|
Element e, prev;
|
|
Element[] replacement;
|
|
int nchanges = 0;
|
|
boolean jointYet = false;
|
|
|
|
for (int i=0; i<this.size(); i++) {
|
|
e = (Element) this.get(i);
|
|
if (e.isjoint())
|
|
jointYet = true;
|
|
if (e.isjoint())
|
|
continue;
|
|
if ((i == 0) || !jointYet) // leave initial const xform
|
|
continue;
|
|
prev = (Element) this.get(i-1);
|
|
//System.out.println("in ToZ2: " + e + " " + prev);
|
|
replacement = e.substituteToZ(prev);
|
|
if (replacement != null) {
|
|
// diagnostic string
|
|
System.out.print("ReplaceToZ2: " + e + " := ");
|
|
for (int j=0; j<replacement.length; j++)
|
|
System.out.print(replacement[j]);
|
|
System.out.println();
|
|
|
|
this.remove(i);
|
|
for (int j=replacement.length-1; j>=0; j--)
|
|
this.add(i, replacement[j]);
|
|
i += replacement.length-1;
|
|
nchanges++;
|
|
}
|
|
}
|
|
return nchanges;
|
|
}
|
|
|
|
/**
|
|
* substitute transforms according to rules.
|
|
*/
|
|
public int substituteY() {
|
|
Element e, prev, next;
|
|
Element[] replacement;
|
|
int nchanges = 0;
|
|
boolean jointYet = false;
|
|
|
|
for (int i=1; i<this.size(); i++) {
|
|
e = (Element) this.get(i);
|
|
if (e.isjoint())
|
|
jointYet = true;
|
|
if ((i == 0) || !jointYet) // leave initial const xform
|
|
continue;
|
|
prev = (Element) this.get(i-1);
|
|
if ((i+1) < this.size())
|
|
next = (Element) this.get(i+1);
|
|
else
|
|
next = null;
|
|
replacement = e.substituteY(prev, next);
|
|
if (replacement != null) {
|
|
// diagnostic string
|
|
System.out.print("ReplaceY: " + prev + e + " := ");
|
|
for (int j=0; j<replacement.length; j++)
|
|
System.out.print(replacement[j]);
|
|
System.out.println();
|
|
|
|
this.remove(i);
|
|
this.remove(i-1);
|
|
for (int j=replacement.length-1; j>=0; j--)
|
|
this.add(i-1, replacement[j]);
|
|
i += replacement.length-2;
|
|
nchanges++;
|
|
}
|
|
}
|
|
return nchanges;
|
|
}
|
|
|
|
/**
|
|
* merge adjacent transforms according to rules.
|
|
*/
|
|
public int merge() {
|
|
int nchanges = 0;
|
|
Element e;
|
|
|
|
for (int i=0; i<(this.size()-1); i++) {
|
|
e = (Element) this.get(i);
|
|
e = e.merge( (Element) this.get(i+1));
|
|
if (e == this.get(i))
|
|
continue;
|
|
this.remove(i);
|
|
this.remove(i);
|
|
if (e != null)
|
|
this.add(i, e);
|
|
nchanges++;
|
|
}
|
|
|
|
return nchanges;
|
|
}
|
|
|
|
/**
|
|
* simplify expression. Cycle continually around merging, substituting,
|
|
* and swapping until no more changes occur.
|
|
*/
|
|
public void simplify() {
|
|
int nchanges;
|
|
int nloops = 0;
|
|
|
|
/*
|
|
* simplify as much as possible, then subsitute for all
|
|
* joint variables not in Z.
|
|
*/
|
|
this.merge();
|
|
this.swap(Element.DH_STANDARD);
|
|
this.merge();
|
|
System.out.println(this);
|
|
this.floatRight();
|
|
this.merge();
|
|
System.out.println("initial merge + swap");
|
|
System.out.println(this);
|
|
this.substituteToZ();
|
|
this.merge();
|
|
System.out.println("joint vars to Z");
|
|
System.out.println(this);
|
|
System.out.println("0---------------------------------------");
|
|
do {
|
|
nchanges = 0;
|
|
|
|
nchanges += this.merge();
|
|
nchanges += this.swap(Element.DH_STANDARD);
|
|
nchanges += this.merge();
|
|
nchanges += this.substituteY();
|
|
nchanges += this.merge();
|
|
System.out.println(this);
|
|
System.out.println("1---------------------------------------");
|
|
if (nchanges == 0) {
|
|
System.out.println("** deal with Ry/Ty");
|
|
nchanges += this.substituteToZ2();
|
|
nchanges += this.merge();
|
|
}
|
|
} while ((nchanges > 0) && (nloops++ < 10));
|
|
}
|
|
|
|
public ElementList() { // constructur, use superclass
|
|
super();
|
|
}
|
|
|
|
public String toString() {
|
|
String s = "";
|
|
|
|
for (int i=0; i<this.size(); i++)
|
|
s += this.get(i) + (i < (this.size()-1) ? "." : "");
|
|
|
|
return s;
|
|
}
|
|
|
|
/*
|
|
static String jstr2offset(String s) {
|
|
int i;
|
|
|
|
if (s != null) {
|
|
i = s.indexOf("+");
|
|
if (i >= 0)
|
|
return s.substring(i);
|
|
i = s.indexOf("-");
|
|
if (i >= 0)
|
|
return s.substring(i);
|
|
}
|
|
return "0";
|
|
}
|
|
|
|
static String convertMatlab(String s)
|
|
{
|
|
if (s == null)
|
|
return " 0";
|
|
return " " + s;
|
|
}
|
|
|
|
public String toMatlab(String robot) {
|
|
String dh = "[";
|
|
String offs = "[";
|
|
String base = "";
|
|
String tool = "";
|
|
String theta, a, d, alpha;
|
|
int dhSeenYet = 0;
|
|
Element e;
|
|
|
|
for (int i=0; i<this.size(); i++) {
|
|
e = (Element) this.get(i);
|
|
if (e.type == Element.DH_STANDARD) {
|
|
dhSeenYet = 1;
|
|
// build up the string: theta a d alpha
|
|
if (e.prismatic == 1) {
|
|
// prismatic joint
|
|
d = "0"; // by definition
|
|
offs += jstr2offset(e.D) + " ";
|
|
theta = (e.theta == null) ? "0" : e.theta;
|
|
} else {
|
|
// revolute joint
|
|
theta = "0"; // by definition
|
|
offs += jstr2offset(e.theta) + " ";
|
|
d = (e.D == null) ? "0" : e.D;
|
|
}
|
|
|
|
a = (e.A == null) ? "0" : e.A;
|
|
alpha = (e.alpha == null) ? "0" : e.alpha;
|
|
|
|
s += theta;
|
|
s += ", ";
|
|
s += a;
|
|
s += ", ";
|
|
s += d;
|
|
s += ", ";
|
|
s += alpha;
|
|
s += "; ";
|
|
} else {
|
|
// found some primitive transform, these will be
|
|
// part of base or tool
|
|
// scrape the leading * off
|
|
if (xform.length() > 0)
|
|
xform = xform.substring(2);
|
|
|
|
// assign this string to base or tool depending on
|
|
// whether or not we've seen the DH terms go by
|
|
if (dhSeenYet == 0)
|
|
base = xform;
|
|
else
|
|
tool = xform;
|
|
}
|
|
}
|
|
dh += "]";
|
|
offs += "]";
|
|
|
|
// build the matlab string
|
|
|
|
s = "robot(" + dh + ", " + robot ;
|
|
|
|
if (base.length() > 0)
|
|
s += ", 'base', " + base;
|
|
if (tool.length() > 0)
|
|
s += ", 'tool', " + tool;
|
|
s += ");";
|
|
|
|
return s;
|
|
}
|
|
*/
|
|
}
|
|
|
|
public class DHFactor {
|
|
|
|
ElementList results;
|
|
|
|
// Matlab callable constructor
|
|
public DHFactor(String src) {
|
|
results = parseString(src);
|
|
if (!this.isValid())
|
|
System.out.println("DHFactor: error: Incomplete factorization, no DH equivalent found");
|
|
}
|
|
|
|
private String angle(Element e) {
|
|
return angle(e.constant);
|
|
}
|
|
|
|
|
|
public String toString() {
|
|
return results.toString();
|
|
}
|
|
|
|
public String display() {
|
|
return results.toString();
|
|
}
|
|
|
|
private String angle(int a)
|
|
{
|
|
if (a == 0)
|
|
return "0";
|
|
else if (a == 90)
|
|
return "pi/2";
|
|
else if (a == -90)
|
|
return "-pi/2";
|
|
else
|
|
throw new IllegalArgumentException("bad transform angle");
|
|
}
|
|
|
|
private String el2matlab(int from, int to)
|
|
{
|
|
String xform = "";
|
|
int i;
|
|
|
|
for (i=from; i<to; i++) {
|
|
Element e = (Element) results.get(i);
|
|
|
|
if (xform.length() > 0)
|
|
xform += "*";
|
|
|
|
switch (e.type) {
|
|
case Element.RX: xform += "trotx(" + angle(e) + ")"; break;
|
|
case Element.RY: xform += "troty(" + angle(e) + ")"; break;
|
|
case Element.RZ: xform += "trotz(" + angle(e) + ")"; break;
|
|
case Element.TX: xform += "transl(" + e.symconst + ",0,0)"; break;
|
|
case Element.TY: xform += "transl(0, " + e.symconst + ",0)"; break;
|
|
case Element.TZ: xform += "transl(0,0," + e.symconst + ")"; break;
|
|
}
|
|
}
|
|
if (xform.length() == 0)
|
|
xform = "eye(4,4)";
|
|
return xform;
|
|
}
|
|
|
|
/*
|
|
* Create a Toolbox legacy DH matrix. The column order is:
|
|
*
|
|
* theta d a alpha
|
|
*/
|
|
public String dh() {
|
|
String s = "[";
|
|
String theta, d;
|
|
Element e;
|
|
|
|
for (int i=0; i<results.size(); i++) {
|
|
e = (Element) results.get(i);
|
|
if (e.type == Element.DH_STANDARD) {
|
|
// build up the string: theta d a alpha
|
|
if (e.prismatic == 1) {
|
|
// prismatic joint
|
|
d = "0"; // by definition
|
|
theta = angle(e.theta);
|
|
} else {
|
|
// revolute joint
|
|
theta = "0"; // by definition
|
|
d = (e.D == null) ? "0" : e.D;
|
|
}
|
|
|
|
s += theta; s += ", ";
|
|
s += d; s += ", ";
|
|
s += (e.A == null) ? "0" : e.A; s += ", ";
|
|
s += angle(e.alpha);
|
|
s += ", " + e.prismatic;
|
|
s += "; ";
|
|
};
|
|
}
|
|
s += "]";
|
|
return s;
|
|
}
|
|
|
|
/*
|
|
* Check the transform string is valid
|
|
*
|
|
*/
|
|
public boolean isValid() {
|
|
Element e;
|
|
int iprev = -1;
|
|
|
|
for (int i=0; i<results.size(); i++) {
|
|
e = (Element) results.get(i);
|
|
if (e.type == Element.DH_STANDARD) {
|
|
if (iprev >= 0) {
|
|
// we've seen a DH factor before
|
|
if ((i-iprev) > 1) {
|
|
// but it was too long ago, fail!
|
|
return false;
|
|
}
|
|
}
|
|
iprev = i; // note where we saw it
|
|
};
|
|
}
|
|
return true;
|
|
}
|
|
|
|
public String offset() {
|
|
String s = "[";
|
|
Element e;
|
|
|
|
for (int i=0; i<results.size(); i++) {
|
|
e = (Element) results.get(i);
|
|
if (e.type == Element.DH_STANDARD) {
|
|
s += angle(e.offset) + " ";
|
|
};
|
|
}
|
|
s += "]";
|
|
return s;
|
|
}
|
|
|
|
// return base transform string in Matlab Toolbox form
|
|
public String base() {
|
|
int i;
|
|
|
|
for (i=0; i<results.size(); i++) {
|
|
Element e = (Element)results.get(i);
|
|
|
|
if ( (e.type == Element.DH_STANDARD) || (e.type == Element.DH_MODIFIED) )
|
|
return el2matlab(0, i);
|
|
}
|
|
|
|
return "eye(4,4)";
|
|
}
|
|
|
|
// return base transform string in Matlab Toolbox form
|
|
public String tool() {
|
|
int i;
|
|
|
|
for (i=results.size()-1; i>=0; i--) {
|
|
Element e = (Element)results.get(i);
|
|
|
|
if ( (e.type == Element.DH_STANDARD) || (e.type == Element.DH_MODIFIED) )
|
|
return el2matlab(i, results.size());
|
|
}
|
|
|
|
return "eye(4,4)";
|
|
}
|
|
|
|
|
|
// return Matlab Toolbox robot creation command
|
|
public String command(String name) {
|
|
if (this.isValid())
|
|
return "SerialLink(" + this.dh() + ", 'name', '" + name +
|
|
"', 'base', " + this.base() +
|
|
", 'tool', " + this.tool() +
|
|
", 'offset', " + this.offset() + ")";
|
|
else
|
|
return "error('incompletely factored transform string')";
|
|
}
|
|
|
|
public static ElementList parseFile(String filename) {
|
|
BufferedReader src;
|
|
String buffer;
|
|
|
|
try {
|
|
File file = new File(filename);
|
|
|
|
if (!file.canRead() || !file.isFile())
|
|
throw new IOException("dh: file access/type error");
|
|
|
|
src = new BufferedReader(new FileReader(file));
|
|
|
|
// read the file and parse it
|
|
src = new BufferedReader(new FileReader(file));
|
|
buffer = src.readLine();
|
|
|
|
return parseString(buffer);
|
|
}
|
|
catch (FileNotFoundException e) {
|
|
System.err.println(e.getMessage());
|
|
System.exit(1);
|
|
}
|
|
catch (IOException e) {
|
|
System.err.println(e.getMessage());
|
|
System.exit(1);
|
|
}
|
|
return null;
|
|
}
|
|
|
|
public static ElementList parseString(String buffer) {
|
|
ElementList l = new ElementList();
|
|
|
|
try {
|
|
System.out.println(buffer);
|
|
StringTokenizer tokens = new StringTokenizer(buffer, " *.");
|
|
|
|
while (tokens.hasMoreTokens())
|
|
l.add( new Element(tokens.nextToken()) );
|
|
|
|
System.out.println(l);
|
|
|
|
l.simplify();
|
|
System.out.println(l);
|
|
|
|
l.factorize(Element.DH_STANDARD, 0);
|
|
System.out.println(l);
|
|
|
|
return l;
|
|
}
|
|
catch (IllegalArgumentException e) {
|
|
System.err.println(e.getMessage());
|
|
System.exit(1);
|
|
}
|
|
return null;
|
|
}
|
|
|
|
// command line instantiation
|
|
// dhfactor file
|
|
// dhfactor < stdin
|
|
public static void main(String args[]) {
|
|
|
|
if (args.length > 0) {
|
|
|
|
ElementList l = parseFile(args[0]);
|
|
System.err.println( l );
|
|
|
|
} else {
|
|
System.err.println("no file name specified\n");
|
|
Element.showRuleUsage();
|
|
System.exit(1);
|
|
}
|
|
}
|
|
}
|