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

runPosIC, VelIC, AccIC numerically correct

Browse Source
This commit is contained in:
Aik-Siong Koh
2023-06-24 23:08:29 -06:00
parent 371b13a9e0
commit c30ee64b89
110 changed files with 2171 additions and 129 deletions
Download Patch File Download Diff File Expand all files Collapse all files

22
MbDCode/TranslationConstraintIqcJc.cpp
View File

@@ -56,3 +56,25 @@ void MbD::TranslationConstraintIqcJc::fillPosKineJacob(SpMatDsptr mat)
mat->atijplusFullRow(iG, iqXI, pGpXI);
mat->atijplusFullRow(iG, iqEI, pGpEI);
}
void MbD::TranslationConstraintIqcJc::fillVelICJacob(SpMatDsptr mat)
{
mat->atijplusFullRow(iG, iqXI, pGpXI);
mat->atijplusFullColumn(iqXI, iG, pGpXI->transpose());
mat->atijplusFullRow(iG, iqEI, pGpEI);
mat->atijplusFullColumn(iqEI, iG, pGpEI->transpose());
}
void MbD::TranslationConstraintIqcJc::fillAccICIterError(FColDsptr col)
{
col->atiplusFullVectortimes(iqXI, pGpXI, lam);
col->atiplusFullVectortimes(iqEI, pGpEI, lam);
auto efrmIqc = std::static_pointer_cast<EndFrameqc>(frmI);
auto qXdotI = efrmIqc->qXdot();
auto qEdotI = efrmIqc->qEdot();
auto sum = pGpXI->timesFullColumn(efrmIqc->qXddot());
sum += pGpEI->timesFullColumn(efrmIqc->qEddot());
sum += 2.0 * (qXdotI->transposeTimesFullColumn(ppGpXIpEI->timesFullColumn(qEdotI)));
sum += qEdotI->transposeTimesFullColumn(ppGpEIpEI->timesFullColumn(qEdotI));
col->atiplusNumber(iG, sum);
}