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

MBDynCaseJose.mbd .mov

Browse Source 
# Conflicts:
#	testapp/OndselSolver.cpp
This commit is contained in:
Aik-Siong Koh
2023-11-14 15:18:10 -07:00
parent 9e9a930622
commit aa8d647ce8
27 changed files with 6437 additions and 380 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
OndselSolver/ASMTPrincipalMassMarker.h
View File

@@ -27,7 +27,7 @@ namespace MbD {
double mass = 1.0;
double density = 10.0;
DiagMatDsptr momentOfInertias = std::make_shared<DiagonalMatrix<double>>(ListD{ 1.0, 2.0, 3.0 });
DiagMatDsptr momentOfInertias = std::make_shared<DiagonalMatrix>(ListD{ 1.0, 2.0, 3.0 });
};
}

297
OndselSolver/CrankSlider2.mbd
View File

@@ -1,297 +0,0 @@
-----------------------------------------------------------------------------
# [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: 6;
end: control data;
#-----------------------------------------------------------------------------
# [Design Variables]
#Generic bodies
#body: 2
set: integer body_2 = 2; #body label
set: real mass_2 = 3.805252376198168; #mass [kg]
set: real volume_2 = 0.0004816775159744516; #volume [m^3]
#body: 3
set: integer body_3 = 3; #body label
set: real mass_3 = 15.238784954845523; #mass [kg]
set: real volume_3 = 0.0019289601208665218; #volume [m^3]
#body: 4
set: integer body_4 = 4; #body label
set: real mass_4 = 2.865603331977783; #mass [kg]
set: real volume_4 = 0.0003627345989845295; #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
#Nodes: initial conditions
#node: 1
set: real Px_1 = -0.06210573361337854; #X component of the absolute position [m]
set: real Py_1 = 0.048526435375479564; #Y component of the absolute position [m]
set: real Pz_1 = -4.033966837940965e-17; #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.011666006676941875; #X component of the absolute position [m]
set: real Py_2 = 0.15999999999999778; #Y component of the absolute position [m]
set: real Pz_2 = -1.2084363289349542e-19; #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.2111281366952498; #X component of the absolute position [m]
set: real Py_3 = 0.16; #Y component of the absolute position [m]
set: real Pz_3 = -2.0217697810416158e-18; #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.1812239275015207; #X component of the absolute position [m]
set: real Py_4 = 0.16000000169909329; #Y component of the absolute position [m]
set: real Pz_4 = -4.340477856936436e-12; #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.031674420620509; #moment of inertia [kg*m^2]
set: real Iyy_2 = 0.029604112147595; #moment of inertia [kg*m^2]
set: real Izz_2 = 0.002867529429125; #moment of inertia [kg*m^2]
set: real Rx_2 = 0.0; #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 = 2.0843632893495426e-20; #Z component of the relative center of mass [m]
#body 3:
set: real Ixx_3 = 0.09813066341583701; #moment of inertia [kg*m^2]
set: real Iyy_3 = 0.095433846761275; #moment of inertia [kg*m^2]
set: real Izz_3 = 0.077043262824289; #moment of inertia [kg*m^2]
set: real Rx_3 = 0.0; #X component of the relative center of mass [m]
set: real Ry_3 = 0.0; #Y component of the relative center of mass [m]
set: real Rz_3 = 2.1769781041615487e-20; #Z component of the relative center of mass [m]
#body 4:
set: real Ixx_4 = 0.010133521085753; #moment of inertia [kg*m^2]
set: real Iyy_4 = 0.006853402672398001; #moment of inertia [kg*m^2]
set: real Izz_4 = 0.00669113151275; #moment of inertia [kg*m^2]
set: real Rx_4 = 0.0; #X component of the relative center of mass [m]
set: real Ry_4 = 0.0; #Y component of the relative center of mass [m]
set: real Rz_4 = -4.306356366563123e-20; #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]
body: body_2,
structural_node_2, #<node_label>
mass_2, #<mass> [kg]
Rx_2, Ry_2, Rz_2, #<relative_center_of_mass> [m]
diag, Ixx_2, Iyy_2, Izz_2, #<inertia matrix> [kg*m^2]
orientation, 3, 1.0, 0.0, 0.0, 2, 0.0, 0.9999999999999999, 9.62964972193618e-35;
body: body_3,
structural_node_3, #<node_label>
mass_3, #<mass> [kg]
Rx_3, Ry_3, Rz_3, #<relative_center_of_mass> [m]
diag, Ixx_3, Iyy_3, Izz_3, #<inertia matrix> [kg*m^2]
orientation, 3, 0.0, 0.0, 1.0, 2, 1.0, 0.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]
diag, Ixx_4, Iyy_4, Izz_4, #<inertia matrix> [kg*m^2]
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.06210573361337854, 0.048526435375479564, -4.033966837940965e-17, #<absolute_pin_position> [m]
3, 0.0, 0.0, 1.0, 2, 0.0, 1.0, 0.0; #<absolute_orientation_matrix>
joint: joint_2,
axial rotation,
structural_node_1, #<node_1_label>
0.2521057336133785, 0.11147356462452043, 0.13500000000000004, #<relative_offset_1> [m]
orientation, 3, 0.0, 0.0, 1.0, 2, -1.0, 2.220446049250313e-16, 0.0, #<relative_orientation_matrix_1>
structural_node_3, #<node_2_label>
-0.021128136695249794, 0.0, 0.135, #<relative_offset_2> [m]
orientation, 3, 0.0, 0.0, 1.0, 2, -1.0, 2.220446049250313e-16, 0.0, #<relative_orientation_matrix_2>
string, "model::drive(1, Time)"; #<angular_velocity> [rad/s]
joint: joint_3,
revolute hinge,
structural_node_2, #<node_1_label>
0.08833399332305812, 2.2168933355715126e-15, 0.065, #<relative_position_1> [m]
orientation, 3, 0.0, 0.0, 1.0, 2, -1.0, 2.220446049250313e-16, 0.0, #<relative_pin_orientation_matrix_1>
structural_node_3, #<node_2_label>
-0.11112813669524979, 0.0, 0.065, #<relative_position_2> [m]
orientation, 3, 0.0, 0.0, 1.0, 2, -1.0, 2.220446049250313e-16, 0.0; #<relative_pin_orientation_matrix_2>
joint: joint_4,
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, 1, 2, 0, 1, 0; #relative_orientation_matrix_2>
joint: joint_5,
in line,
structural_node_1, #<node_1_label>
0.0, 0.11147355803101042, 2.4118673970779456e-17, #<relative_line_position> [m]
3, -1.0, 1.1102230246251565e-16, -2.220446049250313e-16, 2, -1.1102230246251565e-16, 0.0, 1.0, #<relative_orientation>
structural_node_4, #<node_2_label>
offset, 0.0, -8.292603282173139e-09, 4.340448411165876e-12; #<relative_offset> [m]
joint: joint_6,
in line,
structural_node_2, #<node_1_label>
-0.1616660066769419, 2.2168933355715126e-15, 0.0, #<relative_line_position> [m]
3, 0.0, -0.0, -1.0, 2, -1.0, -2.220446049250313e-16, 0.0, #<relative_orientation>
structural_node_4, #<node_2_label>
offset, 0.031223927501520705, -1.69909327496498e-09, 7.275957614183426e-15; #<relative_offset> [m]
#-----------------------------------------------------------------------------
# [Drive callers]
drive caller: 1, name,"drive:1", ramp, 10.0, 0.0, 1.0, 0.0;
end: elements;

343
OndselSolver/EndFrameqct.cpp
View File

