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

Implement Limits

Browse Source
This commit is contained in:
Aik-Siong Koh
2024-04-02 14:30:25 -06:00
committed by PaddleStroke
parent 3a9e7cecab
commit ad4a563b42
184 changed files with 7716 additions and 6083 deletions
Download Patch File Download Diff File Expand all files Collapse all files

16
OndselSolver/VelICSolver.cpp
View File

@@ -55,24 +55,24 @@ void VelICSolver::run()
void VelICSolver::runBasic()
{
//| qsudotOld qsudotWeights qsudotlam |
system->partsJointsMotionsDo([](std::shared_ptr<Item> item) { item->preVelIC(); });
system->partsJointsMotionsLimitsDo([](std::shared_ptr<Item> item) { item->preVelIC(); });
this->assignEquationNumbers();
system->partsJointsMotionsDo([](std::shared_ptr<Item> item) { item->useEquationNumbers(); });
system->partsJointsMotionsLimitsDo([](std::shared_ptr<Item> item) { item->useEquationNumbers(); });
auto qsudotOld = std::make_shared<FullColumn<double>>(nqsu);
auto qsudotWeights = std::make_shared<DiagonalMatrix<double>>(nqsu);
errorVector = std::make_shared<FullColumn<double>>(n);
jacobian = std::make_shared<SparseMatrix<double>>(n, n);
system->partsJointsMotionsDo([&](std::shared_ptr<Item> item) { item->fillqsudot(qsudotOld); });
system->partsJointsMotionsDo([&](std::shared_ptr<Item> item) { item->fillqsudotWeights(qsudotWeights); });
system->partsJointsMotionsLimitsDo([&](std::shared_ptr<Item> item) { item->fillqsudot(qsudotOld); });
system->partsJointsMotionsLimitsDo([&](std::shared_ptr<Item> item) { item->fillqsudotWeights(qsudotWeights); });
errorVector->zeroSelf();
errorVector->atiplusFullColumn(0, qsudotWeights->timesFullColumn(qsudotOld));
system->partsJointsMotionsDo([&](std::shared_ptr<Item> item) { item->fillVelICError(errorVector); });
system->partsJointsMotionsLimitsDo([&](std::shared_ptr<Item> item) { item->fillVelICError(errorVector); });
jacobian->zeroSelf();
jacobian->atijplusDiagonalMatrix(0, 0, qsudotWeights);
system->partsJointsMotionsDo([&](std::shared_ptr<Item> item) { item->fillVelICJacob(jacobian); });
system->partsJointsMotionsLimitsDo([&](std::shared_ptr<Item> item) { item->fillVelICJacob(jacobian); });
matrixSolver = this->matrixSolverClassNew();
this->solveEquations();
auto& qsudotlam = this->x;
system->partsJointsMotionsDo([&](std::shared_ptr<Item> item) { item->setqsudotlam(qsudotlam); });
system->partsJointsMotionsDo([](std::shared_ptr<Item> item) { item->postVelIC(); });
system->partsJointsMotionsLimitsDo([&](std::shared_ptr<Item> item) { item->setqsudotlam(qsudotlam); });
system->partsJointsMotionsLimitsDo([](std::shared_ptr<Item> item) { item->postVelIC(); });
}