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

Backhoe issues (#67)

Browse Source 
* backhoe issues

* runDragStep edit

* backhoe issues

* runDragStep edit

* Reduce large drag step progressively until convergence.
This commit is contained in:
aiksiongkoh
2024-06-24 22:24:38 -06:00
committed by GitHub
parent 79a1e4ed4c
commit 9058e0849d
20 changed files with 2224 additions and 1505 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2443
OndselSolver/ASMTAssembly.cpp
View File

File diff suppressed because it is too large Load Diff

2
OndselSolver/ASMTAssembly.h
View File

@@ -79,6 +79,7 @@ namespace MbD {
void outputFor(AnalysisType type);
void preMbDrun(std::shared_ptr<System> mbdSys);
void preMbDrunDragStep(std::shared_ptr<System> mbdSys, std::shared_ptr<std::vector<std::shared_ptr<Part>>> dragParts);
void postMbDrun();
void calcCharacteristicDimensions();
double calcCharacteristicTime() const;
@@ -97,6 +98,7 @@ namespace MbD {
void runPreDrag();
void runDragStep(std::shared_ptr<std::vector<std::shared_ptr<ASMTPart>>> dragParts);
void runPostDrag();
void restorePosRot();
void runKINEMATIC();
void initprincipalMassMarker();
std::shared_ptr<ASMTSpatialContainer> spatialContainerAt(std::shared_ptr<ASMTAssembly> self, std::string& longname) const;

7
OndselSolver/ASMTPart.cpp
View File

@@ -126,6 +126,13 @@ void MbD::ASMTPart::createMbD(std::shared_ptr<System> mbdSys, std::shared_ptr<Un
if (isFixed) std::static_pointer_cast<Part>(mbdObject)->asFixed();
}
void MbD::ASMTPart::preMbDrunDragStep(std::shared_ptr<System> mbdSys, std::shared_ptr<Units> mbdUnits)
{
auto mbdPart = std::static_pointer_cast<Part>(mbdObject);
mbdPart->qX(rOcmO()->times(1.0 / mbdUnits->length));
mbdPart->qE(qEp());
}
void MbD::ASMTPart::storeOnLevel(std::ofstream& os, size_t level)
{
storeOnLevelString(os, level, "Part");

1
OndselSolver/ASMTPart.h
View File

@@ -26,6 +26,7 @@ namespace MbD {
FColDsptr omeOpO() override;
ASMTPart* part() override;
void createMbD(std::shared_ptr<System> mbdSys, std::shared_ptr<Units> mbdUnits) override;
void preMbDrunDragStep(std::shared_ptr<System> mbdSys, std::shared_ptr<Units> mbdUnits);
void storeOnLevel(std::ofstream& os, size_t level) override;
void storeOnLevelMassMarker(std::ofstream& os, size_t level);
void storeOnTimeSeries(std::ofstream& os) override;

2
OndselSolver/ASMTSpatialContainer.cpp
View File

@@ -376,9 +376,11 @@ void MbD::ASMTSpatialContainer::updateFromMbD()
auto& aAPp = principalMassMarker->rotationMatrix;
auto aAOP = aAOp->timesTransposeFullMatrix(aAPp);
rotationMatrix = aAOP;
oldRotMat = rotationMatrix;
auto rPcmO = aAOP->timesFullColumn(rPcmP);
auto rOPO = rOcmO->minusFullColumn(rPcmO);
position3D = rOPO;
oldPos3D = position3D;
auto vOPO = vOcmO->minusFullColumn(omeOPO->cross(rPcmO));
velocity3D = vOPO;
auto aOPO = aOcmO->minusFullColumn(alpOPO->cross(rPcmO))->minusFullColumn(omeOPO->cross(omeOPO->cross(rPcmO)));

6
OndselSolver/ASMTSpatialItem.cpp
View File

@@ -169,3 +169,9 @@ FMatDsptr MbD::ASMTSpatialItem::getRotationMatrix(size_t i)
bryantAngles->calc();
return bryantAngles->aA;
}
void MbD::ASMTSpatialItem::restorePosRot()
{
position3D = oldPos3D;
rotationMatrix = oldRotMat;
}

5
OndselSolver/ASMTSpatialItem.h
View File

@@ -36,6 +36,7 @@ namespace MbD {
void storeOnLevelRotationMatrixRaw(std::ofstream& os, size_t level);
FColDsptr getPosition3D(size_t i);
FMatDsptr getRotationMatrix(size_t i);
void restorePosRot();
FColDsptr position3D = std::make_shared<FullColumn<double>>(3);
FMatDsptr rotationMatrix = std::make_shared<FullMatrix<double>>(ListListD{
@@ -43,7 +44,9 @@ namespace MbD {
{ 0, 1, 0 },
{ 0, 0, 1 }
});
FRowDsptr xs, ys, zs, bryxs, bryys, bryzs;
FColDsptr oldPos3D = position3D;
FMatDsptr oldRotMat = rotationMatrix;
FRowDsptr xs, ys, zs, bryxs, bryys, bryzs;
FRowDsptr inxs, inys, inzs, inbryxs, inbryys, inbryzs;
};

718
OndselSolver/EulerParameters.h
View File

@@ -5,347 +5,405 @@
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#pragma once
#include "EulerArray.h"
#include "FullColumn.h"
#include "FullMatrix.h"
namespace MbD {
namespace MbD
{
template<typename T>
class EulerParameters : public EulerArray<T>
{
//Quarternion = {q0, q1, q2, q3}
//EulerParameters = {qE1, qE2, qE3, qE4} is preferred because Smalltalk uses one-based indexing.
// q0 = qE4
//Note: It is tempting to use quarternions in C++ because of zero-based indexing.
//Note: But that will make it harder to compare computation results with Smalltalk
//aA aB aC pApE
public:
EulerParameters() : EulerArray<T>(4) {}
EulerParameters(size_t count) : EulerArray<T>(count) {}
EulerParameters(size_t 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->initialize();
this->calc();
}
template<typename T>
class EulerParameters: public EulerArray<T>
{
// Quarternion = {q0, q1, q2, q3}
// EulerParameters = {qE1, qE2, qE3, qE4} is preferred because Smalltalk uses one-based
// indexing.
// q0 = qE4
// Note: It is tempting to use quarternions in C++ because of zero-based indexing.
// Note: But that will make it harder to compare computation results with Smalltalk
// aA aB aC pApE
public:
EulerParameters()
: EulerArray<T>(4)
{
this->initialize();
}
EulerParameters(size_t count)
: EulerArray<T>(count)
{
this->initialize();
}
EulerParameters(size_t count, const T& value)
: EulerArray<T>(count, value)
{
this->initialize();
}
EulerParameters(std::initializer_list<T> list)
: EulerArray<T> {list}
{
this->initialize();
}
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->initialize();
this->calc();
}
static std::shared_ptr<FullMatrix<FColsptr<T>>> ppApEpEtimesColumn(FColDsptr col);
static FMatDsptr pCpEtimesColumn(FColDsptr col);
static FMatDsptr pCTpEtimesColumn(FColDsptr col);
static std::shared_ptr<FullMatrix<FMatsptr<T>>> ppApEpEtimesMatrix(FMatDsptr mat);
static std::shared_ptr<FullMatrix<FColsptr<T>>> ppApEpEtimesColumn(FColDsptr col);
static FMatDsptr pCpEtimesColumn(FColDsptr col);
static FMatDsptr pCTpEtimesColumn(FColDsptr col);
static std::shared_ptr<FullMatrix<FMatsptr<T>>> ppApEpEtimesMatrix(FMatDsptr mat);
void initialize() override;
void calc() override;
void calcABC();
void calcpApE();
void conditionSelf() override;
void initialize() override;
void calc() override;
void calcABC();
void calcpApE();
void conditionSelf() override;
std::shared_ptr<EulerParameters<T>> times(T a);
std::shared_ptr<EulerParameters<T>>
plusFullColumn(std::shared_ptr<EulerParameters<T>> eulerPars);
std::shared_ptr<EulerParameters<T>> copy();
FMatDsptr aA;
FMatDsptr aB;
FMatDsptr aC;
FColFMatDsptr pApE;
};
FMatDsptr aA;
FMatDsptr aB;
FMatDsptr aC;
FColFMatDsptr pApE;
};
template<>
inline FMatFColDsptr EulerParameters<double>::ppApEpEtimesColumn(FColDsptr col)
{
double a2c0 = 2 * col->at(0);
double a2c1 = 2 * col->at(1);
double a2c2 = 2 * col->at(2);
double m2c0 = 0 - a2c0;
double m2c1 = 0 - a2c1;
double m2c2 = 0 - a2c2;
auto col00 = std::make_shared<FullColumn<double>>(ListD{ a2c0, m2c1, m2c2 });
auto col01 = std::make_shared<FullColumn<double>>(ListD{ a2c1, a2c0, 0 });
auto col02 = std::make_shared<FullColumn<double>>(ListD{ a2c2, 0, a2c0 });
auto col03 = std::make_shared<FullColumn<double>>(ListD{ 0, m2c2, a2c1 });
auto col11 = std::make_shared<FullColumn<double>>(ListD{ m2c0, a2c1, m2c2 });
auto col12 = std::make_shared<FullColumn<double>>(ListD{ 0, a2c2, a2c1 });
auto col13 = std::make_shared<FullColumn<double>>(ListD{ a2c2, 0, m2c0 });
auto col22 = std::make_shared<FullColumn<double>>(ListD{ m2c0, m2c1, a2c2 });
auto col23 = std::make_shared<FullColumn<double>>(ListD{ m2c1, a2c0, 0 });
auto col33 = std::make_shared<FullColumn<double>>(ListD{ a2c0, a2c1, a2c2 });
auto answer = std::make_shared<FullMatrix<FColDsptr>>(4, 4);
auto& row0 = answer->at(0);
row0->at(0) = col00;
row0->at(1) = col01;
row0->at(2) = col02;
row0->at(3) = col03;
auto& row1 = answer->at(1);
row1->at(0) = col01;
row1->at(1) = col11;
row1->at(2) = col12;
row1->at(3) = col13;
auto& row2 = answer->at(2);
row2->at(0) = col02;
row2->at(1) = col12;
row2->at(2) = col22;
row2->at(3) = col23;
auto& row3 = answer->at(3);
row3->at(0) = col03;
row3->at(1) = col13;
row3->at(2) = col23;
row3->at(3) = col33;
return answer;
}
template<>
inline FMatDsptr EulerParameters<double>::pCpEtimesColumn(FColDsptr col)
{
//"col size = 4."
auto c0 = col->at(0);
auto c1 = col->at(1);
auto c2 = col->at(2);
auto mc0 = -c0;
auto mc1 = -c1;
auto mc2 = -c2;
auto mc3 = -col->at(3);
auto answer = std::make_shared<FullMatrix<double>>(3, 4);
auto& row0 = answer->at(0);
auto& row1 = answer->at(1);
auto& row2 = answer->at(2);
row0->atiput(0, mc3);
row0->atiput(1, mc2);
row0->atiput(2, c1);
row0->atiput(3, c0);
row1->atiput(0, c2);
row1->atiput(1, mc3);
row1->atiput(2, mc0);
row1->atiput(3, c1);
row2->atiput(0, mc1);
row2->atiput(1, c0);
row2->atiput(2, mc3);
row2->atiput(3, c2);
return answer;
}
template<typename T>
inline FMatDsptr EulerParameters<T>::pCTpEtimesColumn(FColDsptr col)
{
//"col size = 3."
auto c0 = col->at(0);
auto c1 = col->at(1);
auto c2 = col->at(2);
auto mc0 = -c0;
auto mc1 = -c1;
auto mc2 = -c2;
auto answer = std::make_shared<FullMatrix<double>>(4, 4);
auto& row0 = answer->at(0);
auto& row1 = answer->at(1);
auto& row2 = answer->at(2);
auto& row3 = answer->at(3);
row0->atiput(0, 0.0);
row0->atiput(1, c2);
row0->atiput(2, mc1);
row0->atiput(3, c0);
row1->atiput(0, mc2);
row1->atiput(1, 0.0);
row1->atiput(2, c0);
row1->atiput(3, c1);
row2->atiput(0, c1);
row2->atiput(1, mc0);
row2->atiput(2, 0.0);
row2->atiput(3, c2);
row3->atiput(0, mc0);
row3->atiput(1, mc1);
row3->atiput(2, mc2);
row3->atiput(3, 0.0);
return answer;
}
template<>
inline FMatFMatDsptr EulerParameters<double>::ppApEpEtimesMatrix(FMatDsptr mat)
{
FRowDsptr a2m0 = mat->at(0)->times(2.0);
FRowDsptr a2m1 = mat->at(1)->times(2.0);
FRowDsptr a2m2 = mat->at(2)->times(2.0);
FRowDsptr m2m0 = a2m0->negated();
FRowDsptr m2m1 = a2m1->negated();
FRowDsptr m2m2 = a2m2->negated();
FRowDsptr zero = std::make_shared<FullRow<double>>(3, 0.0);
auto mat00 = std::make_shared<FullMatrix<double>>(ListFRD{ a2m0, m2m1, m2m2 });
auto mat01 = std::make_shared<FullMatrix<double>>(ListFRD{ a2m1, a2m0, zero });
auto mat02 = std::make_shared<FullMatrix<double>>(ListFRD{ a2m2, zero, a2m0 });
auto mat03 = std::make_shared<FullMatrix<double>>(ListFRD{ zero, m2m2, a2m1 });
auto mat11 = std::make_shared<FullMatrix<double>>(ListFRD{ m2m0, a2m1, m2m2 });
auto mat12 = std::make_shared<FullMatrix<double>>(ListFRD{ zero, a2m2, a2m1 });
auto mat13 = std::make_shared<FullMatrix<double>>(ListFRD{ a2m2, zero, m2m0 });
auto mat22 = std::make_shared<FullMatrix<double>>(ListFRD{ m2m0, m2m1, a2m2 });
auto mat23 = std::make_shared<FullMatrix<double>>(ListFRD{ m2m1, a2m0, zero });
auto mat33 = std::make_shared<FullMatrix<double>>(ListFRD{ a2m0, a2m1, a2m2 });
auto answer = std::make_shared<FullMatrix<FMatDsptr>>(4, 4);
auto& row0 = answer->at(0);
row0->at(0) = mat00;
row0->at(1) = mat01;
row0->at(2) = mat02;
row0->at(3) = mat03;
auto& row1 = answer->at(1);
row1->at(0) = mat01;
row1->at(1) = mat11;
row1->at(2) = mat12;
row1->at(3) = mat13;
auto& row2 = answer->at(2);
row2->at(0) = mat02;
row2->at(1) = mat12;
row2->at(2) = mat22;
row2->at(3) = mat23;
auto& row3 = answer->at(3);
row3->at(0) = mat03;
row3->at(1) = mat13;
row3->at(2) = mat23;
row3->at(3) = mat33;
return answer;
}
template<>
inline void EulerParameters<double>::initialize()
{
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);
for (size_t i = 0; i < 4; i++)
{
pApE->at(i) = std::make_shared<FullMatrix<double>>(3, 3);
}
}
template<typename T>
inline void EulerParameters<T>::calc()
{
this->calcABC();
this->calcpApE();
}
template<>
inline void EulerParameters<double>::calcABC()
{
double aE0 = this->at(0);
double aE1 = this->at(1);
double aE2 = this->at(2);
double aE3 = this->at(3);
double mE0 = -aE0;
double mE1 = -aE1;
double mE2 = -aE2;
FRowDsptr aBi;
aBi = aB->at(0);
aBi->at(0) = aE3;
aBi->at(1) = mE2;
aBi->at(2) = aE1;
aBi->at(3) = mE0;
aBi = aB->at(1);
aBi->at(0) = aE2;
aBi->at(1) = aE3;
aBi->at(2) = mE0;
aBi->at(3) = mE1;
aBi = aB->at(2);
aBi->at(0) = mE1;
aBi->at(1) = aE0;
aBi->at(2) = aE3;
aBi->at(3) = mE2;
FRowDsptr aCi;
aCi = aC->at(0);
aCi->at(0) = aE3;
aCi->at(1) = aE2;
aCi->at(2) = mE1;
aCi->at(3) = mE0;
aCi = aC->at(1);
aCi->at(0) = mE2;
aCi->at(1) = aE3;
aCi->at(2) = aE0;
aCi->at(3) = mE1;
aCi = aC->at(2);
aCi->at(0) = aE1;
aCi->at(1) = mE0;
aCi->at(2) = aE3;
aCi->at(3) = mE2;
aA = aB->timesTransposeFullMatrix(aC);
}
template<>
inline void EulerParameters<double>::calcpApE()
{
double a2E0 = 2.0 * (this->at(0));
double a2E1 = 2.0 * (this->at(1));
double a2E2 = 2.0 * (this->at(2));
double a2E3 = 2.0 * (this->at(3));
double m2E0 = -a2E0;
double m2E1 = -a2E1;
double m2E2 = -a2E2;
double m2E3 = -a2E3;
FMatDsptr pApEk;
pApEk = pApE->at(0);
FRowDsptr pAipEk;
pAipEk = pApEk->at(0);
pAipEk->at(0) = a2E0;
pAipEk->at(1) = a2E1;
pAipEk->at(2) = a2E2;
pAipEk = pApEk->at(1);
pAipEk->at(0) = a2E1;
pAipEk->at(1) = m2E0;
pAipEk->at(2) = m2E3;
pAipEk = pApEk->at(2);
pAipEk->at(0) = a2E2;
pAipEk->at(1) = a2E3;
pAipEk->at(2) = m2E0;
//
pApEk = pApE->at(1);
pAipEk = pApEk->at(0);
pAipEk->at(0) = m2E1;
pAipEk->at(1) = a2E0;
pAipEk->at(2) = a2E3;
pAipEk = pApEk->at(1);
pAipEk->at(0) = a2E0;
pAipEk->at(1) = a2E1;
pAipEk->at(2) = a2E2;
pAipEk = pApEk->at(2);
pAipEk->at(0) = m2E3;
pAipEk->at(1) = a2E2;
pAipEk->at(2) = m2E1;
//
pApEk = pApE->at(2);
pAipEk = pApEk->at(0);
pAipEk->at(0) = m2E2;
pAipEk->at(1) = m2E3;
pAipEk->at(2) = a2E0;
pAipEk = pApEk->at(1);
pAipEk->at(0) = a2E3;
pAipEk->at(1) = m2E2;
pAipEk->at(2) = a2E1;
pAipEk = pApEk->at(2);
pAipEk->at(0) = a2E0;
pAipEk->at(1) = a2E1;
pAipEk->at(2) = a2E2;
//
pApEk = pApE->at(3);
pAipEk = pApEk->at(0);
pAipEk->at(0) = a2E3;
pAipEk->at(1) = m2E2;
pAipEk->at(2) = a2E1;
pAipEk = pApEk->at(1);
pAipEk->at(0) = a2E2;
pAipEk->at(1) = a2E3;
pAipEk->at(2) = m2E0;
pAipEk = pApEk->at(2);
pAipEk->at(0) = m2E1;
pAipEk->at(1) = a2E0;
pAipEk->at(2) = a2E3;
}
template<typename T>
inline void EulerParameters<T>::conditionSelf()
{
EulerArray<T>::conditionSelf();
this->normalizeSelf();
}
template<>
inline FMatFColDsptr EulerParameters<double>::ppApEpEtimesColumn(FColDsptr col)
{
double a2c0 = 2 * col->at(0);
double a2c1 = 2 * col->at(1);
double a2c2 = 2 * col->at(2);
double m2c0 = 0 - a2c0;
double m2c1 = 0 - a2c1;
double m2c2 = 0 - a2c2;
auto col00 = std::make_shared<FullColumn<double>>(ListD {a2c0, m2c1, m2c2});
auto col01 = std::make_shared<FullColumn<double>>(ListD {a2c1, a2c0, 0});
auto col02 = std::make_shared<FullColumn<double>>(ListD {a2c2, 0, a2c0});
auto col03 = std::make_shared<FullColumn<double>>(ListD {0, m2c2, a2c1});
auto col11 = std::make_shared<FullColumn<double>>(ListD {m2c0, a2c1, m2c2});
auto col12 = std::make_shared<FullColumn<double>>(ListD {0, a2c2, a2c1});
auto col13 = std::make_shared<FullColumn<double>>(ListD {a2c2, 0, m2c0});
auto col22 = std::make_shared<FullColumn<double>>(ListD {m2c0, m2c1, a2c2});
auto col23 = std::make_shared<FullColumn<double>>(ListD {m2c1, a2c0, 0});
auto col33 = std::make_shared<FullColumn<double>>(ListD {a2c0, a2c1, a2c2});
auto answer = std::make_shared<FullMatrix<FColDsptr>>(4, 4);
auto& row0 = answer->at(0);
row0->at(0) = col00;
row0->at(1) = col01;
row0->at(2) = col02;
row0->at(3) = col03;
auto& row1 = answer->at(1);
row1->at(0) = col01;
row1->at(1) = col11;
row1->at(2) = col12;
row1->at(3) = col13;
auto& row2 = answer->at(2);
row2->at(0) = col02;
row2->at(1) = col12;
row2->at(2) = col22;
row2->at(3) = col23;
auto& row3 = answer->at(3);
row3->at(0) = col03;
row3->at(1) = col13;
row3->at(2) = col23;
row3->at(3) = col33;
return answer;
}
template<>
inline FMatDsptr EulerParameters<double>::pCpEtimesColumn(FColDsptr col)
{
//"col size = 4."
auto c0 = col->at(0);
auto c1 = col->at(1);
auto c2 = col->at(2);
auto mc0 = -c0;
auto mc1 = -c1;
auto mc2 = -c2;
auto mc3 = -col->at(3);
auto answer = std::make_shared<FullMatrix<double>>(3, 4);
auto& row0 = answer->at(0);
auto& row1 = answer->at(1);
auto& row2 = answer->at(2);
row0->atiput(0, mc3);
row0->atiput(1, mc2);
row0->atiput(2, c1);
row0->atiput(3, c0);
row1->atiput(0, c2);
row1->atiput(1, mc3);
row1->atiput(2, mc0);
row1->atiput(3, c1);
row2->atiput(0, mc1);
row2->atiput(1, c0);
row2->atiput(2, mc3);
row2->atiput(3, c2);
return answer;
}
template<typename T>
inline FMatDsptr EulerParameters<T>::pCTpEtimesColumn(FColDsptr col)
{
//"col size = 3."
auto c0 = col->at(0);
auto c1 = col->at(1);
auto c2 = col->at(2);
auto mc0 = -c0;
auto mc1 = -c1;
auto mc2 = -c2;
auto answer = std::make_shared<FullMatrix<double>>(4, 4);
auto& row0 = answer->at(0);
auto& row1 = answer->at(1);
auto& row2 = answer->at(2);
auto& row3 = answer->at(3);
row0->atiput(0, 0.0);
row0->atiput(1, c2);
row0->atiput(2, mc1);
row0->atiput(3, c0);
row1->atiput(0, mc2);
row1->atiput(1, 0.0);
row1->atiput(2, c0);
row1->atiput(3, c1);
row2->atiput(0, c1);
row2->atiput(1, mc0);
row2->atiput(2, 0.0);
row2->atiput(3, c2);
row3->atiput(0, mc0);
row3->atiput(1, mc1);
row3->atiput(2, mc2);
row3->atiput(3, 0.0);
return answer;
}
template<>
inline FMatFMatDsptr EulerParameters<double>::ppApEpEtimesMatrix(FMatDsptr mat)
{
FRowDsptr a2m0 = mat->at(0)->times(2.0);
FRowDsptr a2m1 = mat->at(1)->times(2.0);
FRowDsptr a2m2 = mat->at(2)->times(2.0);
FRowDsptr m2m0 = a2m0->negated();
FRowDsptr m2m1 = a2m1->negated();
FRowDsptr m2m2 = a2m2->negated();
FRowDsptr zero = std::make_shared<FullRow<double>>(3, 0.0);
auto mat00 = std::make_shared<FullMatrix<double>>(ListFRD {a2m0, m2m1, m2m2});
auto mat01 = std::make_shared<FullMatrix<double>>(ListFRD {a2m1, a2m0, zero});
auto mat02 = std::make_shared<FullMatrix<double>>(ListFRD {a2m2, zero, a2m0});
auto mat03 = std::make_shared<FullMatrix<double>>(ListFRD {zero, m2m2, a2m1});
auto mat11 = std::make_shared<FullMatrix<double>>(ListFRD {m2m0, a2m1, m2m2});
auto mat12 = std::make_shared<FullMatrix<double>>(ListFRD {zero, a2m2, a2m1});
auto mat13 = std::make_shared<FullMatrix<double>>(ListFRD {a2m2, zero, m2m0});
auto mat22 = std::make_shared<FullMatrix<double>>(ListFRD {m2m0, m2m1, a2m2});
auto mat23 = std::make_shared<FullMatrix<double>>(ListFRD {m2m1, a2m0, zero});
auto mat33 = std::make_shared<FullMatrix<double>>(ListFRD {a2m0, a2m1, a2m2});
auto answer = std::make_shared<FullMatrix<FMatDsptr>>(4, 4);
auto& row0 = answer->at(0);
row0->at(0) = mat00;
row0->at(1) = mat01;
row0->at(2) = mat02;
row0->at(3) = mat03;
auto& row1 = answer->at(1);
row1->at(0) = mat01;
row1->at(1) = mat11;
row1->at(2) = mat12;
row1->at(3) = mat13;
auto& row2 = answer->at(2);
row2->at(0) = mat02;
row2->at(1) = mat12;
row2->at(2) = mat22;
row2->at(3) = mat23;
auto& row3 = answer->at(3);
row3->at(0) = mat03;
row3->at(1) = mat13;
row3->at(2) = mat23;
row3->at(3) = mat33;
return answer;
}
template<>
inline void EulerParameters<double>::initialize()
{
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);
for (size_t i = 0; i < 4; i++) {
pApE->at(i) = std::make_shared<FullMatrix<double>>(3, 3);
}
}
template<typename T>
inline void EulerParameters<T>::calc()
{
this->calcABC();
this->calcpApE();
}
template<>
inline void EulerParameters<double>::calcABC()
{
double aE0 = this->at(0);
double aE1 = this->at(1);
double aE2 = this->at(2);
double aE3 = this->at(3);
double mE0 = -aE0;
double mE1 = -aE1;
double mE2 = -aE2;
FRowDsptr aBi;
aBi = aB->at(0);
aBi->at(0) = aE3;
aBi->at(1) = mE2;
aBi->at(2) = aE1;
aBi->at(3) = mE0;
aBi = aB->at(1);
aBi->at(0) = aE2;
aBi->at(1) = aE3;
aBi->at(2) = mE0;
aBi->at(3) = mE1;
aBi = aB->at(2);
aBi->at(0) = mE1;
aBi->at(1) = aE0;
aBi->at(2) = aE3;
aBi->at(3) = mE2;
FRowDsptr aCi;
aCi = aC->at(0);
aCi->at(0) = aE3;
aCi->at(1) = aE2;
aCi->at(2) = mE1;
aCi->at(3) = mE0;
aCi = aC->at(1);
aCi->at(0) = mE2;
aCi->at(1) = aE3;
aCi->at(2) = aE0;
aCi->at(3) = mE1;
aCi = aC->at(2);
aCi->at(0) = aE1;
aCi->at(1) = mE0;
aCi->at(2) = aE3;
aCi->at(3) = mE2;
aA = aB->timesTransposeFullMatrix(aC);
}
template<>
inline void EulerParameters<double>::calcpApE()
{
double a2E0 = 2.0 * (this->at(0));
double a2E1 = 2.0 * (this->at(1));
double a2E2 = 2.0 * (this->at(2));
double a2E3 = 2.0 * (this->at(3));
double m2E0 = -a2E0;
double m2E1 = -a2E1;
double m2E2 = -a2E2;
double m2E3 = -a2E3;
FMatDsptr pApEk;
pApEk = pApE->at(0);
FRowDsptr pAipEk;
pAipEk = pApEk->at(0);
pAipEk->at(0) = a2E0;
pAipEk->at(1) = a2E1;
pAipEk->at(2) = a2E2;
pAipEk = pApEk->at(1);
pAipEk->at(0) = a2E1;
pAipEk->at(1) = m2E0;
pAipEk->at(2) = m2E3;
pAipEk = pApEk->at(2);
pAipEk->at(0) = a2E2;
pAipEk->at(1) = a2E3;
pAipEk->at(2) = m2E0;
//
pApEk = pApE->at(1);
pAipEk = pApEk->at(0);
pAipEk->at(0) = m2E1;
pAipEk->at(1) = a2E0;
pAipEk->at(2) = a2E3;
pAipEk = pApEk->at(1);
pAipEk->at(0) = a2E0;
pAipEk->at(1) = a2E1;
pAipEk->at(2) = a2E2;
pAipEk = pApEk->at(2);
pAipEk->at(0) = m2E3;
pAipEk->at(1) = a2E2;
pAipEk->at(2) = m2E1;
//
pApEk = pApE->at(2);
pAipEk = pApEk->at(0);
pAipEk->at(0) = m2E2;
pAipEk->at(1) = m2E3;
pAipEk->at(2) = a2E0;
pAipEk = pApEk->at(1);
pAipEk->at(0) = a2E3;
pAipEk->at(1) = m2E2;
pAipEk->at(2) = a2E1;
pAipEk = pApEk->at(2);
pAipEk->at(0) = a2E0;
pAipEk->at(1) = a2E1;
pAipEk->at(2) = a2E2;
//
pApEk = pApE->at(3);
pAipEk = pApEk->at(0);
pAipEk->at(0) = a2E3;
pAipEk->at(1) = m2E2;
pAipEk->at(2) = a2E1;
pAipEk = pApEk->at(1);
pAipEk->at(0) = a2E2;
pAipEk->at(1) = a2E3;
pAipEk->at(2) = m2E0;
pAipEk = pApEk->at(2);
pAipEk->at(0) = m2E1;
pAipEk->at(1) = a2E0;
pAipEk->at(2) = a2E3;
}
template<typename T>
inline void EulerParameters<T>::conditionSelf()
{
EulerArray<T>::conditionSelf();
this->normalizeSelf();
}
template<>
inline std::shared_ptr<EulerParameters<double>> EulerParameters<double>::times(double a)
{
auto n = this->size();
auto answer = std::make_shared<EulerParameters<double>>(n);
for (size_t i = 0; i < n; i++) {
answer->at(i) = this->at(i) * a;
}
return answer;
}
template<typename T>
inline std::shared_ptr<EulerParameters<T>> EulerParameters<T>::times(T)
{
assert(false);
}
template<typename T>
inline std::shared_ptr<EulerParameters<T>>
EulerParameters<T>::plusFullColumn(std::shared_ptr<EulerParameters<T>> eulerPars)
{
auto n = this->size();
auto answer = std::make_shared<EulerParameters<T>>(n);
for (size_t i = 0; i < n; i++) {
answer->at(i) = this->at(i) + eulerPars->at(i);
}
return answer;
}
template<typename T>
inline std::shared_ptr<EulerParameters<T>> EulerParameters<T>::copy()
{
auto n = this->size();
auto answer = std::make_shared<EulerParameters<double>>(n);
for (size_t i = 0; i < n; i++) {
answer->at(i) = this->at(i);
}
return answer;
}
} // namespace MbD

5
OndselSolver/ExternalSystem.cpp
View File

@@ -26,6 +26,11 @@ void MbD::ExternalSystem::preMbDrun(std::shared_ptr<System> mbdSys)
}
}
void MbD::ExternalSystem::preMbDrunDragStep(std::shared_ptr<System> mbdSys, std::shared_ptr<std::vector<std::shared_ptr<Part>>> dragParts)
{
asmtAssembly->preMbDrunDragStep(mbdSys, dragParts);
}
void MbD::ExternalSystem::updateFromMbD()
{
if (cadSystem) {

2
OndselSolver/ExternalSystem.h
View File

@@ -14,6 +14,7 @@
//#include "CADSystem.h"
//#include "ASMTAssembly.h"
#include "Part.h"
namespace MbD {
class CADSystem;
@@ -25,6 +26,7 @@ namespace MbD {
//
public:
void preMbDrun(std::shared_ptr<System> mbdSys);
void preMbDrunDragStep(std::shared_ptr<System> mbdSys, std::shared_ptr<std::vector<std::shared_ptr<Part>>> dragParts);
void updateFromMbD();
void outputFor(AnalysisType type);
void logString(std::string& str);

36
OndselSolver/OndselSolver.vcxproj
View File

@@ -778,27 +778,59 @@
</ItemGroup>
<ItemGroup>
<None Include="..\testapp\00backhoe.asmt" />
<None Include="..\testapp\00camrocker.asmt" />
<None Include="..\testapp\anglejoint.asmt" />
<None Include="..\testapp\camfollower.asmt" />
<None Include="..\testapp\circular.asmt" />
<None Include="..\testapp\circularPendulum.asmt" />
<None Include="..\testapp\cirpendu.asmt" />
<None Include="..\testapp\cirpendu2.asmt" />
<None Include="..\testapp\coastharbor2.asmt" />
<None Include="..\testapp\constvel.asmt" />
<None Include="..\testapp\CrankSlider2.mbd" />
<None Include="..\testapp\crank_slider.asmt" />
<None Include="..\testapp\crank_slider.mbd" />
<None Include="..\testapp\crank_sliderX.mbd" />
<None Include="..\testapp\CylSphJt.asmt" />
<None Include="..\testapp\dragCrankSlider.asmt" />
<None Include="..\testapp\dragCrankSlider.asmt.bak" />
<None Include="..\testapp\dxfmotion.asmt" />
<None Include="..\testapp\dxftree.asmt" />
<None Include="..\testapp\engine1.asmt" />
<None Include="..\testapp\fixplnplnpln.asmt" />
<None Include="..\testapp\fixplnplnplnNoOffset.asmt" />
<None Include="..\testapp\fourbar.asmt" />
<None Include="..\testapp\fourbot.asmt" />
<None Include="..\testapp\Gears.asmt" />
<None Include="..\testapp\gyro.asmt" />
<None Include="..\testapp\InitialConditions.mbd" />
<None Include="..\testapp\JB_2bars.asmt" />
<None Include="..\testapp\limits.asmt" />
<None Include="..\testapp\mateproblembobcat.asmt" />
<None Include="..\testapp\parm.asmt" />
<None Include="..\testapp\part32.asmt" />
<None Include="..\testapp\piston.asmt" />
<None Include="..\testapp\pistonAllowZRotation.asmt" />
<None Include="..\testapp\pistonWithLimits.asmt" />
<None Include="..\testapp\planarbug.asmt" />
<None Include="..\testapp\quasikine.asmt" />
<None Include="..\testapp\rackPinion.asmt" />
<None Include="..\testapp\rackPinion3.asmt" />
<None Include="..\testapp\rackscrew.asmt" />
<None Include="..\testapp\RevCylJt.asmt" />
<None Include="..\testapp\RevRevJt.asmt" />
<None Include="..\testapp\robot.asmt" />
<None Include="..\testapp\runPreDrag.asmt" />
<None Include="..\testapp\runPreDragBackhoe.asmt" />
<None Include="..\testapp\runPreDragBackhoe2.asmt" />
<None Include="..\testapp\runPreDragBackhoe3.asmt" />
<None Include="..\testapp\Schmidt_Coupling_Ass_1-1.asmt" />
<None Include="..\testapp\Schmidt_Coupling_Ass_1-2.asmt" />
<None Include="..\testapp\Schmidt_Coupling_Ass_1a.asmt" />
<None Include="..\testapp\SphericalHinge.mbd" />
<None Include="..\testapp\SphSphJt.asmt" />
<None Include="..\testapp\wobpump.asmt" />
<None Include="..\testapp\__cubes.asmt" />
</ItemGroup>
<ItemGroup>
<Media Include="..\testapp\CrankSlider2.mov" />
@@ -808,6 +840,10 @@
</ItemGroup>
<ItemGroup>
<Text Include="..\CMakeLists.txt" />
<Text Include="..\testapp\dragging.log" />
<Text Include="..\testapp\draggingBackhoe.log" />
<Text Include="..\testapp\draggingBackhoe2.log" />
<Text Include="..\testapp\draggingBackhoe3.log" />
</ItemGroup>
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.targets" />
<ImportGroup Label="ExtensionTargets">

36
OndselSolver/OndselSolver.vcxproj.filters
View File

@@ -1954,6 +1954,38 @@
<None Include="..\testapp\constvel.asmt" />
<None Include="..\testapp\anglejoint.asmt" />
<None Include="..\testapp\Gears.asmt" />
<None Include="..\testapp\runPreDragBackhoe.asmt" />
<None Include="..\testapp\runPreDragBackhoe2.asmt" />
<None Include="..\testapp\runPreDragBackhoe3.asmt" />
<None Include="..\testapp\__cubes.asmt" />
<None Include="..\testapp\runPreDrag.asmt" />
<None Include="..\testapp\limits.asmt" />
<None Include="..\testapp\rackPinion.asmt" />
<None Include="..\testapp\rackPinion3.asmt" />
<None Include="..\testapp\dragCrankSlider.asmt" />
<None Include="..\testapp\pistonAllowZRotation.asmt" />
<None Include="..\testapp\pistonWithLimits.asmt" />
<None Include="..\testapp\dragCrankSlider.asmt.bak" />
<None Include="..\testapp\Schmidt_Coupling_Ass_1-1.asmt" />
<None Include="..\testapp\Schmidt_Coupling_Ass_1-2.asmt" />
<None Include="..\testapp\Schmidt_Coupling_Ass_1a.asmt" />
<None Include="..\testapp\CylSphJt.asmt" />
<None Include="..\testapp\part32.asmt" />
<None Include="..\testapp\RevCylJt.asmt" />
<None Include="..\testapp\RevRevJt.asmt" />
<None Include="..\testapp\SphSphJt.asmt" />
<None Include="..\testapp\00camrocker.asmt" />
<None Include="..\testapp\camfollower.asmt" />
<None Include="..\testapp\circularPendulum.asmt" />
<None Include="..\testapp\coastharbor2.asmt" />
<None Include="..\testapp\crank_slider.asmt" />
<None Include="..\testapp\dxfmotion.asmt" />
<None Include="..\testapp\dxftree.asmt" />
<None Include="..\testapp\fixplnplnpln.asmt" />
<None Include="..\testapp\fixplnplnplnNoOffset.asmt" />
<None Include="..\testapp\JB_2bars.asmt" />
<None Include="..\testapp\mateproblembobcat.asmt" />
<None Include="..\testapp\parm.asmt" />
</ItemGroup>
<ItemGroup>
<Media Include="..\testapp\CrankSlider2.mov" />
@@ -1963,5 +1995,9 @@
</ItemGroup>
<ItemGroup>
<Text Include="..\CMakeLists.txt" />
<Text Include="..\testapp\draggingBackhoe.log" />
<Text Include="..\testapp\draggingBackhoe2.log" />
<Text Include="..\testapp\draggingBackhoe3.log" />
<Text Include="..\testapp\dragging.log" />
</ItemGroup>
</Project>

3
OndselSolver/System.cpp
View File

@@ -125,8 +125,9 @@ void MbD::System::runPreDrag(std::shared_ptr<System> self)
externalSystem->updateFromMbD();
}
void MbD::System::runDragStep(std::shared_ptr<std::vector<std::shared_ptr<Part>>> dragParts)
void MbD::System::runDragStep(std::shared_ptr<System> self, std::shared_ptr<std::vector<std::shared_ptr<Part>>> dragParts)
{
externalSystem->preMbDrunDragStep(self, dragParts);
partsJointsMotionsLimitsForcesTorquesDo([](std::shared_ptr<Item> item) { item->postInput(); });
systemSolver->runDragStep(dragParts);
externalSystem->updateFromMbD();

2
OndselSolver/System.h
View File

@@ -45,7 +45,7 @@ namespace MbD {
void initializeGlobally() override;
void clear();
void runPreDrag(std::shared_ptr<System> self);
void runDragStep(std::shared_ptr<std::vector<std::shared_ptr<Part>>> dragParts);
void runDragStep(std::shared_ptr<System> self, std::shared_ptr<std::vector<std::shared_ptr<Part>>> dragParts);
void runKINEMATIC(std::shared_ptr<System> self);
std::shared_ptr<std::vector<std::string>> discontinuitiesAtIC();
void jointsMotionsDo(const std::function <void(std::shared_ptr<Joint>)>& f);

2
OndselSolver/SystemNewtonRaphson.cpp
View File

@@ -76,7 +76,7 @@ void SystemNewtonRaphson::handleSingularMatrix()
if (str.find("GESpMatParPvPrecise") != std::string::npos) {
str = "MbD: Singular Matrix Error. ";
system->logString(str);
matrixSolver = this->matrixSolverClassNew();
matrixSolver->throwSingularMatrixError("SystemNewtonRaphson");
}
else {
assert(false);

4
OndselSolver/VelSolver.cpp
View File

@@ -34,8 +34,8 @@ void VelSolver::handleSingularMatrix()
str = typeid(r).name();
if (str.find("GESpMatParPvPrecise") != std::string::npos) {
this->logSingularMatrixMessage();
matrixSolver = this->matrixSolverClassNew();
}
matrixSolver->throwSingularMatrixError("VelSolver");
}
else {
assert(false);
}

452
OndselSolver/runDragStep.asmt Normal file
View File

File diff suppressed because one or more lines are too long

1
testapp/draggingBackhoe.log
View File

@@ -179,3 +179,4 @@ runDragStep
1 9.20182797045567e-16 -6.010048927763159e-16
-6.010048927763159e-16 0.91566281475896383 0.40194727224810411
9.20182797045567e-16 -0.40194727224810411 0.91566281475896383
runPostDrag

1
testapp/draggingBackhoe2.log
View File

@@ -17,3 +17,4 @@ runDragStep
1 2.2308684799113093e-15 -8.776108359573272e-16
-1.0922759481826108e-15 0.74987897373328916 0.66157503335049539
2.1339888020027877e-15 -0.66157503335049539 0.74987897373328916
runPostDrag

1
testapp/draggingBackhoe3.log
View File

@@ -26,3 +26,4 @@ runDragStep
1 -4.5522801212393094e-16 -1.9958935382340208e-15
-1.2577026389407931e-15 0.63264657285670811 -0.77444064579066485
1.6152422824418599e-15 0.77444064579066485 0.63264657285670811
runPostDrag