@@ -322,346 +322,3 @@ namespace MbD {
}
}
EndFrameqct::EndFrameqct(const char* str) : EndFrameqc(str) {
}
void EndFrameqct::initialize()
{
EndFrameqc::initialize();
rmem = std::make_shared<FullColumn<double>>(3);
prmempt = std::make_shared<FullColumn<double>>(3);
pprmemptpt = std::make_shared<FullColumn<double>>(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);
pprOeOptpt = std::make_shared<FullColumn<double>>(3);
ppAOepEpt = std::make_shared<FullColumn<FMatDsptr>>(4);
ppAOeptpt = std::make_shared<FullMatrix<double>>(3, 3);
}
void EndFrameqct::initializeLocally()
{
if (!rmemBlks) {
rmem->zeroSelf();
prmempt->zeroSelf();
pprmemptpt->zeroSelf();
}
if (!phiThePsiBlks) {
aAme->identity();
pAmept->zeroSelf();
ppAmeptpt->zeroSelf();
}
}
void EndFrameqct::initializeGlobally()
{
if (rmemBlks) {
initprmemptBlks();
initpprmemptptBlks();
}
if (phiThePsiBlks) {
initpPhiThePsiptBlks();
initppPhiThePsiptptBlks();
}
}
void EndFrameqct::initprmemptBlks()
{
auto& mbdTime = this->root()->time;
prmemptBlks = std::make_shared< FullColumn<Symsptr>>(3);
for (int i = 0; i < 3; i++) {
auto& disp = rmemBlks->at(i);
auto var = disp->differentiateWRT(mbdTime);
auto vel = var->simplified(var);
prmemptBlks->at(i) = vel;
}
}
void EndFrameqct::initpprmemptptBlks()
{
auto& mbdTime = this->root()->time;
pprmemptptBlks = std::make_shared< FullColumn<Symsptr>>(3);
for (int i = 0; i < 3; i++) {
auto& vel = prmemptBlks->at(i);
auto var = vel->differentiateWRT(mbdTime);
auto acc = var->simplified(var);
pprmemptptBlks->at(i) = acc;
}
}
void EndFrameqct::initpPhiThePsiptBlks()
{
auto& mbdTime = this->root()->time;
pPhiThePsiptBlks = std::make_shared< FullColumn<Symsptr>>(3);
for (int i = 0; i < 3; i++) {
auto& angle = phiThePsiBlks->at(i);
auto var = angle->differentiateWRT(mbdTime);
//std::cout << "var " << *var << std::endl;
auto vel = var->simplified(var);
//std::cout << "vel " << *vel << std::endl;
pPhiThePsiptBlks->at(i) = vel;
//std::cout << *angle << std::endl;
//std::cout << *vel << std::endl;
}
}
void EndFrameqct::initppPhiThePsiptptBlks()
{
auto& mbdTime = this->root()->time;
ppPhiThePsiptptBlks = std::make_shared< FullColumn<Symsptr>>(3);
for (int i = 0; i < 3; i++) {
auto& angleVel = pPhiThePsiptBlks->at(i);
auto var = angleVel->differentiateWRT(mbdTime);
auto angleAcc = var->simplified(var);
ppPhiThePsiptptBlks->at(i) = angleAcc;
//std::cout << *angleVel << std::endl;
//std::cout << *angleAcc << std::endl;
}
}
void EndFrameqct::postInput()
{
this->evalrmem();
this->evalAme();
Item::postInput();
}
void EndFrameqct::calcPostDynCorrectorIteration()
{
auto& rOmO = markerFrame->rOmO;
auto& aAOm = markerFrame->aAOm;
rOeO = rOmO->plusFullColumn(aAOm->timesFullColumn(rmem));
auto& prOmOpE = markerFrame->prOmOpE;
auto& pAOmpE = markerFrame->pAOmpE;
for (int i = 0; i < 3; i++)
{
auto& prOmOpEi = prOmOpE->at(i);
auto& prOeOpEi = prOeOpE->at(i);
for (int j = 0; j < 4; j++)
{
auto prOeOpEij = prOmOpEi->at(j) + pAOmpE->at(j)->at(i)->timesFullColumn(rmem);
prOeOpEi->at(j) = prOeOpEij;
}
}
auto rpep = markerFrame->rpmp->plusFullColumn(markerFrame->aApm->timesFullColumn(rmem));
pprOeOpEpE = EulerParameters<double>::ppApEpEtimesColumn(rpep);
aAOe = aAOm->timesFullMatrix(aAme);
for (int i = 0; i < 4; i++)
{
pAOepE->at(i) = pAOmpE->at(i)->timesFullMatrix(aAme);
}
auto aApe = markerFrame->aApm->timesFullMatrix(aAme);
ppAOepEpE = EulerParameters<double>::ppApEpEtimesMatrix(aApe);
}
void EndFrameqct::prePosIC()
{
time = this->root()->mbdTimeValue();
this->evalrmem();
this->evalAme();
EndFrameqc::prePosIC();
}
void EndFrameqct::evalrmem()
{
if (rmemBlks) {
for (int i = 0; i < 3; i++)
{
auto& expression = rmemBlks->at(i);
double value = expression->getValue();
rmem->at(i) = value;
}
}
}
void EndFrameqct::evalAme()
{
if (phiThePsiBlks) {
auto phiThePsi = CREATE<EulerAngleszxz<double>>::With();
for (int i = 0; i < 3; i++)
{
auto& expression = phiThePsiBlks->at(i);
auto value = expression->getValue();
phiThePsi->at(i) = value;
}
phiThePsi->calc();
aAme = phiThePsi->aA;
}
}
void EndFrameqct::preVelIC()
{
time = this->root()->mbdTimeValue();
this->evalrmem();
this->evalAme();
Item::preVelIC();
this->evalprmempt();
this->evalpAmept();
auto& aAOm = markerFrame->aAOm;
prOeOpt = aAOm->timesFullColumn(prmempt);
pAOept = aAOm->timesFullMatrix(pAmept);
}
void EndFrameqct::postVelIC()
{
auto& pAOmpE = markerFrame->pAOmpE;
for (int i = 0; i < 3; i++)
{
auto& pprOeOpEpti = pprOeOpEpt->at(i);
for (int j = 0; j < 4; j++)
{
auto pprOeOpEptij = pAOmpE->at(j)->at(i)->dot(prmempt);
pprOeOpEpti->atiput(j, pprOeOpEptij);
}
}
for (int i = 0; i < 4; i++)
{
ppAOepEpt->atiput(i, pAOmpE->at(i)->timesFullMatrix(pAmept));
}
}
FColDsptr EndFrameqct::pAjOept(int j)
{
return pAOept->column(j);
}
FMatDsptr EndFrameqct::ppAjOepETpt(int jj)
{
auto answer = std::make_shared<FullMatrix<double>>(4, 3);
for (int i = 0; i < 4; i++)
{
auto& answeri = answer->at(i);
auto& ppAOepEipt = ppAOepEpt->at(i);
for (int j = 0; j < 3; j++)
{
auto& answerij = ppAOepEipt->at(j)->at(jj);
answeri->atiput(j, answerij);
}
}
return answer;
}
FColDsptr EndFrameqct::ppAjOeptpt(int j)
{
return ppAOeptpt->column(j);
}
double EndFrameqct::priOeOpt(int i)
{
return prOeOpt->at(i);
}
FRowDsptr EndFrameqct::ppriOeOpEpt(int i)
{
return pprOeOpEpt->at(i);
}
double EndFrameqct::ppriOeOptpt(int i)
{
return pprOeOptpt->at(i);
}
void EndFrameqct::evalprmempt()
{
if (rmemBlks) {
for (int i = 0; i < 3; i++)
{
auto& derivative = prmemptBlks->at(i);
auto value = derivative->getValue();
prmempt->at(i) = value;
}
}
}
void EndFrameqct::evalpAmept()
{
if (phiThePsiBlks) {
auto phiThePsi = CREATE<EulerAngleszxz<double>>::With();
auto phiThePsiDot = CREATE<EulerAngleszxzDot<double>>::With();
phiThePsiDot->phiThePsi = phiThePsi;
for (int i = 0; i < 3; i++)
{
auto& expression = phiThePsiBlks->at(i);
auto& derivative = pPhiThePsiptBlks->at(i);
auto value = expression->getValue();
auto valueDot = derivative->getValue();
phiThePsi->at(i) = value;
phiThePsiDot->at(i) = valueDot;
}
phiThePsi->calc();
phiThePsiDot->calc();
pAmept = phiThePsiDot->aAdot;
}
}
void EndFrameqct::evalpprmemptpt()
{
if (rmemBlks) {
for (int i = 0; i < 3; i++)
{
auto& secondDerivative = pprmemptptBlks->at(i);
auto value = secondDerivative->getValue();
pprmemptpt->atiput(i, value);
}
}
}
void EndFrameqct::evalppAmeptpt()
{
if (phiThePsiBlks) {
auto phiThePsi = CREATE<EulerAngleszxz<double>>::With();
auto phiThePsiDot = CREATE<EulerAngleszxzDot<double>>::With();
phiThePsiDot->phiThePsi = phiThePsi;
auto phiThePsiDDot = CREATE<EulerAngleszxzDDot<double>>::With();
phiThePsiDDot->phiThePsiDot = phiThePsiDot;
for (int i = 0; i < 3; i++)
{
auto& expression = phiThePsiBlks->at(i);
auto& derivative = pPhiThePsiptBlks->at(i);
auto& secondDerivative = ppPhiThePsiptptBlks->at(i);
auto value = expression->getValue();
auto valueDot = derivative->getValue();
auto valueDDot = secondDerivative->getValue();
phiThePsi->atiput(i, value);
phiThePsiDot->atiput(i, valueDot);
phiThePsiDDot->atiput(i, valueDDot);
}
phiThePsi->calc();
phiThePsiDot->calc();
phiThePsiDDot->calc();
ppAmeptpt = phiThePsiDDot->aAddot;
}
}
FColDsptr EndFrameqct::rmeO()
{
return markerFrame->aAOm->timesFullColumn(rmem);
}
FColDsptr EndFrameqct::rpep()
{
auto& rpmp = markerFrame->rpmp;
auto& aApm = markerFrame->aApm;
auto rpep = rpmp->plusFullColumn(aApm->timesFullColumn(rmem));
return rpep;
}
void EndFrameqct::preAccIC()
{
time = this->root()->mbdTimeValue();
this->evalrmem();
this->evalAme();
Item::preVelIC();
this->evalprmempt();
this->evalpAmept();
auto& aAOm = markerFrame->aAOm;
prOeOpt = aAOm->timesFullColumn(prmempt);
pAOept = aAOm->timesFullMatrix(pAmept);
Item::preAccIC();
this->evalpprmemptpt();
this->evalppAmeptpt();
aAOm = markerFrame->aAOm;
pprOeOptpt = aAOm->timesFullColumn(pprmemptpt);
ppAOeptpt = aAOm->timesFullMatrix(ppAmeptpt);
}

