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

crank_slider.mbd MBDyn file working

Browse Source
This commit is contained in:
Aik-Siong Koh
2023-10-13 13:01:39 -06:00
parent e4e98755db
commit 84cb979354
95 changed files with 2693 additions and 558 deletions
Download Patch File Download Diff File Expand all files Collapse all files

113
OndselSolver/ASMTAssembly.cpp
View File

@@ -43,6 +43,11 @@
using namespace MbD;
MbD::ASMTAssembly::ASMTAssembly() : ASMTSpatialContainer()
{
times = std::make_shared<FullRow<double>>();
}
void MbD::ASMTAssembly::runSinglePendulumSuperSimplified()
{
//In this version we skip declaration of variables that don't need as they use default values.
@@ -82,6 +87,64 @@ void MbD::ASMTAssembly::runSinglePendulumSuperSimplified()
assembly->runKINEMATIC();
}
void MbD::ASMTAssembly::runSinglePendulumSuperSimplified2()
{
//In this version we skip declaration of variables that don't need as they use default values.
auto assembly = CREATE<ASMTAssembly>::With();
assembly->setName("OndselAssembly");
auto mkr = CREATE<ASMTMarker>::With();
mkr->setName("marker1");
assembly->addMarker(mkr);
auto part = CREATE<ASMTPart>::With();
part->setName("part1");
assembly->addPart(part);
auto marker1 = CREATE<ASMTMarker>::With();
marker1->setName("FixingMarker");
part->addMarker(marker1);
auto marker2 = CREATE<ASMTMarker>::With();
marker2->setName("marker2");
marker2->setPosition3D(20.0, 10.0, 0.0);
part->addMarker(marker2);
auto part2 = CREATE<ASMTPart>::With();
part2->setName("part2");
part2->setPosition3D(20.0, 10.0, 0.0);
assembly->addPart(part2);
auto marker3 = CREATE<ASMTMarker>::With();
marker3->setName("marker2");
marker3->setPosition3D(50.0, 10.0, 0.0);
part2->addMarker(marker3);
/*Ground joint*/
auto joint = CREATE<ASMTFixedJoint>::With();
joint->setName("Joint1");
joint->setMarkerI("/OndselAssembly/marker1");
joint->setMarkerJ("/OndselAssembly/part1/FixingMarker");
assembly->addJoint(joint);
auto joint2 = CREATE<ASMTRevoluteJoint>::With();
joint2->setName("Joint2");
joint2->setMarkerI("/OndselAssembly/part1/marker2");
joint2->setMarkerJ("/OndselAssembly/part2/marker2");
assembly->addJoint(joint2);
auto simulationParameters = CREATE<ASMTSimulationParameters>::With();
simulationParameters->settstart(0.0);
simulationParameters->settend(0.0); //tstart == tend Initial Conditions only.
simulationParameters->sethmin(1.0e-9);
simulationParameters->sethmax(1.0);
simulationParameters->sethout(0.04);
simulationParameters->seterrorTol(1.0e-6);
assembly->setSimulationParameters(simulationParameters);
assembly->runKINEMATIC();
}
void MbD::ASMTAssembly::runSinglePendulumSimplified()
{
auto assembly = CREATE<ASMTAssembly>::With();
@@ -96,7 +159,7 @@ void MbD::ASMTAssembly::runSinglePendulumSimplified()
assembly->setVelocity3D(0, 0, 0);
assembly->setOmega3D(0, 0, 0);
auto massMarker = CREATE<ASMTPrincipalMassMarker>::With();
auto massMarker = std::make_shared<ASMTPrincipalMassMarker>();
massMarker->setMass(0.0);
massMarker->setDensity(0.0);
massMarker->setMomentOfInertias(0, 0, 0);
@@ -110,7 +173,7 @@ void MbD::ASMTAssembly::runSinglePendulumSimplified()
auto mkr = CREATE<ASMTMarker>::With();
mkr->setName("Marker1");
mkr->setPosition3D(0, 0, 0);
massMarker->setRotationMatrix(
mkr->setRotationMatrix(
1, 0, 0,
0, 1, 0,
0, 0, 1);
@@ -127,7 +190,7 @@ void MbD::ASMTAssembly::runSinglePendulumSimplified()
part->setOmega3D(0, 0, 0);
assembly->addPart(part);
massMarker = CREATE<ASMTPrincipalMassMarker>::With();
massMarker = std::make_shared<ASMTPrincipalMassMarker>();
massMarker->setMass(0.2);
massMarker->setDensity(10.0);
massMarker->setMomentOfInertias(8.3333333333333e-4, 0.016833333333333, 0.017333333333333);
@@ -195,7 +258,7 @@ void MbD::ASMTAssembly::runSinglePendulum()
auto ome3D = std::make_shared<FullColumn<double>>(ListD{ 0, 0, 0 });
assembly->setOmega3D(ome3D);
//
auto massMarker = CREATE<ASMTPrincipalMassMarker>::With();
auto massMarker = std::make_shared<ASMTPrincipalMassMarker>();
massMarker->setMass(0.0);
massMarker->setDensity(0.0);
auto aJ = std::make_shared<DiagonalMatrix<double>>(ListD{ 0, 0, 0 });
@@ -220,7 +283,7 @@ void MbD::ASMTAssembly::runSinglePendulum()
{ 0, 1, 0 },
{ 0, 0, 1 }
});
massMarker->setRotationMatrix(rotMat);
mkr->setRotationMatrix(rotMat);
assembly->addMarker(mkr);
//
auto part = CREATE<ASMTPart>::With();
@@ -240,7 +303,7 @@ void MbD::ASMTAssembly::runSinglePendulum()
part->setOmega3D(ome3D);
assembly->addPart(part);
//
massMarker = CREATE<ASMTPrincipalMassMarker>::With();
massMarker = std::make_shared<ASMTPrincipalMassMarker>();
massMarker->setMass(0.2);
massMarker->setDensity(10.0);
aJ = std::make_shared<DiagonalMatrix<double>>(ListD{ 8.3333333333333e-4, 0.016833333333333, 0.017333333333333 });
@@ -729,6 +792,7 @@ void MbD::ASMTAssembly::calcCharacteristicDimensions()
auto unitLength = this->calcCharacteristicLength();
auto unitAngle = 1.0;
this->mbdUnits = std::make_shared<Units>(unitTime, unitMass, unitLength, unitAngle);
this->mbdUnits = std::make_shared<Units>(1.0, 1.0, 1.0, 1.0); //for debug
}
double MbD::ASMTAssembly::calcCharacteristicTime()
@@ -861,31 +925,20 @@ void MbD::ASMTAssembly::solve()
void MbD::ASMTAssembly::runKINEMATIC()
{
auto mbdSystem = CREATE<System>::With();
auto mbdSystem = std::make_shared<System>();
mbdObject = mbdSystem;
mbdSystem->externalSystem->asmtAssembly = this;
mbdSystem->runKINEMATIC(mbdSystem);
}
//
//void MbD::ASMTAssembly::initprincipalMassMarker()
//{
// //Choose first refPoint as center of mass
// assert(!refPoints->empty());
// auto refPoint = refPoints->at(0);
// principalMassMarker = CREATE<ASMTPrincipalMassMarker>::With();
// principalMassMarker->position3D = refPoint->position3D;
// principalMassMarker->rotationMatrix = refPoint->rotationMatrix;
//}
void MbD::ASMTAssembly::initprincipalMassMarker()
{
principalMassMarker = CREATE<ASMTPrincipalMassMarker>::With();
principalMassMarker = std::make_shared<ASMTPrincipalMassMarker>();
principalMassMarker->mass = 0.0;
principalMassMarker->density = 0.0;
principalMassMarker->momentOfInertias = std::make_shared<DiagonalMatrix<double>>(3, 0);
//principalMassMarker->position3D = std::make_shared<FullColumn<double>>(3, 0);
//principalMassMarker->rotationMatrix = std::make_shared<FullMatrix<double>>(3, 3);
//principalMassMarker->rotationMatrix->identity();
//principalMassMarker->rotationMatrix = FullMatrix<double>>::identitysptr(3);
}
std::shared_ptr<ASMTSpatialContainer> MbD::ASMTAssembly::spatialContainerAt(std::shared_ptr<ASMTAssembly> self, std::string& longname)
@@ -961,6 +1014,7 @@ void MbD::ASMTAssembly::updateFromMbD()
{
ASMTSpatialContainer::updateFromMbD();
times->push_back(asmtTime->getValue());
std::cout << "Time = " << asmtTime->getValue() << std::endl;
for (auto& part : *parts) part->updateFromMbD();
for (auto& joint : *joints) joint->updateFromMbD();
for (auto& motion : *motions) motion->updateFromMbD();
@@ -1016,4 +1070,23 @@ void MbD::ASMTAssembly::setSimulationParameters(std::shared_ptr<ASMTSimulationPa
parameters->owner = this;
}
std::shared_ptr<ASMTPart> MbD::ASMTAssembly::partNamed(std::string partName)
{
auto it = std::find_if(parts->begin(), parts->end(), [&](const std::shared_ptr<ASMTPart>& prt) {
return prt->fullName("") == partName;
});
auto& part = *it;
return part;
}
std::shared_ptr<ASMTPart> MbD::ASMTAssembly::partPartialNamed(std::string partialName)
{
auto it = std::find_if(parts->begin(), parts->end(), [&](const std::shared_ptr<ASMTPart>& prt) {
auto fullName = prt->fullName("");
return fullName.find(partialName) != std::string::npos;
});
auto part = *it;
return part;
}

7
OndselSolver/ASMTAssembly.h
View File

@@ -12,6 +12,7 @@
#include "ASMTSpatialContainer.h"
#include "FullColumn.h"
#include "FullMatrix.h"
#include "MBDynSystem.h"
namespace MbD {
class ASMTRefPoint;
@@ -35,7 +36,9 @@ namespace MbD {
{
//
public:
ASMTAssembly();
static void runSinglePendulumSuperSimplified();
static void runSinglePendulumSuperSimplified2();
static void runSinglePendulumSimplified();
static void runSinglePendulum();
static void runFile(const char* chars);
@@ -101,6 +104,8 @@ namespace MbD {
void addMotion(std::shared_ptr<ASMTMotion> motion);
void setConstantGravity(std::shared_ptr<ASMTConstantGravity> constantGravity);
void setSimulationParameters(std::shared_ptr<ASMTSimulationParameters> simulationParameters);
std::shared_ptr<ASMTPart> partNamed(std::string partName);
std::shared_ptr<ASMTPart> partPartialNamed(std::string partialName);
std::string notes;
std::shared_ptr<std::vector<std::shared_ptr<ASMTPart>>> parts = std::make_shared<std::vector<std::shared_ptr<ASMTPart>>>();
@@ -115,7 +120,7 @@ namespace MbD {
std::shared_ptr<std::vector<double>> times;
std::shared_ptr<ASMTTime> asmtTime = std::make_shared<ASMTTime>();
std::shared_ptr<Units> mbdUnits;
MBDynSystem* mbdynItem = nullptr;
};
}

2
OndselSolver/ASMTFixedJoint.h
View File

@@ -16,7 +16,7 @@ namespace MbD {
{
//
public:
virtual std::shared_ptr<Joint> mbdClassNew() override;
std::shared_ptr<Joint> mbdClassNew() override;
};
}

18
OndselSolver/ASMTGeneralMotion.cpp
View File

@@ -30,7 +30,7 @@ void MbD::ASMTGeneralMotion::parseASMT(std::vector<std::string>& lines)
void MbD::ASMTGeneralMotion::readrIJI(std::vector<std::string>& lines)
{
rIJI = std::make_shared<std::vector<std::string>>(3);
rIJI = std::make_shared<FullColumn<std::string>>(3);
assert(lines[0].find("rIJI1") != std::string::npos);
lines.erase(lines.begin());
@@ -48,7 +48,7 @@ void MbD::ASMTGeneralMotion::readrIJI(std::vector<std::string>& lines)
void MbD::ASMTGeneralMotion::readangIJJ(std::vector<std::string>& lines)
{
angIJJ = std::make_shared<std::vector<std::string>>(3);
angIJJ = std::make_shared<FullColumn<std::string>>(3);
assert(lines[0].find("angIJJ1") != std::string::npos);
lines.erase(lines.begin());
@@ -80,7 +80,7 @@ std::shared_ptr<Joint> MbD::ASMTGeneralMotion::mbdClassNew()
void MbD::ASMTGeneralMotion::createMbD(std::shared_ptr<System> mbdSys, std::shared_ptr<Units> mbdUnits)
{
ASMTMotion::createMbD(mbdSys, mbdUnits);
auto parser = CREATE<SymbolicParser>::With();
auto parser = std::make_shared<SymbolicParser>();
parser->owner = this;
auto geoTime = owner->root()->geoTime();
parser->variables->insert(std::make_pair("time", geoTime));
@@ -88,21 +88,21 @@ void MbD::ASMTGeneralMotion::createMbD(std::shared_ptr<System> mbdSys, std::shar
auto fullMotion = std::static_pointer_cast<FullMotion>(mbdObject);
//rIJI
userFunc = CREATE<BasicUserFunction>::With(rIJI->at(0), 1.0);
userFunc = std::make_shared<BasicUserFunction>(rIJI->at(0), 1.0);
parser->parseUserFunction(userFunc);
auto geoX = parser->stack->top();
geoX = Symbolic::times(geoX, std::make_shared<Constant>(1.0 / mbdUnits->length));
geoX->createMbD(mbdSys, mbdUnits);
auto xBlk = geoX->simplified(geoX);
userFunc = CREATE<BasicUserFunction>::With(rIJI->at(1), 1.0);
userFunc = std::make_shared<BasicUserFunction>(rIJI->at(1), 1.0);
parser->parseUserFunction(userFunc);
auto geoY = parser->stack->top();
geoY = Symbolic::times(geoY, std::make_shared<Constant>(1.0 / mbdUnits->length));
geoY->createMbD(mbdSys, mbdUnits);
auto yBlk = geoY->simplified(geoY);
userFunc = CREATE<BasicUserFunction>::With(rIJI->at(2), 1.0);
userFunc = std::make_shared<BasicUserFunction>(rIJI->at(2), 1.0);
parser->parseUserFunction(userFunc);
auto geoZ = parser->stack->top();
geoZ = Symbolic::times(geoZ, std::make_shared<Constant>(1.0 / mbdUnits->length));
@@ -113,21 +113,21 @@ void MbD::ASMTGeneralMotion::createMbD(std::shared_ptr<System> mbdSys, std::shar
fullMotion->frIJI = std::make_shared<FullColumn<Symsptr>>(xyzBlkList);
//angIJJ
userFunc = CREATE<BasicUserFunction>::With(angIJJ->at(0), 1.0);
userFunc = std::make_shared<BasicUserFunction>(angIJJ->at(0), 1.0);
parser->parseUserFunction(userFunc);
auto geoPhi = parser->stack->top();
geoPhi = Symbolic::times(geoPhi, std::make_shared<Constant>(1.0 / mbdUnits->angle));
geoPhi->createMbD(mbdSys, mbdUnits);
auto phiBlk = geoPhi->simplified(geoPhi);
userFunc = CREATE<BasicUserFunction>::With(angIJJ->at(1), 1.0);
userFunc = std::make_shared<BasicUserFunction>(angIJJ->at(1), 1.0);
parser->parseUserFunction(userFunc);
auto geoThe = parser->stack->top();
geoThe = Symbolic::times(geoThe, std::make_shared<Constant>(1.0 / mbdUnits->angle));
geoThe->createMbD(mbdSys, mbdUnits);
auto theBlk = geoThe->simplified(geoThe);
userFunc = CREATE<BasicUserFunction>::With(angIJJ->at(2), 1.0);
userFunc = std::make_shared<BasicUserFunction>(angIJJ->at(2), 1.0);
parser->parseUserFunction(userFunc);
auto geoPsi = parser->stack->top();
geoPsi = Symbolic::times(geoPsi, std::make_shared<Constant>(1.0 / mbdUnits->angle));

2
OndselSolver/ASMTGeneralMotion.h
View File

@@ -22,7 +22,7 @@ namespace MbD {
std::shared_ptr<Joint> mbdClassNew() override;
void createMbD(std::shared_ptr<System> mbdSys, std::shared_ptr<Units> mbdUnits) override;
std::shared_ptr<std::vector<std::string>> rIJI, angIJJ;
std::shared_ptr<FullColumn<std::string>> rIJI, angIJJ;
std::string rotationOrder;
};

12
OndselSolver/ASMTItem.cpp
View File

@@ -19,7 +19,12 @@ ASMTAssembly* MbD::ASMTItem::root()
return owner->root();
}
std::shared_ptr<ASMTSpatialContainer> MbD::ASMTItem::part()
ASMTSpatialContainer* MbD::ASMTItem::partOrAssembly()
{
return owner->partOrAssembly();
}
ASMTPart* MbD::ASMTItem::part()
{
return owner->part();
}
@@ -28,6 +33,11 @@ void MbD::ASMTItem::initialize()
{
}
void MbD::ASMTItem::noop()
{
//No Operations
}
void MbD::ASMTItem::setName(std::string str)
{
name = str;

7
OndselSolver/ASMTItem.h
View File

@@ -14,15 +14,18 @@ namespace MbD {
class ASMTAssembly;
class Units;
class ASMTSpatialContainer;
class ASMTPart;
class EXPORT ASMTItem
{
//
public:
virtual ASMTAssembly* root();
virtual std::shared_ptr<ASMTSpatialContainer> part();
virtual ASMTSpatialContainer* partOrAssembly();
virtual ASMTPart* part();
virtual void initialize();
void noop();
void setName(std::string str);
virtual void parseASMT(std::vector<std::string>& lines);
FRowDsptr readRowOfDoubles(std::string& line);
@@ -43,7 +46,7 @@ namespace MbD {
std::shared_ptr<Constant> sptrConstant(double value);
std::string name;
ASMTItem* owner;
ASMTItem* owner = nullptr;
std::shared_ptr<Item> mbdObject;
};
}

10
OndselSolver/ASMTItemIJ.cpp
View File

@@ -8,6 +8,16 @@
#include "ASMTItemIJ.h"
MbD::ASMTItemIJ::ASMTItemIJ()
{
fxs = std::make_shared<FullRow<double>>();
fys = std::make_shared<FullRow<double>>();
fzs = std::make_shared<FullRow<double>>();
txs = std::make_shared<FullRow<double>>();
tys = std::make_shared<FullRow<double>>();
tzs = std::make_shared<FullRow<double>>();
}
void MbD::ASMTItemIJ::initialize()
{
fxs = std::make_shared<FullRow<double>>();

1
OndselSolver/ASMTItemIJ.h
View File

@@ -15,6 +15,7 @@ namespace MbD {
{
//
public:
ASMTItemIJ();
void initialize() override;
void setMarkerI(std::string mkrI);
void setMarkerJ(std::string mkrJ);

2
OndselSolver/ASMTMarker.cpp
View File

@@ -53,7 +53,7 @@ FMatDsptr MbD::ASMTMarker::aApm()
void MbD::ASMTMarker::createMbD(std::shared_ptr<System> mbdSys, std::shared_ptr<Units> mbdUnits)
{
auto mkr = CREATE<MarkerFrame>::With(name.c_str());
auto prt = std::static_pointer_cast<Part>(part()->mbdObject);
auto prt = std::static_pointer_cast<Part>(partOrAssembly()->mbdObject);
prt->partFrame->addMarkerFrame(mkr);
mkr->rpmp = rpmp()->times(1.0 / mbdUnits->length);

1
OndselSolver/ASMTMarker.h
View File

@@ -11,6 +11,7 @@
#include "ASMTSpatialItem.h"
#include "FullColumn.h"
#include "FullMatrix.h"
#include "ASMTPart.h"
namespace MbD {
class EXPORT ASMTMarker : public ASMTSpatialItem

17
OndselSolver/ASMTNoRotationJoint.cpp Normal file
View File

@@ -0,0 +1,17 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#include "ASMTNoRotationJoint.h"
#include "NoRotationJoint.h"
using namespace MbD;
std::shared_ptr<Joint> MbD::ASMTNoRotationJoint::mbdClassNew()
{
return CREATE<NoRotationJoint>::With();
}

12
OndselSolver/MBDynNodes.h → OndselSolver/ASMTNoRotationJoint.h
View File

@@ -7,17 +7,15 @@
***************************************************************************/
#pragma once
#include "MBDynItem.h"
#include "ASMTJoint.h"
namespace MbD {
class MBDynNode;
class MBDynNodes : public MBDynItem
class EXPORT ASMTNoRotationJoint : public ASMTJoint
{
//
public:
void initialize() override;
void parseMBDyn(std::vector<std::string>& lines) override;
std::shared_ptr<Joint> mbdClassNew() override;
std::shared_ptr<std::vector<std::shared_ptr<MBDynNode>>> nodes;
};
}

14
OndselSolver/ASMTPart.cpp
View File

@@ -9,6 +9,7 @@
#include "ASMTPart.h"
#include "CREATE.h"
#include "ASMTPrincipalMassMarker.h"
#include "Part.h"
using namespace MbD;
@@ -55,7 +56,7 @@ void MbD::ASMTPart::readPrincipalMassMarker(std::vector<std::string>& lines)
{
assert(lines[0].find("PrincipalMassMarker") != std::string::npos);
lines.erase(lines.begin());
principalMassMarker = CREATE<ASMTPrincipalMassMarker>::With();
principalMassMarker = std::make_shared<ASMTPrincipalMassMarker>();
principalMassMarker->parseASMT(lines);
principalMassMarker->owner = this;
}
@@ -104,3 +105,14 @@ FColDsptr MbD::ASMTPart::omeOpO()
{
return omega3D;
}
ASMTPart* MbD::ASMTPart::part()
{
return this;
}
void MbD::ASMTPart::createMbD(std::shared_ptr<System> mbdSys, std::shared_ptr<Units> mbdUnits)
{
ASMTSpatialContainer::createMbD(mbdSys, mbdUnits);
if (isFixed) std::static_pointer_cast<Part>(mbdObject)->asFixed();
}

3
OndselSolver/ASMTPart.h
View File

@@ -23,9 +23,12 @@ namespace MbD {
void readPartSeries(std::vector<std::string>& lines);
FColDsptr vOcmO() override;
FColDsptr omeOpO() override;
ASMTPart* part() override;
void createMbD(std::shared_ptr<System> mbdSys, std::shared_ptr<Units> mbdUnits) override;
//std::shared_ptr<std::vector<std::shared_ptr<ASMTFeature>>> featureOrder;
std::shared_ptr<std::vector<std::shared_ptr<PosVelAccData>>> partSeries;
bool isFixed = false;
};
}

17
OndselSolver/ASMTPointInLineJoint.cpp Normal file
View File

@@ -0,0 +1,17 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#include "ASMTPointInLineJoint.h"
#include "PointInLineJoint.h"
using namespace MbD;
std::shared_ptr<Joint> MbD::ASMTPointInLineJoint::mbdClassNew()
{
return CREATE<PointInLineJoint>::With();
}

18
OndselSolver/MBDynElements.h → OndselSolver/ASMTPointInLineJoint.h
View File

@@ -7,17 +7,15 @@
***************************************************************************/
#pragma once
#include "MBDynBlock.h"
#include "ASMTJoint.h"
namespace MbD {
class MBDynElement;
class EXPORT ASMTPointInLineJoint : public ASMTJoint
{
//
public:
std::shared_ptr<Joint> mbdClassNew() override;
class MBDynElements : public MBDynBlock
{
public:
void initialize() override;
void parseMBDyn(std::vector<std::string>& lines) override;
std::shared_ptr<std::vector<std::shared_ptr<MBDynElement>>> elements;
};
};
}

2
OndselSolver/ASMTPointInPlaneJoint.h
View File

@@ -15,7 +15,7 @@ namespace MbD {
{
//
public:
virtual std::shared_ptr<Joint> mbdClassNew() override;
std::shared_ptr<Joint> mbdClassNew() override;
void parseASMT(std::vector<std::string>& lines) override;
void readOffset(std::vector<std::string>& lines);
void createMbD(std::shared_ptr<System> mbdSys, std::shared_ptr<Units> mbdUnits) override;

2
OndselSolver/ASMTRevoluteJoint.h
View File

@@ -16,7 +16,7 @@ namespace MbD {
{
//
public:
virtual std::shared_ptr<Joint> mbdClassNew() override;
std::shared_ptr<Joint> mbdClassNew() override;
};
}

4
OndselSolver/ASMTRotationalMotion.cpp
View File

@@ -50,11 +50,11 @@ void MbD::ASMTRotationalMotion::initMarkers()
void MbD::ASMTRotationalMotion::createMbD(std::shared_ptr<System> mbdSys, std::shared_ptr<Units> mbdUnits)
{
ASMTMotion::createMbD(mbdSys, mbdUnits);
auto parser = CREATE<SymbolicParser>::With();
auto parser = std::make_shared<SymbolicParser>();
parser->owner = this;
auto geoTime = owner->root()->geoTime();
parser->variables->insert(std::make_pair("time", geoTime));
auto userFunc = CREATE<BasicUserFunction>::With(rotationZ, 1.0);
auto userFunc = std::make_shared<BasicUserFunction>(rotationZ, 1.0);
parser->parseUserFunction(userFunc);
auto geoPhi = parser->stack->top();
geoPhi = Symbolic::times(geoPhi, std::make_shared<Constant>(1.0 / mbdUnits->angle));

6
OndselSolver/ASMTSimulationParameters.cpp
View File

@@ -72,3 +72,9 @@ void MbD::ASMTSimulationParameters::seterrorTol(double tol)
{
errorTol = tol;
}
void MbD::ASMTSimulationParameters::setmaxIter(int maxIter)
{
iterMaxPosKine = maxIter;
iterMaxAccKine = maxIter;
}

1
OndselSolver/ASMTSimulationParameters.h
View File

@@ -22,6 +22,7 @@ namespace MbD {
void sethmax(double hmax);
void sethout(double hout);
void seterrorTol(double errorTol);
void setmaxIter(int maxIter);
double tstart = 0.0, tend = 1.0, hmin = 1.0e-9, hmax = 1.0e9, hout = 0.1, errorTol = 1.0e-6;
double errorTolPosKine = 1.0e-6, errorTolAccKine = 1.0e-6, corAbsTol = 1.0e-6, corRelTol = 1.0e-6;

60
OndselSolver/ASMTSpatialContainer.cpp
View File

@@ -13,11 +13,38 @@
#include "ASMTRefPoint.h"
#include "ASMTRefCurve.h"
#include "ASMTRefSurface.h"
#include <algorithm>
//#include "ASMTPrincipalMassMarker.h"
using namespace MbD;
MbD::ASMTSpatialContainer::ASMTSpatialContainer()
{
refPoints = std::make_shared<std::vector<std::shared_ptr<ASMTRefPoint>>>();
refCurves = std::make_shared<std::vector<std::shared_ptr<ASMTRefCurve>>>();
refSurfaces = std::make_shared<std::vector<std::shared_ptr<ASMTRefSurface>>>();
xs = std::make_shared<FullRow<double>>();
ys = std::make_shared<FullRow<double>>();
zs = std::make_shared<FullRow<double>>();
bryxs = std::make_shared<FullRow<double>>();
bryys = std::make_shared<FullRow<double>>();
bryzs = std::make_shared<FullRow<double>>();
vxs = std::make_shared<FullRow<double>>();
vys = std::make_shared<FullRow<double>>();
vzs = std::make_shared<FullRow<double>>();
omexs = std::make_shared<FullRow<double>>();
omeys = std::make_shared<FullRow<double>>();
omezs = std::make_shared<FullRow<double>>();
axs = std::make_shared<FullRow<double>>();
ays = std::make_shared<FullRow<double>>();
azs = std::make_shared<FullRow<double>>();
alpxs = std::make_shared<FullRow<double>>();
alpys = std::make_shared<FullRow<double>>();
alpzs = std::make_shared<FullRow<double>>();
}
void MbD::ASMTSpatialContainer::initialize()
{
refPoints = std::make_shared<std::vector<std::shared_ptr<ASMTRefPoint>>>();
@@ -294,9 +321,9 @@ FColDsptr MbD::ASMTSpatialContainer::omeOpO()
return FColDsptr();
}
std::shared_ptr<ASMTSpatialContainer> MbD::ASMTSpatialContainer::part()
ASMTSpatialContainer* MbD::ASMTSpatialContainer::partOrAssembly()
{
return std::make_shared<ASMTSpatialContainer>(*this);
return this;
}
void MbD::ASMTSpatialContainer::updateFromMbD()
@@ -433,3 +460,32 @@ void MbD::ASMTSpatialContainer::addMarker(std::shared_ptr<ASMTMarker> marker)
addRefPoint(refPoint);
refPoint->addMarker(marker);
}
std::string MbD::ASMTSpatialContainer::generateUniqueMarkerName()
{
auto aItemList = markerList();
auto markerNames = std::vector<std::string>();
for (auto& mkr : *aItemList) {
markerNames.push_back(mkr->name);
}
std::stringstream ss;
auto count = 0;
while (true) {
ss.str("");
ss << "Marker";
ss << count;
if (std::find(markerNames.begin(), markerNames.end(), ss.str()) == markerNames.end()) break;
count++;
}
return ss.str();
}
std::shared_ptr<std::vector<std::shared_ptr<ASMTMarker>>> MbD::ASMTSpatialContainer::markerList()
{
auto markers = std::make_shared<std::vector<std::shared_ptr<ASMTMarker>>>();
for (auto& refPoint : *refPoints) {
auto refmarkers = refPoint->markers;
markers->insert(markers->end(), refmarkers->begin(), refmarkers->end());
}
return markers;
}

5
OndselSolver/ASMTSpatialContainer.h
View File

@@ -33,6 +33,7 @@ namespace MbD {
{
//
public:
ASMTSpatialContainer();
void initialize() override;
void setPrincipalMassMarker(std::shared_ptr<ASMTPrincipalMassMarker> aJ);
void readRefPoints(std::vector<std::string>& lines);
@@ -64,12 +65,14 @@ namespace MbD {
std::shared_ptr<EulerParameters<double>> qEp();
virtual FColDsptr vOcmO();
virtual FColDsptr omeOpO();
std::shared_ptr<ASMTSpatialContainer> part() override;
ASMTSpatialContainer* partOrAssembly() override;
void updateFromMbD() override;
void compareResults(AnalysisType type) override;
void outputResults(AnalysisType type) override;
void addRefPoint(std::shared_ptr<ASMTRefPoint> refPoint);
void addMarker(std::shared_ptr<ASMTMarker> marker);
std::string generateUniqueMarkerName();
std::shared_ptr<std::vector<std::shared_ptr<ASMTMarker>>> markerList();
std::shared_ptr<std::vector<std::shared_ptr<ASMTRefPoint>>> refPoints;
std::shared_ptr<std::vector<std::shared_ptr<ASMTRefCurve>>> refCurves;

2
OndselSolver/ASMTSphericalJoint.h
View File

@@ -16,7 +16,7 @@ namespace MbD {
{
//
public:
virtual std::shared_ptr<Joint> mbdClassNew() override;
std::shared_ptr<Joint> mbdClassNew() override;
};
}

4
OndselSolver/ASMTTranslationalMotion.cpp
View File

@@ -49,11 +49,11 @@ void MbD::ASMTTranslationalMotion::initMarkers()
void MbD::ASMTTranslationalMotion::createMbD(std::shared_ptr<System> mbdSys, std::shared_ptr<Units> mbdUnits)
{
ASMTMotion::createMbD(mbdSys, mbdUnits);
auto parser = CREATE<SymbolicParser>::With();
auto parser = std::make_shared<SymbolicParser>();
parser->owner = this;
auto geoTime = owner->root()->geoTime();
parser->variables->insert(std::make_pair("time", geoTime));
auto userFunc = CREATE<BasicUserFunction>::With(translationZ, 1.0);
auto userFunc = std::make_shared<BasicUserFunction>(translationZ, 1.0);
parser->parseUserFunction(userFunc);
auto zIJ = parser->stack->top();
zIJ = Symbolic::times(zIJ, std::make_shared<Constant>(1.0 / mbdUnits->length));

2
OndselSolver/ASMTUniversalJoint.h
View File

@@ -15,7 +15,7 @@ namespace MbD {
{
//
public:
virtual std::shared_ptr<Joint> mbdClassNew() override;
std::shared_ptr<Joint> mbdClassNew() override;
};
}

5
OndselSolver/AnyPosICNewtonRaphson.cpp
View File

@@ -44,9 +44,10 @@ void AnyPosICNewtonRaphson::fillY()
y->atiminusFullColumn(0, (qsuWeights->timesFullColumn(newMinusOld)));
system->partsJointsMotionsDo([&](std::shared_ptr<Item> item) {
item->fillPosICError(y);
//std::cout << *y << std::endl;
//std::cout << item->name << *y << std::endl;
//noop();
});
//std::cout << *y << std::endl;
//std::cout << "Final" << *y << std::endl;
}
void AnyPosICNewtonRaphson::fillPyPx()

4
OndselSolver/Array.h
View File

@@ -41,11 +41,7 @@ namespace MbD {
virtual double maxMagnitude() = 0;
double maxMagnitudeOfVector();
void equalArrayAt(std::shared_ptr<Array<T>> array, int i);
//virtual void normalizeSelf();
//virtual void conditionSelf();
//virtual void conditionSelfWithTol(double tol);
virtual void atiput(int i, T value);
//double length();
void magnifySelf(T factor);
void atitimes(int i, double factor);

1
OndselSolver/BasicUserFunction.cpp
View File

@@ -14,6 +14,7 @@ using namespace MbD;
MbD::BasicUserFunction::BasicUserFunction(const std::string& expression, double myUnt) : funcText(expression), myUnit(myUnt)
{
units = std::make_shared<Units>();
}
void MbD::BasicUserFunction::initialize()

11
OndselSolver/ClassDiagram1.cd Normal file
View File

@@ -0,0 +1,11 @@
<?xml version="1.0" encoding="utf-8"?>
<ClassDiagram MajorVersion="1" MinorVersion="1">
<Class Name="MbD::Solver" Collapsed="true">
<Position X="0.5" Y="0.5" Width="1.5" />
<TypeIdentifier>
<HashCode>AKAAAgAAAAAAgAAAEAEAAIABAAAAAAAACIAAIAAAAAI=</HashCode>
<FileName>Solver.h</FileName>
</TypeIdentifier>
</Class>
<Font Name="Segoe UI" Size="9" />
</ClassDiagram>

2
OndselSolver/CylindricalJoint.cpp
View File

@@ -32,6 +32,6 @@ void CylindricalJoint::initializeGlobally()
this->root()->hasChanged = true;
}
else {
InLineJoint::initializeGlobally();
Joint::initializeGlobally();
}
}

1
OndselSolver/EigenDecomposition.cpp Normal file
View File

@@ -0,0 +1 @@
#include "EigenDecomposition.h"

13
OndselSolver/MBDynLabels.h → OndselSolver/EigenDecomposition.h
View File

@@ -7,15 +7,16 @@
***************************************************************************/
#pragma once
#include "MBDynItem.h"
#include "MatrixSolver.h"
namespace MbD {
class MBDynLabels : public MBDynItem
class EigenDecomposition : public MatrixSolver
{
public:
void initialize() override;
void parseMBDyn(std::vector<std::string>& lines) override;
std::shared_ptr<std::map<std::string, int>> labels;
//
public:
};
}

2
OndselSolver/EndFramec.h
View File

@@ -44,7 +44,7 @@ namespace MbD {
MarkerFrame* markerFrame; //Use raw pointer when pointing backwards.
FColDsptr rOeO = std::make_shared<FullColumn<double>>(3);
FMatDsptr aAOe = std::make_shared<FullMatrix<double>>(3, 3);
FMatDsptr aAOe = FullMatrix<double>::identitysptr(3);
};
//using EndFrmsptr = std::shared_ptr<EndFramec>;
}

2
OndselSolver/EndFrameqct.cpp
View File

@@ -31,7 +31,7 @@ void EndFrameqct::initialize()
rmem = std::make_shared<FullColumn<double>>(3);
prmempt = std::make_shared<FullColumn<double>>(3);
pprmemptpt = std::make_shared<FullColumn<double>>(3);
aAme = std::make_shared<FullMatrix<double>>(3, 3);
aAme = FullMatrix<double>::identitysptr(3);
pAmept = std::make_shared<FullMatrix<double>>(3, 3);
ppAmeptpt = std::make_shared<FullMatrix<double>>(3, 3);
pprOeOpEpt = std::make_shared<FullMatrix<double>>(3, 4);

32
OndselSolver/EulerAngles.h
View File

@@ -30,6 +30,7 @@ namespace MbD {
void initialize() override;
void calc() override;
std::shared_ptr<EulerAnglesDot<T>> differentiateWRT(T var);
void setRotOrder(int i, int j, int k);
std::shared_ptr<FullColumn<int>> rotOrder;
FColFMatDsptr cA;
@@ -64,6 +65,29 @@ namespace MbD {
}
aA = cA->at(0)->timesFullMatrix(cA->at(1)->timesFullMatrix(cA->at(2)));
}
template<>
inline void EulerAngles<double>::calc()
{
cA = std::make_shared<FullColumn<FMatDsptr>>(3);
for (int i = 0; i < 3; i++)
{
auto axis = rotOrder->at(i);
auto angle = this->at(i);
if (axis == 1) {
cA->atiput(i, FullMatrix<double>::rotatex(angle));
}
else if (axis == 2) {
cA->atiput(i, FullMatrix<double>::rotatey(angle));
}
else if (axis == 3) {
cA->atiput(i, FullMatrix<double>::rotatez(angle));
}
else {
throw std::runtime_error("Euler angle rotation order must be any permutation of 1,2,3 without consecutive repeats.");
}
}
aA = cA->at(0)->timesFullMatrix(cA->at(1)->timesFullMatrix(cA->at(2)));
}
template<typename T>
inline void EulerAngles<T>::calc()
{
@@ -79,5 +103,13 @@ namespace MbD {
derivatives->aEulerAngles = this;
return derivatives;
}
template<typename T>
inline void EulerAngles<T>::setRotOrder(int i, int j, int k)
{
rotOrder = std::make_shared<FullColumn<int>>(3);
rotOrder->at(0) = i;
rotOrder->at(1) = j;
rotOrder->at(2) = k;
}
}

9
OndselSolver/EulerAngleszxz.h
View File

@@ -29,12 +29,9 @@ namespace MbD {
template<typename T>
inline void EulerAngleszxz<T>::initialize()
{
phiA = std::make_shared<FullMatrix<double>>(3, 3);
phiA->identity();
theA = std::make_shared<FullMatrix<double>>(3, 3);
theA->identity();
psiA = std::make_shared<FullMatrix<double>>(3, 3);
psiA->identity();
phiA = FullMatrix<T>::identitysptr(3);
theA = FullMatrix<T>::identitysptr(3);
psiA = FullMatrix<T>::identitysptr(3);
}
template<typename T>
inline void EulerAngleszxz<T>::calc()

12
OndselSolver/EulerParameters.h
View File

@@ -28,6 +28,16 @@ namespace MbD {
EulerParameters(int count) : EulerArray<T>(count) {}
EulerParameters(int count, const T& value) : EulerArray<T>(count, value) {}
EulerParameters(std::initializer_list<T> list) : EulerArray<T>{ list } {}
EulerParameters(FColDsptr axis, double theta) : EulerArray<T>(4) {
auto halfTheta = theta / 2.0;
auto sinHalfTheta = std::sin(halfTheta);
auto cosHalfTheta = std::cos(halfTheta);
axis->normalizeSelf();
this->atiputFullColumn(0, axis->times(sinHalfTheta));
this->atiput(3, cosHalfTheta);
this->conditionSelf();
this->calc();
}
static std::shared_ptr<FullMatrix<FColsptr<T>>> ppApEpEtimesColumn(FColDsptr col);
static FMatDsptr pCpEtimesColumn(FColDsptr col);
@@ -201,7 +211,7 @@ namespace MbD {
template<>
inline void EulerParameters<double>::initialize()
{
aA = std::make_shared<FullMatrix<double>>(3, 3);
aA = FullMatrix<double>::identitysptr(3);
aB = std::make_shared<FullMatrix<double>>(3, 4);
aC = std::make_shared<FullMatrix<double>>(3, 4);
pApE = std::make_shared<FullColumn<FMatDsptr>>(4);

70
OndselSolver/FullMatrix.h
View File

@@ -12,6 +12,7 @@
#include <memory>
#include "RowTypeMatrix.h"
#include "FullColumn.h"
namespace MbD {
template<typename T>
@@ -26,6 +27,9 @@ namespace MbD {
class FullRow;
template<typename T>
class EulerParameters;
template<typename T>
class DiagonalMatrix;
using FMatFColDsptr = std::shared_ptr<FullMatrix<FColDsptr>>;
using FMatFMatDsptr = std::shared_ptr<FullMatrix<FMatDsptr>>;
using FColFMatDsptr = std::shared_ptr<FullColumn<FMatDsptr>>;
@@ -66,6 +70,8 @@ namespace MbD {
static FMatsptr<T> rotatexrotDotrotDDot(T angle, T angleDot, T angleDDot);
static FMatsptr<T> rotateyrotDotrotDDot(T angle, T angleDot, T angleDDot);
static FMatsptr<T> rotatezrotDotrotDDot(T angle, T angleDot, T angleDDot);
static FMatsptr<T> identitysptr(int n);
static FMatsptr<T> tildeMatrix(FColDsptr col);
void identity();
FColsptr<T> column(int j);
FColsptr<T> timesFullColumn(FColsptr<T> fullCol);
@@ -95,6 +101,9 @@ namespace MbD {
T trace();
double maxMagnitude() override;
FColsptr<T> bryantAngles();
bool isDiagonal();
std::shared_ptr<DiagonalMatrix<T>> asDiagonalMatrix();
void conditionSelfWithTol(double tol);
std::ostream& printOn(std::ostream& s) const override;
};
@@ -296,6 +305,33 @@ namespace MbD {
row2->atiput(2, 0.0);
return rotMat;
}
template<typename T>
inline FMatsptr<T> FullMatrix<T>::identitysptr(int n)
{
auto mat = std::make_shared<FullMatrix<T>>(n, n);
mat->identity();
return mat;
}
template<typename T>
inline FMatsptr<T> FullMatrix<T>::tildeMatrix(FColDsptr col)
{
//"tildeMatrix is skew symmetric matrix related to angular velocity and cross product."
if (col->size() != 3) throw std::runtime_error("Column is not of dimension 3");
auto tilde = std::make_shared<FullMatrix<double>>(3, 3);
auto c0 = col->at(0);
auto c1 = col->at(1);
auto c2 = col->at(2);
tilde->atijput(0, 0, 0.0);
tilde->atijput(1, 1, 0.0);
tilde->atijput(2, 2, 0.0);
tilde->atijput(1, 2, -c0);
tilde->atijput(0, 2, c1);
tilde->atijput(0, 1, -c2);
tilde->atijput(1, 0, c2);
tilde->atijput(2, 0, -c1);
tilde->atijput(2, 1, c0);
return tilde;
}
template<>
inline void FullMatrix<double>::zeroSelf()
{
@@ -628,6 +664,40 @@ namespace MbD {
return answer;
}
template<typename T>
inline bool FullMatrix<T>::isDiagonal()
{
auto m = this->nrow();
auto n = this->ncol();
if (m != n) return false;
for (int i = 0; i < m; i++)
{
auto rowi = this->at(i);
for (int j = 0; j < n; j++)
{
if (i != j && rowi->at(j) != 0) return false;
}
}
return true;
}
template<typename T>
inline std::shared_ptr<DiagonalMatrix<T>> FullMatrix<T>::asDiagonalMatrix()
{
int nrow = this->nrow();
auto diagMat = std::make_shared<DiagonalMatrix<T>>(nrow);
for (int i = 0; i < nrow; i++)
{
diagMat->atiput(i, this->at(i)->at(i));
}
return diagMat;
}
template<typename T>
inline void FullMatrix<T>::conditionSelfWithTol(double tol)
{
for (auto row : *this) {
row->conditionSelfWithTol(tol);
}
}
template<typename T>
inline FColsptr<T> FullMatrix<T>::timesFullColumn(FColsptr<T> fullCol)
{
return this->timesFullColumn(fullCol.get());

4
OndselSolver/FullVector.h
View File

@@ -5,7 +5,7 @@
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#pragma once
#include <ostream>
@@ -37,7 +37,7 @@ namespace MbD {
void normalizeSelf();
double length();
virtual void conditionSelf();
virtual void conditionSelfWithTol(double tol);
void conditionSelfWithTol(double tol);
std::ostream& printOn(std::ostream& s) const override;

3
OndselSolver/InLineJoint.h
View File

@@ -13,10 +13,11 @@
namespace MbD {
class InLineJoint : public Joint
{
//
//Abstract class. Create subclasses only.
public:
InLineJoint();
InLineJoint(const char* str);
virtual void initializeGlobally() = 0; //To prevent instantiation of this class
void createInLineConstraints();

3
OndselSolver/InPlaneJoint.h
View File

@@ -17,7 +17,8 @@ namespace MbD {
public:
InPlaneJoint();
InPlaneJoint(const char* str);
virtual void initializeGlobally() = 0; //To prevent instantiation of this class
void createInPlaneConstraint();
double offset;

5
OndselSolver/Item.cpp
View File

@@ -28,6 +28,11 @@ System* Item::root()
return owner->root();
}
void MbD::Item::noop()
{
//No Operations
}
void Item::initialize()
{
}

1
OndselSolver/Item.h
View File

@@ -29,6 +29,7 @@ namespace MbD {
Item();
Item(const char* str);
virtual System* root();
void noop();
virtual void calcPostDynCorrectorIteration();
virtual void checkForCollisionDiscontinuityBetweenand(double impulsePrevious, double impulse);

2
OndselSolver/LineInPlaneJoint.cpp
View File

@@ -29,6 +29,6 @@ void MbD::LineInPlaneJoint::initializeGlobally()
this->root()->hasChanged = true;
}
else {
InPlaneJoint::initializeGlobally();
Joint::initializeGlobally();
}
}

70
OndselSolver/MBDynBody.cpp
View File

@@ -1,9 +1,10 @@
#include "MBDynBody.h"
#include "MBDynReferences.h"
#include "MBDynReference.h"
#include "MBDynStructural.h"
#include "SymbolicParser.h"
#include "BasicUserFunction.h"
#include "MBDynVariables.h"
#include "ASMTPart.h"
#include "ASMTAssembly.h"
using namespace MbD;
@@ -18,31 +19,76 @@ void MbD::MBDynBody::parseMBDyn(std::string line)
auto pos = line.find(":");
auto front = line.substr(previousPos, pos - previousPos);
assert(front.find("body") != std::string::npos);
auto arguments = std::make_shared<std::vector<std::string>>();
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);
arguments.push_back(argument);
}
else {
argument = line.substr(previousPos + 1);
arguments->push_back(argument);
arguments.push_back(argument);
break;
}
}
auto iss = std::istringstream(arguments->at(0));
auto iss = std::istringstream(arguments.at(0));
iss >> name;
arguments->erase(arguments->begin());
iss = std::istringstream(arguments->at(0));
iss >> node;
arguments->erase(arguments->begin());
rOfO = readPosition(arguments);
arguments.erase(arguments.begin());
iss = std::istringstream(arguments.at(0));
iss >> nodeName;
arguments.erase(arguments.begin());
readMass(arguments);
rPcmP = readPosition(arguments);
readInertiaMatrix(arguments);
}
void MbD::MBDynBody::readInertiaMatrix(std::shared_ptr<std::vector<std::string>>& args)
void MbD::MBDynBody::readMass(std::vector<std::string>& args)
{
auto parser = std::make_shared<SymbolicParser>();
parser->variables = mbdynVariables();
auto userFunc = std::make_shared<BasicUserFunction>(popOffTop(args), 1.0);
parser->parseUserFunction(userFunc);
auto sym = parser->stack->top();
mass = sym->getValue();
}
void MbD::MBDynBody::readInertiaMatrix(std::vector<std::string>& args)
{
auto parser = std::make_shared<SymbolicParser>();
parser->variables = mbdynVariables();
aJmat = std::make_shared<FullMatrix<double>>(3, 3);
auto& str = args.at(0);
if (str.find("diag") != std::string::npos) {
args.erase(args.begin());
for (int i = 0; i < 3; i++)
{
auto userFunc = std::make_shared<BasicUserFunction>(popOffTop(args), 1.0);
parser->parseUserFunction(userFunc);
auto sym = parser->stack->top();
aJmat->at(i)->at(i) = sym->getValue();
}
}
else if (str.find("eye") != std::string::npos) {
args.erase(args.begin());
aJmat->identity();
}
else {
aJmat = readBasicOrientation(args);
}
}
void MbD::MBDynBody::createASMT()
{
auto asmtMassMarker = std::make_shared<ASMTPrincipalMassMarker>();
asmtItem = asmtMassMarker;
asmtMassMarker->setMass(mass);
assert(aJmat->isDiagonal());
asmtMassMarker->setMomentOfInertias(aJmat->asDiagonalMatrix());
asmtMassMarker->setPosition3D(rPcmP);
asmtMassMarker->setRotationMatrix(FullMatrix<double>::identitysptr(3));
auto asmtPart = asmtAssembly()->partPartialNamed(nodeName);
asmtPart->setPrincipalMassMarker(asmtMassMarker);
}

10
OndselSolver/MBDynBody.h
View File

@@ -15,12 +15,14 @@ namespace MbD {
public:
void initialize() override;
void parseMBDyn(std::string line);
void readInertiaMatrix(std::shared_ptr<std::vector<std::string>>& args);
void readMass(std::vector<std::string>& args);
void readInertiaMatrix(std::vector<std::string>& args);
void createASMT() override;
std::string bodyString, name, node;
std::string bodyString, name, nodeName;
double mass;
FColDsptr rOfO;
FMatDsptr aAOf;
FColDsptr rPcmP;
FMatDsptr aJmat;
};
}

349
OndselSolver/MBDynCase.mbd Normal file
View File

@@ -0,0 +1,349 @@
######################################################################################################################
# MBDyn is a free and open-source general purpose multi-body dynamics software. See https://www.mbdyn.org/ for details.
# This input file was automatically generated by the FreeCAD "MBD workbench".
# To learn more about MBDyn input files, you can visit the website: https://www.sky-engin.jp/en/MBDynTutorial/index.html
# Details about the structure of input files can be studied in the input file manuals: https://www.mbdyn.org/?Software_Download
# Although MBDyn has already reached a mature stage and is used by several industries,
# the FreeCAD dynamics workbench is still under development, and it is likely to have bugs.
# Please be aware of this fact before you use this input file for any critical application.
# If you think you have found a bug or have any suggestion, please send your comments to Jose Egas:
# josegegas@gmail.com
######################################################################################################################
#-----------------------------------------------------------------------------
# [Data Block]
begin: data;
problem: initial value;
end: data;
#-----------------------------------------------------------------------------
# [Problem Block]
begin: initial value;
initial time: 0.0;
final time: 8.0;
time step: 0.01;
max iterations: 100;
tolerance: 1e-06;
derivatives tolerance: 0.0001;
derivatives max iterations: 100;
derivatives coefficient: auto;
end: initial value;
#-----------------------------------------------------------------------------
# [Control Data Block]
begin: control data;
max iterations: 1000;
default orientation: euler321;
omega rotates: no;
print: none;
initial stiffness: 1.0, 1.0;
structural nodes: 4;
rigid bodies: 3;
joints: 7;
end: control data;
#-----------------------------------------------------------------------------
# [Design Variables]
#Generic bodies
#body: 2
set: integer body_2 = 2; #body label
set: real mass_2 = 1.4486361883511716; #mass [kg]
set: real volume_2 = 0.0001833716694115407; #volume [m^3]
#body: 3
set: integer body_3 = 3; #body label
set: real mass_3 = 2.85294865570677; #mass [kg]
set: real volume_3 = 0.00036113274122870505; #volume [m^3]
#body: 4
set: integer body_4 = 4; #body label
set: real mass_4 = 10.859427202622147; #mass [kg]
set: real volume_4 = 0.0013746110383066007; #volume [m^3]
#Nodes
#node: 1
set: integer structural_node_1 = 1; #node label
#node: 2
set: integer structural_node_2 = 2; #node label
#node: 3
set: integer structural_node_3 = 3; #node label
#node: 4
set: integer structural_node_4 = 4; #node label
#Joints
#joint: 1
set: integer joint_1 = 1; #joint label
#joint: 2
set: integer joint_2 = 2; #joint label
#joint: 3
set: integer joint_3 = 3; #joint label
#joint: 4
set: integer joint_4 = 4; #joint label
#joint: 5
set: integer joint_5 = 5; #joint label
#joint: 6
set: integer joint_6 = 6; #joint label
#joint: 7
set: integer joint_7 = 7; #joint label
#Nodes: initial conditions
#node: 1
set: real Px_1 = -0.121; #X component of the absolute position [m]
set: real Py_1 = -8.796847998598882e-19; #Y component of the absolute position [m]
set: real Pz_1 = -0.08; #Z component of the absolute position [m]
set: real Vx_1 = 0.0; #X component of the absolute velocity [m/s]
set: real Vy_1 = 0.0; #Y component of the absolute velocity [m/s]
set: real Vz_1 = 0.0; #Z component of the absolute velocity [m/s]
set: real Wx_1 = 0.0; #X component of the absolute angular velocity [rad/s]
set: real Wy_1 = 0.0; #Y component of the absolute angular velocity [rad/s]
set: real Wz_1 = 0.0; #Z component of the absolute angular velocity [rad/s]
#node: 2
set: real Px_2 = -0.015; #X component of the absolute position [m]
set: real Py_2 = 0.092; #Y component of the absolute position [m]
set: real Pz_2 = 0.008; #Z component of the absolute position [m]
set: real Vx_2 = 0.0; #X component of the absolute velocity [m/s]
set: real Vy_2 = 0.0; #Y component of the absolute velocity [m/s]
set: real Vz_2 = 0.0; #Z component of the absolute velocity [m/s]
set: real Wx_2 = 0.0; #X component of the absolute angular velocity [rad/s]
set: real Wy_2 = 0.0; #Y component of the absolute angular velocity [rad/s]
set: real Wz_2 = 0.0; #Z component of the absolute angular velocity [rad/s]
#node: 3
set: real Px_3 = 0.088; #X component of the absolute position [m]
set: real Py_3 = -0.055; #Y component of the absolute position [m]
set: real Pz_3 = 0.05; #Z component of the absolute position [m]
set: real Vx_3 = 0.0; #X component of the absolute velocity [m/s]
set: real Vy_3 = 0.0; #Y component of the absolute velocity [m/s]
set: real Vz_3 = 0.0; #Z component of the absolute velocity [m/s]
set: real Wx_3 = 0.0; #X component of the absolute angular velocity [rad/s]
set: real Wy_3 = 0.0; #Y component of the absolute angular velocity [rad/s]
set: real Wz_3 = 0.0; #Z component of the absolute angular velocity [rad/s]
#node: 4
set: real Px_4 = 0.3200000010688326; #X component of the absolute position [m]
set: real Py_4 = 1.4796688528733327e-10; #Y component of the absolute position [m]
set: real Pz_4 = 0.04999999225971574; #Z component of the absolute position [m]
set: real Vx_4 = 0.0; #X component of the absolute velocity [m/s]
set: real Vy_4 = 0.0; #Y component of the absolute velocity [m/s]
set: real Vz_4 = 0.0; #Z component of the absolute velocity [m/s]
set: real Wx_4 = 0.0; #X component of the absolute angular velocity [rad/s]
set: real Wy_4 = 0.0; #Y component of the absolute angular velocity [rad/s]
set: real Wz_4 = 0.0; #Z component of the absolute angular velocity [rad/s]
#-----------------------------------------------------------------------------
# [Intermediate Variables]
#Moments of inertia and relative center of mass
#body 2:
set: real Ixx_2 = 0.0012769356301204219; #moment of inertia [kg*m^2]
set: real Ixy_2 = 4.67e-20; #moment of inertia [kg*m^2]
set: real Ixz_2 = -0.0009495625871945231; #moment of inertia [kg*m^2]
set: real Iyx_2 = 4.67e-20; #moment of inertia [kg*m^2]
set: real Iyy_2 = 0.0028717510150978666; #moment of inertia [kg*m^2]
set: real Iyz_2 = 4.4900000000000004e-20; #moment of inertia [kg*m^2]
set: real Izx_2 = -0.0009495625871945231; #moment of inertia [kg*m^2]
set: real Izy_2 = 4.4900000000000004e-20; #moment of inertia [kg*m^2]
set: real Izz_2 = 0.002296471669735477; #moment of inertia [kg*m^2]
set: real Rx_2 = -0.03260146715730948; #X component of the relative center of mass [m]
set: real Ry_2 = 0.0; #Y component of the relative center of mass [m]
set: real Rz_2 = -0.028286762255221056; #Z component of the relative center of mass [m]
#body 3:
set: real Ixx_3 = 0.0019563103180210077; #moment of inertia [kg*m^2]
set: real Ixy_3 = -7.580852e-16; #moment of inertia [kg*m^2]
set: real Ixz_3 = 5.386829000000001e-16; #moment of inertia [kg*m^2]
set: real Iyx_3 = -7.580852e-16; #moment of inertia [kg*m^2]
set: real Iyy_3 = 0.03371514809951082; #moment of inertia [kg*m^2]
set: real Iyz_3 = -4.95494e-17; #moment of inertia [kg*m^2]
set: real Izx_3 = 5.386829000000001e-16; #moment of inertia [kg*m^2]
set: real Izy_3 = -4.95494e-17; #moment of inertia [kg*m^2]
set: real Izz_3 = 0.03383792198797204; #moment of inertia [kg*m^2]
set: real Rx_3 = 7.87281351222191e-15; #X component of the relative center of mass [m]
set: real Ry_3 = 1.0302869668521452e-15; #Y component of the relative center of mass [m]
set: real Rz_3 = -0.024999999999998045; #Z component of the relative center of mass [m]
#body 4:
set: real Ixx_4 = 0.07706742098795094; #moment of inertia [kg*m^2]
set: real Ixy_4 = 8.01291908309e-10; #moment of inertia [kg*m^2]
set: real Ixz_4 = -1.2135214817691901e-08; #moment of inertia [kg*m^2]
set: real Iyx_4 = 8.01291908309e-10; #moment of inertia [kg*m^2]
set: real Iyy_4 = 0.06635181579849883; #moment of inertia [kg*m^2]
set: real Iyz_4 = -3.32678908792e-11; #moment of inertia [kg*m^2]
set: real Izx_4 = -1.2135214817691901e-08; #moment of inertia [kg*m^2]
set: real Izy_4 = -3.32678908792e-11; #moment of inertia [kg*m^2]
set: real Izz_4 = 0.06179235045624136; #moment of inertia [kg*m^2]
set: real Rx_4 = 0.045580834634119355; #X component of the relative center of mass [m]
set: real Ry_4 = 2.0299354041266675e-10; #Y component of the relative center of mass [m]
set: real Rz_4 = -1.2562251448855477e-08; #Z component of the relative center of mass [m]
#-----------------------------------------------------------------------------
# [Nodes Block]
begin: nodes;
structural: structural_node_1,
static,
Px_1, Py_1, Pz_1, #<absolute_position> [m]
3, 0.0, 0.0, 1.0, 2, 0.0, 1.0, 0.0, #<absolute_orientation_matrix>
Vx_1, Vy_1, Vz_1, #<absolute_velocity> [m/s]
Wx_1, Wy_1, Wz_1; #<absolute_angular_velocity> [rad/s]
structural: structural_node_2,
dynamic,
Px_2, Py_2, Pz_2, #<absolute_position> [m]
3, 0.0, 0.0, 1.0, 2, 0.0, 1.0, 0.0, #<absolute_orientation_matrix>
Vx_2, Vy_2, Vz_2, #<absolute_velocity> [m/s]
Wx_2, Wy_2, Wz_2; #<absolute_angular_velocity> [rad/s]
structural: structural_node_3,
dynamic,
Px_3, Py_3, Pz_3, #<absolute_position> [m]
3, 0.0, 0.0, 1.0, 2, 0.0, 1.0, 0.0, #<absolute_orientation_matrix>
Vx_3, Vy_3, Vz_3, #<absolute_velocity> [m/s]
Wx_3, Wy_3, Wz_3; #<absolute_angular_velocity> [rad/s]
structural: structural_node_4,
dynamic,
Px_4, Py_4, Pz_4, #<absolute_position> [m]
3, 0.0, 0.0, 1.0, 2, 0.0, 1.0, 0.0, #<absolute_orientation_matrix>
Vx_4, Vy_4, Vz_4, #<absolute_velocity> [m/s]
Wx_4, Wy_4, Wz_4; #<absolute_angular_velocity> [rad/s]
end: nodes;
#-----------------------------------------------------------------------------
# [Elements Block]
begin: elements;
#-----------------------------------------------------------------------------
# [Bodies]
#IMPORTANT NOTE: FreeCAD provides the inertia matrix in the global reference frame,
#while by default, MBDyn assumes it to be in the reference frame of the node,
#thus, the matrix of inertia must be rotated, using the inverse of the placement
#matrix of the node. This is done using the inertial keyword and the following
#orientation matrix.
body: body_2,
structural_node_2, #<node_label>
mass_2, #<mass> [kg]
Rx_2, Ry_2, Rz_2, #<relative_center_of_mass> [m]
Ixx_2, Ixy_2, Ixz_2, #<inertia_matrix> [kg*m^2]
Iyx_2, Iyy_2, Iyz_2,
Izx_2, Izy_2, Izz_2,
orientation, 3, 0.0, 0.0, 1.0, 2, 0.0, 1.0, 0.0;
body: body_3,
structural_node_3, #<node_label>
mass_3, #<mass> [kg]
Rx_3, Ry_3, Rz_3, #<relative_center_of_mass> [m]
Ixx_3, Ixy_3, Ixz_3, #<inertia_matrix> [kg*m^2]
Iyx_3, Iyy_3, Iyz_3,
Izx_3, Izy_3, Izz_3,
orientation, 3, 0.0, 0.0, 1.0, 2, 0.0, 1.0, 0.0;
body: body_4,
structural_node_4, #<node_label>
mass_4, #<mass> [kg]
Rx_4, Ry_4, Rz_4, #<relative_center_of_mass> [m]
Ixx_4, Ixy_4, Ixz_4, #<inertia_matrix> [kg*m^2]
Iyx_4, Iyy_4, Iyz_4,
Izx_4, Izy_4, Izz_4,
orientation, 3, 0.0, 0.0, 1.0, 2, 0.0, 1.0, 0.0;
#-----------------------------------------------------------------------------
# [Joints]
joint: joint_1,
clamp,
structural_node_1, #<node_label>
-0.121, -8.796847998598882e-19, -0.08, #<absolute_pin_position> [m]
3, 0.0, 0.0, 1.0, 2, 0.0, 1.0, 0.0; #<absolute_orientation_matrix>
joint: joint_2,
spherical hinge,
structural_node_1, #<node_1_label>
0.0, 0.0, 0.0, #<relative_pin_position_1> [m]
structural_node_2, #<node_2_label>
-0.07, 0.0, -0.055; #<relative_pin_position_2> [m]
joint: joint_3,
revolute hinge,
structural_node_2, #<node_1_label>
0.0, 0.0, 0.0, #<relative_position_1> [m]
orientation, 3, 0.0, 0.0, 1.0, 2, 0.0, 1.0, 0.0, #<relative_pin_orientation_matrix_1>
structural_node_3, #<node_2_label>
-0.14, 0.0, -0.024999999999999998, #<relative_position_2> [m]
orientation, 3, 0.0, 0.0, 1.0, 2, guess; #<relative_pin_orientation_matrix_2>
joint: joint_4,
drive hinge,
structural_node_1, #<node_1_label>
orientation, 3, 0.0, 0.0, 1.0, 2, 0.0, 1.0, 0.0, #<relative_orientation_matrix_1>
structural_node_2, #<node_2_label>
orientation, 3, 0.0, 0.0, 1.0, 2, 0.0, 1.0, 0.0, #<relative_orientation_matrix_2>
single, 0., 0., 1., #<hinge_orientation>
string, "0"; #<hinge_orientation> [rad]
joint: joint_5,
in line,
structural_node_4, #<node_1_label>
0.0, 0.0, 0.0, #<relative_line_position> [m]
3, 0.0, 0.0, 1.0, 2, 0.0, 1.0, 0.0, #<relative_orientation>
structural_node_3, #<node_2_label>
offset, 0.14, 0.0, -0.024999999999999998; #<relative_offset> [m]
joint: joint_6,
prismatic,
structural_node_1, #<node_1_label>
orientation, 3, 0.0, 0.0, 1.0, 2, 0.0, 1.0, 0.0, #relative_orientation_matrix_1>
structural_node_4, #<node_2_label>
orientation, 3, 0.0, 0.0, 1.0, 2, 0.0, 1.0, 0.0; #relative_orientation_matrix_2>
joint: joint_7,
in line,
structural_node_1, #<node_1_label>
0.0, 8.796847998604521e-19, 0.08, #<relative_line_position> [m]
3, 1.0, -2.220446049250313e-16, 2.220446049250313e-16, 2, -2.220446049250313e-16, 0.0, 1.0, #<relative_orientation>
structural_node_4, #<node_2_label>
offset, 0.0, 0.0, 0.0; #<relative_offset> [m]
end: elements;

92
OndselSolver/MBDynControlData.cpp
View File

@@ -8,42 +8,126 @@ void MbD::MBDynControlData::initialize()
void MbD::MBDynControlData::parseMBDyn(std::vector<std::string>& lines)
{
readMaxIterations(lines);
readDefaultOrientation(lines);
readOmegaRotates(lines);
readPrint(lines);
readInitialStiffness(lines);
readStructuralNodes(lines);
readRigidBodies(lines);
readJoints(lines);
assert(lines.size() == 2);
}
void MbD::MBDynControlData::readMaxIterations(std::vector<std::string>& lines)
{
//max iterations: 1000;
std::vector<std::string> tokens{ "max", "iterations:" };
auto it = findLineWith(lines, tokens);
if (it == lines.end()) return;
std::istringstream iss(*it);
std::string str;
iss >> str;
iss >> str;
iss >> maxIterations;
lines.erase(it);
}
void MbD::MBDynControlData::readDefaultOrientation(std::vector<std::string>& lines)
{
//default orientation: euler321;
std::vector<std::string> tokens{ "default", "orientation:" };
auto it = findLineWith(lines, tokens);
if (it == lines.end()) return;
std::istringstream iss(*it);
std::string str;
iss >> str;
iss >> str;
iss >> defaultOrientation;
lines.erase(it);
}
void MbD::MBDynControlData::readOmegaRotates(std::vector<std::string>& lines)
{
//omega rotates: no;
std::vector<std::string> tokens{ "omega", "rotates:" };
auto it = findLineWith(lines, tokens);
if (it == lines.end()) return;
std::istringstream iss(*it);
std::string str;
iss >> str;
iss >> str;
iss >> omegaRotates;
lines.erase(it);
}
void MbD::MBDynControlData::readPrint(std::vector<std::string>& lines)
{
//print: none;
std::vector<std::string> tokens{ "print:" };
auto it = findLineWith(lines, tokens);
if (it == lines.end()) return;
std::istringstream iss(*it);
std::string str;
iss >> str;
iss >> str;
iss >> print;
lines.erase(it);
}
void MbD::MBDynControlData::readInitialStiffness(std::vector<std::string>& lines)
{
//initial stiffness: 1.0, 1.0;
std::vector<std::string> tokens{ "initial", "stiffness:" };
auto it = findLineWith(lines, tokens);
if (it == lines.end()) return;
std::istringstream iss(*it);
std::string str;
iss >> str;
iss >> str;
iss >> initialStiffness;
iss >> str;
initialStiffness.append(str);
lines.erase(it);
}
void MbD::MBDynControlData::readStructuralNodes(std::vector<std::string>& lines)
{
{
//structural nodes: 4;
std::vector<std::string> tokens{"structural", "nodes:"};
std::vector<std::string> tokens{ "structural", "nodes:" };
auto it = findLineWith(lines, tokens);
if (it == lines.end()) return;
std::istringstream iss(*it);
std::string str;
iss >> str;
iss >> str;
iss >> structuralNodes;
lines.erase(it);
}
void MbD::MBDynControlData::readRigidBodies(std::vector<std::string>& lines)
{
//rigid bodies: 3;
std::vector<std::string> tokens{"rigid", "bodies:"};
std::vector<std::string> tokens{ "rigid", "bodies:" };
auto it = findLineWith(lines, tokens);
if (it == lines.end()) return;
std::istringstream iss(*it);
std::string str;
iss >> str;
iss >> str;
iss >> rigidBodies;
lines.erase(it);
}
void MbD::MBDynControlData::readJoints(std::vector<std::string>& lines)
{
//joints: 6;
std::vector<std::string> tokens{"joints:"};
std::vector<std::string> tokens{ "joints:" };
auto it = findLineWith(lines, tokens);
if (it == lines.end()) return;
std::istringstream iss(*it);
std::string str;
iss >> str;
iss >> joints;
lines.erase(it);
}

10
OndselSolver/MBDynControlData.h
View File

@@ -15,10 +15,20 @@ namespace MbD {
public:
void initialize() override;
void parseMBDyn(std::vector<std::string>& lines) override;
void readMaxIterations(std::vector<std::string>& lines);
void readDefaultOrientation(std::vector<std::string>& lines);
void readOmegaRotates(std::vector<std::string>& lines);
void readPrint(std::vector<std::string>& lines);
void readInitialStiffness(std::vector<std::string>& lines);
void readStructuralNodes(std::vector<std::string>& lines);
void readRigidBodies(std::vector<std::string>& lines);
void readJoints(std::vector<std::string>& lines);
int maxIterations = 1000;
std::string defaultOrientation = "euler321";
std::string omegaRotates = "no";
std::string print = "none";
std::string initialStiffness = "1.0, 1.0";
int structuralNodes, rigidBodies, joints;
};
}

2
OndselSolver/MBDynData.cpp
View File

@@ -9,7 +9,7 @@ void MbD::MBDynData::initialize()
void MbD::MBDynData::parseMBDyn(std::vector<std::string>& lines)
{
assert(lines.size() == 3);
std::vector<std::string> tokens{"problem:", "initial", "value;"};
std::vector<std::string> tokens{ "problem:", "initial", "value" };
auto problemit = findLineWith(lines, tokens);
assert(problemit != lines.end());
}

37
OndselSolver/MBDynElements.cpp
View File

@@ -1,37 +0,0 @@
#include "MBDynElements.h"
#include "MBDynBody.h"
using namespace MbD;
void MbD::MBDynElements::initialize()
{
}
void MbD::MBDynElements::parseMBDyn(std::vector<std::string>& lines)
{
elements = std::make_shared<std::vector<std::shared_ptr<MBDynElement>>>();
std::vector<std::string> bodyToken{ "body:" };
std::vector<std::string> jointToken{ "joint:" };
std::vector<std::string>::iterator it;
while (true) {
it = findLineWith(lines, bodyToken);
if (it != lines.end()) {
auto body = std::make_shared<MBDynBody>();
body->owner = this;
body->parseMBDyn(*it);
elements->push_back(body);
lines.erase(it);
continue;
}
it = findLineWith(lines, jointToken);
if (it != lines.end()) {
auto body = std::make_shared<MBDynBody>();
body->owner = this;
body->parseMBDyn(*it);
elements->push_back(body);
lines.erase(it);
continue;
}
break;
}
}

85
OndselSolver/MBDynInitialValue.cpp
View File

@@ -1,4 +1,6 @@
#include "MBDynInitialValue.h"
#include "ASMTSimulationParameters.h"
#include "ASMTAssembly.h"
using namespace MbD;
@@ -13,63 +15,134 @@ void MbD::MBDynInitialValue::parseMBDyn(std::vector<std::string>& lines)
readTimeStep(lines);
readMaxIterations(lines);
readTolerance(lines);
readDerivativesTolerance(lines);
readDerivativesMaxIterations(lines);
readDerivativesCoefficient(lines);
assert(lines.size() == 2);
}
void MbD::MBDynInitialValue::readInitialTime(std::vector<std::string>& lines)
{
//initial time: 0.;
std::vector<std::string> tokens{"initial", "time:"};
std::vector<std::string> tokens{ "initial", "time:" };
auto it = findLineWith(lines, tokens);
if (it == lines.end()) return;
std::istringstream iss(*it);
std::string str;
iss >> str;
iss >> str;
iss >> initialTime;
iss >> initialTime;
lines.erase(it);
}
void MbD::MBDynInitialValue::readFinalTime(std::vector<std::string>& lines)
{
//final time: 5.;
std::vector<std::string> tokens{"final", "time:"};
std::vector<std::string> tokens{ "final", "time:" };
auto it = findLineWith(lines, tokens);
if (it == lines.end()) return;
std::istringstream iss(*it);
std::string str;
iss >> str;
iss >> str;
iss >> finalTime;
lines.erase(it);
}
void MbD::MBDynInitialValue::readTimeStep(std::vector<std::string>& lines)
{
//time step: 1.e-2;
std::vector<std::string> tokens{"time", "step:"};
std::vector<std::string> tokens{ "time", "step:" };
auto it = findLineWith(lines, tokens);
if (it == lines.end()) return;
std::istringstream iss(*it);
std::string str;
iss >> str;
iss >> str;
iss >> timeStep;
lines.erase(it);
}
void MbD::MBDynInitialValue::readMaxIterations(std::vector<std::string>& lines)
{
//max iterations: 10;
std::vector<std::string> tokens{"max", "iterations:"};
std::vector<std::string> tokens{ "max", "iterations:" };
auto it = findLineWith(lines, tokens);
if (it == lines.end()) return;
std::istringstream iss(*it);
std::string str;
iss >> str;
iss >> str;
iss >> maxIterations;
lines.erase(it);
}
void MbD::MBDynInitialValue::readTolerance(std::vector<std::string>& lines)
{
//tolerance: 1.e-6;
std::vector<std::string> tokens{"tolerance:"};
std::vector<std::string> tokens{ "tolerance:" };
auto it = findLineWith(lines, tokens);
if (it == lines.end()) return;
std::istringstream iss(*it);
std::string str;
iss >> str;
iss >> tolerance;
lines.erase(it);
}
void MbD::MBDynInitialValue::readDerivativesTolerance(std::vector<std::string>& lines)
{
//derivatives tolerance: 0.0001;
std::vector<std::string> tokens{ "derivatives", "tolerance:" };
auto it = findLineWith(lines, tokens);
if (it == lines.end()) return;
std::istringstream iss(*it);
std::string str;
iss >> str;
iss >> str;
iss >> derivativesTolerance;
lines.erase(it);
}
void MbD::MBDynInitialValue::readDerivativesMaxIterations(std::vector<std::string>& lines)
{
//derivatives max iterations: 100;
std::vector<std::string> tokens{ "derivatives", "max", "iterations:" };
auto it = findLineWith(lines, tokens);
if (it == lines.end()) return;
std::istringstream iss(*it);
std::string str;
iss >> str;
iss >> str;
iss >> str;
iss >> derivativesMaxIterations;
lines.erase(it);
}
void MbD::MBDynInitialValue::readDerivativesCoefficient(std::vector<std::string>& lines)
{
//derivatives coefficient: auto;
std::vector<std::string> tokens{ "derivatives", "coefficient:" };
auto it = findLineWith(lines, tokens);
if (it == lines.end()) return;
std::istringstream iss(*it);
std::string str;
iss >> str;
iss >> str;
iss >> derivativesCoefficient;
lines.erase(it);
}
void MbD::MBDynInitialValue::createASMT()
{
auto simulationParameters = std::make_shared<ASMTSimulationParameters>();
asmtItem = simulationParameters;
simulationParameters->settstart(initialTime);
simulationParameters->settend(finalTime); //tstart == tend Initial Conditions only.
simulationParameters->sethmin(1.0e-9);
simulationParameters->sethmax(1.0);
simulationParameters->sethout(timeStep);
simulationParameters->seterrorTol(tolerance);
simulationParameters->setmaxIter(maxIterations);
asmtAssembly()->setSimulationParameters(simulationParameters);
}

11
OndselSolver/MBDynInitialValue.h
View File

@@ -20,8 +20,15 @@ namespace MbD {
void readTimeStep(std::vector<std::string>& lines);
void readMaxIterations(std::vector<std::string>& lines);
void readTolerance(std::vector<std::string>& lines);
void readDerivativesTolerance(std::vector<std::string>& lines);
void readDerivativesMaxIterations(std::vector<std::string>& lines);
void readDerivativesCoefficient(std::vector<std::string>& lines);
void createASMT() override;
double initialTime, finalTime, timeStep, tolerance;
int maxIterations;
double initialTime = 0.0, finalTime = 5.0, timeStep = 1.0e-2, tolerance = 1.0e-6;
int maxIterations = 10;
double derivativesTolerance = 1.0e-4;
int derivativesMaxIterations = 100;
std::string derivativesCoefficient = "auto";
};
}

298
OndselSolver/MBDynItem.cpp
View File

@@ -1,12 +1,13 @@
#include "MBDynItem.h"
#include "MBDynSystem.h"
#include "MBDynVariables.h"
#include "SymbolicParser.h"
#include "BasicUserFunction.h"
#include "EulerAngles.h"
#include "Constant.h"
#include "MBDynReferences.h"
#include "MBDynReference.h"
#include "FullMatrix.h"
#include "ASMTItem.h"
#include "MBDynBody.h"
using namespace MbD;
@@ -21,6 +22,11 @@ void MbD::MBDynItem::initialize()
assert(false);
}
void MbD::MBDynItem::noop()
{
//No Operations
}
void MbD::MBDynItem::parseMBDyn(std::vector<std::string>& lines)
{
assert(false);
@@ -45,108 +51,288 @@ bool MbD::MBDynItem::lineHasTokens(const std::string& line, std::vector<std::str
return true;
}
std::shared_ptr<MBDynVariables> MbD::MBDynItem::mbdynVariables()
std::shared_ptr<std::vector<std::shared_ptr<MBDynNode>>> MbD::MBDynItem::mbdynNodes()
{
return owner->mbdynNodes();
}
std::vector<std::string> MbD::MBDynItem::nodeNames()
{
return owner->nodeNames();
}
std::shared_ptr<std::map<std::string, Symsptr>> MbD::MBDynItem::mbdynVariables()
{
return owner->mbdynVariables();
}
std::shared_ptr<MBDynReferences> MbD::MBDynItem::mbdynReferences()
std::shared_ptr<std::map<std::string, std::shared_ptr<MBDynReference>>> MbD::MBDynItem::mbdynReferences()
{
return owner->mbdynReferences();
}
FColDsptr MbD::MBDynItem::readPosition(std::shared_ptr<std::vector<std::string>>& args)
void MbD::MBDynItem::createASMT()
{
assert(false);
}
std::shared_ptr<MBDynNode> MbD::MBDynItem::nodeAt(std::string nodeName)
{
return owner->nodeAt(nodeName);
}
int MbD::MBDynItem::nodeidAt(std::string nodeName)
{
return owner->nodeidAt(nodeName);
}
std::shared_ptr<MBDynBody> MbD::MBDynItem::bodyWithNode(std::string nodeName)
{
return owner->bodyWithNode(nodeName);
}
std::shared_ptr<ASMTAssembly> MbD::MBDynItem::asmtAssembly()
{
return owner->asmtAssembly();
}
FColDsptr MbD::MBDynItem::readVector3(std::vector<std::string>& args)
{
auto parser = std::make_shared<SymbolicParser>();
parser->variables = mbdynVariables();
auto rFfF = std::make_shared<FullColumn<double>>(3);
auto& str = args.at(0);
if (str.find("null") != std::string::npos) {
args.erase(args.begin());
}
else {
for (int i = 0; i < 3; i++)
{
auto userFunc = std::make_shared<BasicUserFunction>(popOffTop(args), 1.0);
parser->parseUserFunction(userFunc);
auto sym = parser->stack->top();
rFfF->at(i) = sym->getValue();
}
}
return rFfF;
}
FColDsptr MbD::MBDynItem::readPosition(std::vector<std::string>& args)
{
auto rOfO = std::make_shared<FullColumn<double>>(3);
auto str = args->at(0);
if (str.find("reference") != std::string::npos) {
args->erase(args->begin());
std::string refName;
std::istringstream iss(args->at(0));
iss >> refName;
auto ref = mbdynReferences()->references->at(refName);
args->erase(args->begin());
if (args.empty()) return rOfO;
auto& str = args.at(0);
if (str.find("orientation") != std::string::npos) {
//Do nothing
}
else if (str.find("reference") != std::string::npos) {
args.erase(args.begin());
auto refName = readStringOffTop(args);
auto ref = mbdynReferences()->at(refName);
auto rFfF = readBasicPosition(args);
auto rOFO = ref->rOfO;
auto aAOF = ref->aAOf;
rOfO = rOFO->plusFullColumn(aAOF->timesFullColumn(rFfF));
}
else if (str.find("offset") != std::string::npos) {
args.erase(args.begin());
rOfO = readBasicPosition(args);
}
else if (str.find("null") != std::string::npos) {
args.erase(args.begin());
}
else {
rOfO = readBasicPosition(args);
}
return rOfO;
}
FColDsptr MbD::MBDynItem::readBasicPosition(std::shared_ptr<std::vector<std::string>>& args)
FColDsptr MbD::MBDynItem::readBasicPosition(std::vector<std::string>& args)
{
auto parser = CREATE<SymbolicParser>::With();
parser->variables = mbdynVariables()->variables;
auto rFfF = std::make_shared<FullColumn<double>>(3);
std::string str;
for (int i = 0; i < 3; i++)
{
str = args->at(0);
args->erase(args->begin());
auto userFunc = CREATE<BasicUserFunction>::With(str, 1.0);
parser->parseUserFunction(userFunc);
auto sym = parser->stack->top();
rFfF->at(i) = sym->getValue();
}
return rFfF;
return readVector3(args);
}
FMatDsptr MbD::MBDynItem::readOrientation(std::shared_ptr<std::vector<std::string>>& args)
FMatDsptr MbD::MBDynItem::readOrientation(std::vector<std::string>& args)
{
auto aAOf = std::make_shared<FullMatrix<double>>(3, 3);
auto str = args->at(0);
auto aAOf = FullMatrix<double>::identitysptr(3);
if (args.empty()) return aAOf;
auto& str = args.at(0);
if (str.find("reference") != std::string::npos) {
args->erase(args->begin());
std::string refName;
std::istringstream iss(args->at(0));
iss >> refName;
auto ref = mbdynReferences()->references->at(refName);
args->erase(args->begin());
args.erase(args.begin());
auto refName = readStringOffTop(args);
auto ref = mbdynReferences()->at(refName);
auto aAFf = readBasicOrientation(args);
auto aAOF = ref->aAOf;
aAOf = aAOF->timesFullMatrix(aAFf);
}
else if (str.find("hinge") != std::string::npos) {
args.erase(args.begin());
aAOf = readOrientation(args);
}
else if (str.find("orientation") != std::string::npos) {
args.erase(args.begin());
aAOf = readOrientation(args);
}
else {
aAOf = readBasicOrientation(args);
}
return aAOf;
}
FMatDsptr MbD::MBDynItem::readBasicOrientation(std::shared_ptr<std::vector<std::string>>& args)
FMatDsptr MbD::MBDynItem::readBasicOrientation(std::vector<std::string>& args)
{
auto parser = CREATE<SymbolicParser>::With();
parser->variables = mbdynVariables()->variables;
auto euler = std::make_shared<EulerAngles<Symsptr>>();
auto str = args->at(0);
auto parser = std::make_shared<SymbolicParser>();
parser->variables = mbdynVariables();
auto& str = args.at(0);
if (str.find("euler") != std::string::npos) {
args->erase(args->begin());
args.erase(args.begin());
auto euler = std::make_shared<EulerAngles<Symsptr>>();
euler->rotOrder = std::make_shared<FullColumn<int>>(std::initializer_list<int>{ 1, 2, 3 });
for (int i = 0; i < 3; i++)
{
str = args->at(0);
args->erase(args->begin());
auto userFunc = CREATE<BasicUserFunction>::With(str, 1.0);
auto userFunc = std::make_shared<BasicUserFunction>(popOffTop(args), 1.0);
parser->parseUserFunction(userFunc);
auto sym = parser->stack->top();
euler->at(i) = sym;
}
euler->calc();
auto aAFf = euler->aA;
return aAFf;
}
else if (str.find("eye") != std::string::npos) {
args->erase(args->begin());
euler->rotOrder = std::make_shared<FullColumn<int>>(std::initializer_list<int>{ 1, 2, 3 });
for (int i = 0; i < 3; i++)
if (str.find("eye") != std::string::npos) {
args.erase(args.begin());
auto aAFf = FullMatrix<double>::identitysptr(3);
return aAFf;
}
auto iss = std::istringstream(str);
int integer;
iss >> integer;
if (integer == 1) {
args.erase(args.begin());
FColDsptr vecX, vecY, vecZ, vec;
vecX = readPosition(args);
vecX->normalizeSelf();
auto axis = stoi(popOffTop(args));
str = args.at(0);
if (str.find("guess") != std::string::npos) {
args.erase(args.begin());
double min = std::numeric_limits<double>::max();
double max = -1.0;
int imin, imax;
for (int i = 0; i < 3; i++)
{
auto mag = std::abs(vecX->at(i));
if (mag > max) {
imax = i;
max = mag;
}
if (mag < min) {
imin = i;
min = mag;
}
}
vec = std::make_shared<FullColumn<double>>(3);
vec->at(imin) = 1.0;
vec->at(imax) = -vecX->at(imin) / vecX->at(imax);
}
else {
vec = readPosition(args);
}
vec->normalizeSelf();
if (axis == 2) {
vecZ = vecX->cross(vec);
vecY = vecZ->cross(vecX);
}
else if (axis == 3) {
vecY = vec->cross(vecX);
vecZ = vecX->cross(vecY);
}
else {
assert(false);
}
auto aAFf = FullMatrix<double>::identitysptr(3);
aAFf->atijputFullColumn(0, 0, vecX);
aAFf->atijputFullColumn(0, 1, vecY);
aAFf->atijputFullColumn(0, 2, vecZ);
return aAFf;
}
if (integer == 3) {
args.erase(args.begin());
FColDsptr vecX, vecY, vecZ, vec;
vecZ = readPosition(args);
vecZ->normalizeSelf();
auto axis = stoi(popOffTop(args));
str = args.at(0);
if (str.find("guess") != std::string::npos) {
args.erase(args.begin());
double min = std::numeric_limits<double>::max();
double max = -1.0;
int imin, imax;
for (int i = 0; i < 3; i++)
{
auto mag = std::abs(vecZ->at(i));
if (mag > max) {
imax = i;
max = mag;
}
if (mag < min) {
imin = i;
min = mag;
}
}
vec = std::make_shared<FullColumn<double>>(3);
vec->at(imin) = 1.0;
vec->at(imax) = -vecZ->at(imin) / vecZ->at(imax);
}
else {
vec = readPosition(args);
}
vec->normalizeSelf();
if (axis == 2) {
vecX = vec->cross(vecZ);
vecY = vecZ->cross(vecX);
}
else if (axis == 1) {
vecY = vecZ->cross(vec);
vecX = vecY->cross(vecZ);
}
else {
assert(false);
}
auto aAFf = FullMatrix<double>::identitysptr(3);
aAFf->atijputFullColumn(0, 0, vecX);
aAFf->atijputFullColumn(0, 1, vecY);
aAFf->atijputFullColumn(0, 2, vecZ);
return aAFf;
}
auto aAFf = FullMatrix<double>::identitysptr(3);
for (int i = 0; i < 3; i++)
{
auto rowi = aAFf->at(i);
for (int j = 0; j < 3; j++)
{
euler->at(i) = std::make_shared<Constant>(0.0);
auto userFunc = std::make_shared<BasicUserFunction>(popOffTop(args), 1.0);
parser->parseUserFunction(userFunc);
auto sym = parser->stack->top();
rowi->at(j) = sym->getValue();
}
}
else {
assert(false);
}
euler->calc();
auto aAFf = euler->aA;
return aAFf;
}
std::string MbD::MBDynItem::popOffTop(std::vector<std::string>& args)
{
auto str = args.at(0);
args.erase(args.begin());
return str;
}
std::string MbD::MBDynItem::readStringOffTop(std::vector<std::string>& args)
{
auto iss = std::istringstream(args.at(0));
args.erase(args.begin());
std::string str;
iss >> str;
return str;
}

34
OndselSolver/MBDynItem.h
View File

@@ -11,8 +11,12 @@
namespace MbD {
class MBDynSystem;
class MBDynVariables;
class MBDynReferences;
class MBDynVariable;
class MBDynReference;
class MBDynNode;
class ASMTItem;
class MBDynBody;
class ASMTAssembly;
class MBDynItem
{
@@ -21,20 +25,32 @@ namespace MbD {
virtual MBDynSystem* root();
virtual void initialize();
void noop();
//void setName(std::string str);
virtual void parseMBDyn(std::vector<std::string>& lines);
std::vector<std::string>::iterator findLineWith(std::vector<std::string>& lines, std::vector<std::string>& tokens);
bool lineHasTokens(const std::string& line, std::vector<std::string>& tokens);
virtual std::shared_ptr<MBDynVariables> mbdynVariables();
virtual std::shared_ptr<MBDynReferences> mbdynReferences();
FColDsptr readPosition(std::shared_ptr<std::vector<std::string>>& args);
FColDsptr readBasicPosition(std::shared_ptr<std::vector<std::string>>& args);
FMatDsptr readOrientation(std::shared_ptr<std::vector<std::string>>& args);
FMatDsptr readBasicOrientation(std::shared_ptr<std::vector<std::string>>& args);
virtual std::shared_ptr<std::vector<std::shared_ptr<MBDynNode>>> mbdynNodes();
virtual std::vector<std::string> nodeNames();
virtual std::shared_ptr<std::map<std::string, Symsptr>> mbdynVariables();
virtual std::shared_ptr<std::map<std::string, std::shared_ptr<MBDynReference>>> mbdynReferences();
virtual void createASMT();
virtual std::shared_ptr<MBDynNode> nodeAt(std::string nodeName);
virtual int nodeidAt(std::string nodeName);
virtual std::shared_ptr<MBDynBody> bodyWithNode(std::string nodeName);
virtual std::shared_ptr<ASMTAssembly> asmtAssembly();
FColDsptr readVector3(std::vector<std::string>& args);
FColDsptr readPosition(std::vector<std::string>& args);
FColDsptr readBasicPosition(std::vector<std::string>& args);
FMatDsptr readOrientation(std::vector<std::string>& args);
FMatDsptr readBasicOrientation(std::vector<std::string>& args);
std::string popOffTop(std::vector<std::string>& args);
std::string readStringOffTop(std::vector<std::string>& args);
std::string name;
MBDynItem* owner;
std::shared_ptr<Item> mbdObject;
std::shared_ptr<ASMTItem> asmtItem;
};

231
OndselSolver/MBDynJoint.cpp
View File

@@ -1,4 +1,14 @@
#include <regex>
#include "MBDynJoint.h"
#include "ASMTMarker.h"
#include "ASMTPart.h"
#include "ASMTAssembly.h"
#include "ASMTRevoluteJoint.h"
#include "ASMTRotationalMotion.h"
#include "ASMTPointInLineJoint.h"
#include "ASMTNoRotationJoint.h"
#include "ASMTFixedJoint.h"
using namespace MbD;
@@ -6,7 +16,224 @@ void MbD::MBDynJoint::initialize()
{
}
void MbD::MBDynJoint::parseMBDyn(std::vector<std::string>& lines)
void MbD::MBDynJoint::parseMBDyn(std::string line)
{
assert(false);
jointString = line;
size_t previousPos = 0;
auto pos = line.find(":");
auto front = line.substr(previousPos, pos - previousPos);
assert(front.find("joint") != 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;
}
}
auto ss = std::stringstream();
auto iss = std::istringstream(arguments.at(0));
iss >> name;
arguments.erase(arguments.begin());
iss = std::istringstream(arguments.at(0));
iss >> joint_type;
if (joint_type == "axial") {
ss << joint_type;
iss >> joint_type;
if (joint_type == "rotation") {
ss << " " << joint_type;
joint_type = ss.str();
}
else {
assert(false);
}
arguments.erase(arguments.begin());
readMarkerI(arguments);
readMarkerJ(arguments);
readFunction(arguments);
return;
}
else if (joint_type == "revolute") {
ss << joint_type;
iss >> joint_type;
if (joint_type == "hinge") {
ss << " " << joint_type;
joint_type = ss.str();
}
else {
assert(false);
}
}
else if (joint_type == "spherical") {
ss << joint_type;
iss >> joint_type;
if (joint_type == "hinge") {
ss << " " << joint_type;
joint_type = ss.str();
}
else {
assert(false);
}
}
else if (joint_type == "drive") {
ss << joint_type;
iss >> joint_type;
if (joint_type == "hinge") {
ss << " " << joint_type;
joint_type = ss.str();
}
else {
assert(false);
}
arguments.erase(arguments.begin());
readMarkerI(arguments);
readMarkerJ(arguments);
readFunction(arguments);
return;
}
else if (joint_type == "in") {
ss << joint_type;
iss >> joint_type;
if (joint_type == "line") {
ss << " " << joint_type;
joint_type = ss.str();
}
else {
assert(false);
}
}
else if (joint_type == "clamp") {
//mkr1 should be on assembly which doesn't exist in MBDyn
//mkr2 is on the node
arguments.erase(arguments.begin());
mkr1 = std::make_shared<MBDynMarker>();
mkr1->owner = this;
mkr1->nodeStr = "Assembly";
mkr1->rPmP = std::make_shared<FullColumn<double>>(3);
mkr1->aAPm = FullMatrix<double>::identitysptr(3);
readMarkerJ(arguments);
return;
}
else if (joint_type == "prismatic") {
noop();
}
else {
assert(false);
}
arguments.erase(arguments.begin());
readMarkerI(arguments);
readMarkerJ(arguments);
}
void MbD::MBDynJoint::readMarkerI(std::vector<std::string>& args)
{
mkr1 = std::make_shared<MBDynMarker>();
mkr1->owner = this;
mkr1->nodeStr = readStringOffTop(args);
auto _nodeNames = nodeNames();
std::string nodeName;
auto it = std::find_if(args.begin(), args.end(), [&](const std::string& s) {
auto iss = std::istringstream(s);
iss >> nodeName;
if (std::find(_nodeNames.begin(), _nodeNames.end(), nodeName) != _nodeNames.end()) return true;
return false;
});
auto markerArgs = std::vector<std::string>(args.begin(), it);
args.erase(args.begin(), it);
mkr1->parseMBDyn(markerArgs);
}
void MbD::MBDynJoint::readMarkerJ(std::vector<std::string>& args)
{
if (args.empty()) return;
mkr2 = std::make_shared<MBDynMarker>();
mkr2->owner = this;
mkr2->nodeStr = readStringOffTop(args);
mkr2->parseMBDyn(args);
}
void MbD::MBDynJoint::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 slope, initValue, initTime, finalTime;
slope = popOffTop(args);
initTime = popOffTop(args);
finalTime = popOffTop(args);
initValue = popOffTop(args);
//f = slope*(time - t0) + f0
auto ss = std::stringstream();
ss << slope << "*(time - " << initTime << ") + " << initValue;
formula = ss.str();
} else if (str.find("single") != std::string::npos) {
args.erase(args.begin());
auto vec3 = readVector3(args);
assert(vec3->at(0) == 0 && vec3->at(1) == 0 && vec3->at(2) == 1);
assert(readStringOffTop(args) == "string");
formula = popOffTop(args);
formula = std::regex_replace(formula, std::regex("\""), "");
}
else {
assert(false);
}
}
void MbD::MBDynJoint::createASMT()
{
mkr1->createASMT();
if (mkr2) mkr2->createASMT();
std::shared_ptr<ASMTJoint> asmtJoint;
if (joint_type == "clamp") {
auto asmtAsm = asmtAssembly();
asmtJoint = std::make_shared<ASMTFixedJoint>();
asmtJoint->setName(name);
asmtJoint->setMarkerI(mkr1->asmtItem->fullName(""));
asmtJoint->setMarkerJ(mkr2->asmtItem->fullName(""));
asmtAsm->addJoint(asmtJoint);
return;
}
if (joint_type == "axial rotation") {
auto asmtAsm = asmtAssembly();
asmtJoint = std::make_shared<ASMTRevoluteJoint>();
asmtItem = asmtJoint;
asmtJoint->setName(name);
asmtJoint->setMarkerI(mkr1->asmtItem->fullName(""));
asmtJoint->setMarkerJ(mkr2->asmtItem->fullName(""));
asmtAsm->addJoint(asmtJoint);
auto asmtMotion = std::make_shared<ASMTRotationalMotion>();
asmtItem = asmtMotion;
asmtMotion->setName(name.append("Motion"));
asmtMotion->setMotionJoint(asmtJoint->fullName(""));
asmtMotion->setRotationZ(formula);
asmtAsm->addMotion(asmtMotion);
return;
}
if (joint_type == "revolute hinge") {
asmtJoint = std::make_shared<ASMTRevoluteJoint>();
}
else if (joint_type == "in line") {
asmtJoint = std::make_shared<ASMTPointInLineJoint>();
}
else if (joint_type == "prismatic") {
asmtJoint = std::make_shared<ASMTNoRotationJoint>();
}
else {
assert(false);
}
asmtItem = asmtJoint;
asmtJoint->setName(name);
asmtJoint->setMarkerI(mkr1->asmtItem->fullName(""));
asmtJoint->setMarkerJ(mkr2->asmtItem->fullName(""));
asmtAssembly()->addJoint(asmtJoint);
}

10
OndselSolver/MBDynJoint.h
View File

@@ -8,13 +8,21 @@
#pragma once
#include "MBDynElement.h"
#include "MBDynMarker.h"
namespace MbD {
class MBDynJoint : public MBDynElement
{
public:
void initialize() override;
void parseMBDyn(std::vector<std::string>& lines) override;
void parseMBDyn(std::string line);
void readMarkerI(std::vector<std::string>& args);
void readMarkerJ(std::vector<std::string>& args);
void readFunction(std::vector<std::string>& args);
void createASMT() override;
std::string jointString, joint_type, node_1_label, node_2_label;
std::shared_ptr<MBDynMarker> mkr1, mkr2;
std::string formula;
};
}

31
OndselSolver/MBDynLabels.cpp
View File

@@ -1,31 +0,0 @@
#include "MBDynLabels.h"
using namespace MbD;
void MbD::MBDynLabels::initialize()
{
}
void MbD::MBDynLabels::parseMBDyn(std::vector<std::string>& lines)
{
labels = std::make_shared<std::map<std::string, int>>();
std::string str, label;
int intValue;
std::vector<std::string> tokens{"set:", "integer"};
while (true) {
auto it = findLineWith(lines, tokens);
if (it != lines.end()) {
std::istringstream iss(*it);
iss >> str;
iss >> str;
iss >> label;
iss >> str;
iss >> intValue;
labels->insert(std::make_pair(label, intValue));
lines.erase(it);
}
else {
break;
}
}
}

55
OndselSolver/MBDynMarker.cpp Normal file
View File

@@ -0,0 +1,55 @@
#include "MBDynMarker.h"
#include "MBDynStructural.h"
#include "ASMTMarker.h"
#include "ASMTPart.h"
#include "ASMTAssembly.h"
#include "MBDynBody.h"
#include "ASMTSpatialContainer.h"
using namespace MbD;
void MbD::MBDynMarker::parseMBDyn(std::vector<std::string>& args)
{
rPmP = std::make_shared<FullColumn<double>>(3);
aAPm = FullMatrix<double>::identitysptr(3);
if (args.empty()) return;
auto& str = args.at(0);
if (str.find("reference") != std::string::npos) {
auto strucNode = std::static_pointer_cast<MBDynStructural>(nodeAt(nodeStr));
auto rOPO = strucNode->rOfO;
auto aAOP = strucNode->aAOf;
auto rOmO = readPosition(args);
auto aAOm = readOrientation(args);
rPmP = aAOP->transposeTimesFullColumn(rOmO->minusFullColumn(rOPO));
aAPm = aAOP->transposeTimesFullMatrix(aAOm);
}
else if (str.find("offset") != std::string::npos) {
rPmP = readPosition(args);
}
else {
rPmP = readPosition(args);
aAPm = readOrientation(args);
}
}
void MbD::MBDynMarker::createASMT()
{
auto asmtAsm = asmtAssembly();
if (nodeStr == "Assembly") {
auto mkr = std::make_shared<ASMTMarker>();
asmtItem = mkr;
mkr->setName(asmtAsm->generateUniqueMarkerName());
mkr->setPosition3D(rPmP);
mkr->setRotationMatrix(aAPm);
asmtAsm->addMarker(mkr);
}
else {
auto asmtPart = asmtAsm->partPartialNamed(nodeStr);
auto mkr = std::make_shared<ASMTMarker>();
asmtItem = mkr;
mkr->setName(asmtPart->generateUniqueMarkerName());
mkr->setPosition3D(rPmP);
mkr->setRotationMatrix(aAPm);
asmtPart->addMarker(mkr);
}
}

11
OndselSolver/MBDynVariables.h → OndselSolver/MBDynMarker.h
View File

@@ -10,12 +10,15 @@
#include "MBDynItem.h"
namespace MbD {
class MBDynVariables : public MBDynItem
class MBDynMarker : public MBDynItem
{
public:
void initialize() override;
void parseMBDyn(std::vector<std::string>& lines) override;
void parseMBDyn(std::vector<std::string>& args);
void createASMT() override;
std::shared_ptr<std::map<std::string, Symsptr>> variables;
std::string nodeStr;
FColDsptr rPmP; //part to marker
FMatDsptr aAPm;
};
}

35
OndselSolver/MBDynNode.cpp
View File

@@ -1,4 +1,6 @@
#include "MBDynNode.h"
#include "ASMTPart.h"
#include "EulerAngles.h"
using namespace MbD;
@@ -10,3 +12,36 @@ void MbD::MBDynNode::parseMBDyn(std::vector<std::string>& lines)
{
assert(false);
}
void MbD::MBDynNode::outputLine(int i, std::ostream& os)
{
auto id = nodeidAt(name);
auto asmtPart = std::static_pointer_cast<ASMTPart>(asmtItem);
auto x = asmtPart->xs->at(i);
auto y = asmtPart->ys->at(i);
auto z = asmtPart->zs->at(i);
auto bryantAngles = std::make_shared<EulerAngles<double>>();
bryantAngles->setRotOrder(1, 2, 3);
bryantAngles->at(0) = asmtPart->bryxs->at(i);
bryantAngles->at(1) = asmtPart->bryys->at(i);
bryantAngles->at(2) = asmtPart->bryzs->at(i);
bryantAngles->calc();
auto aA = bryantAngles->aA;
auto vx = asmtPart->vxs->at(i);
auto vy = asmtPart->vys->at(i);
auto vz = asmtPart->vzs->at(i);
auto omex = asmtPart->omexs->at(i);
auto omey = asmtPart->omeys->at(i);
auto omez = asmtPart->omezs->at(i);
os << id << " ";
os << x << " " << y << " " << z << " ";
auto row = aA->at(0);
os << row->at(0) << " " << row->at(1) << " " << row->at(2) << " ";
row = aA->at(1);
os << row->at(0) << " " << row->at(1) << " " << row->at(2) << " ";
row = aA->at(2);
os << row->at(0) << " " << row->at(1) << " " << row->at(2) << " ";
os << vx << " " << vy << " " << vz << " ";
os << omex << " " << omey << " " << omez << " ";
os << std::endl;
}

1
OndselSolver/MBDynNode.h
View File

@@ -15,6 +15,7 @@ namespace MbD {
public:
void initialize() override;
void parseMBDyn(std::vector<std::string>& lines) override;
void outputLine(int i, std::ostream& os);
};
}

28
OndselSolver/MBDynNodes.cpp
View File

@@ -1,28 +0,0 @@
#include "MBDynNodes.h"
#include "MBDynNode.h"
#include "MBDynStructural.h"
using namespace MbD;
void MbD::MBDynNodes::initialize()
{
}
void MbD::MBDynNodes::parseMBDyn(std::vector<std::string>& lines)
{
nodes = std::make_shared<std::vector<std::shared_ptr<MBDynNode>>>();
std::vector<std::string> tokens{ "structural:" };
while (true) {
auto it = findLineWith(lines, tokens);
if (it != lines.end()) {
auto structural = std::make_shared<MBDynStructural>();
structural->owner = this;
structural->parseMBDyn(*it);
nodes->push_back(structural);
lines.erase(it);
}
else {
break;
}
}
}

39
OndselSolver/MBDynReference.cpp
View File

@@ -1,5 +1,4 @@
#include "MBDynReference.h"
#include "MBDynVariables.h"
#include "SymbolicParser.h"
#include "BasicUserFunction.h"
#include "EulerAngles.h"
@@ -17,24 +16,24 @@ void MbD::MBDynReference::parseMBDyn(std::string line)
auto pos = line.find(":");
auto front = line.substr(previousPos, pos - previousPos);
assert(front.find("reference") != std::string::npos);
auto arguments = std::make_shared<std::vector<std::string>>();
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);
arguments.push_back(argument);
}
else {
argument = line.substr(previousPos + 1);
arguments->push_back(argument);
arguments.push_back(argument);
break;
}
}
std::istringstream iss(arguments->at(0));
std::istringstream iss(arguments.at(0));
iss >> name;
arguments->erase(arguments->begin());
arguments.erase(arguments.begin());
rOfO = readPosition(arguments);
aAOf = readOrientation(arguments);
@@ -42,22 +41,20 @@ void MbD::MBDynReference::parseMBDyn(std::string line)
readOmega(arguments);
}
void MbD::MBDynReference::readVelocity(std::shared_ptr<std::vector<std::string>>& args)
void MbD::MBDynReference::readVelocity(std::vector<std::string>& args)
{
auto parser = CREATE<SymbolicParser>::With();
parser->variables = mbdynVariables()->variables;
auto parser = std::make_shared<SymbolicParser>();
parser->variables = mbdynVariables();
vOfO = std::make_shared<FullColumn<double>>(3);
auto str = args->at(0);
auto& str = args.at(0);
if (str.find("null") != std::string::npos) {
args->erase(args->begin());
args.erase(args.begin());
return;
}
else {
for (int i = 0; i < 3; i++)
{
str = args->at(0);
args->erase(args->begin());
auto userFunc = CREATE<BasicUserFunction>::With(str, 1.0);
auto userFunc = std::make_shared<BasicUserFunction>(popOffTop(args), 1.0);
parser->parseUserFunction(userFunc);
auto sym = parser->stack->top();
vOfO->at(i) = sym->getValue();
@@ -65,22 +62,20 @@ void MbD::MBDynReference::readVelocity(std::shared_ptr<std::vector<std::string>>
}
}
void MbD::MBDynReference::readOmega(std::shared_ptr<std::vector<std::string>>& args)
void MbD::MBDynReference::readOmega(std::vector<std::string>& args)
{
auto parser = CREATE<SymbolicParser>::With();
parser->variables = mbdynVariables()->variables;
auto parser = std::make_shared<SymbolicParser>();
parser->variables = mbdynVariables();
omeOfO = std::make_shared<FullColumn<double>>(3);
auto str = args->at(0);
auto& str = args.at(0);
if (str.find("null") != std::string::npos) {
args->erase(args->begin());
args.erase(args.begin());
return;
}
else {
for (int i = 0; i < 3; i++)
{
str = args->at(0);
args->erase(args->begin());
auto userFunc = CREATE<BasicUserFunction>::With(str, 1.0);
auto userFunc = std::make_shared<BasicUserFunction>(popOffTop(args), 1.0);
parser->parseUserFunction(userFunc);
auto sym = parser->stack->top();
omeOfO->at(i) = sym->getValue();

4
OndselSolver/MBDynReference.h
View File

@@ -16,8 +16,8 @@ namespace MbD {
public:
void initialize() override;
void parseMBDyn(std::string line);
void readVelocity(std::shared_ptr<std::vector<std::string>>& args);
void readOmega(std::shared_ptr<std::vector<std::string>>& args);
void readVelocity(std::vector<std::string>& args);
void readOmega(std::vector<std::string>& args);
std::string refString, name;
FColDsptr rOfO, vOfO, omeOfO;

29
OndselSolver/MBDynReferences.cpp
View File

@@ -1,29 +0,0 @@
#include "MBDynReferences.h"
#include "MBDynReference.h"
using namespace MbD;
void MbD::MBDynReferences::initialize()
{
}
void MbD::MBDynReferences::parseMBDyn(std::vector<std::string>& lines)
{
references = std::make_shared<std::map<std::string, std::shared_ptr<MBDynReference>>>();
std::string str, refName;
double doubleValue;
std::vector<std::string> tokens{"reference:"};
while (true) {
auto it = findLineWith(lines, tokens);
if (it != lines.end()) {
auto reference = std::make_shared<MBDynReference>();
reference->owner = this;
reference->parseMBDyn(*it);
references->insert(std::make_pair(reference->name, reference));
lines.erase(it);
}
else {
break;
}
}
}

24
OndselSolver/MBDynReferences.h
View File

@@ -1,24 +0,0 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#pragma once
#include "MBDynItem.h"
namespace MbD {
class MBDynReference;
class MBDynReferences : public MBDynItem
{
public:
void initialize() override;
void parseMBDyn(std::vector<std::string>& lines) override;
std::shared_ptr<std::map<std::string, std::shared_ptr<MBDynReference>>> references;
};
}

64
OndselSolver/MBDynStructural.cpp
View File

@@ -1,16 +1,20 @@
#include "MBDynStructural.h"
#include "MBDynVariables.h"
#include "SymbolicParser.h"
#include "BasicUserFunction.h"
#include "EulerAngles.h"
#include "Constant.h"
#include "MBDynReferences.h"
#include "MBDynReference.h"
#include "ASMTPart.h"
#include "ASMTAssembly.h"
using namespace MbD;
void MbD::MBDynStructural::initialize()
MbD::MBDynStructural::MBDynStructural()
{
rOfO = std::make_shared<FullColumn<double>>(3);
aAOf = FullMatrix<double>::identitysptr(3);
vOfO = std::make_shared<FullColumn<double>>(3);
omeOfO = std::make_shared<FullColumn<double>>(3);
}
void MbD::MBDynStructural::parseMBDyn(std::string line)
@@ -20,27 +24,27 @@ void MbD::MBDynStructural::parseMBDyn(std::string line)
auto pos = line.find(":");
auto front = line.substr(previousPos, pos - previousPos);
assert(front.find("structural") != std::string::npos);
auto arguments = std::make_shared<std::vector<std::string>>();
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);
arguments.push_back(argument);
}
else {
argument = line.substr(previousPos + 1);
arguments->push_back(argument);
arguments.push_back(argument);
break;
}
}
std::istringstream iss(arguments->at(0));
std::istringstream iss(arguments.at(0));
iss >> name;
arguments->erase(arguments->begin());
iss = std::istringstream(arguments->at(0));
arguments.erase(arguments.begin());
iss = std::istringstream(arguments.at(0));
iss >> type;
arguments->erase(arguments->begin());
arguments.erase(arguments.begin());
rOfO = readPosition(arguments);
aAOf = readOrientation(arguments);
@@ -48,22 +52,20 @@ void MbD::MBDynStructural::parseMBDyn(std::string line)
readOmega(arguments);
}
void MbD::MBDynStructural::readVelocity(std::shared_ptr<std::vector<std::string>>& args)
void MbD::MBDynStructural::readVelocity(std::vector<std::string>& args)
{
auto parser = CREATE<SymbolicParser>::With();
parser->variables = mbdynVariables()->variables;
auto parser = std::make_shared<SymbolicParser>();
parser->variables = mbdynVariables();
vOfO = std::make_shared<FullColumn<double>>(3);
auto str = args->at(0);
auto& str = args.at(0);
if (str.find("null") != std::string::npos) {
args->erase(args->begin());
args.erase(args.begin());
return;
}
else {
for (int i = 0; i < 3; i++)
{
str = args->at(0);
args->erase(args->begin());
auto userFunc = CREATE<BasicUserFunction>::With(str, 1.0);
auto userFunc = std::make_shared<BasicUserFunction>(popOffTop(args), 1.0);
parser->parseUserFunction(userFunc);
auto sym = parser->stack->top();
vOfO->at(i) = sym->getValue();
@@ -71,25 +73,35 @@ void MbD::MBDynStructural::readVelocity(std::shared_ptr<std::vector<std::string>
}
}
void MbD::MBDynStructural::readOmega(std::shared_ptr<std::vector<std::string>>& args)
void MbD::MBDynStructural::readOmega(std::vector<std::string>& args)
{
auto parser = CREATE<SymbolicParser>::With();
parser->variables = mbdynVariables()->variables;
auto parser = std::make_shared<SymbolicParser>();
parser->variables = mbdynVariables();
omeOfO = std::make_shared<FullColumn<double>>(3);
auto str = args->at(0);
auto& str = args.at(0);
if (str.find("null") != std::string::npos) {
args->erase(args->begin());
args.erase(args.begin());
return;
}
else {
for (int i = 0; i < 3; i++)
{
str = args->at(0);
args->erase(args->begin());
auto userFunc = CREATE<BasicUserFunction>::With(str, 1.0);
auto userFunc = std::make_shared<BasicUserFunction>(popOffTop(args), 1.0);
parser->parseUserFunction(userFunc);
auto sym = parser->stack->top();
omeOfO->at(i) = sym->getValue();
}
}
}
void MbD::MBDynStructural::createASMT()
{
auto asmtPart = std::make_shared<ASMTPart>();
asmtItem = asmtPart;
asmtPart->setName(name);
asmtPart->setPosition3D(rOfO);
asmtPart->setRotationMatrix(aAOf);
asmtPart->setVelocity3D(vOfO);
asmtPart->setOmega3D(omeOfO);
asmtAssembly()->addPart(asmtPart);
}

9
OndselSolver/MBDynStructural.h
View File

@@ -13,12 +13,13 @@ namespace MbD {
class MBDynStructural : public MBDynNode
{
public:
void initialize() override;
MBDynStructural();
void parseMBDyn(std::string line);
void readVelocity(std::shared_ptr<std::vector<std::string>>& args);
void readOmega(std::shared_ptr<std::vector<std::string>>& args);
void readVelocity(std::vector<std::string>& args);
void readOmega(std::vector<std::string>& args);
void createASMT() override;
std::string strucString, name, type;
std::string strucString, type;
FColDsptr rOfO, vOfO, omeOfO;
FMatDsptr aAOf;
};

286
OndselSolver/MBDynSystem.cpp
View File

@@ -10,11 +10,16 @@
#include "MBDynInitialValue.h"
#include "MBDynData.h"
#include "MBDynControlData.h"
#include "MBDynLabels.h"
#include "MBDynVariables.h"
#include "MBDynReferences.h"
#include "MBDynNodes.h"
#include "MBDynElements.h"
#include "ASMTAssembly.h"
#include "ASMTConstantGravity.h"
#include "ASMTTime.h"
#include "MBDynBody.h"
#include "MBDynStructural.h"
#include "SymbolicParser.h"
#include "BasicUserFunction.h"
#include "MBDynJoint.h"
#include "MBDynReference.h"
#include <iomanip>
using namespace MbD;
@@ -48,19 +53,95 @@ void MbD::MBDynSystem::parseMBDyn(std::vector<std::string>& lines)
}
std::shared_ptr<MBDynVariables> MbD::MBDynSystem::mbdynVariables()
std::shared_ptr<std::vector<std::shared_ptr<MBDynNode>>> MbD::MBDynSystem::mbdynNodes()
{
return nodes;
}
std::shared_ptr<std::map<std::string, Symsptr>> MbD::MBDynSystem::mbdynVariables()
{
return variables;
}
std::shared_ptr<MBDynReferences> MbD::MBDynSystem::mbdynReferences()
std::shared_ptr<std::map<std::string, std::shared_ptr<MBDynReference>>> MbD::MBDynSystem::mbdynReferences()
{
return references;
}
void MbD::MBDynSystem::createASMT()
{
auto asmtAsm = std::make_shared<ASMTAssembly>();
asmtAsm->mbdynItem = this;
asmtItem = asmtAsm;
asmtItem->setName("Assembly");
initialValue->createASMT();
for (auto& node : *nodes) node->createASMT();
for (auto& element : *elements) element->createASMT();
}
std::shared_ptr<MBDynNode> MbD::MBDynSystem::nodeAt(std::string nodeName)
{
for (auto& node : *nodes) {
if (node->name == nodeName) return node;
}
return nullptr;
}
int MbD::MBDynSystem::nodeidAt(std::string nodeName)
{
return labels->at(nodeName);
}
std::shared_ptr<MBDynBody> MbD::MBDynSystem::bodyWithNode(std::string nodeName)
{
for (auto& element : *elements) {
std::string str = typeid(element).name();
if (str.find("MBDynBody") != std::string::npos) {
auto body = std::static_pointer_cast<MBDynBody>(element);
if (body->nodeName == nodeName) return body;
}
}
return nullptr;
}
std::shared_ptr<ASMTAssembly> MbD::MBDynSystem::asmtAssembly()
{
return std::static_pointer_cast<ASMTAssembly>(asmtItem);
}
std::vector<std::string> MbD::MBDynSystem::nodeNames()
{
auto nodeNames = std::vector<std::string>();
for (auto& node : *nodes) {
nodeNames.push_back(node->name);
}
return nodeNames;
}
void MbD::MBDynSystem::runKINEMATIC()
{
assert(false);
createASMT();
std::static_pointer_cast<ASMTAssembly>(asmtItem)->runKINEMATIC();
outputFiles();
}
void MbD::MBDynSystem::outputFiles()
{
auto movFile = filename.substr(0, filename.find_last_of('.')) + ".mov";
auto asmtAsm = asmtAssembly();
auto asmtTimes = asmtAsm->times;
auto asmtParts = asmtAsm->parts;
auto asmtJoints = asmtAsm->joints;
auto asmtMotions = asmtAsm->motions;
std::ofstream os(movFile);
os << std::setprecision(std::numeric_limits<double>::digits10 + 1);
for (int i = 1; i < asmtTimes->size(); i++)
{
for (auto& node : *nodes) {
node->outputLine(i, os);
}
}
os.close();
}
void MbD::MBDynSystem::setFilename(std::string str)
@@ -70,87 +151,75 @@ void MbD::MBDynSystem::setFilename(std::string str)
void MbD::MBDynSystem::readDataBlock(std::vector<std::string>& lines)
{
std::vector<std::string> tokens{"begin:", "data;"};
std::vector<std::string> tokens{ "begin:", "data" };
auto beginit = findLineWith(lines, tokens);
std::vector<std::string> tokens1{"end:", "data;"};
std::vector<std::string> tokens1{ "end:", "data" };
auto endit = findLineWith(lines, tokens1);
std::vector<std::string> blocklines = { beginit, endit + 1 };
dataBlk = std::make_shared<MBDynData>();
dataBlk->owner = this;
dataBlk->parseMBDyn(blocklines);
parseMBDynData(blocklines);
lines.erase(beginit, endit + 1);
}
void MbD::MBDynSystem::readInitialValueBlock(std::vector<std::string>& lines)
{
std::vector<std::string> tokens{"begin:", "initial", "value"};
std::vector<std::string> tokens{ "begin:", "initial", "value" };
auto beginit = findLineWith(lines, tokens);
std::vector<std::string> tokens1{"end:", "initial", "value"};
std::vector<std::string> tokens1{ "end:", "initial", "value" };
auto endit = findLineWith(lines, tokens1);
std::vector<std::string> blocklines = { beginit, endit + 1 };
initialValueBlk = std::make_shared<MBDynInitialValue>();
initialValueBlk->owner = this;
initialValueBlk->parseMBDyn(blocklines);
initialValue = std::make_shared<MBDynInitialValue>();
initialValue->owner = this;
initialValue->parseMBDyn(blocklines);
lines.erase(beginit, endit + 1);
}
void MbD::MBDynSystem::readControlDataBlock(std::vector<std::string>& lines)
{
std::vector<std::string> tokens{"begin:", "control", "data"};
std::vector<std::string> tokens{ "begin:", "control", "data" };
auto beginit = findLineWith(lines, tokens);
std::vector<std::string> tokens1{"end:", "control", "data"};
std::vector<std::string> tokens1{ "end:", "control", "data" };
auto endit = findLineWith(lines, tokens1);
std::vector<std::string> blocklines = { beginit, endit + 1 };
controlDataBlk = std::make_shared<MBDynControlData>();
controlDataBlk->owner = this;
controlDataBlk->parseMBDyn(blocklines);
controlData = std::make_shared<MBDynControlData>();
controlData->owner = this;
controlData->parseMBDyn(blocklines);
lines.erase(beginit, endit + 1);
}
void MbD::MBDynSystem::readLabels(std::vector<std::string>& lines)
{
labels = std::make_shared<MBDynLabels>();
labels->owner = this;
labels->parseMBDyn(lines);
parseMBDynLabels(lines);
}
void MbD::MBDynSystem::readVariables(std::vector<std::string>& lines)
{
variables = std::make_shared<MBDynVariables>();
variables->owner = this;
variables->parseMBDyn(lines);
parseMBDynVariables(lines);
}
void MbD::MBDynSystem::readReferences(std::vector<std::string>& lines)
{
references = std::make_shared<MBDynReferences>();
references->owner = this;
references->parseMBDyn(lines);
parseMBDynReferences(lines);
}
void MbD::MBDynSystem::readNodesBlock(std::vector<std::string>& lines)
{
std::vector<std::string> tokens{ "begin:", "nodes;" };
std::vector<std::string> tokens{ "begin:", "nodes" };
auto beginit = findLineWith(lines, tokens);
std::vector<std::string> tokens1{ "end:", "nodes;" };
std::vector<std::string> tokens1{ "end:", "nodes" };
auto endit = findLineWith(lines, tokens1);
std::vector<std::string> blocklines = { beginit, endit + 1 };
nodesBlk = std::make_shared<MBDynNodes>();
nodesBlk->owner = this;
nodesBlk->parseMBDyn(blocklines);
parseMBDynNodes(blocklines);
lines.erase(beginit, endit + 1);
}
void MbD::MBDynSystem::readElementsBlock(std::vector<std::string>& lines)
{
std::vector<std::string> tokens{ "begin:", "elements;" };
std::vector<std::string> tokens{ "begin:", "elements" };
auto beginit = findLineWith(lines, tokens);
std::vector<std::string> tokens1{ "end:", "elements;" };
std::vector<std::string> tokens1{ "end:", "elements" };
auto endit = findLineWith(lines, tokens1);
std::vector<std::string> blocklines = { beginit, endit + 1 };
elementsBlk = std::make_shared<MBDynElements>();
elementsBlk->owner = this;
elementsBlk->parseMBDyn(blocklines);
parseMBDynElements(blocklines);
lines.erase(beginit, endit + 1);
}
@@ -181,7 +250,7 @@ std::vector<std::string> MbD::MBDynSystem::collectStatements(std::vector<std::st
lines.erase(lines.begin());
auto i = line.find(';');
if (i != std::string::npos) {
ss << line.substr(0, i + 1);
ss << line.substr(0, i);
if (line.size() > i + 1) {
auto remainder = line.substr(i + 1);
auto it = std::find_if(remainder.begin(), remainder.end(), [](unsigned char ch) { return !std::isspace(ch); });
@@ -201,3 +270,134 @@ std::vector<std::string> MbD::MBDynSystem::collectStatements(std::vector<std::st
void MbD::MBDynSystem::initialize()
{
}
void MbD::MBDynSystem::parseMBDynData(std::vector<std::string>& lines)
{
assert(lines.size() == 3);
std::vector<std::string> tokens{ "problem:", "initial", "value" };
auto problemit = findLineWith(lines, tokens);
assert(problemit != lines.end());
data = *problemit;
}
void MbD::MBDynSystem::parseMBDynNodes(std::vector<std::string>& lines)
{
nodes = std::make_shared<std::vector<std::shared_ptr<MBDynNode>>>();
std::vector<std::string> tokens{ "structural:" };
while (true) {
auto it = findLineWith(lines, tokens);
if (it != lines.end()) {
auto structural = std::make_shared<MBDynStructural>();
structural->owner = this;
structural->parseMBDyn(*it);
nodes->push_back(structural);
lines.erase(it);
}
else {
break;
}
}
}
void MbD::MBDynSystem::parseMBDynElements(std::vector<std::string>& lines)
{
elements = std::make_shared<std::vector<std::shared_ptr<MBDynElement>>>();
std::vector<std::string> bodyToken{ "body:" };
std::vector<std::string> jointToken{ "joint:" };
std::vector<std::string>::iterator it;
while (true) {
it = findLineWith(lines, bodyToken);
if (it != lines.end()) {
auto body = std::make_shared<MBDynBody>();
body->owner = this;
body->parseMBDyn(*it);
elements->push_back(body);
lines.erase(it);
continue;
}
it = findLineWith(lines, jointToken);
if (it != lines.end()) {
auto joint = std::make_shared<MBDynJoint>();
joint->owner = this;
joint->parseMBDyn(*it);
elements->push_back(joint);
lines.erase(it);
continue;
}
break;
}
}
void MbD::MBDynSystem::parseMBDynVariables(std::vector<std::string>& lines)
{
variables = std::make_shared<std::map<std::string, Symsptr>>();
std::string str, variable;
double doubleValue;
std::vector<std::string> tokens{ "set:", "real" };
while (true) {
auto it = findLineWith(lines, tokens);
if (it != lines.end()) {
std::istringstream iss(*it);
iss >> str;
iss >> str;
iss >> variable;
iss >> str;
iss >> str;
auto parser = std::make_shared<SymbolicParser>();
parser->variables = variables;
auto userFunc = std::make_shared<BasicUserFunction>(str, 1.0);
parser->parseUserFunction(userFunc);
auto sym = parser->stack->top();
variables->insert(std::make_pair(variable, sym));
lines.erase(it);
}
else {
break;
}
}
}
void MbD::MBDynSystem::parseMBDynLabels(std::vector<std::string>& lines)
{
labels = std::make_shared<std::map<std::string, int>>();
std::string str, label;
int intValue;
std::vector<std::string> tokens{ "set:", "integer" };
while (true) {
auto it = findLineWith(lines, tokens);
if (it != lines.end()) {
std::istringstream iss(*it);
iss >> str;
iss >> str;
iss >> label;
iss >> str;
iss >> intValue;
labels->insert(std::make_pair(label, intValue));
lines.erase(it);
}
else {
break;
}
}
}
void MbD::MBDynSystem::parseMBDynReferences(std::vector<std::string>& lines)
{
references = std::make_shared<std::map<std::string, std::shared_ptr<MBDynReference>>>();
std::string str, refName;
double doubleValue;
std::vector<std::string> tokens{ "reference:" };
while (true) {
auto it = findLineWith(lines, tokens);
if (it != lines.end()) {
auto reference = std::make_shared<MBDynReference>();
reference->owner = this;
reference->parseMBDyn(*it);
references->insert(std::make_pair(reference->name, reference));
lines.erase(it);
}
else {
break;
}
}
}

44
OndselSolver/MBDynSystem.h
View File

@@ -16,11 +16,11 @@ namespace MbD {
class MBDynData;
class MBDynInitialValue;
class MBDynControlData;
class MBDynNodes;
class MBDynElements;
class MBDynVariables;
class MBDynLabels;
class MBDynReferences;
class MBDynNode;
class MBDynElement;
class MBDynVariable;
class MBDynLabel;
class MBDynReference;
class MBDynSystem : public MBDynItem
{
@@ -30,10 +30,24 @@ namespace MbD {
static std::vector<std::string> collectStatements(std::vector<std::string>& lines);
void initialize() override;
void parseMBDyn(std::vector<std::string>& lines) override;
std::shared_ptr<MBDynVariables> mbdynVariables() override;
std::shared_ptr<MBDynReferences> mbdynReferences() override;
void parseMBDynData(std::vector<std::string>& lines);
void parseMBDynNodes(std::vector<std::string>& lines);
void parseMBDynElements(std::vector<std::string>& lines);
void parseMBDynVariables(std::vector<std::string>& lines);
void parseMBDynLabels(std::vector<std::string>& lines);
void parseMBDynReferences(std::vector<std::string>& lines);
std::shared_ptr<std::vector<std::shared_ptr<MBDynNode>>> mbdynNodes() override;
std::shared_ptr<std::map<std::string, Symsptr>> mbdynVariables() override;
std::shared_ptr<std::map<std::string, std::shared_ptr<MBDynReference>>> mbdynReferences() override;
void createASMT() override;
std::shared_ptr<MBDynNode> nodeAt(std::string nodeName) override;
int nodeidAt(std::string nodeName) override;
std::shared_ptr<MBDynBody> bodyWithNode(std::string nodeName) override;
std::shared_ptr<ASMTAssembly> asmtAssembly() override;
std::vector<std::string> nodeNames() override;
void runKINEMATIC();
void outputFiles();
void setFilename(std::string filename);
void readDataBlock(std::vector<std::string>& lines);
void readInitialValueBlock(std::vector<std::string>& lines);
@@ -45,14 +59,14 @@ namespace MbD {
void readElementsBlock(std::vector<std::string>& lines);
std::string filename = "";
std::shared_ptr<MBDynData> dataBlk;
std::shared_ptr<MBDynInitialValue> initialValueBlk;
std::shared_ptr<MBDynControlData> controlDataBlk;
std::shared_ptr<MBDynNodes> nodesBlk;
std::shared_ptr<MBDynElements> elementsBlk;
std::shared_ptr<MBDynVariables> variables;
std::shared_ptr<MBDynLabels> labels;
std::shared_ptr<MBDynReferences> references;
std::string data;
std::shared_ptr<MBDynInitialValue> initialValue;
std::shared_ptr<MBDynControlData> controlData;
std::shared_ptr<std::vector<std::shared_ptr<MBDynNode>>> nodes;
std::shared_ptr<std::vector<std::shared_ptr<MBDynElement>>> elements;
std::shared_ptr<std::map<std::string, Symsptr>> variables;
std::shared_ptr<std::map<std::string, int>> labels;
std::shared_ptr<std::map<std::string, std::shared_ptr<MBDynReference>>> references;
};
}

38
OndselSolver/MBDynVariables.cpp
View File

@@ -1,38 +0,0 @@
#include "MBDynVariables.h"
#include "SymbolicParser.h"
#include "BasicUserFunction.h"
using namespace MbD;
void MbD::MBDynVariables::initialize()
{
}
void MbD::MBDynVariables::parseMBDyn(std::vector<std::string>& lines)
{
variables = std::make_shared<std::map<std::string, Symsptr>>();
std::string str, variable;
double doubleValue;
std::vector<std::string> tokens{"set:", "real"};
while (true) {
auto it = findLineWith(lines, tokens);
if (it != lines.end()) {
std::istringstream iss(*it);
iss >> str;
iss >> str;
iss >> variable;
iss >> str;
iss >> str;
auto parser = CREATE<SymbolicParser>::With();
parser->variables = variables;
auto userFunc = CREATE<BasicUserFunction>::With(str, 1.0);
parser->parseUserFunction(userFunc);
auto sym = parser->stack->top();
variables->insert(std::make_pair(variable, sym));
lines.erase(it);
}
else {
break;
}
}
}

4
OndselSolver/MarkerFrame.h
View File

@@ -72,9 +72,9 @@ namespace MbD {
PartFrame* partFrame; //Use raw pointer when pointing backwards.
FColDsptr rpmp = std::make_shared<FullColumn<double>>(3);
FMatDsptr aApm = std::make_shared<FullMatrix<double>>(3, 3);
FMatDsptr aApm = FullMatrix<double>::identitysptr(3);
FColDsptr rOmO = std::make_shared<FullColumn<double>>(3);
FMatDsptr aAOm = std::make_shared<FullMatrix<double>>(3, 3);
FMatDsptr aAOm = FullMatrix<double>::identitysptr(3);
FMatDsptr prOmOpE;
FColFMatDsptr pAOmpE;
FMatFColDsptr pprOmOpEpE;

157
OndselSolver/MomentOfInertiaSolver.cpp Normal file
View File

@@ -0,0 +1,157 @@
#include "MomentOfInertiaSolver.h"
#include "EulerParameters.h"
#include <iostream>
using namespace MbD;
void MbD::MomentOfInertiaSolver::example1()
{
auto aJpp = std::make_shared<FullMatrix<double>>(ListListD{
{ 1, 0, 0 },
{ 0, 2, 0 },
{ 0, 0, 3 }
});
auto rpPp = std::make_shared<FullColumn<double>>(ListD{ 0, 0, 1 });
auto axis = std::make_shared<FullColumn<double>>(ListD{ 0, 0, 1 });
auto aApP = std::make_shared<EulerParameters<double>>(axis, 0.1)->aA;
auto solver = std::make_shared<MomentOfInertiaSolver>();
solver->setm(4.0);
solver->setJPP(aJpp);
solver->setrPoP(rpPp);
solver->setAPo(aApP);
auto rPcmP = std::make_shared<FullColumn<double>>(ListD{ 0, 0, 0 });
solver->setrPcmP(rPcmP);
solver->calc();
auto aJPP = solver->getJoo();
std::cout << solver->getJoo();
std::cout << solver->getJpp();
std::cout << solver->getAPp();
std::cout << std::endl;
solver->setm(4.0);
solver->setJPP(aJPP);
auto rPoP = aApP->transposeTimesFullColumn(rpPp->negated());
solver->setrPoP(rPoP);
auto aAPo = aApP->transpose();
solver->setAPo(aAPo);
solver->setrPcmP(rPoP);
solver->calc();
std::cout << solver->getJoo();
std::cout << solver->getJpp();
std::cout << solver->getAPp();
std::cout << std::endl;
}
void MbD::MomentOfInertiaSolver::forwardEliminateWithPivot(int p)
{
}
void MbD::MomentOfInertiaSolver::backSubstituteIntoDU()
{
}
void MbD::MomentOfInertiaSolver::postSolve()
{
}
FColDsptr MbD::MomentOfInertiaSolver::basicSolvewithsaveOriginal(FMatDsptr fullMat, FColDsptr fullCol, bool saveOriginal)
{
return FColDsptr();
}
FColDsptr MbD::MomentOfInertiaSolver::basicSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal)
{
return FColDsptr();
}
void MbD::MomentOfInertiaSolver::preSolvewithsaveOriginal(FMatDsptr fullMat, FColDsptr fullCol, bool saveOriginal)
{
}
void MbD::MomentOfInertiaSolver::preSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal)
{
}
double MbD::MomentOfInertiaSolver::getmatrixArowimaxMagnitude(int i)
{
return 0.0;
}
void MbD::MomentOfInertiaSolver::doPivoting(int p)
{
}
void MbD::MomentOfInertiaSolver::setm(double mass)
{
m = mass;
}
void MbD::MomentOfInertiaSolver::setJPP(FMatDsptr mat)
{
aJPP = mat;
}
void MbD::MomentOfInertiaSolver::setrPoP(FColDsptr col)
{
rPoP = col;
}
void MbD::MomentOfInertiaSolver::setAPo(FMatDsptr mat)
{
aAPo = mat;
}
void MbD::MomentOfInertiaSolver::setrPcmP(FColDsptr col)
{
rPcmP = col;
}
FMatDsptr MbD::MomentOfInertiaSolver::getJoo()
{
return aJoo;
}
FMatDsptr MbD::MomentOfInertiaSolver::getJpp()
{
return aJpp;
}
FMatDsptr MbD::MomentOfInertiaSolver::getAPp()
{
return aAPp;
}
void MbD::MomentOfInertiaSolver::calc()
{
calcJoo();
calcJpp();
calcAPp();
}
void MbD::MomentOfInertiaSolver::calcJoo()
{
//"aJoo = aAPoT*[aJPP + mass*(rPoPTilde*rPoPTilde + rPoPTilde*rocmPTilde + rocmPTilde*rPoPTilde)]*aAPo"
if (!rPoP) {
rPoP = rPcmP;
aAPo = FullMatrix<double>::identitysptr(3);
}
auto rocmPtilde = FullMatrix<double>::tildeMatrix(rPcmP->minusFullColumn(rPoP));
auto rPoPtilde = FullMatrix<double>::tildeMatrix(rPoP);
auto term1 = aJPP;
auto term21 = rPoPtilde->timesFullMatrix(rPoPtilde);
auto term22 = rPoPtilde->timesFullMatrix(rocmPtilde);
auto term23 = term22->transpose();
auto term2 = term21->plusFullMatrix(term22)->plusFullMatrix(term23)->times(m);
aJoo = aAPo->transposeTimesFullMatrix(term1->plusFullMatrix(term2))->timesFullMatrix(aAPo);
aJoo->symLowerWithUpper();
aJoo->conditionSelfWithTol(aJoo->maxMagnitude() * 1.0e-6);
}
void MbD::MomentOfInertiaSolver::calcJpp()
{
}
void MbD::MomentOfInertiaSolver::calcAPp()
{
}

50
OndselSolver/MomentOfInertiaSolver.h Normal file
View File

@@ -0,0 +1,50 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#pragma once
#include "EigenDecomposition.h"
namespace MbD {
class MomentOfInertiaSolver : public EigenDecomposition
{
//See document 9100moment.fodt
//m aJPP aJoo rPoP aAPo aJcmP aJcmPcopy rPcmP aJpp aAPp colOrder
//aJoo == aJpp when rPoP == rPcmP and aAPo == aAPp
public:
static void example1();
void forwardEliminateWithPivot(int p) override;
void backSubstituteIntoDU() override;
void postSolve() override;
FColDsptr basicSolvewithsaveOriginal(FMatDsptr fullMat, FColDsptr fullCol, bool saveOriginal) override;
FColDsptr basicSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal) override;
void preSolvewithsaveOriginal(FMatDsptr fullMat, FColDsptr fullCol, bool saveOriginal) override;
void preSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal) override;
double getmatrixArowimaxMagnitude(int i) override;
void doPivoting(int p) override;
void setm(double mass);
void setJPP(FMatDsptr mat);
void setrPoP(FColDsptr col);
void setAPo(FMatDsptr mat);
void setrPcmP(FColDsptr col);
FMatDsptr getJoo();
FMatDsptr getJpp();
FMatDsptr getAPp();
void calc();
void calcJoo();
void calcJpp();
void calcAPp();
double m;
FMatDsptr aJPP, aJoo, aAPo, aJcmP, aJcmPcopy, aJpp, aAPp;
FColDsptr rPoP, rPcmP;
std::shared_ptr<FullRow<int>> colOrder;
};
}

21
OndselSolver/OndselSolver.cpp
View File

@@ -5,12 +5,12 @@
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
/*********************************************************************
* @file MbDCode.cpp
*
* @brief Program to assemble a piston crank system.
*********************************************************************/
/*********************************************************************
* @file MbDCode.cpp
*
* @brief Program to assemble a piston crank system.
*********************************************************************/
#include <filesystem>
#include "CADSystem.h"
@@ -24,10 +24,13 @@ void runSpMat();
int main()
{
//MBDynSystem::runFile("crank_slider.mbd"); //To be completed
//MBDynSystem::runFile("MBDynCase.mbd"); //To be completed
MBDynSystem::runFile("crank_slider.mbd"); //To be completed
//ASMTAssembly::runSinglePendulumSuperSimplified(); //Mass is missing
ASMTAssembly::runSinglePendulumSimplified();
ASMTAssembly::runSinglePendulum();
//ASMTAssembly::runSinglePendulumSuperSimplified2(); //DOF has infinite acceleration due to zero mass and inertias
//ASMTAssembly::runSinglePendulumSimplified();
ASMTAssembly::runSinglePendulum();
ASMTAssembly::runFile("piston.asmt");
ASMTAssembly::runFile("00backhoe.asmt");
//ASMTAssembly::runFile("circular.asmt"); //Needs checking

21
OndselSolver/OndselSolver.vcxproj
View File

@@ -159,12 +159,14 @@
<ClCompile Include="ASMTFixedJoint.cpp" />
<ClCompile Include="ASMTForceTorque.cpp" />
<ClCompile Include="ASMTGeneralMotion.cpp" />
<ClCompile Include="ASMTPointInLineJoint.cpp" />
<ClCompile Include="ASMTItem.cpp" />
<ClCompile Include="ASMTItemIJ.cpp" />
<ClCompile Include="ASMTJoint.cpp" />
<ClCompile Include="ASMTKinematicIJ.cpp" />
<ClCompile Include="ASMTMarker.cpp" />
<ClCompile Include="ASMTMotion.cpp" />
<ClCompile Include="ASMTNoRotationJoint.cpp" />
<ClCompile Include="ASMTPart.cpp" />
<ClCompile Include="ASMTPointInPlaneJoint.cpp" />
<ClCompile Include="ASMTPrincipalMassMarker.cpp" />
@@ -206,6 +208,7 @@
<ClCompile Include="CylindricalJoint.cpp" />
<ClCompile Include="CylSphJoint.cpp" />
<ClCompile Include="DifferentiatedGeneralSpline.cpp" />
<ClCompile Include="EigenDecomposition.cpp" />
<ClCompile Include="EndFrameqct2.cpp" />
<ClCompile Include="EulerAngles.cpp" />
<ClCompile Include="EulerAnglesDDot.cpp" />
@@ -224,18 +227,15 @@
<ClCompile Include="MBDynControlData.cpp" />
<ClCompile Include="MBDynData.cpp" />
<ClCompile Include="MBDynElement.cpp" />
<ClCompile Include="MBDynElements.cpp" />
<ClCompile Include="MBDynInitialValue.cpp" />
<ClCompile Include="MBDynItem.cpp" />
<ClCompile Include="MBDynJoint.cpp" />
<ClCompile Include="MBDynLabels.cpp" />
<ClCompile Include="MBDynMarker.cpp" />
<ClCompile Include="MBDynNode.cpp" />
<ClCompile Include="MBDynNodes.cpp" />
<ClCompile Include="MBDynReference.cpp" />
<ClCompile Include="MBDynReferences.cpp" />
<ClCompile Include="MBDynStructural.cpp" />
<ClCompile Include="MBDynSystem.cpp" />
<ClCompile Include="MBDynVariables.cpp" />
<ClCompile Include="MomentOfInertiaSolver.cpp" />
<ClCompile Include="Negative.cpp" />
<ClCompile Include="PosVelAccData.cpp" />
<ClCompile Include="DiagonalMatrix.cpp" />
@@ -445,12 +445,14 @@
<ClInclude Include="ASMTFixedJoint.h" />
<ClInclude Include="ASMTForceTorque.h" />
<ClInclude Include="ASMTGeneralMotion.h" />
<ClInclude Include="ASMTPointInLineJoint.h" />
<ClInclude Include="ASMTItem.h" />
<ClInclude Include="ASMTItemIJ.h" />
<ClInclude Include="ASMTJoint.h" />
<ClInclude Include="ASMTKinematicIJ.h" />
<ClInclude Include="ASMTMarker.h" />
<ClInclude Include="ASMTMotion.h" />
<ClInclude Include="ASMTNoRotationJoint.h" />
<ClInclude Include="ASMTPart.h" />
<ClInclude Include="ASMTPointInPlaneJoint.h" />
<ClInclude Include="ASMTPrincipalMassMarker.h" />
@@ -492,6 +494,7 @@
<ClInclude Include="CylindricalJoint.h" />
<ClInclude Include="CylSphJoint.h" />
<ClInclude Include="DifferentiatedGeneralSpline.h" />
<ClInclude Include="EigenDecomposition.h" />
<ClInclude Include="EndFrameqct2.h" />
<ClInclude Include="EulerAngles.h" />
<ClInclude Include="EulerAnglesDDot.h" />
@@ -510,18 +513,15 @@
<ClInclude Include="MBDynControlData.h" />
<ClInclude Include="MBDynData.h" />
<ClInclude Include="MBDynElement.h" />
<ClInclude Include="MBDynElements.h" />
<ClInclude Include="MBDynInitialValue.h" />
<ClInclude Include="MBDynItem.h" />
<ClInclude Include="MBDynJoint.h" />
<ClInclude Include="MBDynLabels.h" />
<ClInclude Include="MBDynMarker.h" />
<ClInclude Include="MBDynNode.h" />
<ClInclude Include="MBDynNodes.h" />
<ClInclude Include="MBDynReference.h" />
<ClInclude Include="MBDynReferences.h" />
<ClInclude Include="MBDynStructural.h" />
<ClInclude Include="MBDynSystem.h" />
<ClInclude Include="MBDynVariables.h" />
<ClInclude Include="MomentOfInertiaSolver.h" />
<ClInclude Include="Negative.h" />
<ClInclude Include="PosVelAccData.h" />
<ClInclude Include="DiagonalMatrix.h" />
@@ -715,6 +715,7 @@
<None Include="circular.asmt" />
<None Include="cirpendu.asmt" />
<None Include="ClassDiagram.cd" />
<None Include="ClassDiagram1.cd" />
<None Include="crank_slider.mbd" />
<None Include="engine1.asmt" />
<None Include="fourbar.asmt" />

61
OndselSolver/OndselSolver.vcxproj.filters
View File

@@ -819,9 +819,6 @@
<ClCompile Include="MBDynData.cpp">
<Filter>Source Files</Filter>
</ClCompile>
<ClCompile Include="MBDynNodes.cpp">
<Filter>Source Files</Filter>
</ClCompile>
<ClCompile Include="MBDynSystem.cpp">
<Filter>Source Files</Filter>
</ClCompile>
@@ -831,18 +828,9 @@
<ClCompile Include="MBDynInitialValue.cpp">
<Filter>Source Files</Filter>
</ClCompile>
<ClCompile Include="MBDynElements.cpp">
<Filter>Source Files</Filter>
</ClCompile>
<ClCompile Include="MBDynControlData.cpp">
<Filter>Source Files</Filter>
</ClCompile>
<ClCompile Include="MBDynVariables.cpp">
<Filter>Source Files</Filter>
</ClCompile>
<ClCompile Include="MBDynLabels.cpp">
<Filter>Source Files</Filter>
</ClCompile>
<ClCompile Include="MBDynNode.cpp">
<Filter>Source Files</Filter>
</ClCompile>
@@ -858,15 +846,27 @@
<ClCompile Include="MBDynJoint.cpp">
<Filter>Source Files</Filter>
</ClCompile>
<ClCompile Include="MBDynReferences.cpp">
<Filter>Source Files</Filter>
</ClCompile>
<ClCompile Include="MBDynReference.cpp">
<Filter>Source Files</Filter>
</ClCompile>
<ClCompile Include="ArcSine.cpp">
<Filter>Source Files</Filter>
</ClCompile>
<ClCompile Include="MBDynMarker.cpp">
<Filter>Source Files</Filter>
</ClCompile>
<ClCompile Include="ASMTPointInLineJoint.cpp">
<Filter>Source Files</Filter>
</ClCompile>
<ClCompile Include="ASMTNoRotationJoint.cpp">
<Filter>Source Files</Filter>
</ClCompile>
<ClCompile Include="EigenDecomposition.cpp">
<Filter>Source Files</Filter>
</ClCompile>
<ClCompile Include="MomentOfInertiaSolver.cpp">
<Filter>Source Files</Filter>
</ClCompile>
</ItemGroup>
<ItemGroup>
<ClInclude Include="Array.h">
@@ -1679,9 +1679,6 @@
<ClInclude Include="MBDynData.h">
<Filter>Header Files</Filter>
</ClInclude>
<ClInclude Include="MBDynNodes.h">
<Filter>Header Files</Filter>
</ClInclude>
<ClInclude Include="MBDynSystem.h">
<Filter>Header Files</Filter>
</ClInclude>
@@ -1691,18 +1688,9 @@
<ClInclude Include="MBDynInitialValue.h">
<Filter>Header Files</Filter>
</ClInclude>
<ClInclude Include="MBDynElements.h">
<Filter>Header Files</Filter>
</ClInclude>
<ClInclude Include="MBDynControlData.h">
<Filter>Header Files</Filter>
</ClInclude>
<ClInclude Include="MBDynVariables.h">
<Filter>Header Files</Filter>
</ClInclude>
<ClInclude Include="MBDynLabels.h">
<Filter>Header Files</Filter>
</ClInclude>
<ClInclude Include="MBDynNode.h">
<Filter>Header Files</Filter>
</ClInclude>
@@ -1718,15 +1706,27 @@
<ClInclude Include="MBDynJoint.h">
<Filter>Header Files</Filter>
</ClInclude>
<ClInclude Include="MBDynReferences.h">
<Filter>Header Files</Filter>
</ClInclude>
<ClInclude Include="MBDynReference.h">
<Filter>Header Files</Filter>
</ClInclude>
<ClInclude Include="ArcSine.h">
<Filter>Header Files</Filter>
</ClInclude>
<ClInclude Include="MBDynMarker.h">
<Filter>Header Files</Filter>
</ClInclude>
<ClInclude Include="ASMTPointInLineJoint.h">
<Filter>Header Files</Filter>
</ClInclude>
<ClInclude Include="ASMTNoRotationJoint.h">
<Filter>Header Files</Filter>
</ClInclude>
<ClInclude Include="EigenDecomposition.h">
<Filter>Header Files</Filter>
</ClInclude>
<ClInclude Include="MomentOfInertiaSolver.h">
<Filter>Header Files</Filter>
</ClInclude>
</ItemGroup>
<ItemGroup>
<ResourceCompile Include="OndselSolver.rc">
@@ -1746,5 +1746,6 @@
<None Include="robot.asmt" />
<None Include="wobpump.asmt" />
<None Include="crank_slider.mbd" />
<None Include="ClassDiagram1.cd" />
</ItemGroup>
</Project>

2
OndselSolver/PlanarJoint.cpp
View File

@@ -30,6 +30,6 @@ void MbD::PlanarJoint::initializeGlobally()
this->root()->hasChanged = true;
}
else {
InPlaneJoint::initializeGlobally();
Joint::initializeGlobally();
}
}

2
OndselSolver/PointInLineJoint.cpp
View File

@@ -28,6 +28,6 @@ void MbD::PointInLineJoint::initializeGlobally()
this->root()->hasChanged = true;
}
else {
InLineJoint::initializeGlobally();
Joint::initializeGlobally();
}
}

2
OndselSolver/PointInPlaneJoint.cpp
View File

@@ -27,6 +27,6 @@ void MbD::PointInPlaneJoint::initializeGlobally()
this->root()->hasChanged = true;
}
else {
InPlaneJoint::initializeGlobally();
Joint::initializeGlobally();
}
}

10
OndselSolver/PosKineNewtonRaphson.cpp
View File

@@ -5,12 +5,13 @@
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#include "PosKineNewtonRaphson.h"
#include "SystemSolver.h"
#include "Part.h"
#include "NotKinematicError.h"
#include "Constraint.h"
#include <iostream>
using namespace MbD;
@@ -80,5 +81,10 @@ void PosKineNewtonRaphson::preRun()
void PosKineNewtonRaphson::fillY()
{
y->zeroSelf();
system->partsJointsMotionsDo([&](std::shared_ptr<Item> item) { item->fillPosKineError(y); });
system->partsJointsMotionsDo([&](std::shared_ptr<Item> item) {
item->fillPosKineError(y);
//std::cout << item->name << *y << std::endl;
//noop();
});
//std::cout << "Final" << *y << std::endl;
}

5
OndselSolver/Solver.cpp
View File

@@ -13,6 +13,11 @@
using namespace MbD;
void MbD::Solver::noop()
{
//No Operations
}
void Solver::initialize()
{
}

1
OndselSolver/Solver.h
View File

@@ -15,6 +15,7 @@ namespace MbD {
{
//statistics
public:
void noop();
virtual void initialize();
virtual void initializeLocally();
virtual void initializeGlobally();

7
OndselSolver/SymbolicParser.cpp
View File

@@ -26,6 +26,13 @@
#include "GeneralSpline.h"
#include "ArcSine.h"
MbD::SymbolicParser::SymbolicParser()
{
variables = std::make_shared<std::map<std::string, Symsptr>>();
stack = std::make_shared<std::stack<Symsptr>>();
buffer = std::make_shared<std::stringstream>();
}
void MbD::SymbolicParser::initialize()
{
variables = std::make_shared<std::map<std::string, Symsptr>>();

1
OndselSolver/SymbolicParser.h
View File

@@ -21,6 +21,7 @@ namespace MbD {
{
//
public:
SymbolicParser();
void initialize();
void parseUserFunction(Symsptr userFunc);
void parseString(std::string expr);

12
OndselSolver/System.cpp
View File

@@ -21,6 +21,12 @@
using namespace MbD;
System::System() {
externalSystem = std::make_shared<ExternalSystem>();
time = std::make_shared<Time>();
parts = std::make_shared<std::vector<std::shared_ptr<Part>>>();
jointsMotions = std::make_shared<std::vector<std::shared_ptr<Joint>>>();
forcesTorques = std::make_shared<std::vector<std::shared_ptr<ForceTorqueItem>>>();
systemSolver = std::make_shared<SystemSolver>(this);
}
System::System(const char* str) : Item(str) {
@@ -33,12 +39,6 @@ System* MbD::System::root()
void System::initialize()
{
externalSystem = std::make_shared<ExternalSystem>();
time = CREATE<Time>::With();
parts = std::make_shared<std::vector<std::shared_ptr<Part>>>();
jointsMotions = std::make_shared<std::vector<std::shared_ptr<Joint>>>();
forcesTorques = std::make_shared<std::vector<std::shared_ptr<ForceTorqueItem>>>();
systemSolver = std::make_shared<SystemSolver>(this);
}
void System::addPart(std::shared_ptr<Part> part)

4
OndselSolver/Time.cpp
View File

@@ -12,10 +12,10 @@ using namespace MbD;
Time::Time()
{
std::string str = "t";
this->setName(str);
}
void Time::initialize()
{
std::string str = "t";
this->setName(str);
}

2
OndselSolver/TranslationalJoint.cpp
View File

@@ -33,6 +33,6 @@ void MbD::TranslationalJoint::initializeGlobally()
this->root()->hasChanged = true;
}
else {
InLineJoint::initializeGlobally();
Joint::initializeGlobally();
}
}

2
OndselSolver/crank_slider.mbd
View File

@@ -13,7 +13,7 @@ end: data;
begin: initial value;
initial time: 0.;
final time: 5.;
time step: 1.e-2;
time step: 1.e-1;
max iterations: 10;
tolerance: 1.e-6;
end: initial value;

204
OndselSolver/crank_slider.mov Normal file
View File

@@ -0,0 +1,204 @@
1 0 0 0 1 0 0 0 1 0 0 0 1 5.551115123125783e-17 -1.110223024624046e-16 0 -0 0 0
2 0.1 0 0 1 0 0 0 1 0 0 0 1 -6.993747673745632e-18 0.6283185307179585 0 0 0 6.283185307179586
3 0.3984313483298443 0.025 0 0.9921567416492215 -0.125 0 0.125 0.9921567416492215 0 0 0 1 0.07916069411491507 0.6283185307179583 0 0 0 -3.166427764596603
4 0.5968626966596886 0.05 0 1 0 0 0 1 0 0 0 1 0.1583213882298301 -5.551115123120231e-17 0 0 0 0
1 0 0 0 1 0 0 0 1 0 0 0 1 0 0 0 0 0 0
2 0.08090169943749474 0.05877852522924731 0 0.8090169943749475 -0.5877852522924731 0 0.5877852522924731 0.8090169943749475 0 0 0 1 -0.3693163660980913 0.5083203692315259 0 0 0 6.283185307179585
3 0.3589302900728601 0.08377852522924729 0 0.9856344559893528 0.1688926261462365 0 -0.1688926261462365 0.9856344559893528 0 0 0 1 -0.8257355739865109 0.5083203692315259 0 0 0 -2.578645491452954
4 0.5560571812707307 0.05 0 1 0 0 0 1 0 0 0 1 -0.9128384157768391 0 -0 0 0 0
1 0 0 0 1 0 0 0 1 0 0 0 1 -0 0 0 0 0 0
2 0.03090169943749474 0.09510565162951536 0 0.3090169943749475 -0.9510565162951535 0 0.9510565162951535 0.3090169943749475 0 0 0 1 -0.5975664329483111 0.1941611038725468 0 0 0 6.283185307179586
3 0.2491138300036403 0.1201056516295154 0 0.936552155643254 0.3505282581475768 0 -0.3505282581475768 0.936552155643254 0 0 0 1 -1.267802554531004 0.1941611038725468 0 0 0 -1.03657389875522
4 0.436424261132291 0.05 0 1 0 0 0 1 0 0 0 1 -1.340472243165386 0 -0 0 0 0
1 0 0 0 1 0 0 0 1 0 0 0 1 -0 0 0 0 0 0
2 -0.03090169943749476 0.09510565162951534 0 -0.3090169943749476 -0.9510565162951535 0 0.9510565162951535 -0.3090169943749476 0 0 0 1 -0.5975664329483108 -0.1941611038725468 0 0 0 6.283185307179586
3 0.1255070322536613 0.1201056516295154 0 0.936552155643254 0.3505282581475768 0 -0.3505282581475768 0.936552155643254 0 0 0 1 -1.12246317726224 -0.1941611038725468 0 0 -0 1.036573898755221
4 0.3128174633823121 0.05 0 1 0 0 0 1 0 0 0 1 -1.049793488627858 0 -0 0 0 0
1 0 0 0 1 0 0 0 1 0 0 0 1 0 0 0 0 0 0
2 -0.08090169943749473 0.05877852522924733 0 -0.8090169943749473 -0.5877852522924732 0 0.5877852522924732 -0.8090169943749473 0 0 0 1 -0.3693163660980914 -0.5083203692315259 0 0 0 6.283185307179587
3 0.03532349232288116 0.08377852522924732 0 0.9856344559893528 0.1688926261462365 0 -0.1688926261462365 0.9856344559893528 0 0 0 1 -0.6515298904058543 -0.5083203692315257 0 0 -0 2.578645491452954
4 0.2324503835207517 0.05 0 1 0 0 0 1 0 0 0 1 -0.564427048615526 0 -0 0 0 0
1 0 0 0 1 0 0 0 1 0 0 0 1 0 0 0 0 0 0
2 -0.1 1.224646799190013e-17 0 -1 -1.224646799147353e-16 0 1.224646799147353e-16 -1 0 0 0 1 0 -0.6283185307179586 0 0 0 6.283185307179586
3 -0.001568651670155672 0.02500000000000002 0 0.9921567416492215 -0.1249999999999999 0 0.1249999999999999 0.9921567416492215 0 0 0 1 -0.07916069411491503 -0.6283185307179584 0 0 0 3.166427764596603
4 0.1968626966596886 0.05 0 1 0 0 0 1 0 0 0 1 -0.15832138822983 0 -0 0 0 0
1 0 0 0 1 0 0 0 1 0 0 0 1 0 0 0 0 0 0
2 -0.08090169943749474 -0.05877852522924731 0 -0.8090169943749473 0.5877852522924732 0 -0.5877852522924732 -0.8090169943749473 0 0 0 1 0.3693163660980912 -0.5083203692315259 0 0 0 6.283185307179584
3 0.01980375602374343 -0.03377852522924731 0 0.9080357744936647 -0.4188926261462365 0 0.4188926261462365 0.9080357744936647 0 0 0 1 0.5041357438922219 -0.5083203692315259 0 0 0 2.799010697100417
4 0.2014109109224763 0.05 0 1 0 0 0 1 0 0 0 1 0.2696387555882615 0 -0 0 0 0
1 0 0 0 1 0 0 0 1 0 0 0 1 0 0 0 0 0 0
2 -0.03090169943749475 -0.09510565162951536 0 -0.3090169943749476 0.9510565162951535 0 -0.9510565162951535 -0.3090169943749476 0 0 0 1 0.5975664329483111 -0.1941611038725468 0 0 0 6.283185307179586
3 0.0981173078726031 -0.07010565162951535 0 0.7996035337379629 -0.6005282581475767 0 0.6005282581475767 0.7996035337379629 0 0 0 1 1.049311562477018 -0.1941611038725468 0 0 0 1.214108590571681
4 0.2580380146201957 0.05 0 1 0 0 0 1 0 0 0 1 0.9034902590574138 0 -0 0 0 0
1 0 0 0 1 0 0 0 1 0 0 0 1 0 0 0 0 0 0
2 0.03090169943749473 -0.09510565162951537 0 0.3090169943749473 0.9510565162951536 0 -0.9510565162951536 0.3090169943749473 0 0 0 1 0.5975664329483112 0.1941611038725467 0 0 0 6.283185307179587
3 0.221724105622582 -0.07010565162951535 0 0.7996035337379628 -0.6005282581475767 0 0.6005282581475767 0.7996035337379628 0 0 0 1 1.340954169316227 0.1941611038725468 0 0 0 -1.21410859057168
4 0.3816448123701746 0.05 0 1 0 0 0 1 0 0 0 1 1.486775472735831 0 -0 0 0 0
1 0 0 0 1 0 0 0 1 0 0 0 1 0 0 0 0 0 0
2 0.08090169943749467 -0.05877852522924741 0 0.8090169943749468 0.587785252292474 0 -0.587785252292474 0.8090169943749468 0 0 0 1 0.3693163660980919 0.5083203692315256 0 0 0 6.283185307179587
3 0.3434105537737223 -0.0337785252292474 0 0.9080357744936643 -0.418892626146237 0 0.418892626146237 0.9080357744936643 0 0 0 1 0.9731297205001445 0.5083203692315256 0 0 0 -2.799010697100416
4 0.5250177086724551 0.05 0 1 0 0 0 1 0 0 0 1 1.207626708804105 0 -0 0 0 0
1 0 0 0 1 0 0 0 1 0 0 0 1 0 0 0 0 0 0
2 0.1 -1.133107779525203e-16 0 1 1.133106425970147e-15 0 -1.133106425970147e-15 1 0 0 0 1 0 0.6283185307179586 0 0 0 6.283185307179586
3 0.3984313483298443 0.02499999999999989 0 0.9921567416492214 -0.1250000000000006 0 0.1250000000000006 0.9921567416492214 0 0 0 1 0.07916069411491541 0.6283185307179587 0 0 0 -3.166427764596602
4 0.5968626966596887 0.05 0 1 0 0 0 1 0 0 0 1 0.1583213882298308 0 -0 0 0 0
1 0 0 0 1 0 0 0 1 0 0 0 1 0 0 0 0 0 0
2 0.08090169943749481 0.05877852522924722 0 0.8090169943749481 -0.5877852522924722 0 0.5877852522924722 0.8090169943749481 0 0 0 1 -0.3693163660980908 0.5083203692315263 0 0 0 6.283185307179585
3 0.3589302900728603 0.08377852522924722 0 0.985634455989353 0.1688926261462361 0 -0.1688926261462361 0.985634455989353 0 0 0 1 -0.8257355739865098 0.5083203692315265 0 0 0 -2.578645491452955
4 0.5560571812707309 0.05 0 1 0 0 0 1 0 0 0 1 -0.9128384157768378 0 -0 0 0 0
1 0 0 0 1 0 0 0 1 0 0 0 1 0 0 0 0 0 0
2 0.03090169943749477 0.09510565162951536 0 0.3090169943749477 -0.9510565162951535 0 0.9510565162951535 0.3090169943749477 0 0 0 1 -0.597566432948311 0.1941611038725467 0 0 0 6.283185307179584
3 0.2491138300036404 0.1201056516295154 0 0.936552155643254 0.3505282581475768 0 -0.3505282581475768 0.936552155643254 0 0 0 1 -1.267802554531004 0.1941611038725467 0 0 0 -1.03657389875522
4 0.4364242611322912 0.05 0 1 0 0 0 1 0 0 0 1 -1.340472243165386 0 -0 0 0 0
1 0 0 0 1 0 0 0 1 0 0 0 1 0 0 0 0 0 0
2 -0.03090169943749472 0.09510565162951537 0 -0.3090169943749471 -0.9510565162951536 0 0.9510565162951536 -0.3090169943749471 0 0 0 1 -0.5975664329483112 -0.1941611038725465 0 0 0 6.283185307179587
3 0.1255070322536614 0.1201056516295154 0 0.936552155643254 0.3505282581475768 0 -0.3505282581475768 0.936552155643254 0 0 0 1 -1.122463177262241 -0.1941611038725465 0 0 -0 1.036573898755219
4 0.3128174633823122 0.05 0 1 0 0 0 1 0 0 0 1 -1.049793488627859 0 -0 0 0 0
1 0 0 0 1 0 0 0 1 0 0 0 1 -0 0 0 0 0 0
2 -0.08090169943749473 0.05877852522924734 0 -0.8090169943749473 -0.5877852522924732 0 0.5877852522924732 -0.8090169943749473 0 0 0 1 -0.3693163660980915 -0.5083203692315258 0 0 0 6.283185307179585
3 0.03532349232288117 0.08377852522924735 0 0.9856344559893528 0.1688926261462367 0 -0.1688926261462367 0.9856344559893528 0 0 0 1 -0.6515298904058544 -0.5083203692315257 0 0 -0 2.578645491452953
4 0.2324503835207518 0.05 0 1 0 0 0 1 0 0 0 1 -0.564427048615526 0 -0 0 0 0
1 0 0 0 1 0 0 0 1 0 0 0 1 0 0 0 0 0 0
2 -0.1 -1.408962799657787e-16 0 -1 1.454732309464923e-15 0 -1.454732309464923e-15 -1 0 0 0 1 0 -0.6283185307179586 0 -0 0 6.283185307179586
3 -0.001568651670155688 0.02499999999999986 0 0.9921567416492214 -0.1250000000000007 0 0.1250000000000007 0.9921567416492214 0 0 0 1 -0.0791606941149155 -0.6283185307179585 0 0 0 3.166427764596602
4 0.1968626966596886 0.05 0 1 0 0 0 1 0 0 0 1 -0.158321388229831 0 -0 0 0 0
1 0 0 0 1 0 0 0 1 0 0 0 1 -0 0 0 0 0 0
2 -0.08090169943749466 -0.05877852522924742 0 -0.8090169943749466 0.5877852522924742 0 -0.5877852522924742 -0.8090169943749466 0 0 0 1 0.3693163660980921 -0.5083203692315255 0 0 0 6.283185307179585
3 0.01980375602374362 -0.03377852522924742 0 0.9080357744936643 -0.4188926261462371 0 0.4188926261462371 0.9080357744936643 0 0 0 1 0.5041357438922236 -0.5083203692315253 0 0 0 2.799010697100416
4 0.2014109109224765 0.05 0 1 0 0 0 1 0 0 0 1 0.2696387555882631 0 -0 0 0 0
1 0 0 0 1 0 0 0 1 0 0 0 1 0 0 0 0 0 0
2 -0.03090169943749461 -0.09510565162951541 0 -0.3090169943749463 0.951056516295154 0 -0.951056516295154 -0.3090169943749463 0 0 0 1 0.5975664329483114 -0.1941611038725459 0 0 0 6.283185307179586
3 0.09811730787260331 -0.07010565162951539 0 0.7996035337379626 -0.600528258147577 0 0.600528258147577 0.7996035337379626 0 0 0 1 1.049311562477019 -0.1941611038725459 0 0 0 1.214108590571676
4 0.2580380146201958 0.05 0 1 0 0 0 1 0 0 0 1 0.9034902590574154 0 -0 0 0 0
1 0 0 0 1 0 0 0 1 0 0 0 1 0 0 0 0 0 0
2 0.03090169943749504 -0.09510565162951527 0 0.3090169943749504 0.9510565162951526 0 -0.9510565162951526 0.3090169943749504 0 0 0 1 0.5975664329483106 0.1941611038725486 0 0 -0 6.283185307179586
3 0.2217241056225827 -0.07010565162951526 0 0.7996035337379632 -0.6005282581475763 0 0.6005282581475763 0.7996035337379632 0 0 0 1 1.340954169316227 0.1941611038725486 0 0 0 -1.214108590571692
4 0.3816448123701754 0.05 0 1 0 0 0 1 0 0 0 1 1.486775472735832 0 -0 0 0 0
1 0 0 0 1 0 0 0 1 0 0 0 1 0 0 0 0 0 0
2 0.08090169943749494 -0.05877852522924707 0 0.8090169943749492 0.5877852522924706 0 -0.5877852522924706 0.8090169943749492 0 0 0 1 0.3693163660980898 0.5083203692315271 0 0 -0 6.283185307179586
3 0.3434105537737229 -0.03377852522924706 0 0.9080357744936652 -0.4188926261462352 0 0.4188926261462352 0.9080357744936652 0 0 0 1 0.9731297205001397 0.5083203692315272 0 0 0 -2.79901069710042
4 0.525017708672456 0.05 0 1 0 0 0 1 0 0 0 1 1.2076267088041 0 -0 0 0 0
1 0 0 0 1 0 0 0 1 0 0 0 1 0 0 0 0 0 0
2 0.1 3.062854959145498e-16 0 1 -3.062856312695708e-15 0 3.062856312695708e-15 1 0 0 0 1 -9.622246890965959e-16 0.6283185307179586 0 0 0 6.283185307179586
3 0.3984313483298444 0.02500000000000031 0 0.9921567416492216 -0.1249999999999985 0 0.1249999999999985 0.9921567416492216 0 0 0 1 0.07916069411491214 0.6283185307179586 0 0 0 -3.166427764596601
4 0.5968626966596887 0.05 0 1 0 0 0 1 0 0 0 1 0.1583213882298262 0 -0 0 0 0
1 0 0 0 1 0 0 0 1 0 0 0 1 0 0 0 0 0 0
2 0.08090169943749456 0.05877852522924756 0 0.8090169943749457 -0.5877852522924756 0 0.5877852522924756 0.8090169943749457 0 0 0 1 -0.3693163660980929 0.5083203692315248 0 0 0 6.283185307179586
3 0.3589302900728597 0.08377852522924756 0 0.9856344559893526 0.1688926261462378 0 -0.1688926261462378 0.9856344559893526 0 0 0 1 -0.8257355739865145 0.5083203692315248 0 0 0 -2.578645491452948
4 0.5560571812707302 0.05 0 1 0 0 0 1 0 0 0 1 -0.9128384157768438 0 -0 0 0 0
1 0 0 0 1 0 0 0 1 0 0 0 1 0 0 0 0 0 0
2 0.03090169943749446 0.09510565162951545 0 0.3090169943749445 -0.9510565162951545 0 0.9510565162951545 0.3090169943749445 0 0 0 1 -0.5975664329483117 0.1941611038725448 0 0 0 6.283185307179586
3 0.2491138300036397 0.1201056516295154 0 0.9365521556432538 0.3505282581475771 0 -0.3505282581475771 0.9365521556432538 0 0 0 1 -1.267802554531005 0.1941611038725448 0 0 0 -1.03657389875521
4 0.4364242611322905 0.05 0 1 0 0 0 1 0 0 0 1 -1.340472243165386 0 -0 0 0 0
1 0 0 0 1 0 0 0 1 0 0 0 1 0 0 0 0 0 0
2 -0.0309016994374952 0.09510565162951522 0 -0.309016994374952 -0.9510565162951521 0 0.9510565162951521 -0.309016994374952 0 0 0 1 -0.5975664329483101 -0.1941611038725495 0 0 0 6.283185307179584
3 0.1255070322536605 0.1201056516295152 0 0.9365521556432542 0.3505282581475759 0 -0.3505282581475759 0.9365521556432542 0 0 0 1 -1.122463177262238 -0.1941611038725495 0 0 -0 1.036573898755234
4 0.3128174633823114 0.05 0 1 0 0 0 1 0 0 0 1 -1.049793488627854 0 -0 0 0 0
1 0 0 0 1 0 0 0 1 0 0 0 1 0 0 0 0 0 0
2 -0.08090169943749503 0.05877852522924693 0 -0.8090169943749502 -0.5877852522924693 0 0.5877852522924693 -0.8090169943749502 0 0 0 1 -0.3693163660980889 -0.5083203692315278 0 0 0 6.283185307179587
3 0.03532349232288063 0.08377852522924692 0 0.9856344559893532 0.1688926261462345 0 -0.1688926261462345 0.9856344559893532 0 0 0 1 -0.6515298904058502 -0.5083203692315277 0 0 -0 2.578645491452963
4 0.2324503835207513 0.05 0 1 0 0 0 1 0 0 0 1 -0.5644270486155225 0 -0 0 0 0
1 0 0 0 1 0 0 0 1 0 0 0 1 0 0 0 0 0 0
2 -0.1 -4.716747118641288e-16 0 -1 4.563356778415361e-15 0 -4.563356778415361e-15 -1 0 0 0 1 1.481810234907784e-15 -0.6283185307179586 0 0 0 6.283185307179586
3 -0.001568651670155744 0.02499999999999953 0 0.9921567416492212 -0.1250000000000023 0 0.1250000000000023 0.9921567416492212 0 0 0 1 -0.07916069411491361 -0.6283185307179586 0 0 0 3.166427764596603
4 0.1968626966596885 0.05 0 1 0 0 0 1 0 0 0 1 -0.1583213882298302 0 -0 0 0 0
1 0 0 0 1 0 0 0 1 0 0 0 1 0 0 0 0 0 0
2 -0.08090169943749438 -0.05877852522924784 0 -0.8090169943749437 0.5877852522924782 0 -0.5877852522924782 -0.8090169943749437 0 0 0 1 0.3693163660980947 -0.5083203692315237 0 0 0 6.283185307179587
3 0.01980375602374394 -0.03377852522924784 0 0.9080357744936634 -0.4188926261462392 0 0.4188926261462392 0.9080357744936634 0 0 0 1 0.5041357438922281 -0.5083203692315236 0 0 0 2.799010697100409
4 0.2014109109224766 0.05 0 1 0 0 0 1 0 0 0 1 0.2696387555882669 0 -0 0 0 0
1 0 0 0 1 0 0 0 1 0 0 0 1 0 0 0 0 0 0
2 -0.03090169943749413 -0.09510565162951556 0 -0.3090169943749415 0.9510565162951555 0 -0.9510565162951555 -0.3090169943749415 0 0 0 1 0.5975664329483125 -0.1941611038725429 0 0 0 6.283185307179586
3 0.09811730787260418 -0.07010565162951554 0 0.7996035337379621 -0.6005282581475776 0 0.6005282581475776 0.7996035337379621 0 0 0 1 1.049311562477023 -0.1941611038725429 0 0 0 1.214108590571658
4 0.2580380146201966 0.05 0 1 0 0 0 1 0 0 0 1 0.9034902590574215 0 -0 0 0 0
1 0 0 0 1 0 0 0 1 0 0 0 1 -0 0 0 0 0 0
2 0.03090169943749536 -0.09510565162951518 0 0.3090169943749536 0.9510565162951515 0 -0.9510565162951515 0.3090169943749536 0 0 0 1 0.59756643294831 0.1941611038725505 0 0 0 6.283185307179587
3 0.2217241056225834 -0.07010565162951517 0 0.7996035337379634 -0.6005282581475758 0 0.6005282581475758 0.7996035337379634 0 0 0 1 1.340954169316227 0.1941611038725505 0 0 0 -1.214108590571703
4 0.3816448123701761 0.05 0 1 0 0 0 1 0 0 0 1 1.486775472735833 0 -0 0 0 0
1 0 0 0 1 0 0 0 1 0 0 0 1 0 0 0 0 0 0
2 0.08090169943749513 -0.0587785252292468 0 0.8090169943749512 0.5877852522924679 0 -0.5877852522924679 0.8090169943749512 0 0 0 1 0.369316366098088 0.5083203692315285 0 0 0 6.283185307179587
3 0.3434105537737235 -0.03377852522924679 0 0.9080357744936658 -0.418892626146234 0 0.418892626146234 0.9080357744936658 0 0 0 1 0.9731297205001359 0.5083203692315282 0 0 0 -2.799010697100428
4 0.5250177086724566 0.05 0 1 0 0 0 1 0 0 0 1 1.207626708804096 0 -0 0 0 0
1 0 0 0 1 0 0 0 1 0 0 0 1 0 0 0 0 0 0
2 0.1 6.370639278112843e-16 0 1 -6.370640631674362e-15 0 6.370640631674362e-15 1 0 0 0 1 -2.001395780712881e-15 0.6283185307179586 0 0 0 6.283185307179586
3 0.3984313483298444 0.02500000000000064 0 0.9921567416492219 -0.1249999999999968 0 0.1249999999999968 0.9921567416492219 0 0 0 1 0.07916069411490906 0.6283185307179583 0 0 0 -3.166427764596602
4 0.5968626966596887 0.05 0 1 0 0 0 1 0 0 0 1 0.1583213882298221 0 -0 0 0 0
1 0 0 0 1 0 0 0 1 0 0 0 1 0 0 0 0 0 0
2 0.08090169943749416 0.05877852522924812 0 0.8090169943749416 -0.5877852522924812 0 0.5877852522924812 0.8090169943749416 0 0 0 1 -0.3693163660980964 0.5083203692315224 0 0 0 6.283185307179586
3 0.3589302900728588 0.08377852522924813 0 0.9856344559893522 0.1688926261462406 0 -0.1688926261462406 0.9856344559893522 0 0 0 1 -0.8257355739865228 0.5083203692315222 0 0 0 -2.578645491452938
4 0.5560571812707292 0.05 0 1 0 0 0 1 0 0 0 1 -0.912838415776853 0 -0 0 0 0
1 0 0 0 1 0 0 0 1 0 0 0 1 0 0 0 0 0 0
2 0.0309016994374938 0.09510565162951566 0 0.309016994374938 -0.9510565162951566 0 0.9510565162951566 0.309016994374938 0 0 0 1 -0.597566432948313 0.1941611038725406 0 0 0 6.283185307179585
3 0.2491138300036383 0.1201056516295157 0 0.9365521556432533 0.3505282581475783 0 -0.3505282581475783 0.9365521556432533 0 0 0 1 -1.267802554531006 0.1941611038725404 0 0 0 -1.036573898755189
4 0.4364242611322889 0.05 0 1 0 0 0 1 0 0 0 1 -1.340472243165386 0 -0 0 0 0
1 0 0 0 1 0 0 0 1 0 0 0 1 -0 0 0 0 0 0
2 -0.03090169943749568 0.09510565162951506 0 -0.3090169943749567 -0.9510565162951505 0 0.9510565162951505 -0.3090169943749567 0 0 0 1 -0.5975664329483094 -0.1941611038725525 0 0 0 6.283185307179584
3 0.1255070322536596 0.120105651629515 0 0.9365521556432546 0.3505282581475752 0 -0.3505282581475752 0.9365521556432546 0 0 0 1 -1.122463177262235 -0.1941611038725524 0 0 -0 1.03657389875525
4 0.3128174633823105 0.05 0 1 0 0 0 1 0 0 0 1 -1.049793488627851 0 -0 0 0 0
1 0 0 0 1 0 0 0 1 0 0 0 1 0 0 0 0 0 0
2 -0.08090169943749534 0.05877852522924652 0 -0.8090169943749531 -0.5877852522924653 0 0.5877852522924653 -0.8090169943749531 0 0 0 1 -0.3693163660980864 -0.5083203692315297 0 0 0 6.283185307179587
3 0.03532349232288004 0.0837785252292465 0 0.9856344559893535 0.1688926261462325 0 -0.1688926261462325 0.9856344559893535 0 0 0 1 -0.6515298904058457 -0.5083203692315296 0 0 -0 2.578645491452971
4 0.2324503835207507 0.05 0 1 0 0 0 1 0 0 0 1 -0.5644270486155188 0 -0 0 0 0
1 0 0 0 1 0 0 0 1 0 0 0 1 0 0 0 0 0 0
2 -0.1 -9.800888276991112e-16 0 -1 9.892427296616113e-15 0 -9.892427296616113e-15 -1 0 0 0 1 6.15808057239872e-15 -0.6283185307179586 0 0 0 6.283185307179586
3 -0.001568651670155779 0.02499999999999902 0 0.9921567416492209 -0.1250000000000049 0 0.1250000000000049 0.9921567416492209 0 0 0 1 -0.07916069411490589 -0.6283185307179585 0 0 0 3.166427764596604
4 0.1968626966596884 0.05 0 1 0 0 0 1 0 0 0 1 -0.1583213882298241 0 -0 0 0 0
1 0 0 0 1 0 0 0 1 0 0 0 1 0 0 0 0 0 0
2 -0.08090169943749417 -0.0587785252292481 0 -0.8090169943749417 0.5877852522924811 0 -0.5877852522924811 -0.8090169943749417 0 0 0 1 0.3693163660980963 -0.5083203692315224 0 0 0 6.283185307179586
3 0.01980375602374421 -0.0337785252292481 0 0.9080357744936628 -0.4188926261462406 0 0.4188926261462406 0.9080357744936628 0 0 0 1 0.5041357438922311 -0.5083203692315224 0 0 0 2.799010697100403
4 0.2014109109224768 0.05 0 1 0 0 0 1 0 0 0 1 0.2696387555882698 0 -0 0 0 0
1 0 0 0 1 0 0 0 1 0 0 0 1 0 0 0 0 0 0
2 -0.03090169943749381 -0.09510565162951566 0 -0.3090169943749381 0.9510565162951566 0 -0.9510565162951566 -0.3090169943749381 0 0 0 1 0.5975664329483132 -0.1941611038725408 0 0 0 6.283185307179587
3 0.09811730787260473 -0.07010565162951567 0 0.7996035337379617 -0.6005282581475783 0 0.6005282581475783 0.7996035337379617 0 0 0 1 1.049311562477026 -0.1941611038725407 0 0 0 1.214108590571646
4 0.258038014620197 0.05 0 1 0 0 0 1 0 0 0 1 0.9034902590574255 0 -0 0 0 0
1 0 0 0 1 0 0 0 1 0 0 0 1 0 0 0 0 0 0
2 0.03090169943749601 -0.09510565162951497 0 0.3090169943749602 0.9510565162951494 0 -0.9510565162951494 0.3090169943749602 0 0 0 1 0.5975664329483086 0.1941611038725546 0 0 -0 6.283185307179585
3 0.2217241056225849 -0.07010565162951496 0 0.7996035337379643 -0.6005282581475748 0 0.6005282581475748 0.7996035337379643 0 0 0 1 1.340954169316226 0.1941611038725545 0 0 0 -1.214108590571729
4 0.3816448123701777 0.05 0 1 0 0 0 1 0 0 0 1 1.486775472735836 0 -0 0 0 0
1 0 0 0 1 0 0 0 1 0 0 0 1 0 0 0 0 0 0
2 0.08090169943749553 -0.05877852522924624 0 0.8090169943749552 0.5877852522924624 0 -0.5877852522924624 0.8090169943749552 0 0 0 1 0.3693163660980846 0.5083203692315308 0 0 -0 6.283185307179586
3 0.3434105537737245 -0.03377852522924623 0 0.9080357744936671 -0.4188926261462312 0 0.4188926261462312 0.9080357744936671 0 0 0 1 0.9731297205001284 0.5083203692315305 0 0 0 -2.799010697100438
4 0.5250177086724578 0.05 0 1 0 0 0 1 0 0 0 1 1.207626708804088 0 -0 0 0 0
1 0 0 0 1 0 0 0 1 0 0 0 1 0 0 0 0 0 0
2 0.1 9.678423597088266e-16 0 1 -9.678424950646554e-15 0 9.678424950646554e-15 1 0 0 0 1 -6.081133744654275e-15 0.6283185307179586 0 0 0 6.283185307179586
3 0.3984313483298445 0.02500000000000097 0 0.9921567416492221 -0.1249999999999952 0 0.1249999999999952 0.9921567416492221 0 0 0 1 0.07916069411489979 0.6283185307179585 0 0 0 -3.1664277645966
4 0.5968626966596889 0.05 0 1 0 0 0 1 0 0 0 1 0.1583213882298118 0 -0 0 0 0
1 0 0 0 1 0 0 0 1 0 0 0 1 0 0 0 0 0 0
2 0.08090169943749417 0.05877852522924809 0 0.8090169943749419 -0.5877852522924808 0 0.5877852522924808 0.8090169943749419 0 0 0 1 -0.3693163660980962 0.5083203692315226 0 0 0 6.283185307179586
3 0.3589302900728588 0.08377852522924809 0 0.9856344559893522 0.1688926261462404 0 -0.1688926261462404 0.9856344559893522 0 0 0 1 -0.8257355739865224 0.5083203692315226 0 0 0 -2.578645491452938
4 0.5560571812707292 0.05 0 1 0 0 0 1 0 0 0 1 -0.9128384157768521 0 -0 0 0 0
1 0 0 0 1 0 0 0 1 0 0 0 1 0 0 0 0 0 0
2 0.03090169943749416 0.09510565162951555 0 0.3090169943749416 -0.9510565162951555 0 0.9510565162951555 0.3090169943749416 0 0 0 1 -0.5975664329483124 0.194161103872543 0 0 0 6.283185307179585
3 0.2491138300036391 0.1201056516295155 0 0.9365521556432537 0.3505282581475776 0 -0.3505282581475776 0.9365521556432537 0 0 0 1 -1.267802554531005 0.194161103872543 0 0 0 -1.0365738987552
4 0.4364242611322898 0.05 0 1 0 0 0 1 0 0 0 1 -1.340472243165386 0 -0 0 0 0
1 0 0 0 1 0 0 0 1 0 0 0 1 0 0 0 0 0 0
2 -0.03090169943749498 0.0951056516295153 0 -0.3090169943749499 -0.9510565162951528 0 0.9510565162951528 -0.3090169943749499 0 0 0 1 -0.5975664329483107 -0.1941611038725481 0 0 0 6.283185307179586
3 0.1255070322536609 0.1201056516295153 0 0.9365521556432541 0.3505282581475764 0 -0.3505282581475764 0.9365521556432541 0 0 0 1 -1.122463177262239 -0.1941611038725481 0 0 -0 1.036573898755228
4 0.3128174633823118 0.05 0 1 0 0 0 1 0 0 0 1 -1.049793488627856 0 -0 0 0 0
1 0 0 0 1 0 0 0 1 0 0 0 1 0 0 0 0 0 0
2 -0.08090169943749491 0.05877852522924712 0 -0.8090169943749489 -0.587785252292471 0 0.587785252292471 -0.8090169943749489 0 0 0 1 -0.3693163660980902 -0.508320369231527 0 0 0 6.283185307179587
3 0.03532349232288079 0.08377852522924711 0 0.9856344559893531 0.1688926261462356 0 -0.1688926261462356 0.9856344559893531 0 0 0 1 -0.6515298904058522 -0.5083203692315268 0 0 -0 2.57864549145296
4 0.2324503835207514 0.05 0 1 0 0 0 1 0 0 0 1 -0.5644270486155242 0 -0 0 0 0
1 0 0 0 1 0 0 0 1 0 0 0 1 0 0 0 0 0 0
2 -0.1 1.102182119222544e-16 0 -1 -1.010643099614861e-15 0 1.010643099614861e-15 -1 0 0 0 1 0 -0.6283185307179586 0 0 0 6.283185307179586
3 -0.001568651670155674 0.02500000000000012 0 0.9921567416492215 -0.1249999999999994 0 0.1249999999999994 0.9921567416492215 0 0 0 1 -0.07916069411491472 -0.6283185307179585 0 0 0 3.166427764596602
4 0.1968626966596886 0.05 0 1 0 0 0 1 0 0 0 1 -0.1583213882298294 0 -0 0 0 0
1 0 0 0 1 0 0 0 1 0 0 0 1 0 0 0 0 0 0
2 -0.08090169943749502 -0.05877852522924694 0 -0.8090169943749502 0.5877852522924693 0 -0.5877852522924693 -0.8090169943749502 0 0 0 1 0.3693163660980889 -0.5083203692315276 0 0 0 6.283185307179585
3 0.01980375602374309 -0.03377852522924694 0 0.9080357744936655 -0.4188926261462347 0 0.4188926261462347 0.9080357744936655 0 0 0 1 0.5041357438922178 -0.5083203692315276 0 0 0 2.799010697100424
4 0.2014109109224762 0.05 0 1 0 0 0 1 0 0 0 1 0.269638755588258 0 -0 0 0 0
1 0 0 0 1 0 0 0 1 0 0 0 1 0 0 0 0 0 0
2 -0.03090169943749519 -0.09510565162951523 0 -0.309016994374952 0.9510565162951521 0 -0.9510565162951521 -0.309016994374952 0 0 0 1 0.5975664329483102 -0.1941611038725494 0 0 0 6.283185307179586
3 0.09811730787260234 -0.07010565162951521 0 0.7996035337379634 -0.6005282581475759 0 0.6005282581475759 0.7996035337379634 0 0 0 1 1.049311562477015 -0.1941611038725494 0 0 0 1.214108590571696
4 0.258038014620195 0.05 0 1 0 0 0 1 0 0 0 1 0.9034902590574087 0 -0 0 0 0
1 0 0 0 1 0 0 0 1 0 0 0 1 0 0 0 0 0 0
2 0.03090169943749396 -0.09510565162951563 0 0.3090169943749397 0.9510565162951561 0 -0.9510565162951561 0.3090169943749397 0 0 0 1 0.5975664329483128 0.1941611038725417 0 0 0 6.283185307179585
3 0.2217241056225803 -0.07010565162951561 0 0.7996035337379618 -0.600528258147578 0 0.600528258147578 0.7996035337379618 0 0 0 1 1.340954169316227 0.1941611038725416 0 0 0 -1.214108590571651
4 0.3816448123701727 0.05 0 1 0 0 0 1 0 0 0 1 1.486775472735827 0 -0 0 0 0
1 0 0 0 1 0 0 0 1 0 0 0 1 0 0 0 0 0 0
2 0.08090169943749406 -0.05877852522924828 0 0.8090169943749406 0.5877852522924826 0 -0.5877852522924826 0.8090169943749406 0 0 0 1 0.3693163660980975 0.5083203692315217 0 0 0 6.283185307179587
3 0.3434105537737207 -0.03377852522924827 0 0.9080357744936625 -0.4188926261462412 0 0.4188926261462412 0.9080357744936625 0 0 0 1 0.9731297205001567 0.5083203692315218 0 0 0 -2.7990106971004
4 0.5250177086724531 0.05 0 1 0 0 0 1 0 0 0 1 1.207626708804118 0 -0 0 0 0
1 0 0 0 1 0 0 0 1 0 0 0 1 -0 0 0 0 0 0
2 0.1 -1.188278783554951e-15 0 1 1.188278648199769e-14 0 -1.188278648199769e-14 1 0 0 0 1 7.497297506262819e-15 0.6283185307179586 0 0 0 6.283185307179586
3 0.3984313483298442 0.02499999999999881 0 0.9921567416492207 -0.1250000000000059 0 0.1250000000000059 0.9921567416492207 0 0 0 1 0.07916069411493384 0.6283185307179585 0 0 0 -3.166427764596604
4 0.5968626966596884 0.05 0 1 0 0 0 1 0 0 0 1 0.1583213882298527 0 -0 0 0 0

176
OndselSolver/crank_sliderX.mbd Normal file
View File

@@ -0,0 +1,176 @@
# crank_slider.mbd
#-----------------------------------------------------------------------------
# [Data Block]
begin: data;
problem: initial value;
end: data;
#-----------------------------------------------------------------------------
# [<Problem> Block]
begin: initial value;
initial time: 0.;
final time: 1.;
time step: 1.e-1;
max iterations: 10;
tolerance: 1.e-6;
end: initial value;
#-----------------------------------------------------------------------------
# [Control Data Block]
begin: control data;
structural nodes: 4;
rigid bodies: 3;
joints: 6;
end: control data;
#-----------------------------------------------------------------------------
# Design Variables
set: real Mass_Crank = 1.;
set: real Mass_Conrod = 1.;
set: real Mass_Slider = 1.;
set: real Length_Crank = 0.2;
set: real Length_Conrod = 0.4;
set: real Offset_Slider = 0.05;
#-----------------------------------------------------------------------------
# Reference Labels
set: integer Ref_Conrod = 1;
# Node Labels
set: integer Node_Ground = 1;
set: integer Node_Crank = 2;
set: integer Node_Conrod = 3;
set: integer Node_Slider = 4;
# Body Labels
set: integer Body_Crank = 1;
set: integer Body_Conrod = 2;
set: integer Body_Slider = 3;
# Joint Labels
set: integer JoClamp_Ground = 1;
set: integer JoAxrot_Ground_Crank = 2;
set: integer JoRevh_Crank_Conrod = 3;
set: integer JoInlin_Conrod_Slider = 4;
set: integer JoInlin_Ground_Slider = 5;
set: integer JoPrism_Ground_Slider = 6;
#-----------------------------------------------------------------------------
# Intermediate Variables
set: real Izz_Crank = Mass_Crank*Length_Crank^2./12.;
set: real Izz_Conrod = Mass_Conrod*Length_Conrod^2./12.;
#-----------------------------------------------------------------------------
# References
reference: Ref_Conrod,
Length_Crank, 0., 0., # absolute position
euler, 0., 0., asin(Offset_Slider/Length_Conrod), # absolute orientation
null, # absolute velocity
null; # absolute angular velocity
#-----------------------------------------------------------------------------
# [Nodes Block]
begin: nodes;
#-----------------------------------------------------------------------------
# Nodes
structural: Node_Ground, static,
0., 0., 0., # absolute position
eye, # absolute orientation
null, # absolute velocity
null; # absolute angular velocity
structural: Node_Crank, dynamic,
Length_Crank/2., 0., 0., # absolute position
eye, # absolute orientation
null, # absolute velocity
null; # absolute angular velocity
structural: Node_Conrod, dynamic,
reference, Ref_Conrod, Length_Conrod/2., 0., 0., # absolute position
reference, Ref_Conrod, eye, # absolute orientation
null, # absolute velocity
null; # absolute angular velocity
structural: Node_Slider, dynamic,
reference, Ref_Conrod, Length_Conrod, 0., 0., # absolute position
eye, # absolute orientation
null, # absolute velocity
null; # absolute angular velocity
end: nodes;
#-----------------------------------------------------------------------------
# [Elements Block]
begin: elements;
#-----------------------------------------------------------------------------
# Bodies
body: Body_Crank, Node_Crank,
Mass_Crank, # mass
null, # relative center of mass
diag, 1., 1., Izz_Crank; # inertia matrix
body: Body_Conrod, Node_Conrod,
Mass_Conrod, # mass
null, # relative center of mass
diag, 1., 1., Izz_Conrod; # inertia matrix
body: Body_Slider, Node_Slider,
Mass_Slider, # mass
null, # relative center of mass
eye; # inertia matrix
#-----------------------------------------------------------------------------
# Joints
joint: JoClamp_Ground,
clamp,
Node_Ground,
null, # absolute position
eye; # absolute orientation
joint: JoAxrot_Ground_Crank,
axial rotation,
Node_Ground,
null, # relative offset
hinge, eye, # relative orientation
Node_Crank,
-Length_Crank/2., 0., 0., # relative offset
hinge, eye, # relative orientation
ramp, 2.*pi, 0., 1., 0.; # angular velocity
joint: JoRevh_Crank_Conrod,
revolute hinge,
Node_Crank,
reference, Ref_Conrod, null, # relative offset
hinge, reference, Ref_Conrod, eye, # relative axis orientation
Node_Conrod,
reference, Ref_Conrod, null, # relative offset
hinge, reference, Ref_Conrod, eye; # relative axis orientation
joint: JoInlin_Conrod_Slider,
in line,
Node_Conrod,
Length_Conrod/2., 0., 0., # relative line position
eye, # relative orientation
Node_Slider;
joint: JoInlin_Ground_Slider,
in line,
Node_Ground,
0., Offset_Slider, 0., # relative line position
1, 0., 0., -1., 3, 1., 0., 0., # relative orientation
Node_Slider;
joint: JoPrism_Ground_Slider,
prismatic,
Node_Ground,
Node_Slider;
end: elements;

44
OndselSolver/crank_sliderX.mov Normal file
View File

@@ -0,0 +1,44 @@
1 0 0 0 1 0 0 0 1 0 0 0 1 5.551115123125783e-17 -1.110223024624046e-16 0 -0 0 0
2 0.1 0 0 1 0 0 0 1 0 0 0 1 -6.993747673745632e-18 0.6283185307179585 0 0 0 6.283185307179586
3 0.3984313483298443 0.025 0 1 0 0 0 1 0 0 0 1 0.07916069411491507 0.6283185307179583 0 0 0 -3.166427764596603
4 0.5968626966596886 0.05 0 1 0 0 0 1 0 0 0 1 0.1583213882298301 -5.551115123120231e-17 0 0 0 0
1 0 0 0 1 0 0 0 1 0 0 0 1 0 0 0 0 0 0
2 0.08090169943749474 0.05877852522924731 0 1 0 0 0 1 0 0 0 1 -0.3693163660980913 0.5083203692315259 0 0 0 6.283185307179585
3 0.3589302900728601 0.08377852522924729 0 1 0 0 0 1 0 0 0 1 -0.8257355739865109 0.5083203692315259 0 0 0 -2.578645491452954
4 0.5560571812707307 0.05 0 1 0 0 0 1 0 0 0 1 -0.9128384157768391 0 -0 0 0 0
1 0 0 0 1 0 0 0 1 0 0 0 1 -0 0 0 0 0 0
2 0.03090169943749474 0.09510565162951536 0 1 0 0 0 1 0 0 0 1 -0.5975664329483111 0.1941611038725468 0 0 0 6.283185307179586
3 0.2491138300036403 0.1201056516295154 0 1 0 0 0 1 0 0 0 1 -1.267802554531004 0.1941611038725468 0 0 0 -1.03657389875522
4 0.436424261132291 0.05 0 1 0 0 0 1 0 0 0 1 -1.340472243165386 0 -0 0 0 0
1 0 0 0 1 0 0 0 1 0 0 0 1 -0 0 0 0 0 0
2 -0.03090169943749476 0.09510565162951534 0 1 0 0 0 1 0 0 0 1 -0.5975664329483108 -0.1941611038725468 0 0 0 6.283185307179586
3 0.1255070322536613 0.1201056516295154 0 1 0 0 0 1 0 0 0 1 -1.12246317726224 -0.1941611038725468 0 0 -0 1.036573898755221
4 0.3128174633823121 0.05 0 1 0 0 0 1 0 0 0 1 -1.049793488627858 0 -0 0 0 0
1 0 0 0 1 0 0 0 1 0 0 0 1 0 0 0 0 0 0
2 -0.08090169943749473 0.05877852522924733 0 1 0 0 0 1 0 0 0 1 -0.3693163660980914 -0.5083203692315259 0 0 0 6.283185307179587
3 0.03532349232288116 0.08377852522924732 0 1 0 0 0 1 0 0 0 1 -0.6515298904058543 -0.5083203692315257 0 0 -0 2.578645491452954
4 0.2324503835207517 0.05 0 1 0 0 0 1 0 0 0 1 -0.564427048615526 0 -0 0 0 0
1 0 0 0 1 0 0 0 1 0 0 0 1 0 0 0 0 0 0
2 -0.1 1.224646799190013e-17 0 1 0 0 0 1 0 0 0 1 0 -0.6283185307179586 0 0 0 6.283185307179586
3 -0.001568651670155672 0.02500000000000002 0 1 0 0 0 1 0 0 0 1 -0.07916069411491503 -0.6283185307179584 0 0 0 3.166427764596603
4 0.1968626966596886 0.05 0 1 0 0 0 1 0 0 0 1 -0.15832138822983 0 -0 0 0 0
1 0 0 0 1 0 0 0 1 0 0 0 1 0 0 0 0 0 0
2 -0.08090169943749474 -0.05877852522924731 0 1 0 0 0 1 0 0 0 1 0.3693163660980912 -0.5083203692315259 0 0 0 6.283185307179584
3 0.01980375602374343 -0.03377852522924731 0 1 0 0 0 1 0 0 0 1 0.5041357438922219 -0.5083203692315259 0 0 0 2.799010697100417
4 0.2014109109224763 0.05 0 1 0 0 0 1 0 0 0 1 0.2696387555882615 0 -0 0 0 0
1 0 0 0 1 0 0 0 1 0 0 0 1 0 0 0 0 0 0
2 -0.03090169943749475 -0.09510565162951536 0 1 0 0 0 1 0 0 0 1 0.5975664329483111 -0.1941611038725468 0 0 0 6.283185307179586
3 0.0981173078726031 -0.07010565162951535 0 1 0 0 0 1 0 0 0 1 1.049311562477018 -0.1941611038725468 0 0 0 1.214108590571681
4 0.2580380146201957 0.05 0 1 0 0 0 1 0 0 0 1 0.9034902590574138 0 -0 0 0 0
1 0 0 0 1 0 0 0 1 0 0 0 1 0 0 0 0 0 0
2 0.03090169943749473 -0.09510565162951537 0 1 0 0 0 1 0 0 0 1 0.5975664329483112 0.1941611038725467 0 0 0 6.283185307179587
3 0.221724105622582 -0.07010565162951535 0 1 0 0 0 1 0 0 0 1 1.340954169316227 0.1941611038725468 0 0 0 -1.21410859057168
4 0.3816448123701746 0.05 0 1 0 0 0 1 0 0 0 1 1.486775472735831 0 -0 0 0 0
1 0 0 0 1 0 0 0 1 0 0 0 1 0 0 0 0 0 0
2 0.08090169943749467 -0.05877852522924741 0 1 0 0 0 1 0 0 0 1 0.3693163660980919 0.5083203692315256 0 0 0 6.283185307179587
3 0.3434105537737223 -0.0337785252292474 0 1 0 0 0 1 0 0 0 1 0.9731297205001445 0.5083203692315256 0 0 0 -2.799010697100416
4 0.5250177086724551 0.05 0 1 0 0 0 1 0 0 0 1 1.207626708804105 0 -0 0 0 0
1 0 0 0 1 0 0 0 1 0 0 0 1 0 0 0 0 0 0
2 0.1 -1.133107779525203e-16 0 1 0 0 0 1 0 0 0 1 0 0.6283185307179586 0 0 0 6.283185307179586
3 0.3984313483298443 0.02499999999999989 0 1 0 0 0 1 0 0 0 1 0.07916069411491541 0.6283185307179587 0 0 0 -3.166427764596602
4 0.5968626966596887 0.05 0 1 0 0 0 1 0 0 0 1 0.1583213882298308 0 -0 0 0 0