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

Windows version merged

Browse Source
This commit is contained in:
Aik-Siong Koh
2023-11-14 16:04:54 -07:00
parent aa8d647ce8 ff4f8c8fbb
commit 37420b52cf
11 changed files with 2919 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

297
OndselSolver/CrankSlider2.mbd Normal file
View File

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

2
OndselSolver/DiagonalMatrix.h
View File

@@ -25,7 +25,7 @@ namespace MbD {
DiagonalMatrix(int count, const double& value) : Array<double>(count, value) {}
DiagonalMatrix(std::initializer_list<double> list) : Array<double>{ list } {}
void atiputDiagonalMatrix(int i, std::shared_ptr<DiagonalMatrix> diagMat);
DiagMatDsptr times(double factor);
DiagMatDsptr times(double factor);
FColsptr<double> timesFullColumn(FColsptr<double> fullCol);
FMatDsptr timesFullMatrix(FMatDsptr fullMat);
int nrow() {

343
OndselSolver/EndFrameqct.cpp
View File

@@ -322,3 +322,346 @@ 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/FullRow.h
View File

@@ -172,6 +172,37 @@ namespace MbD {
return answer;
}
template<typename T>
inline std::shared_ptr<FullRow<T>> FullRow<T>::clonesptr()
{
return std::make_shared<FullRow<T>>(*this);
}
template<typename T>
inline double FullRow<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>
inline std::shared_ptr<FullVector<T>> FullRow<T>::dot(std::shared_ptr<std::vector<std::shared_ptr<FullColumn<T>>>> vecvec)
{
auto 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>
inline std::ostream& FullRow<T>::printOn(std::ostream& s) const
{
s << "FullRow{";

40
OndselSolver/FullVector.h
View File

@@ -228,6 +228,46 @@ namespace MbD {
return true;
}
template<typename T>
inline std::shared_ptr<FullVector<T>> FullVector<T>::clonesptr()
{
//Return shallow copy of *this wrapped in shared_ptr
assert(false);
return std::make_shared<FullVector<T>>(*this);
}
template<typename T>
inline bool FullVector<T>::isIncreasing()
{
return isIncreasingIfExceptionsAreLessThan(0.0);
}
template<typename T>
inline bool FullVector<T>::isIncreasingIfExceptionsAreLessThan(double tol)
{
//"Test if elements are increasing."
//"Ok if spoilers are less than tol."
auto next = this->at(0);
for (int i = 1; i < this->size(); i++)
{
auto previous = next;
next = this->at(i);
if (previous > next && (previous - tol > next)) return false;
}
return true;
}
template<typename T>
inline bool FullVector<T>::isDecreasingIfExceptionsAreLessThan(double tol)
{
//"Test if elements are increasing."
//"Ok if spoilers are less than tol."
auto next = this->at(0);
for (int i = 1; i < this->size(); i++)
{
auto previous = next;
next = this->at(i);
if (previous < next && (previous + tol < next)) return false;
}
return true;
}
template<typename T>
inline std::ostream& FullVector<T>::printOn(std::ostream& s) const
{
s << "FullVec{";

297
OndselSolver/MBDynCase2.mbd Normal file
View File

@@ -0,0 +1,297 @@
#-----------------------------------------------------------------------------
# [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 Normal file
View File

@@ -0,0 +1,297 @@
#-----------------------------------------------------------------------------
# [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 Normal file
View File

@@ -0,0 +1,321 @@
#-----------------------------------------------------------------------------
# [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 Normal file
View File

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

479
OndselSolver/cirpendu2.asmt Normal file
View File

File diff suppressed because one or more lines are too long

493
OndselSolver/quasikine.asmt Normal file
View File

File diff suppressed because one or more lines are too long