31
OndselSolver/FullColumn.cpp
View File

@@ -169,5 +169,36 @@ namespace MbD {
// instantiate on purpose to make visible in library api:
template class FullColumn<double>;
template class FullColumn<int>;
template<typename T>
double FullColumn<T>::dot(std::shared_ptr<FullVector<T>> vec)
{
int n = (int)this->size();
double answer = 0.0;
for (int i = 0; i < n; i++) {
answer += this->at(i) * vec->at(i);
}
return answer;
}
template<typename T>
std::shared_ptr<FullVector<T>> FullColumn<T>::dot(std::shared_ptr<std::vector<std::shared_ptr<FullColumn<T>>>> vecvec)
{
int ncol = (int)this->size();
auto nelem = vecvec->at(0)->size();
auto answer = std::make_shared<FullVector<T>>(nelem);
for (int k = 0; k < nelem; k++) {
auto sum = 0.0;
for (int i = 0; i < ncol; i++)
{
sum += this->at(i) * vecvec->at(i)->at(k);
}
answer->at(k) = sum;
}
return answer;
}
template<typename T>
std::shared_ptr<FullColumn<T>> FullColumn<T>::clonesptr()
{
return std::make_shared<FullColumn<T>>(*this);
}
}

8
OndselSolver/FullMatrix.h
View File

@@ -107,6 +107,14 @@ namespace MbD {
public:
FullMatrixFullMatrixDouble() = default;
explicit FullMatrixFullMatrixDouble(int m) : RowTypeMatrix<FRowsptr<FMatDsptr>>(m)
{
}
FullMatrixFullMatrixDouble(int m, int n) {
for (int i = 0; i < m; i++) {
auto row = std::make_shared<FullRow<FMatDsptr>>(n);
this->push_back(row);
}
}
double maxMagnitude() override;
void zeroSelf() override;

12
OndselSolver/FullRow.cpp
View File

@@ -22,6 +22,18 @@ namespace MbD {
}
return answer;
}
//template<typename T>
//inline FMatDsptr FullRow<T>::transposeTimesFullRow(FRowDsptr fullRow)
//{
// //"a*b = a(i)b(j)"
// auto nrow = (int)this->size();
// auto answer = std::make_shared<FullMatrixDouble>(nrow);
// for (int i = 0; i < nrow; i++)
// {
// answer->atiput(i, fullRow->times(this->at(i)));
// }
// return answer;
//}
template<>
FRowsptr<double> FullRow<double>::timesTransposeFullMatrix(std::shared_ptr<FullMatrixDouble> fullMat)

15
OndselSolver/FullRow.h
View File

