Skip to content

Limits-are-for-dragging-only #11

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 2 commits into
base: main
Choose a base branch
from
Open

Limits-are-for-dragging-only #11

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
8971c3f
Limits-are-for-dragging-only
aiksiongkoh Oct 4, 2025
b36f06d
Merge branch 'main' into Limits-are-for-dragging-only
aiksiongkoh Oct 4, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
14 changes: 7 additions & 7 deletions OndselSolver/AccNewtonRaphson.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -20,7 +20,7 @@ using namespace MbD;

void AccNewtonRaphson::askSystemToUpdate()
{
system->partsJointsMotionsLimitsForcesTorquesDo([&](std::shared_ptr<Item> item) { item->postAccICIteration(); });
system->partsJointsMotionsForcesTorquesDo([&](std::shared_ptr<Item> item) { item->postAccICIteration(); });
}

void AccNewtonRaphson::assignEquationNumbers()
Expand Down Expand Up @@ -57,15 +57,15 @@ void AccNewtonRaphson::assignEquationNumbers()
void AccNewtonRaphson::fillPyPx()
{
pypx->zeroSelf();
system->partsJointsMotionsLimitsForcesTorquesDo([&](std::shared_ptr<Item> item) {
system->partsJointsMotionsForcesTorquesDo([&](std::shared_ptr<Item> item) {
item->fillAccICIterJacob(pypx);
});
}

void AccNewtonRaphson::fillY()
{
y->zeroSelf();
system->partsJointsMotionsLimitsForcesTorquesDo([&](std::shared_ptr<Item> item) {
system->partsJointsMotionsForcesTorquesDo([&](std::shared_ptr<Item> item) {
item->fillAccICIterError(y);
//std::cout << item->name << *y << std::endl;
});
Expand Down Expand Up @@ -97,7 +97,7 @@ void AccNewtonRaphson::incrementIterNo()
void AccNewtonRaphson::initializeGlobally()
{
SystemNewtonRaphson::initializeGlobally();
system->partsJointsMotionsLimitsDo([&](std::shared_ptr<Item> item) { item->fillqsuddotlam(x); });
system->partsJointsMotionsDo([&](std::shared_ptr<Item> item) { item->fillqsuddotlam(x); });
}

void AccNewtonRaphson::logSingularMatrixMessage()
Expand All @@ -110,17 +110,17 @@ void AccNewtonRaphson::logSingularMatrixMessage()

void AccNewtonRaphson::passRootToSystem()
{
system->partsJointsMotionsLimitsDo([&](std::shared_ptr<Item> item) { item->setqsuddotlam(x); });
system->partsJointsMotionsDo([&](std::shared_ptr<Item> item) { item->setqsuddotlam(x); });
}

void AccNewtonRaphson::postRun()
{
system->partsJointsMotionsLimitsForcesTorquesDo([&](std::shared_ptr<Item> item) { item->postAccIC(); });
system->partsJointsMotionsForcesTorquesDo([&](std::shared_ptr<Item> item) { item->postAccIC(); });
}

void AccNewtonRaphson::preRun()
{
system->partsJointsMotionsLimitsForcesTorquesDo([&](std::shared_ptr<Item> item) { item->preAccIC(); });
system->partsJointsMotionsForcesTorquesDo([&](std::shared_ptr<Item> item) { item->preAccIC(); });
}

void MbD::AccNewtonRaphson::handleSingularMatrix()
Expand Down
8 changes: 4 additions & 4 deletions OndselSolver/AnyPosICNewtonRaphson.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -25,7 +25,7 @@ void AnyPosICNewtonRaphson::initialize()
void AnyPosICNewtonRaphson::initializeGlobally()
{
SystemNewtonRaphson::initializeGlobally();
system->partsJointsMotionsLimitsDo([&](std::shared_ptr<Item> item) {
system->partsJointsMotionsDo([&](std::shared_ptr<Item> item) {
item->fillqsu(qsuOld);
item->fillqsuWeights(qsuWeights);
item->fillqsulam(x);
Expand All @@ -45,7 +45,7 @@ void AnyPosICNewtonRaphson::fillY()
newMinusOld->equalSelfPlusFullColumnAt(x, 0);
y->zeroSelf();
y->atiminusFullColumn(0, (qsuWeights->timesFullColumn(newMinusOld)));
system->partsJointsMotionsLimitsDo([&](std::shared_ptr<Item> item) {
system->partsJointsMotionsDo([&](std::shared_ptr<Item> item) {
item->fillPosICError(y);
//std::cout << item->name << *y << std::endl;
//noop();
Expand All @@ -57,7 +57,7 @@ void AnyPosICNewtonRaphson::fillPyPx()
{
pypx->zeroSelf();
pypx->atijminusDiagonalMatrix(0, 0, qsuWeights);
system->partsJointsMotionsLimitsDo([&](std::shared_ptr<Item> item) {
system->partsJointsMotionsDo([&](std::shared_ptr<Item> item) {
item->fillPosICJacob(pypx);
//std::cout << *(pypx->at(3)) << std::endl;
});
Expand All @@ -66,7 +66,7 @@ void AnyPosICNewtonRaphson::fillPyPx()

void AnyPosICNewtonRaphson::passRootToSystem()
{
system->partsJointsMotionsLimitsDo([&](std::shared_ptr<Item> item) { item->setqsulam(x); });
system->partsJointsMotionsDo([&](std::shared_ptr<Item> item) { item->setqsulam(x); });
}

void MbD::AnyPosICNewtonRaphson::assignEquationNumbers()
Expand Down
15 changes: 1 addition & 14 deletions OndselSolver/PosICDragLimitNewtonRaphson.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -22,19 +22,6 @@ void MbD::PosICDragLimitNewtonRaphson::preRun()

}

void MbD::PosICDragLimitNewtonRaphson::initializeGlobally()
{
AnyPosICNewtonRaphson::initializeGlobally();
iterMax = system->iterMaxPosKine;
dxTol = system->errorTolPosKine;
}

void MbD::PosICDragLimitNewtonRaphson::setdragParts(std::shared_ptr<std::vector<std::shared_ptr<Part>>> dragParts)
{
(void) dragParts;
throw SimulationStoppingError("To be implemented.");
}

void MbD::PosICDragLimitNewtonRaphson::run()
{
preRun();
Expand All @@ -58,5 +45,5 @@ void MbD::PosICDragLimitNewtonRaphson::run()
system->deactivateLimits();
if (system->limitsSatisfied()) return;
}
throw SimulationStoppingError("Limits cannot be satisfiled.");
throw SimulationStoppingError("Limits cannot be satisfied.");
}
11 changes: 4 additions & 7 deletions OndselSolver/PosICDragLimitNewtonRaphson.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -8,21 +8,18 @@

#pragma once

#include "AnyPosICNewtonRaphson.h"
#include "PosICDragNewtonRaphson.h"

namespace MbD {
class Part;

class PosICDragLimitNewtonRaphson : public AnyPosICNewtonRaphson
class PosICDragLimitNewtonRaphson : public PosICDragNewtonRaphson
{
//Kinematics with under constrained system
//Dragging with limits of constrained or under constrained system
//Assume no redundant constraints
public:
static std::shared_ptr<PosICDragLimitNewtonRaphson> With();
void preRun() override;
void initializeGlobally() override;
void setdragParts(std::shared_ptr<std::vector<std::shared_ptr<Part>>> dragParts);
void run() override;

std::shared_ptr<std::vector<std::shared_ptr<Part>>> dragParts;
};
}
89 changes: 88 additions & 1 deletion OndselSolver/PosICDragNewtonRaphson.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -20,9 +20,29 @@ std::shared_ptr<PosICDragNewtonRaphson> MbD::PosICDragNewtonRaphson::With()
return newtonRaphson;
}

void MbD::PosICDragNewtonRaphson::preRun()
{
std::string str("MbD: Assembling system. ");
system->logString(str);
system->partsJointsMotionsLimitsDo([&](std::shared_ptr<Item> item) { item->prePosIC(); });
}

void MbD::PosICDragNewtonRaphson::askSystemToUpdate()
{
system->partsJointsMotionsLimitsDo([&](std::shared_ptr<Item> item) { item->postPosICIteration(); });
}

void MbD::PosICDragNewtonRaphson::postRun()
{
system->partsJointsMotionsLimitsDo([&](std::shared_ptr<Item> item) { item->postPosIC(); });
}

void MbD::PosICDragNewtonRaphson::initializeGlobally()
{
AnyPosICNewtonRaphson::initializeGlobally();
this->assignEquationNumbers();
system->partsJointsMotionsLimitsForcesTorquesDo([&](std::shared_ptr<Item> item) { item->useEquationNumbers(); });
this->createVectorsAndMatrices();
matrixSolver = this->matrixSolverClassNew();
iterMax = system->iterMaxPosKine;
dxTol = system->errorTolPosKine;
for (size_t i = 0; i < qsuWeights->size(); i++)
Expand All @@ -36,6 +56,73 @@ void MbD::PosICDragNewtonRaphson::initializeGlobally()
qsuWeights->at((size_t)iqX + i) = 1.0e6; //maximum weight
}
}
system->partsJointsMotionsLimitsDo([&](std::shared_ptr<Item> item) {
item->fillqsu(qsuOld);
item->fillqsuWeights(qsuWeights);
item->fillqsulam(x);
});
}

void MbD::PosICDragNewtonRaphson::fillY()
{
auto newMinusOld = qsuOld->negated();
newMinusOld->equalSelfPlusFullColumnAt(x, 0);
y->zeroSelf();
y->atiminusFullColumn(0, (qsuWeights->timesFullColumn(newMinusOld)));
system->partsJointsMotionsLimitsDo([&](std::shared_ptr<Item> item) {
item->fillPosICError(y);
//std::cout << item->name << *y << std::endl;
//noop();
});
//std::cout << "Final" << *y << std::endl;
}

void MbD::PosICDragNewtonRaphson::fillPyPx()
{
pypx->zeroSelf();
pypx->atijminusDiagonalMatrix(0, 0, qsuWeights);
system->partsJointsMotionsLimitsDo([&](std::shared_ptr<Item> item) {
item->fillPosICJacob(pypx);
//std::cout << *(pypx->at(3)) << std::endl;
});
//std::cout << *pypx << std::endl;
}

void MbD::PosICDragNewtonRaphson::passRootToSystem()
{
system->partsJointsMotionsLimitsDo([&](std::shared_ptr<Item> item) { item->setqsulam(x); });
}

void MbD::PosICDragNewtonRaphson::assignEquationNumbers()
{
auto parts = system->parts();
//auto contactEndFrames = system->contactEndFrames();
//auto uHolders = system->uHolders();
auto constraints = system->allConstraintsLimits();
size_t eqnNo = 0;
for (auto& part : *parts) {
part->iqX(eqnNo);
eqnNo = eqnNo + 3;
part->iqE(eqnNo);
eqnNo = eqnNo + 4;
}
//for (auto& endFrm : *contactEndFrames) {
// endFrm->is(eqnNo);
// eqnNo = eqnNo + endFrm->sSize();
//}
//for (auto& uHolder : *uHolders) {
// uHolder->iu(eqnNo);
// eqnNo += 1;
//}
auto nEqns = eqnNo; //C++ uses index 0.
nqsu = nEqns;
for (auto& con : *constraints) {
con->iG = eqnNo;
eqnNo += 1;
}
//auto lastEqnNo = eqnNo - 1;
nEqns = eqnNo; //C++ uses index 0.
n = nEqns;
}

void MbD::PosICDragNewtonRaphson::setdragParts(std::shared_ptr<std::vector<std::shared_ptr<Part>>> _dragParts)
Expand Down
16 changes: 14 additions & 2 deletions OndselSolver/PosICDragNewtonRaphson.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -15,11 +15,23 @@ namespace MbD {

class PosICDragNewtonRaphson : public AnyPosICNewtonRaphson
{
//Kinematics with under constrained system
//Dragging of constrained or under constrained system
//Assume no redundant constraints
//Limits are used in dragging only
public:
static std::shared_ptr<PosICDragNewtonRaphson> With();

void initializeGlobally() override;
void setdragParts(std::shared_ptr<std::vector<std::shared_ptr<Part>>> dragParts);
void fillY() override;
void fillPyPx() override;
void passRootToSystem() override;
void assignEquationNumbers() override;

void preRun() override;
void askSystemToUpdate() override;
void postRun() override;

void setdragParts(std::shared_ptr<std::vector<std::shared_ptr<Part>>> _dragParts);

std::shared_ptr<std::vector<std::shared_ptr<Part>>> dragParts;
};
Expand Down
6 changes: 3 additions & 3 deletions OndselSolver/PosICNewtonRaphson.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -34,9 +34,9 @@ void PosICNewtonRaphson::run()
}
catch (const SingularMatrixError& ex) {
auto redundantEqnNos = ex.getRedundantEqnNos();
system->partsJointsMotionsLimitsDo([&](std::shared_ptr<Item> item) { item->removeRedundantConstraints(redundantEqnNos); });
system->partsJointsMotionsLimitsDo([&](std::shared_ptr<Item> item) { item->constraintsReport(); });
system->partsJointsMotionsLimitsDo([&](std::shared_ptr<Item> item) { item->setqsu(qsuOld); });
system->partsJointsMotionsDo([&](std::shared_ptr<Item> item) { item->removeRedundantConstraints(redundantEqnNos); });
system->partsJointsMotionsDo([&](std::shared_ptr<Item> item) { item->constraintsReport(); });
system->partsJointsMotionsDo([&](std::shared_ptr<Item> item) { item->setqsu(qsuOld); });
}
}
}
Expand Down
10 changes: 5 additions & 5 deletions OndselSolver/PosKineNewtonRaphson.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -18,22 +18,22 @@ using namespace MbD;
void PosKineNewtonRaphson::initializeGlobally()
{
SystemNewtonRaphson::initializeGlobally();
system->partsJointsMotionsLimitsDo([&](std::shared_ptr<Item> item) { item->fillqsu(x); });
system->partsJointsMotionsDo([&](std::shared_ptr<Item> item) { item->fillqsu(x); });
iterMax = system->iterMaxPosKine;
dxTol = system->errorTolPosKine;
}

void PosKineNewtonRaphson::fillPyPx()
{
pypx->zeroSelf();
system->partsJointsMotionsLimitsDo([&](std::shared_ptr<Item> item) {
system->partsJointsMotionsDo([&](std::shared_ptr<Item> item) {
item->fillPosKineJacob(pypx);
});
}

void PosKineNewtonRaphson::passRootToSystem()
{
system->partsJointsMotionsLimitsDo([&](std::shared_ptr<Item> item) { item->setqsu(x); });
system->partsJointsMotionsDo([&](std::shared_ptr<Item> item) { item->setqsu(x); });
}

void PosKineNewtonRaphson::assignEquationNumbers()
Expand Down Expand Up @@ -75,13 +75,13 @@ void PosKineNewtonRaphson::preRun()
{
std::string str = "MbD: Solving for kinematic position.";
system->logString(str);
system->partsJointsMotionsLimitsDo([](std::shared_ptr<Item> item) { item->prePosKine(); });
system->partsJointsMotionsDo([](std::shared_ptr<Item> item) { item->prePosKine(); });
}

void PosKineNewtonRaphson::fillY()
{
y->zeroSelf();
system->partsJointsMotionsLimitsDo([&](std::shared_ptr<Item> item) {
system->partsJointsMotionsDo([&](std::shared_ptr<Item> item) {
item->fillPosKineError(y);
//std::cout << item->name << *y << std::endl;
//noop();
Expand Down
6 changes: 3 additions & 3 deletions OndselSolver/PosNewtonRaphson.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,7 @@ using namespace MbD;

void PosNewtonRaphson::preRun()
{
system->partsJointsMotionsLimitsDo([&](std::shared_ptr<Item> item) { item->prePosIC(); });
system->partsJointsMotionsDo([&](std::shared_ptr<Item> item) { item->prePosIC(); });
}

void PosNewtonRaphson::incrementIterNo()
Expand All @@ -44,10 +44,10 @@ void PosNewtonRaphson::incrementIterNo()

void PosNewtonRaphson::askSystemToUpdate()
{
system->partsJointsMotionsLimitsDo([&](std::shared_ptr<Item> item) { item->postPosICIteration(); });
system->partsJointsMotionsDo([&](std::shared_ptr<Item> item) { item->postPosICIteration(); });
}

void PosNewtonRaphson::postRun()
{
system->partsJointsMotionsLimitsDo([&](std::shared_ptr<Item> item) { item->postPosIC(); });
system->partsJointsMotionsDo([&](std::shared_ptr<Item> item) { item->postPosIC(); });
}
Loading