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

second good build. Joints

Browse Source
This commit is contained in:
Aik-Siong Koh
2023-05-05 19:01:53 -06:00
parent 697aad4db9
commit d5ac041906
84 changed files with 1616 additions and 174 deletions
Download Patch File Download Diff File Expand all files Collapse all files

115
MbDCode/MbDCode.cpp
View File

@@ -1,4 +1,10 @@
#include <iostream>
/*********************************************************************
* @file MbDCode.cpp
*
* @brief Program to assemble a piston crank system.
*********************************************************************/
#include <iostream>
#include "System.h"
#include "FullColumn.h"
#include "FullMatrix.h"
@@ -7,6 +13,7 @@
#include "CylindricalJoint.h"
#include "RevoluteJoint.h"
#include "ZRotation.h"
#include "EndFrameqc.h"
#include "MbDCode.h"
using namespace MbD;
@@ -14,10 +21,10 @@ using namespace MbD;
int main()
{
std::cout << "Hello World!\n";
System& TheSystem = System::getInstance();
std::string str = "TheSystem";
TheSystem.setName(str);
//System& TheSystem = System::getInstance();
System& TheSystem = System::getInstance("TheSystem");
std::cout << "TheSystem.getName() " << TheSystem.getName() << std::endl;
std::string str;
FullColDptr qX, qE;
FullColDptr rpmp;
FullMatDptr aApm;
@@ -26,9 +33,7 @@ int main()
fullRow = std::make_shared<FullRow<double>>(4);
fullRow->copy(row);
//
auto assembly1 = std::make_shared<Part>();
str = "Assembly1";
assembly1->setName(str);
auto assembly1 = std::make_shared<Part>("Assembly1");
std::cout << "assembly1->getName() " << assembly1->getName() << std::endl;
qX = std::make_shared<FullColumn<double>>(ListD{ 0, 0, 0 });
qE = std::make_shared<FullColumn<double>>(ListD{ 0, 0, 0, 1 });
@@ -38,9 +43,8 @@ int main()
std::cout << "assembly1->getqE() " << assembly1->getqE()->toString() << std::endl;
TheSystem.addPart(assembly1);
{
auto marker1 = std::make_shared<MarkerFrame>();
str = "Marker1";
marker1->setName(str);
auto partFrame = assembly1->partFrame;
auto marker1 = std::make_shared<MarkerFrame>("Marker1");
rpmp = std::make_shared<FullColumn<double>>(ListD{ 0.38423366582893, 6.8384291794733e-9, -0.048029210642807 });
marker1->setrpmp(rpmp);
aApm = std::make_shared<FullMatrix<double>>(ListListD{
@@ -49,11 +53,9 @@ int main()
{ 0, -1, 0 }
});
marker1->setaApm(aApm);
assembly1->partFrame->addMarkerFrame(marker1);
partFrame->addMarkerFrame(marker1);
//
auto marker2 = std::make_shared<MarkerFrame>();
str = "Marker2";
marker2->setName(str);
auto marker2 = std::make_shared<MarkerFrame>("Marker2");
rpmp = std::make_shared<FullColumn<double>>(ListD{ 0.0, 0.0, 0.0 });
marker2->setrpmp(rpmp);
aApm = std::make_shared<FullMatrix<double>>(ListListD{
@@ -62,21 +64,18 @@ int main()
{ 0, 0, 1 }
});
marker2->setaApm(aApm);
assembly1->partFrame->addMarkerFrame(marker2);
partFrame->addMarkerFrame(marker2);
}
//
auto part1 = std::make_shared<Part>();
str = "Part1";
part1->setName(str);
auto part1 = std::make_shared<Part>("Part1");
qX = std::make_shared<FullColumn<double>>(ListD{ 0.38423366582893, 6.8384291794733e-9, -0.048029210642807 });
qE = std::make_shared<FullColumn<double>>(ListD{ 0.0, 0.0, 1.4248456266393e-10, 1.0 });
part1->setqX(qX);
part1->setqE(qE);
TheSystem.parts.push_back(part1);
TheSystem.parts->push_back(part1);
{
auto marker1 = std::make_shared<MarkerFrame>();
str = "Marker1";
marker1->setName(str);
auto partFrame = part1->partFrame;
auto marker1 = std::make_shared<MarkerFrame>("Marker1");
rpmp = std::make_shared<FullColumn<double>>(ListD{ -0.38423368514246, -2.6661567755108e-17, 0.048029210642807 });
marker1->setrpmp(rpmp);
aApm = std::make_shared<FullMatrix<double>>(ListListD{
@@ -85,11 +84,9 @@ int main()
{ 0, 0, 1 }
});
marker1->setaApm(aApm);
part1->partFrame->addMarkerFrame(marker1);
partFrame->addMarkerFrame(marker1);
//
auto marker2 = std::make_shared<MarkerFrame>();
str = "Marker2";
marker2->setName(str);
auto marker2 = std::make_shared<MarkerFrame>("Marker2");
rpmp = std::make_shared<FullColumn<double>>(ListD{ 0.38423368514246, -2.6661567755108e-17, 0.048029210642807 });
marker2->setrpmp(rpmp);
aApm = std::make_shared<FullMatrix<double>>(ListListD{
@@ -98,21 +95,18 @@ int main()
{ 0, 0, 1 }
});
marker2->setaApm(aApm);
part1->partFrame->addMarkerFrame(marker2);
partFrame->addMarkerFrame(marker2);
}
//
auto part2 = std::make_shared<Part>();
str = "Part2";
part2->setName(str);
auto part2 = std::make_shared<Part>("Part2");
qX = std::make_shared<FullColumn<double>>(ListD{ 0.38423366582893, 0.49215308269277, 0.048029210642807 });
qE = std::make_shared<FullColumn<double>>(ListD{ 0.0, 0.0, 0.89871701272344, 0.4385290538168 });
part2->setqX(qX);
part2->setqE(qE);
TheSystem.parts.push_back(part2);
TheSystem.parts->push_back(part2);
{
auto marker1 = std::make_shared<MarkerFrame>();
str = "Marker1";
marker1->setName(str);
auto partFrame = part2->partFrame;
auto marker1 = std::make_shared<MarkerFrame>("Marker1");
rpmp = std::make_shared<FullColumn<double>>(ListD{ -0.6243797383565, 1.1997705489799e-16, -0.048029210642807 });
marker1->setrpmp(rpmp);
aApm = std::make_shared<FullMatrix<double>>(ListListD{
@@ -121,11 +115,9 @@ int main()
{0.0, 0.0, 1.0}
});
marker1->setaApm(aApm);
part2->partFrame->addMarkerFrame(marker1);
partFrame->addMarkerFrame(marker1);
//
auto marker2 = std::make_shared<MarkerFrame>();
str = "Marker2";
marker2->setName(str);
auto marker2 = std::make_shared<MarkerFrame>("Marker2");
rpmp = std::make_shared<FullColumn<double>>(ListD{ 0.6243797383565, -2.1329254204087e-16, -0.048029210642807 });
marker2->setrpmp(rpmp);
aApm = std::make_shared<FullMatrix<double>>(ListListD{
@@ -134,21 +126,18 @@ int main()
{-2.2204460492503e-16, -4.1633363423443e-17, 1.0}
});
marker2->setaApm(aApm);
part2->partFrame->addMarkerFrame(marker2);
partFrame->addMarkerFrame(marker2);
}
//
auto part3 = std::make_shared<Part>();
str = "Part3";
part3->setName(str);
auto part3 = std::make_shared<Part>("Part3");
qX = std::make_shared<FullColumn<double>>(ListD{ -1.284772285311e-18, 1.4645982581368, -4.788228906425e-17 });
qE = std::make_shared<FullColumn<double>>(ListD{ 0.70710678118655, 0.70710678118655, 0.0, 0.0 });
part3->setqX(qX);
part3->setqE(qE);
TheSystem.parts.push_back(part3);
TheSystem.parts->push_back(part3);
{
auto marker1 = std::make_shared<MarkerFrame>();
str = "Marker1";
marker1->setName(str);
auto partFrame = part3->partFrame;
auto marker1 = std::make_shared<MarkerFrame>("Marker1");
rpmp = std::make_shared<FullColumn<double>>(ListD{ -0.48029210642807, 7.6201599718927e-18, -2.816737703896e-17 });
marker1->setrpmp(rpmp);
aApm = std::make_shared<FullMatrix<double>>(ListListD{
@@ -157,11 +146,9 @@ int main()
{-1.0785207688569e-32, 2.2204460492503e-16, -1.0}
});
marker1->setaApm(aApm);
part3->partFrame->addMarkerFrame(marker1);
partFrame->addMarkerFrame(marker1);
//
auto marker2 = std::make_shared<MarkerFrame>();
str = "Marker2";
marker2->setName(str);
auto marker2 = std::make_shared<MarkerFrame>("Marker2");
rpmp = std::make_shared<FullColumn<double>>(ListD{ 0.48029210642807, 1.7618247880058e-17, 2.5155758471256e-17 });
marker2->setrpmp(rpmp);
aApm = std::make_shared<FullMatrix<double>>(ListListD{
@@ -170,12 +157,28 @@ int main()
{-6.9388939039072e-18, 1.0, -7.4837411882581e-50}
});
marker2->setaApm(aApm);
part3->partFrame->addMarkerFrame(marker2);
partFrame->addMarkerFrame(marker2);
}
//
auto cylJoint4 = std::shared_ptr<CylindricalJoint>();
auto revJoint3 = std::shared_ptr<RevoluteJoint>();
auto revJoint2 = std::shared_ptr<RevoluteJoint>();
auto revJoint1 = std::shared_ptr<RevoluteJoint>();
auto rotMotion1 = std::shared_ptr<ZRotation>();
auto cylJoint4 = std::make_shared<CylindricalJoint>("CylJoint4");
cylJoint4->connectsItoJ(part3->partFrame->endFrame("Marker2"), assembly1->partFrame->endFrame("Marker1"));
TheSystem.jointsMotions->push_back(cylJoint4);
auto revJoint3 = std::make_shared<RevoluteJoint>("RevJoint3");
revJoint3->connectsItoJ(part2->partFrame->endFrame("Marker2"), part3->partFrame->endFrame("Marker1"));
TheSystem.jointsMotions->push_back(revJoint3);
auto revJoint2 = std::make_shared<RevoluteJoint>("RevJoint2");
revJoint2->connectsItoJ(part1->partFrame->endFrame("Marker2"), part2->partFrame->endFrame("Marker1"));
TheSystem.jointsMotions->push_back(revJoint2);
auto revJoint1 = std::make_shared<RevoluteJoint>("RevJoint1");
revJoint1->connectsItoJ(assembly1->partFrame->endFrame("Marker2"), part1->partFrame->endFrame("Marker1"));
TheSystem.jointsMotions->push_back(revJoint1);
auto rotMotion1 = std::make_shared<ZRotation>("RotMotion1");
rotMotion1->connectsItoJ(assembly1->partFrame->endFrame("Marker2"), part1->partFrame->endFrame("Marker1"));
TheSystem.jointsMotions->push_back(rotMotion1);
//
TheSystem.runKINEMATICS();
}