public LbcbConversion(String label, Lbcb lbcb, RigidTransform transform)
{
String newlabel = lbcb.getLabel() + "_" + label;
this.lbcb = new Lbcb( newlabel, lbcb.getActuatorPins());
this.transform = transform;
this.label = label;
}
public void testTransform1()
{
RigidTransform rig = new RigidTransform(new double[] { 3, 0, 0 }, new double[,] {
{1.0, 0.0, 0.0, 0.0, 0.0, 0.0},
{0.0, 1.0, 0.0, 0.0, 0.0, 0.0},
{0.0, 0.0, 1.0, 0.0, 0.0, 0.0},
{0.0, 0.0, 0.0, 1.0, 0.0, 0.0},
{0.0, 0.0, 0.0, 0.0, 1.0, 0.0},
{0.0, 0.0, 0.0, 0.0, 0.0, 1.0}
});
log.Debug("Testing small motion center");
testTransform(rig, mcDisp, mcForces);
}
private void testTransform(RigidTransform rig, double[][] exDisp, double[][] exForce)
{
for (int t = 0; t < 5; t++)
{
CompareDoubleLists cmp = new CompareDoubleLists();
CompareDoubleLists cmpF = new CompareDoubleLists(0.005);
double[] actual = rig.transform(platDisp[t], false);
log.Debug("Checking mc disp");
cmp.Compare(exDisp[t], actual);
actual = rig.transformMoments(platForces[t]);
log.Debug("Checking mc force");
cmpF.Compare(exForce[t], actual);
actual = rig.transform(exDisp[t], true);
log.Debug("Checking plat disp");
cmp.Compare(platDisp[t], actual);
actual = rig.transformMoments(exForce[t]);
log.Debug("Checking plat force");
cmpF.Compare(platForces[t], actual);
}
}
public void testTransform4()
{
RigidTransform rig = new RigidTransform(new double[] { -18.0000, 0, 45.4690 }, new double[,] {
{1.016593, -0.000047, -0.006086, 0.122384, -1.617186, 0.775084},
{-0.009181, 1.003122, -0.028188, 1.501314, -0.060338, -0.176663},
{0.006838, 0.025004, 1.001882, -0.282000, -0.167350, 0.043025},
{-0.000022, -0.000022, 0.000109, 1.001187, 0.000965, -0.007494},
{0.000003, -0.000029, 0.000001, 0.001187, 0.999678, -0.025727},
{0.000667, 0.000011, -0.000276, 0.006026, 0.029385, 1.016522},
});
log.Debug("Testing alignment matrix");
testTransform(rig, mcDisp3, mcForces3);
}
public void testTransform3()
{
RigidTransform rig = new RigidTransform(new double[] { -11, 55, 32.5, }, new double[,] {
{1.0, 0.0, 0.0, 0.0, 0.0, 0.0},
{0.0, 1.0, 0.0, 0.0, 0.0, 0.0},
{0.0, 0.0, 1.0, 0.0, 0.0, 0.0},
{0.0, 0.0, 0.0, 1.0, 0.0, 0.0},
{0.0, 0.0, 0.0, 0.0, 1.0, 0.0},
{0.0, 0.0, 0.0, 0.0, 0.0, 1.0}
});
log.Debug("Testing large motion center");
testTransform(rig, mcDisp2, mcForces2);
}
public void testTransform2()
{
RigidTransform rig = new RigidTransform(new double[] { 32, 0, 65 }, new double[,] {
{1.0, 0.0, 0.0, 0.0, 0.0, 0.0},
{0.0, 1.0, 0.0, 0.0, 0.0, 0.0},
{0.0, 0.0, 1.0, 0.0, 0.0, 0.0},
{0.0, 0.0, 0.0, 1.0, 0.0, 0.0},
{0.0, 0.0, 0.0, 0.0, 1.0, 0.0},
{0.0, 0.0, 0.0, 0.0, 0.0, 1.0}
});
log.Debug("Testing large motion center");
testTransform(rig, mcDisp1, mcForces1);
}
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;
}