@@ -35,7 +35,7 @@ namespace MbD {
FColsptr<T> transpose();
FRowsptr<T> copy();
void atiplusFullRow(int j, FRowsptr<T> fullRow);
FMatsptr<T> transposeTimesFullRow(FRowsptr<T> fullRow);
FMatDsptr transposeTimesFullRow(FRowDsptr fullRow);
std::shared_ptr<FullRow<T>> clonesptr();
//double dot(std::shared_ptr<FullColumn<T>> vec);
//double dot(std::shared_ptr<FullRow<T>> vec);
@@ -45,7 +45,6 @@ namespace MbD {
std::ostream& printOn(std::ostream& s) const override;
std::shared_ptr<FullMatrixDouble> transposeTimesFullRow(FRowsptr<double> fullRow);
FRowsptr<double> timesTransposeFullMatrix(std::shared_ptr<FullMatrixDouble> fullMat);
// FRowsptr<std::shared_ptr<FullMatrixDouble>> timesTransposeFullMatrixForFMFMDsptr(std::shared_ptr<FullMatrixFullMatrixDouble> fullMat);
FRowsptr<double> timesFullMatrix(std::shared_ptr<FullMatrixDouble> fullMat);
@@ -142,18 +141,6 @@ namespace MbD {
}
}
template<typename T>
inline FMatsptr<T> FullRow<T>::transposeTimesFullRow(FRowsptr<T> fullRow)
{
//"a*b = a(i)b(j)"
auto nrow = (int)this->size();
auto answer = std::make_shared<FullMatrix<double>>(nrow);
for (int i = 0; i < nrow; i++)
{
answer->atiput(i, fullRow->times(this->at(i)));
}
return answer;
}
template<typename T>
inline std::shared_ptr<FullRow<T>> FullRow<T>::clonesptr()
{
return std::make_shared<FullRow<T>>(*this);

349
OndselSolver/MBDynCase.mbd
View File

@@ -1,349 +0,0 @@
######################################################################################################################
# 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.1;
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;

297
OndselSolver/MBDynCase2.mbd
View File

@@ -1,297 +0,0 @@
#-----------------------------------------------------------------------------
# [Data Block]
begin: data;
problem: initial value;
end: data;
#-----------------------------------------------------------------------------
# [Problem Block]
begin: initial value;
initial time: 0.0;
final time: 3.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: 6;
end: control data;
#-----------------------------------------------------------------------------
# [Design Variables]
#Generic bodies
#body: 2
set: integer body_2 = 2; #body label
set: real mass_2 = 1.448636188351172; #mass [kg]
set: real volume_2 = 0.00018337166941154076; #volume [m^3]
#body: 3
set: integer body_3 = 3; #body label
set: real mass_3 = 2.8529486557067685; #mass [kg]
set: real volume_3 = 0.0003611327412287049; #volume [m^3]
#body: 4
set: integer body_4 = 4; #body label
set: real mass_4 = 10.859427202622141; #mass [kg]
set: real volume_4 = 0.0013746110383066003; #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
#Nodes: initial conditions
#node: 1
set: real Px_1 = -0.121; #X component of the absolute position [m]
set: real Py_1 = -1.218180697837851e-18; #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.047537048944738425; #X component of the absolute position [m]
set: real Py_2 = 0.09742200410568831; #Y component of the absolute position [m]
set: real Pz_2 = -0.030293476812230588; #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.07099630277370235; #X component of the absolute position [m]
set: real Py_3 = -0.07364765799707981; #Y component of the absolute position [m]
set: real Pz_3 = 0.058407900823760565; #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.3723079639890564; #X component of the absolute position [m]
set: real Py_4 = 0.04333150035096042; #Y component of the absolute position [m]
set: real Pz_4 = 0.008140985321785409; #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.0028717510150880004; #moment of inertia [kg*m^2]
set: real Iyy_2 = 0.002864447840812; #moment of inertia [kg*m^2]
set: real Izz_2 = 0.0007089594589930001; #moment of inertia [kg*m^2]
set: real Rx_2 = 7.105427357601002e-18; #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 = 7.105427357601002e-18; #Z component of the relative center of mass [m]
#body 3:
set: real Ixx_3 = 0.033837921987970004; #moment of inertia [kg*m^2]
set: real Iyy_3 = 0.033715148099504; #moment of inertia [kg*m^2]
set: real Izz_3 = 0.001956310318013; #moment of inertia [kg*m^2]
set: real Rx_3 = 2.842170943040401e-17; #X component of the relative center of mass [m]
set: real Ry_3 = -1.4210854715202004e-17; #Y component of the relative center of mass [m]
set: real Rz_3 = 0.0; #Z component of the relative center of mass [m]
#body 4:
set: real Ixx_4 = 0.07706742098794901; #moment of inertia [kg*m^2]
set: real Iyy_4 = 0.066351815798527; #moment of inertia [kg*m^2]
set: real Izz_4 = 0.061792350456255; #moment of inertia [kg*m^2]
set: real Rx_4 = 2.842170943040401e-16; #X component of the relative center of mass [m]
set: real Ry_4 = 2.1316282072803006e-17; #Y component of the relative center of mass [m]
set: real Rz_4 = 2.2737367544323206e-16; #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.28116838555915347, -0.12674940821214617, 0.9512512425641977, 2, -0.12674940821214617, 0.9776506595433675, 0.16773125949652098, #<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.27178893568691503, -0.25783416049630004, 0.9271743741709766, 2, 0.014330116918634624, 0.9622501868990581, 0.271788935686915, #<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.2588190451025211, 0.0, 0.9659258262890682, 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]
body: body_2,
structural_node_2, #<node_label>
mass_2, #<mass> [kg]
Rx_2, Ry_2, Rz_2, #<relative_center_of_mass> [m]
diag, Ixx_2, Iyy_2, Izz_2, #<inertia matrix> [kg*m^2]
orientation, 3, -0.0054384608129255385, 0.8559103374577453, 0.5150405937686557, 2, 0.9969290667160357, -0.0024978913689130133, 0.0023196850194898005;
body: body_3,
structural_node_3, #<node_label>
mass_3, #<mass> [kg]
Rx_3, Ry_3, Rz_3, #<relative_center_of_mass> [m]
diag, Ixx_3, Iyy_3, Izz_3, #<inertia matrix> [kg*m^2]
orientation, 3, 0.9973238325492868, 0.0012847690482160057, -0.002629529780301787, 2, -0.00011328308715380375, 1.0027304733964244, 0.004450235157436283;
body: body_4,
structural_node_4, #<node_label>
mass_4, #<mass> [kg]
Rx_4, Ry_4, Rz_4, #<relative_center_of_mass> [m]
diag, Ixx_4, Iyy_4, Izz_4, #<inertia matrix> [kg*m^2]
orientation, 3, -8.551421488581235e-05, 0.0, 0.9957768995806164, 2, 0.0, 1.0, 0.0;
#-----------------------------------------------------------------------------
# [Joints]
joint: joint_1,
clamp,
structural_node_1, #<node_label>
-0.121, -1.218180697837851e-18, -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,
axial rotation,
structural_node_1, #<node_1_label>
0.0, 1.218180697837851e-18, 0.05, #<relative_offset_1> [m]
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>
-0.03739853284269051, 0.0, 0.0032867622552210634, #<relative_offset_2> [m]
orientation, 3, 5.204170427930421e-18, 0.0, 1.0, 2, guess, #<relative_orientation_matrix_2>
string, "model::drive(1, Time)"; #<angular_velocity> [rad/s]
joint: joint_3,
revolute hinge,
structural_node_2, #<node_1_label>
0.03260146715730949, 1.4210854715202004e-17, 0.05328676225522106, #<relative_position_1> [m]
orientation, 3, 5.204170427930421e-18, 0.0, 1.0, 2, 0.0, 1.0, 0.0, #<relative_pin_orientation_matrix_1>
structural_node_3, #<node_2_label>
-0.1400000000000079, -1.0444978215673472e-15, 0.024999999999998038, #<relative_position_2> [m]
orientation, 3, -5.551115123125784e-17, -5.984795992119986e-17, 1.0, 2, guess; #<relative_pin_orientation_matrix_2>
joint: joint_4,
in line,
structural_node_3, #<node_1_label>
0.13999999999999133, 4.4231285301066236e-16, 9.79127889877418e-15, #<relative_line_position> [m]
3, 2.7195091690402506e-15, 1.2586758486779817e-14, 1.0, 2, 0.6882367162699149, 0.7254861972346578, -1.1003185610451133e-14, #<relative_orientation>
structural_node_4, #<node_2_label>
offset, -0.045580834634119244, -2.0299354019925886e-10, 1.2562251640702015e-08; #<relative_offset> [m]
joint: joint_5,
in line,
structural_node_1, #<node_1_label>
0.0, 1.2181806978377854e-18, 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.045580834634119244, -2.0299354019925886e-10, 1.2562251640702015e-08; #<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, 1, 2, 0, 1, 0; #relative_orientation_matrix_2>
#-----------------------------------------------------------------------------
# [Drive callers]
drive caller: 1, name,"drive:1", ramp, 6.28, 0.0, 1.0, 0.0;
end: elements;

297
OndselSolver/MBDynCase2orig.mbd
View File

@@ -1,297 +0,0 @@
#-----------------------------------------------------------------------------
# [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: 6;
end: control data;
#-----------------------------------------------------------------------------
# [Design Variables]
#Generic bodies
#body: 2
set: integer body_2 = 2; #body label
set: real mass_2 = 1.448636188351172; #mass [kg]
set: real volume_2 = 0.00018337166941154076; #volume [m^3]
#body: 3
set: integer body_3 = 3; #body label
set: real mass_3 = 2.8529486557067685; #mass [kg]
set: real volume_3 = 0.0003611327412287049; #volume [m^3]
#body: 4
set: integer body_4 = 4; #body label
set: real mass_4 = 10.859427202622141; #mass [kg]
set: real volume_4 = 0.0013746110383066003; #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
#Nodes: initial conditions
#node: 1
set: real Px_1 = -0.121; #X component of the absolute position [m]
set: real Py_1 = -1.218180697837851e-18; #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.047537048944738425; #X component of the absolute position [m]
set: real Py_2 = 0.09742200410568831; #Y component of the absolute position [m]
set: real Pz_2 = -0.030293476812230588; #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.07099630277370235; #X component of the absolute position [m]
set: real Py_3 = -0.07364765799707981; #Y component of the absolute position [m]
set: real Pz_3 = 0.058407900823760565; #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.3723079639890564; #X component of the absolute position [m]
set: real Py_4 = 0.04333150035096042; #Y component of the absolute position [m]
set: real Pz_4 = 0.008140985321785409; #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.0028717510150880004; #moment of inertia [kg*m^2]
set: real Iyy_2 = 0.002864447840812; #moment of inertia [kg*m^2]
set: real Izz_2 = 0.0007089594589930001; #moment of inertia [kg*m^2]
set: real Rx_2 = 7.105427357601002e-18; #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 = 7.105427357601002e-18; #Z component of the relative center of mass [m]
#body 3:
set: real Ixx_3 = 0.033837921987970004; #moment of inertia [kg*m^2]
set: real Iyy_3 = 0.033715148099504; #moment of inertia [kg*m^2]
set: real Izz_3 = 0.001956310318013; #moment of inertia [kg*m^2]
set: real Rx_3 = 2.842170943040401e-17; #X component of the relative center of mass [m]
set: real Ry_3 = -1.4210854715202004e-17; #Y component of the relative center of mass [m]
set: real Rz_3 = 0.0; #Z component of the relative center of mass [m]
#body 4:
set: real Ixx_4 = 0.07706742098794901; #moment of inertia [kg*m^2]
set: real Iyy_4 = 0.066351815798527; #moment of inertia [kg*m^2]
set: real Izz_4 = 0.061792350456255; #moment of inertia [kg*m^2]
set: real Rx_4 = 2.842170943040401e-16; #X component of the relative center of mass [m]
set: real Ry_4 = 2.1316282072803006e-17; #Y component of the relative center of mass [m]
set: real Rz_4 = 2.2737367544323206e-16; #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.28116838555915347, -0.12674940821214617, 0.9512512425641977, 2, -0.12674940821214617, 0.9776506595433675, 0.16773125949652098, #<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.27178893568691503, -0.25783416049630004, 0.9271743741709766, 2, 0.014330116918634624, 0.9622501868990581, 0.271788935686915, #<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.2588190451025211, 0.0, 0.9659258262890682, 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]
body: body_2,
structural_node_2, #<node_label>
mass_2, #<mass> [kg]
Rx_2, Ry_2, Rz_2, #<relative_center_of_mass> [m]
diag, Ixx_2, Iyy_2, Izz_2, #<inertia matrix> [kg*m^2]
orientation, 3, -0.0054384608129255385, 0.8559103374577453, 0.5150405937686557, 2, 0.9969290667160357, -0.0024978913689130133, 0.0023196850194898005;
body: body_3,
structural_node_3, #<node_label>
mass_3, #<mass> [kg]
Rx_3, Ry_3, Rz_3, #<relative_center_of_mass> [m]
diag, Ixx_3, Iyy_3, Izz_3, #<inertia matrix> [kg*m^2]
orientation, 3, 0.9973238325492868, 0.0012847690482160057, -0.002629529780301787, 2, -0.00011328308715380375, 1.0027304733964244, 0.004450235157436283;
body: body_4,
structural_node_4, #<node_label>
mass_4, #<mass> [kg]
Rx_4, Ry_4, Rz_4, #<relative_center_of_mass> [m]
diag, Ixx_4, Iyy_4, Izz_4, #<inertia matrix> [kg*m^2]
orientation, 3, -8.551421488581235e-05, 0.0, 0.9957768995806164, 2, 0.0, 1.0, 0.0;
#-----------------------------------------------------------------------------
# [Joints]
joint: joint_1,
clamp,
structural_node_1, #<node_label>
-0.121, -1.218180697837851e-18, -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,
axial rotation,
structural_node_1, #<node_1_label>
0.0, 1.218180697837851e-18, 0.05, #<relative_offset_1> [m]
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>
-0.03739853284269051, 0.0, 0.0032867622552210634, #<relative_offset_2> [m]
orientation, 3, 5.204170427930421e-18, 0.0, 1.0, 2, guess, #<relative_orientation_matrix_2>
string, "model::drive(1, Time)"; #<angular_velocity> [rad/s]
joint: joint_3,
revolute hinge,
structural_node_2, #<node_1_label>
0.03260146715730949, 1.4210854715202004e-17, 0.05328676225522106, #<relative_position_1> [m]
orientation, 3, 5.204170427930421e-18, 0.0, 1.0, 2, 0.0, 1.0, 0.0, #<relative_pin_orientation_matrix_1>
structural_node_3, #<node_2_label>
-0.1400000000000079, -1.0444978215673472e-15, 0.024999999999998038, #<relative_position_2> [m]
orientation, 3, -5.551115123125784e-17, -5.984795992119986e-17, 1.0, 2, guess; #<relative_pin_orientation_matrix_2>
joint: joint_4,
in line,
structural_node_3, #<node_1_label>
0.13999999999999133, 4.4231285301066236e-16, 9.79127889877418e-15, #<relative_line_position> [m]
3, 2.7195091690402506e-15, 1.2586758486779817e-14, 1.0, 2, 0.6882367162699149, 0.7254861972346578, -1.1003185610451133e-14, #<relative_orientation>
structural_node_4, #<node_2_label>
offset, -0.045580834634119244, -2.0299354019925886e-10, 1.2562251640702015e-08; #<relative_offset> [m]
joint: joint_5,
in line,
structural_node_1, #<node_1_label>
0.0, 1.2181806978377854e-18, 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.045580834634119244, -2.0299354019925886e-10, 1.2562251640702015e-08; #<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, 1, 2, 0, 1, 0; #relative_orientation_matrix_2>
#-----------------------------------------------------------------------------
# [Drive callers]
drive caller: 1, name,"drive:1", ramp, 6.28, 0.0, 1.0, 0.0;
end: elements;

321
OndselSolver/MBDynCaseDebug1.mbd
View File

@@ -1,321 +0,0 @@
#-----------------------------------------------------------------------------
# [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: orientation matrix;
omega rotates: no;
print: none;
initial stiffness: 1.0, 1.0;
structural nodes: 5;
rigid bodies: 3;
joints: 6;
end: control data;
#-----------------------------------------------------------------------------
# [Design Variables]
#Generic bodies
#body: 2
set: integer body_2 = 2; #body label
set: real mass_2 = 0.24147734685710437; #mass [kg]
set: real volume_2 = 3.056675276672207e-05; #volume [m^3]
#body: 4
set: integer body_4 = 4; #body label
set: real mass_4 = 0.11253654770310718; #mass [kg]
set: real volume_4 = 1.4245132620646478e-05; #volume [m^3]
#body: 5
set: integer body_5 = 5; #body label
set: real mass_5 = 0.11253654770310723; #mass [kg]
set: real volume_5 = 1.4245132620646483e-05; #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
#node: 5
set: integer structural_node_5 = 5; #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: 7
set: integer joint_7 = 7; #joint label
#Nodes: initial conditions
#node: 1
set: real Px_1 = 0.16912189837562464; #X component of the absolute position [m]
set: real Py_1 = -0.10055206743263254; #Y component of the absolute position [m]
set: real Pz_1 = 0.07867737409397452; #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.018683478687078307; #X component of the absolute position [m]
set: real Py_2 = -0.05533171845235879; #Y component of the absolute position [m]
set: real Pz_2 = 0.1768528076947657; #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.2890983348369078; #X component of the absolute position [m]
set: real Py_3 = -0.06156645049450425; #Y component of the absolute position [m]
set: real Pz_3 = 0.09724597715885096; #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.11637356459110539; #X component of the absolute position [m]
set: real Py_4 = -0.019655254070140387; #Y component of the absolute position [m]
set: real Pz_4 = 0.20651079866339145; #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]
#node: 5
set: real Px_5 = 0.15064540729834192; #X component of the absolute position [m]
set: real Py_5 = -0.020536937096322302; #Y component of the absolute position [m]
set: real Pz_5 = 0.06548008210568429; #Z component of the absolute position [m]
set: real Vx_5 = 0.0; #X component of the absolute velocity [m/s]
set: real Vy_5 = 0.0; #Y component of the absolute velocity [m/s]
set: real Vz_5 = 0.0; #Z component of the absolute velocity [m/s]
set: real Wx_5 = 0.0; #X component of the absolute angular velocity [rad/s]
set: real Wy_5 = 0.0; #Y component of the absolute angular velocity [rad/s]
set: real Wz_5 = 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 = 6.927961737800001e-05; #moment of inertia [kg*m^2]
set: real Iyy_2 = 5.6689424982e-05; #moment of inertia [kg*m^2]
set: real Izz_2 = 2.9053392577e-05; #moment of inertia [kg*m^2]
set: real Rx_2 = 0.0; #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.0; #Z component of the relative center of mass [m]
#body 4:
set: real Ixx_4 = 7.9157004521e-05; #moment of inertia [kg*m^2]
set: real Iyy_4 = 7.769349168e-05; #moment of inertia [kg*m^2]
set: real Izz_4 = 3.339121993e-06; #moment of inertia [kg*m^2]
set: real Rx_4 = 0.0; #X component of the relative center of mass [m]
set: real Ry_4 = 0.0; #Y component of the relative center of mass [m]
set: real Rz_4 = 0.0; #Z component of the relative center of mass [m]
#body 5:
set: real Ixx_5 = 7.9157004521e-05; #moment of inertia [kg*m^2]
set: real Iyy_5 = 7.769349168e-05; #moment of inertia [kg*m^2]
set: real Izz_5 = 3.339121993e-06; #moment of inertia [kg*m^2]
set: real Rx_5 = 0.0; #X component of the relative center of mass [m]
set: real Ry_5 = 0.0; #Y component of the relative center of mass [m]
set: real Rz_5 = 0.0; #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.3539135011019427, 0.9158836712023025, 0.18947911379030236, 2, -0.5084786583487672, -0.018385530501657588, -0.8608782877224926, #<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,
static,
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]
structural: structural_node_5,
dynamic,
Px_5, Py_5, Pz_5, #<absolute_position> [m]
3, 0.0, 0.0, 1.0, 2, 0.0, 1.0, 0.0, #<absolute_orientation_matrix>
Vx_5, Vy_5, Vz_5, #<absolute_velocity> [m/s]
Wx_5, Wy_5, Wz_5; #<absolute_angular_velocity> [rad/s]
end: nodes;
#-----------------------------------------------------------------------------
# [Elements Block]
begin: elements;
#-----------------------------------------------------------------------------
# [Bodies]
body: body_2,
structural_node_2, #<node_label>
mass_2, #<mass> [kg]
Rx_2, Ry_2, Rz_2, #<relative_center_of_mass> [m]
diag, Ixx_2, Iyy_2, Izz_2, #<inertia matrix> [kg*m^2]
orientation, 3, -0.35, 0.91, 0.21, 2, 0.78, 0.4, -0.47;
body: body_4,
structural_node_4, #<node_label>
mass_4, #<mass> [kg]
Rx_4, Ry_4, Rz_4, #<relative_center_of_mass> [m]
diag, Ixx_4, Iyy_4, Izz_4, #<inertia matrix> [kg*m^2]
orientation, 3, 0.78, 0.4, -0.47, 2, -0.36, 0.91, 0.19;
body: body_5,
structural_node_5, #<node_label>
mass_5, #<mass> [kg]
Rx_5, Ry_5, Rz_5, #<relative_center_of_mass> [m]
diag, Ixx_5, Iyy_5, Izz_5, #<inertia matrix> [kg*m^2]
orientation, 3, -0.36, 0.91, 0.19, 2, 0.78, 0.4, -0.47;
#-----------------------------------------------------------------------------
# [Joints]
joint: joint_1,
clamp,
structural_node_1, #<node_label>
0.16912189837562464, -0.10055206743263254, 0.07867737409397452, #<absolute_pin_position> [m]
3, -0.3539135011019427, 0.9158836712023025, 0.18947911379030236, 2, -0.5084786583487672, -0.018385530501657588, -0.8608782877224926; #<absolute_orientation_matrix>
joint: joint_2,
axial rotation,
structural_node_1, #<node_1_label>
1.0407603667772492e-11, -0.05247682460581396, -0.00025357557055826873, #<relative_offset_1> [m]
orientation, 3, 1.1102230246251565e-16, -1.0, 0.0, 2, guess, #<relative_orientation_matrix_1>
structural_node_2, #<node_2_label>
0.006998226646402806, -0.00490348349806041, 0.007426083408032554, #<relative_offset_2> [m]
orientation, 3, 0.5084786344413901, 0.01838554762064229, 0.8608783014778036, 2, guess, #<relative_orientation_matrix_2>
string, "model::drive(1, Time)"; #<angular_velocity> [rad/s]
joint: joint_3,
clamp,
structural_node_3, #<node_label>
0.2890983348369078, -0.06156645049450425, 0.09724597715885096, #<absolute_pin_position> [m]
3, 0.0, 0.0, 1.0, 2, 0.0, 1.0, 0.0; #<absolute_orientation_matrix>
joint: joint_4,
revolute hinge,
structural_node_2, #<node_1_label>
-0.0011629534118987692, 0.02262071327533141, 0.01746668757489371, #<relative_position_1> [m]
orientation, 3, 0.5084786344412178, 0.018385547620640152, 0.8608783014779054, 2, guess, #<relative_pin_orientation_matrix_1>
structural_node_4, #<node_2_label>
-0.031347177833029705, -0.01617494903756937, 0.018860685712244078, #<relative_position_2> [m]
orientation, 3, 0.5084786455601428, 0.018385495401303797, 0.860878296025734, 2, guess; #<relative_pin_orientation_matrix_2>
joint: joint_5,
revolute hinge,
structural_node_4, #<node_1_label>
0.03388957106083931, 0.016266876514581658, -0.01455629423212079, #<relative_position_1> [m]
orientation, 3, 0.50847864556046, 0.0183854954014063, 0.8608782960255446, 2, guess, #<relative_pin_orientation_matrix_1>
structural_node_5, #<node_2_label>
-0.016813818553224735, 0.036484433510506876, 0.003343892503690739, #<relative_position_2> [m]
orientation, 3, 0.5084787180221569, 0.01838550036990011, 0.8608782531198544, 2, guess; #<relative_pin_orientation_matrix_2>
joint: joint_7,
revolute hinge,
structural_node_3, #<node_1_label>
1.1368683772161603e-16, 0.0, -1.4210854715202004e-17, #<relative_position_1> [m]
orientation, 3, 0.5094306516594838, 0.018659363391322903, 0.8603093858070039, 2, guess, #<relative_pin_orientation_matrix_1>
structural_node_5, #<node_2_label>
0.014271424963118534, -0.03657636101236503, -0.007648283769292526, #<relative_position_2> [m]
orientation, 3, 0.5084787180221835, 0.018385500369866165, 0.8608782531198396, 2, guess; #<relative_pin_orientation_matrix_2>
#-----------------------------------------------------------------------------
# [Drive callers]
drive caller: 1, name,"drive:1", ramp, 5.0, 0.25, 4.0, 0.0;
end: elements;

320
OndselSolver/MBDynCaseDebug2.mbd
View File

@@ -1,320 +0,0 @@
#-----------------------------------------------------------------------------
# [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: orientation matrix;
omega rotates: no;
print: none;
initial stiffness: 1.0, 1.0;
structural nodes: 5;
rigid bodies: 3;
joints: 6;
end: control data;
#-----------------------------------------------------------------------------
# [Design Variables]
#Generic bodies
#body: 2
set: integer body_2 = 2; #body label
set: real mass_2 = 0.24147734685710437; #mass [kg]
set: real volume_2 = 3.056675276672207e-05; #volume [m^3]
#body: 4
set: integer body_4 = 4; #body label
set: real mass_4 = 0.11253654770310718; #mass [kg]
set: real volume_4 = 1.4245132620646478e-05; #volume [m^3]
#body: 5
set: integer body_5 = 5; #body label
set: real mass_5 = 0.11253654770310723; #mass [kg]
set: real volume_5 = 1.4245132620646483e-05; #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
#node: 5
set: integer structural_node_5 = 5; #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
#Nodes: initial conditions
#node: 1
set: real Px_1 = 0.16912189837562464; #X component of the absolute position [m]
set: real Py_1 = -0.10055206743263254; #Y component of the absolute position [m]
set: real Pz_1 = 0.07867737409397452; #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.018683478687078307; #X component of the absolute position [m]
set: real Py_2 = -0.05533171845235879; #Y component of the absolute position [m]
set: real Pz_2 = 0.1768528076947657; #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.2890983348369078; #X component of the absolute position [m]
set: real Py_3 = -0.06156645049450425; #Y component of the absolute position [m]
set: real Pz_3 = 0.09724597715885096; #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.11637356459110539; #X component of the absolute position [m]
set: real Py_4 = -0.019655254070140387; #Y component of the absolute position [m]
set: real Pz_4 = 0.20651079866339145; #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]
#node: 5
set: real Px_5 = 0.15064540729834192; #X component of the absolute position [m]
set: real Py_5 = -0.020536937096322302; #Y component of the absolute position [m]
set: real Pz_5 = 0.06548008210568429; #Z component of the absolute position [m]
set: real Vx_5 = 0.0; #X component of the absolute velocity [m/s]
set: real Vy_5 = 0.0; #Y component of the absolute velocity [m/s]
set: real Vz_5 = 0.0; #Z component of the absolute velocity [m/s]
set: real Wx_5 = 0.0; #X component of the absolute angular velocity [rad/s]
set: real Wy_5 = 0.0; #Y component of the absolute angular velocity [rad/s]
set: real Wz_5 = 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 = 6.927961737800001e-05; #moment of inertia [kg*m^2]
set: real Iyy_2 = 5.6689424982e-05; #moment of inertia [kg*m^2]
set: real Izz_2 = 2.9053392577e-05; #moment of inertia [kg*m^2]
set: real Rx_2 = 0.0; #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.0; #Z component of the relative center of mass [m]
#body 4:
set: real Ixx_4 = 7.9157004521e-05; #moment of inertia [kg*m^2]
set: real Iyy_4 = 7.769349168e-05; #moment of inertia [kg*m^2]
set: real Izz_4 = 3.339121993e-06; #moment of inertia [kg*m^2]
set: real Rx_4 = 0.0; #X component of the relative center of mass [m]
set: real Ry_4 = 0.0; #Y component of the relative center of mass [m]
set: real Rz_4 = 0.0; #Z component of the relative center of mass [m]
#body 5:
set: real Ixx_5 = 7.9157004521e-05; #moment of inertia [kg*m^2]
set: real Iyy_5 = 7.769349168e-05; #moment of inertia [kg*m^2]
set: real Izz_5 = 3.339121993e-06; #moment of inertia [kg*m^2]
set: real Rx_5 = 0.0; #X component of the relative center of mass [m]
set: real Ry_5 = 0.0; #Y component of the relative center of mass [m]
set: real Rz_5 = 0.0; #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.3539135011019427, 0.9158836712023025, 0.18947911379030236, 2, -0.5084786583487672, -0.018385530501657588, -0.8608782877224926, #<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,
static,
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]
structural: structural_node_5,
dynamic,
Px_5, Py_5, Pz_5, #<absolute_position> [m]
3, 0.0, 0.0, 1.0, 2, 0.0, 1.0, 0.0, #<absolute_orientation_matrix>
Vx_5, Vy_5, Vz_5, #<absolute_velocity> [m/s]
Wx_5, Wy_5, Wz_5; #<absolute_angular_velocity> [rad/s]
end: nodes;
#-----------------------------------------------------------------------------
# [Elements Block]
begin: elements;
#-----------------------------------------------------------------------------
# [Bodies]
body: body_2,
structural_node_2, #<node_label>
mass_2, #<mass> [kg]
Rx_2, Ry_2, Rz_2, #<relative_center_of_mass> [m]
diag, Ixx_2, Iyy_2, Izz_2, #<inertia matrix> [kg*m^2]
orientation, 3, -0.35, 0.91, 0.21, 2, 0.78, 0.4, -0.47;
body: body_4,
structural_node_4, #<node_label>
mass_4, #<mass> [kg]
Rx_4, Ry_4, Rz_4, #<relative_center_of_mass> [m]
diag, Ixx_4, Iyy_4, Izz_4, #<inertia matrix> [kg*m^2]
orientation, 3, 0.78, 0.4, -0.47, 2, -0.36, 0.91, 0.19;
body: body_5,
structural_node_5, #<node_label>
mass_5, #<mass> [kg]
Rx_5, Ry_5, Rz_5, #<relative_center_of_mass> [m]
diag, Ixx_5, Iyy_5, Izz_5, #<inertia matrix> [kg*m^2]
orientation, 3, -0.36, 0.91, 0.19, 2, 0.78, 0.4, -0.47;
#-----------------------------------------------------------------------------
# [Joints]
joint: joint_1,
clamp,
structural_node_1, #<node_label>
0.16912189837562464, -0.10055206743263254, 0.07867737409397452, #<absolute_pin_position> [m]
3, -0.3539135011019427, 0.9158836712023025, 0.18947911379030236, 2, -0.5084786583487672, -0.018385530501657588, -0.8608782877224926; #<absolute_orientation_matrix>
joint: joint_2,
axial rotation,
structural_node_1, #<node_1_label>
1.0407603667772492e-11, -0.05247682460581396, -0.00025357557055826873, #<relative_offset_1> [m]
orientation, 3, 1.1102230246251565e-16, -1.0, 0.0, 2, guess, #<relative_orientation_matrix_1>
structural_node_2, #<node_2_label>
0.006998226646402806, -0.00490348349806041, 0.007426083408032554, #<relative_offset_2> [m]
orientation, 3, 0.5084786344413901, 0.01838554762064229, 0.8608783014778036, 2, guess, #<relative_orientation_matrix_2>
string, "model::drive(1, Time)"; #<angular_velocity> [rad/s]
joint: joint_3,
clamp,
structural_node_3, #<node_label>
0.2890983348369078, -0.06156645049450425, 0.09724597715885096, #<absolute_pin_position> [m]
3, 0.0, 0.0, 1.0, 2, 0.0, 1.0, 0.0; #<absolute_orientation_matrix>
joint: joint_4,
revolute hinge,
structural_node_2, #<node_1_label>
-0.0011629534118987692, 0.02262071327533141, 0.01746668757489371, #<relative_position_1> [m]
orientation, 3, 0.5084786344412178, 0.018385547620640152, 0.8608783014779054, 2, guess, #<relative_pin_orientation_matrix_1>
structural_node_4, #<node_2_label>
-0.031347177833029705, -0.01617494903756937, 0.018860685712244078, #<relative_position_2> [m]
orientation, 3, 0.5084786455601428, 0.018385495401303797, 0.860878296025734, 2, guess; #<relative_pin_orientation_matrix_2>
joint: joint_5,
revolute hinge,
structural_node_4, #<node_1_label>
0.03388957106083931, 0.016266876514581658, -0.01455629423212079, #<relative_position_1> [m]
orientation, 3, 0.50847864556046, 0.0183854954014063, 0.8608782960255446, 2, guess, #<relative_pin_orientation_matrix_1>
structural_node_5, #<node_2_label>
-0.016813818553224735, 0.036484433510506876, 0.003343892503690739, #<relative_position_2> [m]
orientation, 3, 0.5084787180221569, 0.01838550036990011, 0.8608782531198544, 2, guess; #<relative_pin_orientation_matrix_2>
joint: joint_6,
in line,
structural_node_3, #<node_1_label>
0.0, 0.0, 0.0, #<relative_line_position> [m]
3, 0.5094306516594838, 0.018659363391322903, 0.8603093858070039, 2, guess, #<relative_orientation>
structural_node_5, #<node_2_label>
offset, 0.014271424963118534, -0.03657636101236503, -0.007648283769292526; #<relative_offset> [m]
#-----------------------------------------------------------------------------
# [Drive callers]
drive caller: 1, name,"drive:1", ramp, 5.0, 0.25, 4.0, 0.0;
end: elements;

