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

add runOndselDoublePendulum for simple case

Browse Source
This commit is contained in:
Aik-Siong Koh
2023-09-26 14:03:05 -06:00
parent 11cbcffacf
commit 2b1ec21b3a
16 changed files with 230 additions and 37 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
MbDCode/ASMTSpatialContainer.cpp
View File

@@ -336,7 +336,6 @@ void MbD::ASMTSpatialContainer::compareResults(AnalysisType type)
auto i = xs->size() - 1;
//Pos
if (!Numeric::equaltol(xs->at(i), inxs->at(i), lengthTol)) {
std::cout << i << " xs " << xs->at(i) << ", " << i<< lengthTol << std::endl;
std::cout << i << " xs " << xs->at(i) << ", " << inxs->at(i) << ", " << lengthTol << std::endl;
}
if (!Numeric::equaltol(ys->at(i), inys->at(i), lengthTol)) {

2
MbDCode/Array.h
View File

@@ -29,7 +29,7 @@ namespace MbD {
Array(std::vector<T> vec) : std::vector<T>(vec) {}
Array(int count) : std::vector<T>(count) {}
Array(int count, const T& value) : std::vector<T>(count, value) {}
Array(std::vector<T>::iterator begin, std::vector<T>::iterator end) : std::vector<T>(begin, end) {}
Array(typename typename std::vector<T>::iterator begin, typename typename std::vector<T>::iterator end) : std::vector<T>(begin, end) {}
Array(std::initializer_list<T> list) : std::vector<T>{ list } {}
virtual void initialize();
void copyFrom(std::shared_ptr<Array<T>> x);

175
MbDCode/CADSystem.cpp
View File

@@ -24,6 +24,7 @@
#include "PartFrame.h"
#include "Time.h"
#include "StateData.h"
#include "EulerParameters.h"
using namespace MbD;
@@ -48,6 +49,180 @@ void CADSystem::logString(double value)
{
}
void CADSystem::runOndselDoublePendulum()
{
//Double pendulum with easy input numbers for exact port from Smalltalk
//GEOAssembly calcCharacteristicDimensions must set mbdUnits to unity.
std::cout << "runOndselDoublePendulum" << std::endl;
auto& TheSystem = mbdSystem;
TheSystem->clear();
std::string name = "TheSystem";
TheSystem->name = name;
std::cout << "TheSystem->name " << TheSystem->name << std::endl;
auto& systemSolver = TheSystem->systemSolver;
systemSolver->errorTolPosKine = 1.0e-6;
systemSolver->errorTolAccKine = 1.0e-6;
systemSolver->iterMaxPosKine = 25;
systemSolver->iterMaxAccKine = 25;
systemSolver->tstart = 0.0;
systemSolver->tend = 0.0;
systemSolver->hmin = 1.0e-9;
systemSolver->hmax = 1.0;
systemSolver->hout = 0.04;
systemSolver->corAbsTol = 1.0e-6;
systemSolver->corRelTol = 1.0e-6;
systemSolver->intAbsTol = 1.0e-6;
systemSolver->intRelTol = 1.0e-6;
systemSolver->iterMaxDyn = 4;
systemSolver->orderMax = 5;
systemSolver->translationLimit = 1.0e10;
systemSolver->rotationLimit = 0.5;
std::string str;
FColDsptr qX, qE, qXdot, omeOpO, qXddot, alpOpO;
FColDsptr rpmp;
FMatDsptr aAap, aApm;
FRowDsptr fullRow;
//
auto assembly1 = CREATE<Part>::With("/Assembly1");
std::cout << "assembly1->name " << assembly1->name << std::endl;
assembly1->m = 0.0;
assembly1->aJ = std::make_shared<DiagonalMatrix<double>>(ListD{ 0, 0, 0 });
qX = std::make_shared<FullColumn<double>>(ListD{ 0, 0, 0 });
aAap = std::make_shared<FullMatrix<double>>(ListListD{
{ 1, 0, 0 },
{ 0, 1, 0 },
{ 0, 0, 1 }
});
assembly1->setqX(qX);
assembly1->setaAap(aAap);
qXdot = std::make_shared<FullColumn<double>>(ListD{ 0, 0, 0 });
omeOpO = std::make_shared<FullColumn<double>>(ListD{ 0, 0, 0 });
assembly1->setqXdot(qXdot);
assembly1->setomeOpO(omeOpO);
TheSystem->addPart(assembly1);
{
auto& partFrame = assembly1->partFrame;
auto marker2 = CREATE<MarkerFrame>::With("/Assembly1/Marker2");
rpmp = std::make_shared<FullColumn<double>>(ListD{ 0.0, 0.0, 0.0 });
marker2->setrpmp(rpmp);
aApm = std::make_shared<FullMatrix<double>>(ListListD{
{ 1, 0, 0 },
{ 0, 1, 0 },
{ 0, 0, 1 }
});
marker2->setaApm(aApm);
partFrame->addMarkerFrame(marker2);
//
auto marker1 = CREATE<MarkerFrame>::With("/Assembly1/Marker1");
rpmp = std::make_shared<FullColumn<double>>(ListD{ 0.0, 3.0, 0.0 });
marker1->setrpmp(rpmp);
aApm = std::make_shared<FullMatrix<double>>(ListListD{
{ 1, 0, 0 },
{ 0, 0, 1 },
{ 0, -1, 0 }
});
marker1->setaApm(aApm);
partFrame->addMarkerFrame(marker1);
}
assembly1->asFixed();
//
auto crankPart1 = CREATE<Part>::With("/Assembly1/Part1");
std::cout << "crankPart1->name " << crankPart1->name << std::endl;
crankPart1->m = 1.0;
crankPart1->aJ = std::make_shared<DiagonalMatrix<double>>(ListD{ 1, 1, 1 });
qX = std::make_shared<FullColumn<double>>(ListD{ 0.4, 0.0, -0.05 });
aAap = std::make_shared<FullMatrix<double>>(ListListD{
{ 1, 0, 0 },
{ 0, 1, 0 },
{ 0, 0, 1 }
});
crankPart1->setqX(qX);
crankPart1->setaAap(aAap);
qXdot = std::make_shared<FullColumn<double>>(ListD{ 0, 0, 0 });
omeOpO = std::make_shared<FullColumn<double>>(ListD{ 0, 0, 0 });
crankPart1->setqXdot(qXdot);
crankPart1->setomeOpO(omeOpO);
TheSystem->addPart(crankPart1);
{
auto& partFrame = crankPart1->partFrame;
auto marker1 = CREATE<MarkerFrame>::With("/Assembly1/Part1/Marker1");
rpmp = std::make_shared<FullColumn<double>>(ListD{ -0.4, 0.0, 0.05 });
marker1->setrpmp(rpmp);
aApm = std::make_shared<FullMatrix<double>>(ListListD{
{ 1, 0, 0 },
{ 0, 1, 0 },
{ 0, 0, 1 }
});
marker1->setaApm(aApm);
partFrame->addMarkerFrame(marker1);
//
auto marker2 = CREATE<MarkerFrame>::With("/Assembly1/Part1/Marker2");
rpmp = std::make_shared<FullColumn<double>>(ListD{ 0.4, 0.0, 0.05 });
marker2->setrpmp(rpmp);
aApm = std::make_shared<FullMatrix<double>>(ListListD{
{ 1, 0, 0 },
{ 0, 1, 0 },
{ 0, 0, 1 }
});
marker2->setaApm(aApm);
partFrame->addMarkerFrame(marker2);
}
//
auto conrodPart2 = CREATE<Part>::With("/Assembly1/Part2");
std::cout << "conrodPart2->name " << conrodPart2->name << std::endl;
conrodPart2->m = 1.0;
conrodPart2->aJ = std::make_shared<DiagonalMatrix<double>>(ListD{ 1, 1, 1 });
qX = std::make_shared<FullColumn<double>>(ListD{ 0.15, 0.1, 0.05 });
qE = std::make_shared<FullColumn<double>>(ListD{ 0.0, 0.0, 1.0, 0.0 });
auto eulerParameters = CREATE<EulerParameters<double>>::With(ListD{ 0.0, 0.0, 1.0, 0.0 });
eulerParameters->calcABC();
aAap = eulerParameters->aA;
conrodPart2->setqX(qX);
conrodPart2->setaAap(aAap);
qXdot = std::make_shared<FullColumn<double>>(ListD{ 0, 0, 0 });
omeOpO = std::make_shared<FullColumn<double>>(ListD{ 0, 0, 0 });
conrodPart2->setqXdot(qXdot);
conrodPart2->setomeOpO(omeOpO);
TheSystem->addPart(conrodPart2);
{
auto& partFrame = conrodPart2->partFrame;
auto marker1 = CREATE<MarkerFrame>::With("/Assembly1/Part2/Marker1");
rpmp = std::make_shared<FullColumn<double>>(ListD{ -0.65, 0.0, -0.05 });
marker1->setrpmp(rpmp);
aApm = std::make_shared<FullMatrix<double>>(ListListD{
{1.0, 0.0, 0.0},
{0.0, 1.0, 0.0},
{0.0, 0.0, 1.0}
});
marker1->setaApm(aApm);
partFrame->addMarkerFrame(marker1);
//
auto marker2 = CREATE<MarkerFrame>::With("/Assembly1/Part2/Marker2");
rpmp = std::make_shared<FullColumn<double>>(ListD{ 0.65, 0.0, -0.05 });
marker2->setrpmp(rpmp);
aApm = std::make_shared<FullMatrix<double>>(ListListD{
{1.0, 0.0, 0.0},
{0.0, 1.0, 0.0},
{0.0, 0.0, 1.0}
});
marker2->setaApm(aApm);
partFrame->addMarkerFrame(marker2);
}
//
auto revJoint1 = CREATE<RevoluteJoint>::With("/Assembly1/Joint1");
std::cout << "revJoint1->name " << revJoint1->name << std::endl;
revJoint1->connectsItoJ(assembly1->partFrame->endFrame("/Assembly1/Marker2"), crankPart1->partFrame->endFrame("/Assembly1/Part1/Marker1"));
TheSystem->addJoint(revJoint1);
auto revJoint2 = CREATE<RevoluteJoint>::With("/Assembly1/Joint2");
std::cout << "revJoint2->name " << revJoint2->name << std::endl;
revJoint2->connectsItoJ(crankPart1->partFrame->endFrame("/Assembly1/Part1/Marker2"), conrodPart2->partFrame->endFrame("/Assembly1/Part2/Marker1"));
TheSystem->addJoint(revJoint2);
//
TheSystem->runKINEMATIC(TheSystem);
}
void CADSystem::runOndselPiston()
{
//Piston with easy input numbers for exact port from Smalltalk

1
MbDCode/CADSystem.h
View File

@@ -27,6 +27,7 @@ namespace MbD {
void outputFor(AnalysisType type);
void logString(std::string& str);
void logString(double value);
void runOndselDoublePendulum();
void runOndselPiston();
void runPiston();
void preMbDrun(std::shared_ptr<System> mbdSys);

2
MbDCode/FullColumn.h
View File

@@ -33,7 +33,7 @@ namespace MbD {
FullColumn(std::vector<T> vec) : FullVector<T>(vec) {}
FullColumn(int count) : FullVector<T>(count) {}
FullColumn(int count, const T& value) : FullVector<T>(count, value) {}
FullColumn(std::vector<T>::iterator begin, std::vector<T>::iterator end) : FullVector<T>(begin, end) {}
FullColumn(typename std::vector<T>::iterator begin, typename std::vector<T>::iterator end) : FullVector<T>(begin, end) {}
FullColumn(std::initializer_list<T> list) : FullVector<T>{ list } {}
FColsptr<T> plusFullColumn(FColsptr<T> fullCol);
FColsptr<T> minusFullColumn(FColsptr<T> fullCol);

2
MbDCode/FullRow.h
View File

@@ -35,7 +35,7 @@ namespace MbD {
FullRow(std::vector<T> vec) : FullVector<T>(vec) {}
FullRow(int count) : FullVector<T>(count) {}
FullRow(int count, const T& value) : FullVector<T>(count, value) {}
FullRow(std::vector<T>::iterator begin, std::vector<T>::iterator end) : FullVector<T>(begin, end) {}
FullRow(typename std::vector<T>::iterator begin, typename std::vector<T>::iterator end) : FullVector<T>(begin, end) {}
FullRow(std::initializer_list<T> list) : FullVector<T>{ list } {}
FRowsptr<T> times(T a);
FRowsptr<T> negated();

2
MbDCode/FullVector.h
View File

@@ -21,7 +21,7 @@ namespace MbD {
FullVector(std::vector<T> vec) : Array<T>(vec) {}
FullVector(int count) : Array<T>(count) {}
FullVector(int count, const T& value) : Array<T>(count, value) {}
FullVector(std::vector<T>::iterator begin, std::vector<T>::iterator end) : Array<T>(begin, end) {}
FullVector(typename std::vector<T>::iterator begin, typename std::vector<T>::iterator end) : Array<T>(begin, end) {}
FullVector(std::initializer_list<T> list) : Array<T>{ list } {}
double dot(std::shared_ptr<FullVector<T>> vec);
void atiplusNumber(int i, T value);

36
MbDCode/GeneralSpline.cpp
View File

@@ -5,7 +5,7 @@
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#include <functional>
#include "GeneralSpline.h"
@@ -27,7 +27,7 @@ double MbD::GeneralSpline::getValue()
Symsptr MbD::GeneralSpline::differentiateWRTx()
{
auto self = clonesptr();
auto arg = std::static_pointer_cast<GeneralSpline>(self)->xx;
auto& arg = std::static_pointer_cast<GeneralSpline>(self)->xx;
auto deriv = std::make_shared<DifferentiatedGeneralSpline>(arg, self, 1);
return deriv;
}
@@ -35,14 +35,14 @@ Symsptr MbD::GeneralSpline::differentiateWRTx()
void MbD::GeneralSpline::arguments(Symsptr args)
{
auto array = args->getTerms();
auto arg = array->at(0);
auto& arg = array->at(0);
int order = (int)array->at(1)->getValue();
int n = (array->size() - 2) / 2;
int n = ((int)array->size() - 2) / 2;
auto xarray = std::make_shared<std::vector<double>>(n);
auto yarray = std::make_shared<std::vector<double>>(n);
for (int i = 0; i < n; i++)
{
size_t ii = 2 * (i + 1);
size_t ii = 2 * ((size_t)i + 1);
xarray->at(i) = array->at(ii)->getValue();
yarray->at(i) = array->at(ii + 1)->getValue();
}
@@ -63,7 +63,7 @@ void MbD::GeneralSpline::computeDerivatives()
{
//"See derivation in MbDTheory 9spline.fodt."
if (degree == 0) throw std::runtime_error("ToDo: Use ZeroDegreeSpline");
auto n = xs->size();
auto n = (int)xs->size();
auto p = degree;
auto np = n * p;
auto matrix = std::make_shared<SparseMatrix<double>>(np, np);
@@ -72,7 +72,7 @@ void MbD::GeneralSpline::computeDerivatives()
auto hmax = 0.0;
for (int i = 0; i < n - 1; i++)
{
auto h = xs->at(i + 1) - xs->at(i);
auto h = xs->at((size_t)i + 1) - xs->at(i);
hmax = std::max(hmax, std::abs(h));
hs->atiput(i, h);
}
@@ -94,7 +94,7 @@ void MbD::GeneralSpline::computeDerivatives()
matrix->atijput(offset + k - j, offset + k, dum);
}
}
bvector->atiput(offset, ys->at(i + 1) - ys->at(i));
bvector->atiput(offset, ys->at((size_t)i + 1) - ys->at(i));
}
if (isCyclic()) {
for (int j = 1; j < p + 1; j++)
@@ -127,7 +127,7 @@ void MbD::GeneralSpline::computeDerivatives()
}
for (int i = 0; i < n; i++)
{
auto derivsi = derivs->at(i);
auto& derivsi = derivs->at(i);
derivsi->equalArrayAt(derivsVector, (i - 1) * p + 1);
for (int j = 0; j < p; j++)
{
@@ -147,13 +147,13 @@ double MbD::GeneralSpline::derivativeAt(int n, double xxx)
//"d2ydx2(x) := d2ydx2i + d3ydx3i*hi + d4ydx4i*hi^2/2! +"
if (n > degree) return 0.0;
calcIndexAndDeltaFor(xxx);
auto derivsi = derivs->at(index);
auto& derivsi = derivs->at(index);
auto sum = 0.0;
for (int j = degree; j >= n + 1; j--)
{
sum = (sum + derivsi->at(j - 1)) * delta / (j - n);
sum = (sum + derivsi->at((size_t)j - 1)) * delta / (j - n);
}
return derivsi->at(n - 1) + sum;
return derivsi->at((size_t)n - 1) + sum;
}
void MbD::GeneralSpline::calcIndexAndDeltaFor(double xxx)
@@ -185,7 +185,7 @@ void MbD::GeneralSpline::calcNonCyclicIndexAndDelta()
}
else {
if (xvalue >= xLast) {
index = xs->size() - 1;
index = (int)xs->size() - 1;
delta = xvalue - xLast;
}
else {
@@ -196,8 +196,8 @@ void MbD::GeneralSpline::calcNonCyclicIndexAndDelta()
void MbD::GeneralSpline::calcIndexAndDelta()
{
if (!(index < xs->size() - 1) || !(xs->at(index) <= xvalue) || !(xvalue < xs->at(index + 1))) {
searchIndexFromto(0, xs->size()); //Using range.
if (!(index < xs->size() - 1) || !(xs->at(index) <= xvalue) || !(xvalue < xs->at((size_t)index + 1))) {
searchIndexFromto(0, (int)xs->size()); //Using range.
}
delta = xvalue - xs->at(index);
}
@@ -209,7 +209,7 @@ void MbD::GeneralSpline::searchIndexFromto(int first, int last)
index = first;
}
else {
auto middle = std::floor((first + last) / 2);
auto middle = (int)std::floor((first + last) / 2);
if (xvalue < xs->at(middle)) {
searchIndexFromto(first, middle);
}
@@ -229,11 +229,11 @@ double MbD::GeneralSpline::y(double xxx)
//"y(x) := yi + dydxi*hi + d2ydx2i*hi^2/2! + d3ydx3i*hi^3/3! +"
calcIndexAndDeltaFor(xxx);
auto derivsi = derivs->at(index);
auto& derivsi = derivs->at(index);
auto sum = 0.0;
for (int j = degree; j >= 1; j--)
{
sum = (sum + derivsi->at(j - 1)) * delta / j;
sum = (sum + derivsi->at((size_t)j - 1)) * delta / j;
}
return ys->at(index) + sum;
}

4
MbDCode/MarkerFrame.cpp
View File

@@ -243,9 +243,9 @@ void MarkerFrame::setrpmp(FColDsptr x)
rpmp->copyFrom(x);
}
void MarkerFrame::setaApm(FMatDsptr x)
void MarkerFrame::setaApm(FMatDsptr mat)
{
aApm->copyFrom(x);
aApm->copyFrom(mat);
}
void MarkerFrame::addEndFrame(EndFrmsptr endFrm)
{

2
MbDCode/MarkerFrame.h
View File

@@ -34,7 +34,7 @@ namespace MbD {
void setPartFrame(PartFrame* partFrm);
PartFrame* getPartFrame();
void setrpmp(FColDsptr x);
void setaApm(FMatDsptr x);
void setaApm(FMatDsptr mat);
void addEndFrame(EndFrmsptr x);
void initializeLocally() override;
void initializeGlobally() override;

23
MbDCode/MbDCode.cpp
View File

@@ -26,19 +26,20 @@ void runSpMat();
int main()
{
ASMTAssembly::runFile("piston.asmt");
ASMTAssembly::runFile("00backhoe.asmt");
ASMTAssembly::runFile("circular.asmt");
ASMTAssembly::runFile("cirpendu.asmt"); //Under constrained. Testing ICKine.
ASMTAssembly::runFile("engine1.asmt");
ASMTAssembly::runFile("fourbar.asmt");
ASMTAssembly::runFile("fourbot.asmt");
ASMTAssembly::runFile("wobpump.asmt");
//ASMTAssembly::runFile("piston.asmt");
//ASMTAssembly::runFile("00backhoe.asmt");
//ASMTAssembly::runFile("circular.asmt");
//ASMTAssembly::runFile("cirpendu.asmt"); //Under constrained. Testing ICKine.
//ASMTAssembly::runFile("engine1.asmt");
//ASMTAssembly::runFile("fourbar.asmt");
//ASMTAssembly::runFile("fourbot.asmt");
//ASMTAssembly::runFile("wobpump.asmt");
auto cadSystem = std::make_shared<CADSystem>();
cadSystem->runOndselPiston();
cadSystem->runPiston();
runSpMat();
cadSystem->runOndselDoublePendulum();
////cadSystem->runOndselPiston();
//cadSystem->runPiston();
//runSpMat();
}
void runSpMat() {

6
MbDCode/MbDCode.vcxproj
View File

@@ -76,6 +76,7 @@
<SDLCheck>true</SDLCheck>
<PreprocessorDefinitions>WIN32;_DEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
<ConformanceMode>true</ConformanceMode>
<MultiProcessorCompilation>true</MultiProcessorCompilation>
</ClCompile>
<Link>
<SubSystem>Console</SubSystem>
@@ -90,6 +91,7 @@
<SDLCheck>true</SDLCheck>
<PreprocessorDefinitions>WIN32;NDEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
<ConformanceMode>true</ConformanceMode>
<MultiProcessorCompilation>true</MultiProcessorCompilation>
</ClCompile>
<Link>
<SubSystem>Console</SubSystem>
@@ -104,6 +106,8 @@
<SDLCheck>true</SDLCheck>
<PreprocessorDefinitions>_DEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
<ConformanceMode>true</ConformanceMode>
<LanguageStandard>stdcpp17</LanguageStandard>
<MultiProcessorCompilation>true</MultiProcessorCompilation>
</ClCompile>
<Link>
<SubSystem>Console</SubSystem>
@@ -118,6 +122,8 @@
<SDLCheck>true</SDLCheck>
<PreprocessorDefinitions>NDEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
<ConformanceMode>true</ConformanceMode>
<LanguageStandard>stdcpp17</LanguageStandard>
<MultiProcessorCompilation>true</MultiProcessorCompilation>
</ClCompile>
<Link>
<SubSystem>Console</SubSystem>

4
MbDCode/Part.cpp
View File

@@ -61,6 +61,10 @@ FColDsptr Part::getqX() {
return partFrame->getqX();
}
void Part::setaAap(FMatDsptr mat) {
partFrame->setaAap(mat);
}
void Part::setqE(FColDsptr x) {
partFrame->setqE(x);
}

1
MbDCode/Part.h
View File

@@ -32,6 +32,7 @@ namespace MbD {
void initializeGlobally() override;
void setqX(FColDsptr x);
FColDsptr getqX();
void setaAap(FMatDsptr mat);
void setqE(FColDsptr x);
FColDsptr getqE();
void setqXdot(FColDsptr x);

5
MbDCode/PartFrame.cpp
View File

@@ -64,6 +64,11 @@ void PartFrame::setqE(FColDsptr x) {
qE->copyFrom(x);
}
void MbD::PartFrame::setaAap(FMatDsptr mat)
{
qE = mat->asEulerParameters();
}
FColDsptr PartFrame::getqE() {
return qE;
}

1
MbDCode/PartFrame.h
View File

@@ -42,6 +42,7 @@ namespace MbD {
void setqX(FColDsptr x);
FColDsptr getqX();
void setqE(FColDsptr x);
void setaAap(FMatDsptr mat);
FColDsptr getqE();
void setqXdot(FColDsptr x);
FColDsptr getqXdot();