kindred/solver
1
0
Fork 0
Code Issues 10 Pull Requests 1 Actions Packages Projects Releases Wiki Activity

MBDyn new joints, sine, cosine

Browse Source
This commit is contained in:
Aik-Siong Koh
2023-12-08 21:39:53 -07:00
parent 98d6d46ebd
commit 56bba4e921
58 changed files with 5895 additions and 1905 deletions
Download Patch File Download Diff File Expand all files Collapse all files

121
OndselSolver/MBDynDrive.cpp
View File

@@ -7,25 +7,7 @@ using namespace MbD;
void MbD::MBDynDrive::parseMBDyn(std::string line)
{
driveString = line;
size_t previousPos = 0;
auto pos = line.find(":");
auto front = line.substr(previousPos, pos - previousPos);
assert(front.find("drive caller") != std::string::npos);
auto arguments = std::vector<std::string>();
std::string argument;
while (true) {
previousPos = pos;
pos = line.find(",", pos + 1);
if (pos != std::string::npos) {
argument = line.substr(previousPos + 1, pos - previousPos - 1);
arguments.push_back(argument);
}
else {
argument = line.substr(previousPos + 1);
arguments.push_back(argument);
break;
}
}
arguments = collectArgumentsFor("drive caller", line);
auto iss = std::istringstream(arguments.at(0));
iss >> name;
arguments.erase(arguments.begin());
@@ -41,11 +23,8 @@ void MbD::MBDynDrive::parseMBDyn(std::string line)
void MbD::MBDynDrive::readFunction(std::vector<std::string>& args)
{
if (args.empty()) return;
std::string str;
auto iss = std::istringstream(args.at(0));
iss >> str;
if (str.find("ramp") != std::string::npos) {
args.erase(args.begin());
std::string str = readStringOffTop(args);
if (str == "ramp") {
std::string slope, initValue, initTime, finalTime;
slope = popOffTop(args);
initTime = popOffTop(args);
@@ -67,6 +46,100 @@ void MbD::MBDynDrive::readFunction(std::vector<std::string>& args)
<< initValue << " + " << slope << "*(" << finalTime << " - " << initTime << "))";
formula = ss.str();
}
else if (str == "cosine") {
std::string initial_time, angular_velocity, amplitude, number_of_cycles, initial_value;
initial_time = popOffTop(args);
angular_velocity = popOffTop(args);
amplitude = popOffTop(args);
number_of_cycles = popOffTop(args);
initial_value = popOffTop(args);
initial_time.erase(remove_if(initial_time.begin(), initial_time.end(), isspace), initial_time.end());
angular_velocity.erase(remove_if(angular_velocity.begin(), angular_velocity.end(), isspace), angular_velocity.end());
amplitude.erase(remove_if(amplitude.begin(), amplitude.end(), isspace), amplitude.end());
number_of_cycles.erase(remove_if(number_of_cycles.begin(), number_of_cycles.end(), isspace), number_of_cycles.end());
initial_value.erase(remove_if(initial_value.begin(), initial_value.end(), isspace), initial_value.end());
//f(t) = initial_value + amplitude * (1 ? cos (angular_velocity * (t ? initial_time)))
double nCycle;
if (number_of_cycles.find("forever") != std::string::npos) {
nCycle = 1.0e9;
}
else if (number_of_cycles.find("one") != std::string::npos) {
nCycle = 1.0;
}
else if (number_of_cycles.find("half") != std::string::npos) {
nCycle = 0.5;
}
else {
nCycle = stod(number_of_cycles);
}
double x0 = stod(initial_time);
double y0 = stod(initial_value);
double omega = stod(angular_velocity);
double amp = stod(amplitude);
double x1 = x0 + (2 * OS_M_PI * nCycle / omega);
double y1 = y0 + amp * (1.0 - std::cos(omega * (x1 - x0)));
double f1 = y0;
auto ss = std::stringstream();
ss << "(" << y0 << " + " << amp << "*(1.0 - cos(" << omega << "*(time - " << x0 << "))))";
std::string f2 = ss.str();
double f3 = y1;
double t1 = x0;
double t2 = x1;
ss = std::stringstream();
ss << "piecewise(time, functions(" << f1 << ", " << f2 << ", " << f3 << "), transitions(" << t1 << ", " << t2 << "))";
formula = ss.str();
}
else if (str == "sine") {
std::string initial_time, angular_velocity, amplitude, number_of_cycles, initial_value;
initial_time = popOffTop(args);
angular_velocity = popOffTop(args);
amplitude = popOffTop(args);
number_of_cycles = popOffTop(args);
initial_value = popOffTop(args);
initial_time.erase(remove_if(initial_time.begin(), initial_time.end(), isspace), initial_time.end());
angular_velocity.erase(remove_if(angular_velocity.begin(), angular_velocity.end(), isspace), angular_velocity.end());
amplitude.erase(remove_if(amplitude.begin(), amplitude.end(), isspace), amplitude.end());
number_of_cycles.erase(remove_if(number_of_cycles.begin(), number_of_cycles.end(), isspace), number_of_cycles.end());
initial_value.erase(remove_if(initial_value.begin(), initial_value.end(), isspace), initial_value.end());
//f(t) = initial_value + amplitude · sin (angular_velocity · (t − initial_time))
double nCycle;
if (number_of_cycles.find("forever") != std::string::npos) {
nCycle = 1.0e9;
}
else if (number_of_cycles.find("one") != std::string::npos) {
nCycle = 0.5; //Different from cosine
}
else if (number_of_cycles.find("half") != std::string::npos) {
nCycle = 0.25; //Different from cosine
}
else {
nCycle = stod(number_of_cycles);
if (nCycle < 0.0) {
nCycle -= 0.75;
}
else if (nCycle > 0.0) {
nCycle -= 0.5;
}
}
double x0 = stod(initial_time);
double y0 = stod(initial_value);
double omega = stod(angular_velocity);
double amp = stod(amplitude);
double x1 = x0 + (2 * OS_M_PI * nCycle / omega);
double y1 = y0 + amp * std::sin(omega * (x1 - x0));
double f1 = y0;
auto ss = std::stringstream();
ss << "(" << y0 << " + " << amp << "*sin(" << omega << "*(time - " << x0 << ")))";
std::string f2 = ss.str();
double f3 = y1;
double t1 = x0;
double t2 = x1;
ss = std::stringstream();
ss << "piecewise(time, functions(" << f1 << ", " << f2 << ", " << f3 << "), transitions(" << t1 << ", " << t2 << "))";
formula = ss.str();
}
else {
assert(false);
}