2
OndselSolver/MBDynMarker.cpp
View File

@@ -37,7 +37,7 @@ void MbD::MBDynMarker::parseMBDynClamp(std::vector<std::string>& args)
//rOmO = rOPO + aAOP*rPmP
//aAOm = aAOP * aAPm
auto rOmO = std::make_shared<FullColumn<double>>(3);
auto aAOm = FullMatrix<double>::identitysptr(3);
auto aAOm = FullMatrixDouble::identitysptr(3);
auto rOPO = readPosition(args);
auto aAOP = readOrientation(args);
rPmP = aAOP->transposeTimesFullColumn(rOmO->minusFullColumn(rOPO));

9
OndselSolver/OndselSolver.vcxproj
View File

@@ -134,6 +134,7 @@
</Link>
</ItemDefinitionGroup>
<ItemGroup>
<ClCompile Include="..\testapp\OndselSolver.cpp" />
<ClCompile Include="Abs.cpp" />
<ClCompile Include="AbsConstraint.cpp" />
<ClCompile Include="AccICKineNewtonRaphson.cpp" />
@@ -347,7 +348,6 @@
<ClCompile Include="NoRotationJoint.cpp" />
<ClCompile Include="NotKinematicError.cpp" />
<ClCompile Include="Numeric.cpp" />
<ClCompile Include="OndselSolver.cpp" />
<ClCompile Include="OrbitAngleZIecJec.cpp" />
<ClCompile Include="OrbitAngleZIeqcJec.cpp" />
<ClCompile Include="OrbitAngleZIeqcJeqc.cpp" />
@@ -426,6 +426,7 @@
<ClCompile Include="ZTranslation.cpp" />
</ItemGroup>
<ItemGroup>
<ClInclude Include="..\testapp\OndselSolver.h" />
<ClInclude Include="Abs.h" />
<ClInclude Include="AbsConstraint.h" />
<ClInclude Include="AccICKineNewtonRaphson.h" />
@@ -499,14 +500,19 @@
<ClInclude Include="CREATE.h" />
<ClInclude Include="CylindricalJoint.h" />
<ClInclude Include="CylSphJoint.h" />
<ClInclude Include="DiagonalMatrix.ref.h" />
<ClInclude Include="DifferentiatedGeneralSpline.h" />
<ClInclude Include="EigenDecomposition.h" />
<ClInclude Include="EndFrameqct2.h" />
<ClInclude Include="EulerAngles.h" />
<ClInclude Include="EulerAnglesDDot.h" />
<ClInclude Include="EulerAnglesDot.h" />
<ClInclude Include="EulerParameters.ref.h" />
<ClInclude Include="Exponential.h" />
<ClInclude Include="ExternalSystem.h" />
<ClInclude Include="FullColumn.ref.h" />
<ClInclude Include="FullMatrix.ref.h" />
<ClInclude Include="FullRow.ref.h" />
<ClInclude Include="FunctionFromData.h" />
<ClInclude Include="FunctionXcParameter.h" />
<ClInclude Include="FunctionXY.h" />
@@ -640,7 +646,6 @@
<ClInclude Include="NoRotationJoint.h" />
<ClInclude Include="NotKinematicError.h" />
<ClInclude Include="Numeric.h" />
<ClInclude Include="OndselSolver.h" />
<ClInclude Include="OrbitAngleZIecJec.h" />
<ClInclude Include="OrbitAngleZIeqcJec.h" />
<ClInclude Include="OrbitAngleZIeqcJeqc.h" />

27
OndselSolver/OndselSolver.vcxproj.filters
View File

@@ -15,9 +15,6 @@
</Filter>
</ItemGroup>
<ItemGroup>
<ClCompile Include="OndselSolver.cpp">
<Filter>Source Files</Filter>
</ClCompile>
<ClCompile Include="Array.cpp">
<Filter>Source Files</Filter>
</ClCompile>
@@ -885,6 +882,9 @@
<ClCompile Include="Integral.cpp">
<Filter>Source Files</Filter>
</ClCompile>
<ClCompile Include="..\testapp\OndselSolver.cpp">
<Filter>Source Files</Filter>
</ClCompile>
</ItemGroup>
<ItemGroup>
<ClInclude Include="Array.h">
@@ -953,9 +953,6 @@
<ClInclude Include="EndFrameqct.h">
<Filter>Header Files</Filter>
</ClInclude>
<ClInclude Include="OndselSolver.h">
<Filter>Header Files</Filter>
</ClInclude>
<ClInclude Include="FullVector.h">
<Filter>Header Files</Filter>
</ClInclude>
@@ -1763,6 +1760,24 @@
<ClInclude Include="Integral.h">
<Filter>Header Files</Filter>
</ClInclude>
<ClInclude Include="..\testapp\OndselSolver.h">
<Filter>Header Files</Filter>
</ClInclude>
<ClInclude Include="FullRow.ref.h">
<Filter>Header Files</Filter>
</ClInclude>
<ClInclude Include="FullMatrix.ref.h">
<Filter>Header Files</Filter>
</ClInclude>
<ClInclude Include="FullColumn.ref.h">
<Filter>Header Files</Filter>
</ClInclude>
<ClInclude Include="EulerParameters.ref.h">
<Filter>Header Files</Filter>
</ClInclude>
<ClInclude Include="DiagonalMatrix.ref.h">
<Filter>Header Files</Filter>
</ClInclude>
</ItemGroup>
<ItemGroup>
<ResourceCompile Include="OndselSolver.rc">

