public double[] transform(double[] displacement, bool isreverse)
{
DenseMatrix xform = (isreverse ? (DenseMatrix)transformation.Inverse() : transformation);
double[] dispcopy = new double[6];
displacement.CopyTo(dispcopy, 0);
DenseMatrix2String m2s = new DenseMatrix2String();
List2String l2s = new List2String();
DenseVector dispV = new DenseVector(dispcopy);
log.Debug("original disp: " + l2s.ToString(displacement));
if (isreverse)
{
dispV = (DenseVector)xform.Multiply(dispV);
log.Debug("transformed Disp: " + l2s.ToString(dispV.Values));
}
DenseVector newDisp = translate(dispV.Values, isreverse);
log.Debug("newDisp: " + l2s.ToString(newDisp.Values));
DenseMatrix rollM = roll.create(dispV.Values[3]);
DenseMatrix pitchM = pitch.create(dispV.Values[4]);
DenseMatrix yawM = yaw.create(dispV.Values[5]);
DenseMatrix rotation = (DenseMatrix)rollM.Multiply(pitchM.Multiply(yawM));
log.Debug("rotation: " + m2s.ToString(rotation));
DenseVector unt = (DenseVector)rotation.Multiply(directionalVector);
log.Debug("unt: " + l2s.ToString(unt.Values));
DenseVector newDisp1 = (DenseVector)unt.Add(newDisp);
log.Debug("newDisp1: " + l2s.ToString(newDisp1.Values));
dispV.SetSubVector(0, 3, newDisp1);
if (isreverse == false)
{
dispV = (DenseVector)xform.Multiply(dispV);
}
log.Debug("resulting Disp: " + l2s.ToString(dispV.Values));
return dispV.Values;
}
public void setLbcb(bool isLbcb2, double[] flattenedPinArray, double[] motionCenter, double[] flattenedTransformation)
{
double[][] pins = new double[6][];
double[,] xform = new double[6, 6];
double[,] xpins = new double[6, 6];
List2String l2s = new List2String();
DenseMatrix2String m2s = new DenseMatrix2String();
for (int r = 0; r < 6; r++)
{
pins[r] = new double[6];
for (int c = 0; c < 6; c++)
{
pins[r][c] = flattenedPinArray[r * 6 + c];
xpins[r,c] = flattenedPinArray[r * 6 + c];
xform[r, c] = flattenedTransformation[r * 6 + c];
}
}
log.Info("Creating " + (isLbcb2 ? "LBCB 2" : "LBCB 1")
+ " with \n]\t Motion Center: " + l2s.ToString(motionCenter)
+ "\n\t Transformation Matrix: " + m2s.ToString(DenseMatrix.OfArray(xform))
+ "\n\t Pins Locations: " + m2s.ToString(DenseMatrix.OfArray(xpins)));
Lbcb lbcb = new Lbcb((isLbcb2 ? "LBCB 2" : "LBCB 1"), pins);
lbcbs[(isLbcb2 ? 1 : 0)] = lbcb;
RigidTransform transform = new RigidTransform(motionCenter,xform);
transforms[(isLbcb2 ? 1 : 0)] = transform;
}
public RigidTransform(double[] motionCenter, double[,] transformation)
{
this.motionCenter = new DenseVector(motionCenter);
this.transformation = DenseMatrix.OfArray(transformation);
List2String l2s = new List2String();
log.Debug("motionCenter: " + l2s.ToString(this.motionCenter.Values) +
" platformCenter: " + l2s.ToString(this.platformCenter.Values));
}
private DenseVector translate(double[] displacement, bool isreverse)
{
double[] ddisp = new double[6];
displacement.CopyTo(ddisp, 0);
DenseVector translation = DenseVector.Create(3, 0.0); ;
List2String l2s = new List2String();
log.Debug("original disp: " + l2s.ToString(displacement));
DenseVector dispV = new DenseVector(ddisp);
dispV.CopySubVectorTo(translation, 0, 0, 3);
log.Debug("given trans:" + l2s.ToString(translation.Values) +
" from: " + l2s.ToString(dispV.Values) +
" orig: " + l2s.ToString(displacement));
DenseVector target;
DenseVector reference;
if (isreverse == false)
{
log.Debug("Normal Translation");
directionalVector = (DenseVector)motionCenter.Subtract(platformCenter);
target = motionCenter;
reference = platformCenter;
}
else
{
log.Debug("Reverse Translation");
directionalVector = (DenseVector)platformCenter.Subtract(motionCenter);
target = platformCenter;
reference = motionCenter;
}
log.Debug("translation: " + l2s.ToString(translation.Values) +
" reference: " + l2s.ToString(reference.Values) +
" target: " + l2s.ToString(target.Values));
DenseVector newDisp = (DenseVector)translation.Add(reference).Subtract(target);
log.Debug("calculated trans:" + l2s.ToString(newDisp.Values) +
" dv: " + l2s.ToString(directionalVector.Values));
return newDisp;
}
private void solveActuator2CartesianDisp(double[] adisp)
{
bool check = false;
DenseVector cartDisp = new DenseVector(6);
DenseVector newAct = new DenseVector(adisp);
DenseVector actError = (DenseVector)newAct.Subtract(actuatorDisp);
cartesianDisp.CopyTo(cartDisp);
int iterations = 0;
while (check == false)
{
List2String l2s = new List2String();
DenseMatrix JacobianMatrix = new DenseMatrix(6, 6);
for (int i = 0; i < 6; i++)
{
DenseVector DL_Dd = actuators[i].calcNewDiffs(cartDisp.Values);
JacobianMatrix.SetRow(i, DL_Dd);
}
DenseVector diffCart = (DenseVector)JacobianMatrix.LU().Solve(actError);
log.Debug("Cartesian differences " + l2s.ToString(diffCart.Values));
cartDisp = (DenseVector)cartDisp.Add(diffCart);
setCartesianDisp(cartDisp.Values);
log.Debug("New cartesian estimate " + this);
actError = (DenseVector)newAct.Subtract(actuatorDisp);
log.Debug("Actuator error " + l2s.ToString(actError.Values));
check = withinErrorWindow(actError);
if (iterations > 20)
{
check = true;
log.Error("Calculations for " + label + " won't converge with " + this);
}
iterations++;
}
}
public override string ToString()
{
List2String l2s = new List2String();
string result = label + ":";
result += "\nCartD: " + l2s.ToString(cartesianDisp.Values);
result += "\nCartF: " + l2s.ToString(cartesianForce.Values);
result += "\nActD: " + l2s.ToString(actuatorDisp.Values);
result += "\nActF: " + l2s.ToString(actuatorForce.Values);
//foreach (LbcbActuator act in actuators)
//{
// result += "\n" + act;
//}
return result;
}
public void ConvertCartesian2Actuator()
{
List2String l2s = new List2String();
foreach (ExpectedLbcbValues ev in expected1)
{
LbcbConversion control1 = lbcbFactory.get("Command 1");
LbcbConversion daq1 = lbcbFactory.get("Readings 1");
CompareDoubleLists cmp = new CompareDoubleLists();
log.Debug("Setting Cart Disp:" + l2s.ToString(ev.getDofs(DofType.CartCommands)));
log.Debug("Before " + control1.getLbcb());
double[] acts = control1.convertCartesian2ActuatorDisplacements(ev.getDofs(DofType.CartCommands));
log.Debug("After " + control1.getLbcb());
cmp.Compare(ev.getDofs(DofType.ActCommands), acts);
acts = control1.getActuatorDisplacements();
cmp.Compare(ev.getDofs(DofType.ActCommands), acts);
acts = daq1.convertCartesian2ActuatorDisplacements(ev.getDofs(DofType.CartDisplacements));
cmp.Compare(ev.getDofs(DofType.ActLvdts), acts);
acts = daq1.getActuatorDisplacements();
cmp.Compare(ev.getDofs(DofType.ActLvdts), acts);
}
foreach (ExpectedLbcbValues ev in expected2)
{
LbcbConversion control2 = lbcbFactory.get("Command 2");
LbcbConversion daq2 = lbcbFactory.get("Readings 2");
CompareDoubleLists cmp = new CompareDoubleLists();
log.Debug("Setting Cart Disp:" + l2s.ToString(ev.getDofs(DofType.CartCommands)));
double[] acts = control2.convertCartesian2ActuatorDisplacements(ev.getDofs(DofType.CartCommands));
cmp.Compare(ev.getDofs(DofType.ActCommands), acts);
acts = control2.getActuatorDisplacements();
cmp.Compare(ev.getDofs(DofType.ActCommands), acts);
acts = daq2.convertCartesian2ActuatorDisplacements(ev.getDofs(DofType.CartDisplacements));
cmp.Compare(ev.getDofs(DofType.ActLvdts), acts);
acts = daq2.getActuatorDisplacements();
cmp.Compare(ev.getDofs(DofType.ActLvdts), acts);
}
}
public double[] calcNewDiffs(double[] cartesian)
{
DenseMatrix rotTrig = DenseMatrix.Create(3,2,0);
double theta_x = cartesian[3];
double theta_y = cartesian[4];
double theta_z = cartesian[5];
var formatProvider = (CultureInfo)CultureInfo.InvariantCulture.Clone();
formatProvider.TextInfo.ListSeparator = " ";
rotTrig.At(0, 0, Math.Sin(theta_x));
rotTrig.At(0, 1, Math.Cos(theta_x));
rotTrig.At(1, 0, Math.Sin(theta_y));
rotTrig.At(1, 1, Math.Cos(theta_y));
rotTrig.At(2, 0, Math.Sin(theta_z));
rotTrig.At(2, 1, Math.Cos(theta_z));
log.Debug("Rotation Angles " + rotTrig.ToString("#000.00000\t", formatProvider));
DenseVector basepin = fixedPin;
DenseVector nominalplatformpin = initialPlatformPin;
double Ppx = nominalplatformpin[0];
double Ppy = nominalplatformpin[1];
double Ppz = nominalplatformpin[2];
log.Debug("Ppx= " + Ppx + "Ppy= " + Ppy + "Ppz= " + Ppz);
DenseVector currentplatformpin = platformPin;
List2String l2s = new List2String();
DenseVector phai = (DenseVector) currentplatformpin.Subtract(basepin);
log.Debug("phai " + l2s.ToString(phai.Values));
if (length == 0.0)
{
log.Error("Actuator length is zero. Caused a divide-by-zero error");
throw new Exception("Actuator length is zero causing a divide by zero error");
}
phai = (DenseVector) phai.Divide(length);
log.Debug("phai " + l2s.ToString(phai.Values));
DenseMatrix J = DenseMatrix.Create(3,3,0);
// Jacobian Element
J[0, 0] = 0;
J[0, 1] = -rotTrig[1, 0] * rotTrig[2, 1] * Ppx + rotTrig[1, 0] * rotTrig[2, 0] * Ppy + rotTrig[1, 1] * Ppz;
J[0, 2] = -rotTrig[1, 1] * rotTrig[2, 0] * Ppx - rotTrig[1, 1] * rotTrig[2, 1] * Ppy;
J[1, 0] = (rotTrig[0, 1] * rotTrig[1, 0] * rotTrig[2, 1] - rotTrig[0, 0] * rotTrig[2, 0]) * Ppx +
(-rotTrig[0, 1] * rotTrig[1, 0] * rotTrig[2, 0] - rotTrig[0, 0] * rotTrig[0, 1]) * Ppy
- rotTrig[0, 1] * rotTrig[1, 1] * Ppz;
J[1, 1] = rotTrig[0, 0] * rotTrig[1, 1] * rotTrig[2, 1] * Ppx - rotTrig[0, 0] * rotTrig[1, 1] * rotTrig[2, 0] * Ppy
- rotTrig[0, 0] * rotTrig[1, 1] * Ppz;
J[1, 2] = (-rotTrig[0, 0] * rotTrig[1, 0] * rotTrig[2, 0] * rotTrig[0, 1] * rotTrig[2, 1]) * Ppx +
(-rotTrig[0, 0] * rotTrig[1, 0] * rotTrig[2, 1] - rotTrig[0, 1] * rotTrig[2, 0]) * Ppy;
J[2, 0] = (rotTrig[0, 0] * rotTrig[1, 0] * rotTrig[2, 1] + rotTrig[0, 1] * rotTrig[2, 0]) * Ppx +
(-rotTrig[0, 0] * rotTrig[1, 0] * rotTrig[2, 0] + rotTrig[0, 1] * rotTrig[2, 1]) * Ppy
- rotTrig[0, 0] * rotTrig[1, 1] * Ppz;
J[2, 1] = -rotTrig[0, 1] * rotTrig[1, 1] * rotTrig[2, 1] * Ppx + rotTrig[0, 1] * rotTrig[1, 1] * rotTrig[2, 0] * Ppy
- rotTrig[0, 1] * rotTrig[1, 0] * Ppz;
J[2, 2] = (rotTrig[0, 1] * rotTrig[1, 0] * rotTrig[2, 0] + rotTrig[0, 0] * rotTrig[2, 1]) * Ppx
+ (rotTrig[0, 1] * rotTrig[1, 0] * rotTrig[2, 1] - rotTrig[0, 0] * rotTrig[2, 0]) * Ppy;
log.Debug("Jacobian " + J.ToString("#000.00000\t", formatProvider));
DenseVector diffs = new DenseVector(6);
diffs.SetSubVector(0, 3, phai);
DenseVector rphai = new DenseVector(new[] { (J[0, 0] * phai[0] + J[1, 0] * phai[1] + J[2, 0] * phai[2]),
(J[0, 1] * phai[0] + J[1, 1] * phai[1] + J[2, 1] * phai[2]),
(J[0, 2] * phai[0] + J[1, 2] * phai[2] + J[2, 2] * phai[2]) });
log.Debug("rphai " + l2s.ToString(rphai.Values));
diffs.SetSubVector(3, 3, rphai);
return diffs.Values;
}
public override string ToString()
{
List2String l2s = new List2String();
string result = label + " ";
result += "\tCurrent: " + l2s.ToString(platformPin.Values);
result += "\tFixed: " + l2s.ToString(fixedPin.Values);
result += "\tInitial: " + l2s.ToString(initialPlatformPin.Values);
result += "\tDisplacement: " + getCurrentDisplacement();
result += "\tLength: " + length;
result += "\tInitial Length: " + initialLength;
return result;
}
