25 const int scalarDataDimension = 1;
26 const int vectorDataDimension = 3;
31 std::vector<std::optional<double>> lowerBoundedL = {0};
32 std::vector<std::optional<double>> lowerBoundedU(scalarDataDimension);
34 std::vector<std::optional<double>> upperBoundedL(scalarDataDimension);
35 std::vector<std::optional<double>> upperBoundedU = {0};
37 std::vector<std::optional<double>> lowerUpperBoundedL = {0};
38 std::vector<std::optional<double>> lowerUpperBoundedU = {1};
40 std::vector<std::optional<double>> zUpperBoundedL(vectorDataDimension);
41 std::vector<std::optional<double>> zUpperBoundedU(vectorDataDimension);
42 zUpperBoundedU[2] = 1;
44 std::vector<std::optional<double>> xLowerBoundedL(vectorDataDimension);
45 std::vector<std::optional<double>> xLowerBoundedU(vectorDataDimension);
46 xLowerBoundedL[0] = 0;
48 std::vector<std::optional<double>> yUpperBoundedL(vectorDataDimension);
49 std::vector<std::optional<double>> yUpperBoundedU(vectorDataDimension);
50 yUpperBoundedU[1] = 1;
52 std::vector<std::optional<double>> xUpperBoundedL(vectorDataDimension);
53 std::vector<std::optional<double>> xUpperBoundedU(vectorDataDimension);
54 xUpperBoundedU[0] = 1;
56 mesh::PtrData lowerBounded(
new mesh::Data(
"lowerBounded", 0, scalarDataDimension, 1, 2, lowerBoundedL, lowerBoundedU));
57 lowerBounded->emplaceSampleAtTime(0.0, {0., 0.});
58 mesh::PtrData upperBounded(
new mesh::Data(
"upperBounded", 1, scalarDataDimension, 1, 2, upperBoundedL, upperBoundedU));
59 upperBounded->emplaceSampleAtTime(0.0, {0., 0.});
60 mesh::PtrData lowerUpperBounded(
new mesh::Data(
"lowerUpperBounded", 2, scalarDataDimension, 1, 2, lowerUpperBoundedL, lowerUpperBoundedU));
61 lowerUpperBounded->emplaceSampleAtTime(0.0, {0., 0.});
63 mesh::PtrData zUpperBounded(
new mesh::Data(
"zUpperBounded", 3, vectorDataDimension, 1, 2, zUpperBoundedL, zUpperBoundedU));
64 zUpperBounded->emplaceSampleAtTime(0.0, {0., 0., 0., 0., 0., 0.});
65 mesh::PtrData xLowerBounded(
new mesh::Data(
"xLowerBounded", 4, vectorDataDimension, 1, 2, xLowerBoundedL, xLowerBoundedU));
66 xLowerBounded->emplaceSampleAtTime(0.0, {0., 0., 0., 0., 0., 0.});
67 mesh::PtrData yUpperBounded(
new mesh::Data(
"yUpperBounded", 5, vectorDataDimension, 1, 2, yUpperBoundedL, yUpperBoundedU));
68 yUpperBounded->emplaceSampleAtTime(0.0, {0., 0., 0., 0., 0., 0.});
69 mesh::PtrData xUpperBounded(
new mesh::Data(
"xUpperBounded", 6, vectorDataDimension, 1, 2, xUpperBoundedL, xUpperBoundedU));
70 xUpperBounded->emplaceSampleAtTime(0.0, {0., 0., 0., 0., 0., 0.});
79 for (
auto &cd : {lowerBoundedPtr, upperBoundedPtr, lowerUpperBoundedPtr, zUpperBoundedPtr, xLowerBoundedPtr, yUpperBoundedPtr, xUpperBoundedPtr}) {
86 {2, lowerUpperBoundedPtr},
87 {3, zUpperBoundedPtr},
88 {4, xLowerBoundedPtr},
89 {5, yUpperBoundedPtr},
90 {6, xUpperBoundedPtr}};
94 Eigen::VectorXd testData(3 * 2 + 4 * 6);
95 testData << 2.0, -0.1, 2.0, 0.1, -0.1, 1.1, 0.1, 0.1, 1.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, -0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 1.1, 0.1, 1.1, 0.1, 0.1, 0.1, 0.1, 0.1;
96 auto violatingIDs =
acceleration.checkBoundViolation(testData, cplData);
98 std::vector<int> expectedViolations{0, 1, 2, 3, 4, 5, 6};
99 BOOST_TEST(violatingIDs == expectedViolations, boost::test_tools::per_element());
107 Eigen::VectorXd _data(6);
108 _data << 0.1, 0.3, 0.4, -0.4, 1.2, 0.5;
109 Eigen::Map<Eigen::MatrixXd> dataMap(_data.data(), 2, 3);
111 Eigen::VectorXd _dataAfterUpdate(6);
112 _dataAfterUpdate << 0.1, 0.3, 0.4, 0.0, 0.5, 0.5;
113 Eigen::Map<Eigen::MatrixXd> dataAfterUpdateMap(_dataAfterUpdate.data(), 2, 3);
115 std::vector<std::optional<double>> _lowerBound(2);
118 std::vector<std::optional<double>> _upperBound(2);
119 _upperBound[0] = 0.5;
123 acceleration.clampToBounds(dataMap, _lowerBound, _upperBound);
134 Eigen::VectorXd _data(6);
135 _data << 0.1, 0.3, 0.4, -0.4, 1.2, 0.5;
136 Eigen::Map<Eigen::MatrixXd> dataMap(_data.data(), 2, 3);
138 Eigen::VectorXd _dataUpdate(6);
139 _dataUpdate << -0.6, -0.3, 0.3, -0.6, 0.8, 0.12;
140 Eigen::Map<Eigen::MatrixXd> dataUpdateMap(_dataUpdate.data(), 2, 3);
142 std::vector<std::optional<double>> _lowerBound(2);
145 std::vector<std::optional<double>> _upperBound(2);
146 _upperBound[0] = 0.5;
150 auto scaleFactor =
acceleration.computeShorteningFactor(dataMap, dataUpdateMap, _lowerBound, _upperBound);
159 Eigen::VectorXd _data(6);
160 _data << -1e-13, -0.3, 1e-13, -1e-13, 1e-13, 1e-13;
161 Eigen::Map<Eigen::MatrixXd> dataMap(_data.data(), 2, 3);
163 Eigen::VectorXd _dataUpdate(6);
164 _dataUpdate << -2e-13, -0.3, 0, 3e-13, -2e-13, 2e-13;
165 Eigen::Map<Eigen::MatrixXd> dataUpdateMap(_dataUpdate.data(), 2, 3);
167 std::vector<std::optional<double>> _lowerBound(2);
170 std::vector<std::optional<double>> _upperBound(2);
171 _upperBound[0] = 0.5;
175 auto scaleFactor =
acceleration.computeShorteningFactor(dataMap, dataUpdateMap, _lowerBound, _upperBound);