2
OndselSolver/Part.cpp
View File

@@ -586,5 +586,5 @@ void Part::postDynStep()
void MbD::Part::postAccIC()
{
calcpdot();
//calcpdot();
}

439
OndselSolver/assembly.asmt Normal file
View File

@@ -0,0 +1,439 @@
OndselSolver
Assembly
Notes
(Text string: '' runs: (Core.RunArray runs: #() values: #()))
Name
Assembly
Position3D
0 0 0
RotationMatrix
1 0 0
0 1 0
0 0 1
Velocity3D
0 0 0
Omega3D
0 0 0
RefPoints
RefPoint
Position3D
0 0 0
RotationMatrix
1 0 0
0 1 0
0 0 1
Markers
Marker
Name
Marker0
Position3D
0 0 0
RotationMatrix
1 0 0
0 1 0
0 0 1
RefCurves
RefSurfaces
Parts
Part
Name
structural_node_1
Position3D
-0.06210573361337854 0.04852643537547956 -4.033966837940965e-17
RotationMatrix
1 0 0
0 1 0
0 0 1
Velocity3D
0 0 0
Omega3D
0 0 0
FeatureOrder
PrincipalMassMarker
Name
MassMarker
Position3D
0 0 0
RotationMatrix
1 0 0
0 1 0
0 0 1
Mass
1
MomentOfInertias
1 2 3
Density
10
RefPoints
RefPoint
Position3D
0 0 0
RotationMatrix
1 0 0
0 1 0
0 0 1
Markers
Marker
Name
Marker0
Position3D
0.06210573361337854 -0.04852643537547956 4.033966837940965e-17
RotationMatrix
1 0 0
0 1 0
0 0 1
RefPoint
Position3D
0 0 0
RotationMatrix
1 0 0
0 1 0
0 0 1
Markers
Marker
Name
Marker1
Position3D
0.2521057336133785 0.1114735646245204 0.135
RotationMatrix
2.220446049250313e-16 -1 0
1 2.220446049250313e-16 0
-0 0 1
RefPoint
Position3D
0 0 0
RotationMatrix
1 0 0
0 1 0
0 0 1
Markers
Marker
Name
Marker2
Position3D
0 0 0
RotationMatrix
1 0 0
0 1 0
0 0 1
RefPoint
Position3D
0 0 0
RotationMatrix
1 0 0
0 1 0
0 0 1
Markers
Marker
Name
Marker3
Position3D
0 0.1114735580310104 2.411867397077946e-17
RotationMatrix
-1.110223024625157e-16 -2.220446049250313e-16 -1
-1 1.232595164407831e-32 1.110223024625157e-16
-1.232595164407831e-32 1 -2.220446049250313e-16
RefCurves
RefSurfaces
Part
Name
structural_node_2
Position3D
0.01166600667694188 0.1599999999999978 -1.208436328934954e-19
RotationMatrix
1 0 0
0 1 0
0 0 1
Velocity3D
0 0 0
Omega3D
0 0 0
FeatureOrder
PrincipalMassMarker
Name
MassMarker
Position3D
0 0 2.084363289349543e-20
RotationMatrix
1 0 0
0 1 0
0 0 1
Mass
3.805252376198168
MomentOfInertias
0.031674420620509 0.029604112147595 0.002867529429125
Density
10
RefPoints
RefPoint
Position3D
0 0 0
RotationMatrix
1 0 0
0 1 0
0 0 1
Markers
Marker
Name
Marker0
Position3D
0.08833399332305812 2.216893335571513e-15 0.065
RotationMatrix
2.220446049250313e-16 -1 0
1 2.220446049250313e-16 0
-0 0 1
RefPoint
Position3D
0 0 0
RotationMatrix
1 0 0
0 1 0
0 0 1
Markers
Marker
Name
Marker1
Position3D
-0.1616660066769419 2.216893335571513e-15 0
RotationMatrix
2.220446049250313e-16 -1 0
-1 -2.220446049250313e-16 -0
0 0 -1
RefCurves
RefSurfaces
Part
Name
structural_node_3
Position3D
0.2111281366952498 0.16 -2.021769781041616e-18
RotationMatrix
1 0 0
0 1 0
0 0 1
Velocity3D
0 0 0
Omega3D
0 0 0
FeatureOrder
PrincipalMassMarker
Name
MassMarker
Position3D
0 0 2.176978104161549e-20
RotationMatrix
1 0 0
0 1 0
0 0 1
Mass
15.23878495484552
MomentOfInertias
0.09813066341583701 0.095433846761275 0.077043262824289
Density
10
RefPoints
RefPoint
Position3D
0 0 0
RotationMatrix
1 0 0
0 1 0
0 0 1
Markers
Marker
Name
Marker0
Position3D
-0.02112813669524979 0 0.135
RotationMatrix
2.220446049250313e-16 -1 0
1 2.220446049250313e-16 0
-0 0 1
RefPoint
Position3D
0 0 0
RotationMatrix
1 0 0
0 1 0
0 0 1
Markers
Marker
Name
Marker1
Position3D
-0.1111281366952498 0 0.065
RotationMatrix
2.220446049250313e-16 -1 0
1 2.220446049250313e-16 0
-0 0 1
RefCurves
RefSurfaces
Part
Name
structural_node_4
Position3D
-0.1812239275015207 0.1600000016990933 -4.340477856936436e-12
RotationMatrix
1 0 0
0 1 0
0 0 1
Velocity3D
0 0 0
Omega3D
0 0 0
FeatureOrder
PrincipalMassMarker
Name
MassMarker
Position3D
0 0 -4.306356366563123e-20
RotationMatrix
1 0 0
0 1 0
0 0 1
Mass
2.865603331977783
MomentOfInertias
0.010133521085753 0.006853402672398001 0.00669113151275
Density
10
RefPoints
RefPoint
Position3D
0 0 0
RotationMatrix
1 0 0
0 1 0
0 0 1
Markers
Marker
Name
Marker0
Position3D
0 0 0
RotationMatrix
1 0 0
0 1 0
0 0 1
RefPoint
Position3D
0 0 0
RotationMatrix
1 0 0
0 1 0
0 0 1
Markers
Marker
Name
Marker1
Position3D
0 -8.292603282173139e-09 4.340448411165876e-12
RotationMatrix
1 0 0
0 1 0
0 0 1
RefPoint
Position3D
0 0 0
RotationMatrix
1 0 0
0 1 0
0 0 1
Markers
Marker
Name
Marker2
Position3D
0.03122392750152071 -1.69909327496498e-09 7.275957614183426e-15
RotationMatrix
1 0 0
0 1 0
0 0 1
RefCurves
RefSurfaces
KinematicIJs
ConstraintSets
Joints
FixedJoint
Name
joint_1
MarkerI
/Assembly/Marker0
MarkerJ
/Assembly/structural_node_1/Marker0
RevoluteJoint
Name
joint_2
MarkerI
/Assembly/structural_node_1/Marker1
MarkerJ
/Assembly/structural_node_3/Marker0
RevoluteJoint
Name
joint_3
MarkerI
/Assembly/structural_node_2/Marker0
MarkerJ
/Assembly/structural_node_3/Marker1
NoRotationJoint
Name
joint_4
MarkerI
/Assembly/structural_node_1/Marker2
MarkerJ
/Assembly/structural_node_4/Marker0
PointInLineJoint
Name
joint_5
MarkerI
/Assembly/structural_node_1/Marker3
MarkerJ
/Assembly/structural_node_4/Marker1
PointInLineJoint
Name
joint_6
MarkerI
/Assembly/structural_node_2/Marker1
MarkerJ
/Assembly/structural_node_4/Marker2
Motions
RotationalMotion
Name
joint_2Motion
MarkerI
/Assembly/structural_node_1/Marker1
MarkerJ
/Assembly/structural_node_3/Marker0
MotionJoint
/Assembly/joint_2
RotationZ
integral(time, rampstep(time, 0.0, 0.0, 1.0, 0.0 + 10.0*(1.0 - 0.0)))
GeneralConstraintSets
ForceTorques
ConstantGravity
0 0 0
SimulationParameters
tstart
0
tend
8
hmin
1e-09
hmax
1
hout
0.01
errorTol
1e-06
AnimationParameters
nframe
1000000
icurrent
1
istart
1
iend
1000000
isForward
true
framesPerSecond
30

479
OndselSolver/cirpendu2.asmt
View File

File diff suppressed because one or more lines are too long

176
OndselSolver/crank_slider.mbd
View File

@@ -1,176 +0,0 @@
# crank_slider.mbd
#-----------------------------------------------------------------------------
# [Data Block]
begin: data;
problem: initial value;
end: data;
#-----------------------------------------------------------------------------
# [<Problem> Block]
begin: initial value;
initial time: 0.;
final time: 5.;
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;

176
OndselSolver/crank_sliderX.mbd
View File

@@ -1,176 +0,0 @@
# 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;

493
OndselSolver/quasikine.asmt
View File

File diff suppressed because one or more lines are too long