Commit 0365002e authored by Hendrik Geisler's avatar Hendrik Geisler

Experimenting with Potential for switching

parent 59032e0f
This source diff could not be displayed because it is stored in LFS. You can view the blob instead.
This source diff could not be displayed because it is stored in LFS. You can view the blob instead.
This source diff could not be displayed because it is stored in LFS. You can view the blob instead.
This source diff could not be displayed because it is stored in LFS. You can view the blob instead.
This source diff could not be displayed because it is stored in LFS. You can view the blob instead.
......@@ -46,6 +46,7 @@ int main()
std::vector<Eigen::Vector<double, 4 * NUM_N>> f_vec;
std::vector<state_type> bc_l_vec;
std::vector<double> times;
std::vector<double> potential;
//Solving the system with odeint
auto start = std::chrono::high_resolution_clock::now();
......@@ -56,7 +57,7 @@ int main()
error_stepper_type::time_type t = 0.0;
error_stepper_type::time_type dt = 0.001;
double t_max = 4.0;
double t_max = 1.0;
double dt_max = 0.0001;
int step = 0;
std::cout << "Step \t t \t dt" << std::endl;
......@@ -73,13 +74,10 @@ int main()
stepper.try_step(Crawler, x, t, dt);
if (t > 2.0) {
}
//Start Motion
if (t < 2.0) {
partLeft = Robot::rampStepUp(0.0, 0.5, 2.0, t);
partRight = Robot::rampStepUp(1.0, 0.5, 2.0, t);
partLeft = Robot::rampStepUp(0.0, 0.5, 2.0, t);
partRight = Robot::rampStepUp(1.0, 0.5, 2.0, t);
}
if (t >= 2.0) {
//partLeft = Robot::rampStepDown(2.0, 0.5, 6.0, t);
......@@ -89,7 +87,74 @@ int main()
Crawler.setRightCurvature(x, -0.3, partRight);
Crawler.setLeftCurvature(x, -0.3, partLeft);
//Do not fall through the ground
for (int i = 0; i < 2 * NUM_N; i = i + 2) {
if (x(i) < -height) {
x(i) = -height;
x(4 * NUM_N + i) = 0;
}
}
step++;
//Push back state
if (fmod(step, 100) == 0) {
std::cout << step << " " << t << " " << dt << std::endl;
bc_l_vec.push_back(Prob->boundaryConditions);
x_vec.push_back(x);
f_vec.push_back(System.F);
times.push_back(t);
}
}
potential.push_back(System.approxPotential(x));
t = 1.0;
while (t < 1.5 && dt > 1E-6) { // && dt > 1E-25
if (dt > dt_max) {
dt = dt_max;
}
//stepper.try_step(Crawler, x, t, dt);
t = t + 1E-2;
//Start Motion
if (t < 2.0) {
partLeft = Robot::rampStepUp(0.0, 0.5, 2.0, t);
partRight = Robot::rampStepUp(1.0, 0.5, 2.0, t);
}
Crawler.setRightCurvature(x, -0.3, partRight);
Crawler.setLeftCurvature(x, -0.3, partLeft);
potential.push_back(System.approxPotential(x));
//Do not fall through the ground
for (int i = 0; i < 2 * NUM_N; i = i + 2) {
if (x(i) < -height) {
x(i) = -height;
x(4 * NUM_N + i) = 0;
}
}
step++;
//Push back state
if (fmod(step, 1) == 0) {
std::cout << step << " " << t << " " << dt << std::endl;
bc_l_vec.push_back(Prob->boundaryConditions);
x_vec.push_back(x);
f_vec.push_back(System.F);
times.push_back(t);
}
}
//If nodes are controlled via boundary conditions, do not let DER change them
//for (int i = 0; i < 2 * NUM_N_LEFT; i = i + 2) {
......@@ -158,29 +223,8 @@ int main()
// }
//}
//Do not fall through the ground
for (int i = 0; i < 2 * NUM_N; i = i + 2) {
if (x(i) < -height) {
x(i) = -height;
x(4 * NUM_N + i) = 0;
}
}
step++;
//Push back state
if (fmod(step, 100) == 0) {
std::cout << step << " " << t << " " << dt << std::endl;
bc_l_vec.push_back(Prob->boundaryConditions);
x_vec.push_back(x);
f_vec.push_back(System.F);
times.push_back(t);
}
//std::cout << t << " " << dt << std::endl;
}
auto stop = std::chrono::high_resolution_clock::now();
......@@ -197,6 +241,8 @@ int main()
std::ofstream xOut("../../../x.csv");
std::ofstream fOut("../../../f.csv");
std::ofstream bcOut("../../../bc.csv");
std::ofstream potOut("../../../pot.csv");
std::ofstream timesOut("../../../t.csv");
// Send data to the stream
for (int i = 0; i < x_vec.size(); i++) {
......@@ -210,10 +256,14 @@ int main()
xOut << "\n";
fOut << "\n";
bcOut << "\n";
potOut << potential[i] << ",";
timesOut << times[i] << ",";
}
// Close the files
xOut.close();
fOut.close();
bcOut.close();
potOut.close();
timesOut.close();
}
\ No newline at end of file
......@@ -243,4 +243,104 @@ void DER::operator() (const state_type& s, state_type& ds, const double t) {
//Boundary conditions
ds = ds.array() * prob->boundaryConditions.array();
}
double DER::approxPotential(const state_type& s) {
constexpr int N = NUM_N;
VectorXd q = s.block(0, 0, 4 * N, 1);
VectorXd dq = s.block(4 * N, 0, 4 * N, 1);
Matrix<double, Dynamic, Dynamic> vX = q.block(0, 0, 2 * N, 1);
vX.resize(2, N);
Matrix<double, Dynamic, Dynamic> vD = q.block(2 * N, 0, 2 * N, 1);
vD.resize(2, N);
//Kinematic Quantities
MatrixXd A = vX.block(0, 1, vX.rows(), vX.cols() - 1);
MatrixXd B = vX.block(0, 0, vX.rows(), vX.cols() - 1);
MatrixXd eE = A - B;
VectorXd eEnorm = eE.colwise().norm();
MatrixXd eT = eE.array().rowwise() / eEnorm.array().transpose().eval(); //Eval to resolve aliasing issues
VectorXd vDnorm = vD.colwise().norm();
MatrixXd vD_hat = vD.array().rowwise() / vDnorm.array().transpose().eval(); //Eval to resolve aliasing issues
MatrixXd vT(2, N);
VectorXd diffX(2);
VectorXd diffE(2);
for (int i = 1; i < N - 1; i++) {
diffX = vX.col(i + 1) - vX.col(i - 1);
diffE = eE.col(i - 1) + eE.col(i);
vT.col(i) = diffX / diffE.norm();
}
vT.col(0) = eT.col(0);
vT.col(N - 1) = eT.col(N - 2);
VectorXd vL(N);
for (int i = 1; i < N - 1; i++) {
vL(i) = 0.5 * (eE.col(i - 1).norm() + eE.col(i).norm());
}
vL(0) = 0.5 * eE.col(0).norm();
vL(N - 1) = 0.5 * eE.col(N - 2).norm();
VectorXd vMu(N);
vMu = vL.array() / prob->vL_Ref.array();
VectorXd vPhi(N);
for (int i = 1; i < N - 1; i++) {
//Hacking the floating point precision.
//The absolut dot product of two normalized vectors is less or equal 1.
double arg = eT.col(i - 1).dot(eT.col(i));
if (arg > 1) {
arg = 1;
}
if (arg < -1) {
arg = -1;
}
vPhi(i) = acos(arg);
}
vPhi(0) = acos(vT.col(0).dot(vT.col(1)));
vPhi(N - 1) = acos(vT.col(N - 2).dot(vT.col(N - 1)));
VectorXd vPhiSign = MatrixXd::Ones(N, 1);
for (int i = 1; i < N - 1; i++) {
vPhiSign(i) = sgn(-(eT(0, i - 1) * eT(1, i) - eT(1, i - 1) * eT(0, i)));
}
vPhi = vPhi.cwiseProduct(vPhiSign);
//Stretches and Strains
VectorXd eMu(N - 1);
VectorXd eERef_norm(N - 1);
eERef_norm = prob->eE_Ref.colwise().norm();
eMu = eEnorm.array() / eERef_norm.array();
VectorXd vPhi_1_2 = vPhi / 2;
VectorXd vKappa = 2 * vL.cwiseInverse().array() * vPhi_1_2.array().tan().array();
VectorXd vTheta(N);
double vTheta_sign;
VectorXd vB(2);
double arg;
for (int i = 0; i < N; i++) {
vB << vT(1, i) * 1, -vT(0, i);
//Fighting floating point precision
arg = vD_hat.col(i).dot(vB);
if (arg > 1) {
arg = 1;
}
if (arg < -1) {
arg = -1;
}
vTheta(i) = acos(arg);
vTheta_sign = sgn(-(vB(0)*vD_hat(1, i) - vB(1)*vD_hat(0, i)));
vTheta(i) = vTheta(i) * vTheta_sign;
}
double pot = 0;
//Calculate potential
for (int i = 0; i < N-1; i++) {
pot += prob->Cpar->k1 * (pow(eMu(i), 2) - eMu(i) + pow(eMu(i), -1)) * eEnorm(i) + prob->Cpar->k3 * pow(vMu(i)*vKappa(i), 2) * prob->vL_Ref(i);
}
return 0.5*pot;
}
\ No newline at end of file
......@@ -27,6 +27,8 @@ public:
*/
void operator() (const state_type&, state_type&, const double /* t */);
double approxPotential(const state_type&); //Calculates approx. potential for given state, using only k1 and k3
//The internal forces
VectorXd F_int_vec;
VectorXd F;
......
f = readmatrix("../f.csv");
x = readmatrix("../x.csv");
bc = readmatrix("../bc.csv");
pot = readmatrix("../pot.csv");
t = readmatrix("../t.csv");
f = f(1:end, 1:end-1);
x = x(1:end, 1:end-1);
bc = bc(1:end, 1:end-1);
......
This diff is collapsed.
This diff is collapsed.
1.76916,1.66967,1.58704,1.52052,1.46823,1.42829,1.39882,1.37801,1.36409,1.35544,1.35064,1.34857,1.34853,1.35019,1.35352,1.35872,1.36624,1.37676,1.39113,1.41031,1.43526,1.46689,1.50605,1.55349,1.60983,1.67561,1.7512,1.83691,1.93287,2.03913,2.15561,2.28212,2.41836,2.56392,2.71833,2.88097,3.05119,3.22823,3.41128,3.59946,3.79184,3.98745,4.18528,4.38429,4.58342,4.78162,4.97781,5.17093,5.35995,5.54383,5.72158,0.52,0.53,0.54,0.55,0.56,0.57,0.58,0.59,0.6,0.61,0.62,0.63,0,5.25476e-303,-3.25082e-05,1.5965e-06,5.25118e-05,5.47798e-07,2.21843e-05,-1.3192e-06,-7.38605e-05,2.15248e-07,1.3852e-05,3.53713e-07,2.85313e-05,1.14588e-07,1.53151e-05,9.3391e-07,-4.5635e-05,2.54728e-07,-4.95482e-06,-7.34622e-07,1.30229e-05,1.95974e-06,-4.37714e-05,-4.47819e-07,2.00812e-05,2.00817e-07,-2.45464e-05,-1.6659e-08,4.34763e-06,3.21868e-08,-8.1635e-06,2.66919e-07,-6.79369e-05,3.52714e-10,-4.84865e-06,2.55585e-07,6.26812e-05,2.12518e-07,4.49455e-05,1.45831e-08,4.79089e-06,-1.43656e-08,-1.05984e-05,-3.82876e-09,-8.40197e-06,-4.45294e-10,-3.66476e-06,-3.0245e-11,-1.13942e-06,-1.34167e-12,-2.75634e-07,-4.15667e-14,-5.42564e-08,-9.21881e-16,-8.82593e-09,-2.211e-17,-1.6805e-09,0,5.01919e-303,0,0,3.91284e-270,5.19588e-303,-0.0380546,-0.216845,0.017062,0.220746,-0.0138944,-0.000439162,-0.0139725,-0.00163562,-0.0141152,-0.000686736,-0.0144984,-0.00015705,-0.0144497,-0.000518009,-0.0137319,-0.00118189,-0.01191,-0.00125407,0.253184,-0.0484943,-0.431108,0.0533181,0.0123808,0.0263625,
\ No newline at end of file
0.01,0.02,0.03,0.04,0.05,0.06,0.07,0.08,0.09,0.1,0.11,0.12,0.13,0.14,0.15,0.16,0.17,0.18,0.19,0.2,0.21,0.22,0.23,0.24,0.25,0.26,0.27,0.28,0.29,0.3,0.31,0.32,0.33,0.34,0.35,0.36,0.37,0.38,0.39,0.4,0.41,0.42,0.43,0.44,0.45,0.46,0.47,0.48,0.49,0.5,0.51,0.52,0.53,0.54,0.55,0.56,0.57,0.58,0.59,0.6,0.61,0.62,0.63,0.64,0.65,0.66,0.67,0.68,0.69,0.7,0.71,0.72,0.73,0.74,0.75,0.76,0.77,0.78,0.79,0.8,0.81,0.82,0.83,0.84,0.85,0.86,0.87,0.88,0.89,0.9,0.91,0.92,0.93,0.94,0.95,0.96,0.97,0.98,0.99,1,1.01,1.02,1.03,1.04,1.05,1.06,1.07,1.08,1.09,1.1,1.11,1.12,1.13,1.14,1.15,1.16,1.17,1.18,1.19,1.2,1.21,1.22,1.23,1.24,1.25,1.26,1.27,1.28,1.29,1.3,1.31,1.32,1.33,1.34,1.35,1.36,1.37,1.38,1.39,1.4,1.41,1.42,1.43,1.44,1.45,1.46,1.47,1.48,1.49,1.5,
\ No newline at end of file
This diff is collapsed.
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment