public void ConvertActuatorForces2Cartesian()
{
foreach (ExpectedLbcbValues ev in expected1)
{
LbcbConversion daq1 = lbcbFactory.get("Readings 1");
CompareDoubleLists cmp = new CompareDoubleLists();
double[] carts = daq1.convertActuator2CartesianDisplacements(ev.getDofs(DofType.ActLvdts));
cmp.Compare(ev.getDofs(DofType.CartDisplacements), carts);
carts = daq1.convertActuator2CartesianForces(ev.getDofs(DofType.ActLoadCells));
cmp.Compare(ev.getDofs(DofType.CartForces), carts);
carts = daq1.getCartesianForces();
cmp.Compare(ev.getDofs(DofType.CartForces), carts);
}
foreach (ExpectedLbcbValues ev in expected2)
{
LbcbConversion daq2 = lbcbFactory.get("Readings 2");
CompareDoubleLists cmp = new CompareDoubleLists();
double [] carts = daq2.convertActuator2CartesianDisplacements(ev.getDofs(DofType.ActLvdts));
cmp.Compare(ev.getDofs(DofType.CartDisplacements), carts);
carts = daq2.convertActuator2CartesianForces(ev.getDofs(DofType.ActLoadCells));
cmp.Compare(ev.getDofs(DofType.CartForces), carts);
carts = daq2.getCartesianForces();
cmp.Compare(ev.getDofs(DofType.CartForces), carts);
}
}
public void test02DiffCalculations()
{
// MatrixDataAccessor mda = new MatrixDataAccessor();
// mda.Run();
double[] cart = new double[] { 0, 0, 0, 0, 0, 0 };
double[] expected0 = new double[] { 0.9507, 0, 0.3101, 7.4413, -3.8690, -22.8173 };
double[] expected1 = new double[] { 0.9507, 0, 0.3101, 7.4612, -3.8691, -22.8591 };
double[] expected2 = new double[] { 0.9507, 0, 0.3101, 7.4413, -3.7207, -22.7798 };
double[] expected3 = new double[] { 0.9507, 0, 0.3101, 7.3994, -3.8319, -22.7258 };
LbcbActuator[] pins1 = l1pins.getActuators();
LbcbActuator x1 = pins1[0];
CompareDoubleLists cdl = new CompareDoubleLists();
cdl.Compare(expected0,x1.calcNewDiffs(cart));
cart[3] = 0.005;
cdl.Compare(expected1, x1.calcNewDiffs(cart));
cart[3] = 0.0;
cart[4] = 0.005;
cdl.Compare(expected2, x1.calcNewDiffs(cart));
cart[4] = 0.0;
cart[5] = 0.005;
cdl.Compare(expected3, x1.calcNewDiffs(cart));
}
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 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 void test03Updates()
{
double[][] cartesians =
{
new double[] {0.1, 0, 0, 0, 0, 0},
new double[] {0, 0.2, 0, 0, 0, 0},
new double[] {0, 0, 0.45, 0, 0, 0},
new double[] {0, 0, 0, 0.005, 0, 0},
new double[] {0, 0, 0, 0, 0.003, 0},
new double[] {0, 0, 0, 0, 0, -0.002}
};
double[][] positions =
{
new double[] {-26.9000, 24.0000, -12.8750},
new double[] {-27.0000, 24.2000, -12.8750},
new double[] {-27.0000, 24.0000, -12.4250},
new double[] {-27.0000, 24.0641, -12.7548},
new double[] {-27.0385, 24.0000, -12.7939},
new double[] {-26.9519, 24.0540, -12.8750},
};
double[] lengths = { 73.0662, 72.9714, 73.1119, 73.0085, 72.9597, 73.0168 };
LbcbActuator[] pins1 = l1pins.getActuators();
LbcbActuator x1 = pins1[0];
for (int t = 0; t < 6; t++)
{
CompareDoubleLists cdl = new CompareDoubleLists();
x1.update(cartesians[t]);
cdl.Compare(positions[t], x1.getPlatformPin());
Assert.That(x1.getLength(), Is.EqualTo(lengths[t]).Within(0.001));
}
}
