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

fix calcdxNorm crash

Browse Source
This commit is contained in:
Aik-Siong Koh
2024-04-24 16:22:29 -06:00
parent 5e19e4acd2
commit 18a8b5cecf
10 changed files with 464 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
.gitignore vendored
View File

@@ -39,3 +39,4 @@ assembly.asmt
build
cmake-build-debug
.idea
temp/

85
OndselSolver/ASMTAssembly.cpp
View File

@@ -441,6 +441,12 @@ void MbD::ASMTAssembly::runFile(const char* fileName)
}
}
void MbD::ASMTAssembly::runDraggingLogTest()
{
auto assembly = ASMTAssembly::assemblyFromFile("../testapp/runPreDrag.asmt");
assembly->runDraggingLog("../testapp/dragging.log");
}
void MbD::ASMTAssembly::runDraggingTest()
{
//auto assembly = ASMTAssembly::assemblyFromFile("../testapp/pistonWithLimits.asmt");
@@ -915,7 +921,12 @@ void MbD::ASMTAssembly::readTimes(std::vector<std::string>& lines)
assert(pos != std::string::npos);
str.erase(0, pos + substr.length());
times = readRowOfDoubles(str);
times->insert(times->begin(), times->at(0)); //The first element is the input state.
if (times->empty()) {
times->insert(times->begin(), 0.0); //The first element is the input state.
}
else {
times->insert(times->begin(), times->at(0)); //The first element is the input state.
}
lines.erase(lines.begin());
}
@@ -1035,6 +1046,47 @@ void MbD::ASMTAssembly::readMotionSeries(std::vector<std::string>& lines)
motion->readMotionSeries(lines);
}
void MbD::ASMTAssembly::runDraggingLog(const char* fileName)
{
std::ifstream stream(fileName);
if (stream.fail()) {
throw std::invalid_argument("File not found.");
}
std::string line;
std::vector<std::string> lines;
while (std::getline(stream, line)) {
lines.push_back(line);
}
assert(readStringOffTop(lines) == "runPreDrag");
runPreDrag();
while (lines[0].find("runDragStep") != std::string::npos)
{
assert(readStringOffTop(lines) == "runDragStep");
auto dragParts = std::make_shared<std::vector<std::shared_ptr<ASMTPart>>>();
while (lines[0].find("Name") != std::string::npos) {
assert(readStringOffTop(lines) == "Name");
auto dragPartName = readStringOffTop(lines);
std::string longerName = "/" + name + "/" + dragPartName;
auto dragPart = partAt(longerName);
dragParts->push_back(dragPart);
assert(readStringOffTop(lines) == "Position3D");
auto dragPartPosition3D = readColumnOfDoublesOffTop(lines);
dragPart->updateMbDFromPosition3D(dragPartPosition3D);
assert(readStringOffTop(lines) == "RotationMatrix");
auto dragPartRotationMatrix = std::make_shared<FullMatrix<double>>(3);
for (size_t i = 0; i < 3; i++)
{
auto row = readRowOfDoublesOffTop(lines);
dragPartRotationMatrix->atiput(i, row);
}
dragPart->updateMbDFromRotationMatrix(dragPartRotationMatrix);
}
runDragStep(dragParts);
}
assert(readStringOffTop(lines) == "runPostDrag");
runPostDrag();
}
void MbD::ASMTAssembly::outputFor(AnalysisType)
{
assert(false);
@@ -1236,15 +1288,10 @@ void MbD::ASMTAssembly::runPreDrag()
}
mbdSystem = std::make_shared<System>();
mbdSystem->externalSystem->asmtAssembly = this;
try {
mbdSystem->runPreDrag(mbdSystem);
}
catch (SimulationStoppingError ex) {
}
mbdSystem->runPreDrag(mbdSystem);
}
void MbD::ASMTAssembly::runDragStep(std::shared_ptr<std::vector<std::shared_ptr<ASMTPart>>> dragParts) const
void MbD::ASMTAssembly::runDragStep(std::shared_ptr<std::vector<std::shared_ptr<ASMTPart>>> dragParts)
{
if (debug) {
std::ofstream os("dragging.log", std::ios_base::app);
@@ -1264,7 +1311,12 @@ void MbD::ASMTAssembly::runDragStep(std::shared_ptr<std::vector<std::shared_ptr<
auto dragMbDPart = std::static_pointer_cast<Part>(dragPart->mbdObject);
dragMbDParts->push_back(dragMbDPart);
}
mbdSystem->runDragStep(dragMbDParts);
try {
mbdSystem->runDragStep(dragMbDParts);
}
catch (...) {
runPreDrag();
}
}
void MbD::ASMTAssembly::runPostDrag()
@@ -1278,12 +1330,7 @@ void MbD::ASMTAssembly::runPostDrag()
debug = false;
mbdSystem = std::make_shared<System>();
mbdSystem->externalSystem->asmtAssembly = this;
try {
mbdSystem->runPreDrag(mbdSystem);
}
catch (SimulationStoppingError ex) {
}
mbdSystem->runPreDrag(mbdSystem);
}
void MbD::ASMTAssembly::runKINEMATIC()
@@ -1318,6 +1365,14 @@ std::shared_ptr<ASMTSpatialContainer> MbD::ASMTAssembly::spatialContainerAt(std:
return part;
}
std::shared_ptr<ASMTPart> MbD::ASMTAssembly::partAt(std::string& longname) const
{
for (auto& part : *parts) {
if (part->fullName("") == longname) return part;
}
return nullptr;
}
std::shared_ptr<ASMTMarker> MbD::ASMTAssembly::markerAt(std::string& longname) const
{
for (auto& refPoint : *refPoints) {

5
OndselSolver/ASMTAssembly.h
View File

@@ -41,6 +41,7 @@ namespace MbD {
static void runSinglePendulum();
static std::shared_ptr<ASMTAssembly> assemblyFromFile(const char* chars);
static void runFile(const char* chars);
static void runDraggingLogTest();
static void runDraggingTest();
static void runDraggingTest2();
static void runDraggingTest3();
@@ -73,6 +74,7 @@ namespace MbD {
void readJointSeries(std::vector<std::string>& lines);
void readMotionSeriesMany(std::vector<std::string>& lines);
void readMotionSeries(std::vector<std::string>& lines);
void runDraggingLog(const char* chars);
void outputFor(AnalysisType type);
void preMbDrun(std::shared_ptr<System> mbdSys);
@@ -92,11 +94,12 @@ namespace MbD {
void solve();
void runPreDrag();
void runDragStep(std::shared_ptr<std::vector<std::shared_ptr<ASMTPart>>> dragParts) const;
void runDragStep(std::shared_ptr<std::vector<std::shared_ptr<ASMTPart>>> dragParts);
void runPostDrag();
void runKINEMATIC();
void initprincipalMassMarker();
std::shared_ptr<ASMTSpatialContainer> spatialContainerAt(std::shared_ptr<ASMTAssembly> self, std::string& longname) const;
std::shared_ptr<ASMTPart> partAt(std::string& longname) const;
std::shared_ptr<ASMTMarker> markerAt(std::string& longname) const;
std::shared_ptr<ASMTJoint> jointAt(std::string& longname) const;
std::shared_ptr<ASMTMotion> motionAt(std::string& longname) const;

12
OndselSolver/ASMTItem.cpp
View File

@@ -83,6 +83,12 @@ FRowDsptr MbD::ASMTItem::readRowOfDoubles(std::string& line)
return readRowOfDoubles;
}
FRowDsptr MbD::ASMTItem::readRowOfDoublesOffTop(std::vector<std::string>& lines)
{
auto str = popOffTop(lines);
return readRowOfDoubles(str);
}
FColDsptr MbD::ASMTItem::readColumnOfDoubles(std::string& line)
{
std::istringstream iss(line);
@@ -94,6 +100,12 @@ FColDsptr MbD::ASMTItem::readColumnOfDoubles(std::string& line)
return readColumnOfDoubles;
}
FColDsptr MbD::ASMTItem::readColumnOfDoublesOffTop(std::vector<std::string>& lines)
{
auto str = popOffTop(lines);
return readColumnOfDoubles(str);
}
double MbD::ASMTItem::readDouble(std::string& line)
{
std::istringstream iss(line);

6
OndselSolver/ASMTItem.h
View File

@@ -30,10 +30,12 @@ namespace MbD {
virtual std::string classname();
void setName(std::string str);
virtual void parseASMT(std::vector<std::string>& lines);
std::string popOffTop(std::vector<std::string>& args);
std::string readStringOffTop(std::vector<std::string>& args);
std::string popOffTop(std::vector<std::string>& lines);
std::string readStringOffTop(std::vector<std::string>& lines);
FRowDsptr readRowOfDoubles(std::string& line);
FRowDsptr readRowOfDoublesOffTop(std::vector<std::string>& lines);
FColDsptr readColumnOfDoubles(std::string& line);
FColDsptr readColumnOfDoublesOffTop(std::vector<std::string>& lines);
double readDouble(std::string& line);
int readInt(std::string& line);
size_t readSize_t(std::string& line);

4
OndselSolver/Part.cpp
View File

@@ -121,9 +121,9 @@ FColDsptr Part::qX()
return partFrame->qX;
}
void Part::qE(std::shared_ptr<EulerParameters<double>> x)
void Part::qE(std::shared_ptr<EulerParameters<double>> qe)
{
partFrame->qE = x;
partFrame->qE = qe;
}
std::shared_ptr<EulerParameters<double>> Part::qE()

6
OndselSolver/PosICDragLimitNewtonRaphson.cpp
View File

@@ -38,7 +38,11 @@ void MbD::PosICDragLimitNewtonRaphson::run()
{
preRun();
system->deactivateLimits();
if (system->limitsSatisfied()) return;
if (system->limitsSatisfied()) {
std::string str("MbD: No limits reached. ");
system->logString(str);
return;
}
auto limits = system->limits();
std::partition(limits->begin(), limits->end(), [](auto limit) { return !limit->satisfied(); });
//Violated limits are in front.

1
testapp/OndselSolver.cpp
View File

@@ -26,6 +26,7 @@ void sharedptrTest();
int main()
{
ASMTAssembly::runDraggingLogTest();
ASMTAssembly::runDraggingTest2();
ASMTAssembly::runDraggingTest3();
ASMTAssembly::runDraggingTest();

20
testapp/dragging.log Normal file
View File

@@ -0,0 +1,20 @@
runPreDrag
runDragStep
Name
Part1
Position3D
4.801714581503802e-15 0.099999999999995245 0.19999999999999998
RotationMatrix
1 -4.7573992180162559e-14 0
4.7573992180162559e-14 1 0
0 0 1
runDragStep
Name
Part1
Position3D
0.2633974596215562 0.063397459621556157 0
RotationMatrix
0.86602540378443871 -0.49999999999999994 0
0.49999999999999994 0.86602540378443871 0
0 0 1
runPostDrag

345
testapp/runPreDrag.asmt Normal file
View File

File diff suppressed because one or more lines are too long