InverseDynamicsStepSolver Class Reference

#include <stepsol.h>

Inheritance diagram for InverseDynamicsStepSolver:

Collaboration diagram for InverseDynamicsStepSolver:

Public Member Functions
	InverseDynamicsStepSolver (const integer MaxIt, const doublereal dT, const doublereal dSolutionTol, const integer stp, const integer sts, const bool bmod_res_test)
	~InverseDynamicsStepSolver (void)
virtual void	EvalProd (doublereal Tau, const VectorHandler &f0, const VectorHandler &w, VectorHandler &z) const
virtual doublereal	TestScale (const NonlinearSolverTest *pTest, doublereal &dCoef) const
virtual doublereal	Advance (Solver *pS, const doublereal TStep, const doublereal dAlph, const StepChange StType, std::deque< MyVectorHandler * > &qX, std::deque< MyVectorHandler * > &qXPrime, MyVectorHandler *const pX, MyVectorHandler *const pXPrime, integer &EffIter, doublereal &Err, doublereal &SolErr)
virtual doublereal	Advance (InverseSolver *pS, const doublereal TStep, const StepChange StType, MyVectorHandler *const pX, MyVectorHandler *const pXPrime, MyVectorHandler *const pXPrimePrime, MyVectorHandler *const pLambda, integer &EffIter, doublereal &Err, doublereal &SolErr)
void	Residual (VectorHandler *pRes) const
void	Jacobian (MatrixHandler *pJac) const
void	Update (const VectorHandler *pSol) const
void	SetOrder (InverseDynamics::Order iOrder)
InverseDynamics::Order	GetOrder (void) const
bool	bJacobian (void) const
Public Member Functions inherited from StepIntegrator
	StepIntegrator (const integer MaxIt, const doublereal dT, const doublereal dSolutionTol, const integer stp, const integer sts)
virtual	~StepIntegrator (void)
void	SetDataManager (DataManager *pDatMan)
virtual integer	GetIntegratorNumPreviousStates (void) const
virtual integer	GetIntegratorNumUnknownStates (void) const
virtual integer	GetIntegratorMaxIters (void) const
virtual doublereal	GetIntegratorDTol (void) const
virtual doublereal	GetIntegratorDSolTol (void) const
virtual void	OutputTypes (const bool fpred)
virtual void	SetDriveHandler (const DriveHandler *pDH)
Public Member Functions inherited from NonlinearProblem
virtual	~NonlinearProblem (void)

Protected Attributes
VectorHandler *	pXCurr
VectorHandler *	pXPrimeCurr
VectorHandler *	pXPrimePrimeCurr
VectorHandler *	pLambdaCurr
bool	bModResTest
Protected Attributes inherited from StepIntegrator
DataManager *	pDM
const DataManager::DofVecType *	pDofs
bool	outputPred
integer	MaxIters
doublereal	dTol
doublereal	dSolTol
integer	steps
integer	unkstates

Private Attributes
MyVectorHandler	XTau
MyVectorHandler	SavedState
MyVectorHandler	SavedDerState
bool	bEvalProdCalledFirstTime
InverseDynamics::Order	iOrder
bool	m_bJacobian

Additional Inherited Members
Public Types inherited from StepIntegrator
enum	{ DIFFERENTIAL = 0, ALGEBRAIC = 1 }
enum	StepChange { NEWSTEP, REPEATSTEP }
Protected Member Functions inherited from StepIntegrator
template<class T >
void	UpdateLoop (const T *const t, void(T::*pUpd)(const int DCount, const DofOrder::Order Order, const VectorHandler *const pSol) const, const VectorHandler *const pSol=0) const

Detailed Description

Definition at line 666 of file stepsol.h.

Constructor & Destructor Documentation

InverseDynamicsStepSolver::InverseDynamicsStepSolver	(	const integer	MaxIt,
		const doublereal	dT,
		const doublereal	dSolutionTol,
		const integer	stp,
		const integer	sts,
		const bool	bmod_res_test
	)

Definition at line 1378 of file stepsol.cc.

: StepIntegrator(MaxIt, dT, dSolutionTol, stp, sts),
XTau(0),
SavedState(0),
SavedDerState(0),
bEvalProdCalledFirstTime(true),
iOrder(InverseDynamics::UNDEFINED),
m_bJacobian(true),
pXCurr(0),
pXPrimeCurr(0),
pXPrimePrimeCurr(0),
pLambdaCurr(0),
bModResTest(bmod_res_test)
{
        return;
}

InverseDynamicsStepSolver::~InverseDynamicsStepSolver

(

void

)

Definition at line 1400 of file stepsol.cc.

References NO_OP.

{
        NO_OP;
}

Member Function Documentation

virtual doublereal InverseDynamicsStepSolver::Advance ( Solver *  pS, const doublereal  TStep, const doublereal  dAlph, const StepChange  StType, std::deque< MyVectorHandler * > &  qX, std::deque< MyVectorHandler * > &  qXPrime, MyVectorHandler *const  pX, MyVectorHandler *const  pXPrime, integer &  EffIter, doublereal &  Err, doublereal &  SolErr  )

inlinevirtual

Implements StepIntegrator.

Definition at line 708 of file stepsol.h.

References MBDYN_EXCEPT_ARGS.

        {
                silent_cerr("InverseDynamicsStepSolver::Advance()");
                throw ErrGeneric(MBDYN_EXCEPT_ARGS);
        };

doublereal InverseDynamicsStepSolver::Advance ( InverseSolver *  pS, const doublereal  TStep, const StepChange  StType, MyVectorHandler *const  pX, MyVectorHandler *const  pXPrime, MyVectorHandler *const  pXPrimePrime, MyVectorHandler *const  pLambda, integer &  EffIter, doublereal &  Err, doublereal &  SolErr  )

virtual

Definition at line 1528 of file stepsol.cc.

References InverseDynamics::ACCELERATION, ASSERT, bJacobian(), StepIntegrator::dSolTol, StepIntegrator::dTol, InverseDynamics::FULLY_ACTUATED_COLLOCATED, DataManager::GetSolver(), DataManager::IDAfterConvergence(), InverseDynamics::INVERSE_DYNAMICS, Jacobian(), DataManager::LinkToSolution(), SolutionManager::MatrReset(), StepIntegrator::MaxIters, SolverDiagnostics::outputJac(), SolverDiagnostics::outputRes(), SolverDiagnostics::outputSol(), StepIntegrator::pDM, Solver::pGetNonlinearSolver(), Solver::pGetSolutionManager(), pLambdaCurr, SolutionManager::pMatHdl(), InverseDynamics::POSITION, SolutionManager::pResHdl(), Solver::PrintResidual(), Solver::PrintSolution(), SolutionManager::pSolHdl(), pXCurr, pXPrimeCurr, pXPrimePrimeCurr, VectorHandler::Reset(), Residual(), SetOrder(), SolutionManager::Solve(), NonlinearSolver::Solve(), SolutionManager::SolveT(), Update(), and InverseDynamics::VELOCITY.

{
        ASSERT(pDM != NULL);
        pXCurr = pX;
        pXPrimeCurr = pXPrime;
        pXPrimePrimeCurr = pXPrimePrime;
        pLambdaCurr = pLambda;

        pDM->LinkToSolution(*pXCurr, *pXPrimeCurr, *pXPrimePrimeCurr, *pLambdaCurr);

        Err = 0.;
        NonlinearSolver *pNLSolver = pS->pGetNonlinearSolver();

        /* Position */
        SetOrder(InverseDynamics::POSITION);

        /* Setting iOrder = 0, the residual is called only
         * for constraints, on positions */
        pNLSolver->Solve(this, pS, MaxIters, dTol,
                        EffIter, Err, dSolTol, SolErr);

        SolutionManager *pSM = pS->pGetSolutionManager();
        MatrixHandler *pMat = pSM->pMatHdl();
        VectorHandler *pRes = pSM->pResHdl();
        VectorHandler *pSol = pSM->pSolHdl();

        /* use position */
        // Update(pSol);

        /* Velocity */
        SetOrder(InverseDynamics::VELOCITY);

        pRes->Reset();
        pSol->Reset();
        /* there's no need to check changes in
         * equation structure... it is already
         * performed by NonlinearSolver->Solve() */
        Residual(pRes);

        if (pS->outputRes()) {
                silent_cout("Residual(velocity):" << std::endl);
                pS->PrintResidual(*pRes, 0);
        }

        if (bJacobian()) {
                pSM->MatrReset();
                Jacobian(pMat);

                if (pS->outputJac()) {
                        silent_cout("Jacobian(velocity):" << std::endl << *pMat);
                }

                EffIter++;
        }

        pSM->Solve();

        if (pS->outputSol()) {
                silent_cout("Solution(velocity):" << std::endl);
                pS->PrintSolution(*pSol, 0);
        }

        /* use velocity */
        Update(pSol);

        // TODO: if UNDERDETERMINED_UNDERACTUATED_COLLOCATED,
        // InverseDynamics::ACCELERATION and InverseDynamics::INVERSE_DYNAMICS
        // are solved together

        /* Acceleration */
        SetOrder(InverseDynamics::ACCELERATION);

        pRes->Reset();
        pSol->Reset();
        Residual(pRes);

        if (pS->outputRes()) {
                silent_cout("Residual(acceleration):" << std::endl);
                pS->PrintResidual(*pRes, 0);
        }

        if (bJacobian()) {
                pSM->MatrReset();
                Jacobian(pMat);

                if (pS->outputJac()) {
                        silent_cout("Jacobian(acceleration):" << std::endl << *pMat);
                }

                EffIter++;
        }

        pSM->Solve();

        if (pS->outputSol()) {
                silent_cout("Solution(acceleration):" << std::endl);
                pS->PrintSolution(*pSol, 0);
        }

        Update(pSol);

        /* Forces */
        SetOrder(InverseDynamics::INVERSE_DYNAMICS);

        pRes->Reset();
        pSol->Reset();
        Residual(pRes);

        if (pS->outputRes()) {
                silent_cout("Residual(inverseDynamics):" << std::endl);
                pS->PrintResidual(*pRes, 0);
        }

        if (bJacobian()) {
                pSM->MatrReset();
                Jacobian(pMat);

                if (pS->outputJac()) {
                        silent_cout("Jacobian(inverseDynamics):" << std::endl << *pMat);
                }

                EffIter++;
        }

        switch (dynamic_cast<const InverseSolver *>(pDM->GetSolver())->GetProblemType()) {
        case InverseDynamics::FULLY_ACTUATED_COLLOCATED:
                pSM->SolveT();
                break;

        default:
                pSM->Solve();
        }

        if (pS->outputSol()) {
                silent_cout("Solution(inverseDynamics):" << std::endl);
                pS->PrintSolution(*pSol, 0);
        }

        Update(pSol);

        pDM->IDAfterConvergence();

        return Err;
}

Here is the call graph for this function:

bool InverseDynamicsStepSolver::bJacobian

(

void

)

const

Definition at line 1522 of file stepsol.cc.

References m_bJacobian.

Referenced by Advance().

{
        return m_bJacobian;
}

void InverseDynamicsStepSolver::EvalProd ( doublereal  Tau, const VectorHandler &  f0, const VectorHandler &  w, VectorHandler &  z  ) const

virtual

Implements NonlinearProblem.

Definition at line 1406 of file stepsol.cc.

References ASSERT, bEvalProdCalledFirstTime, copysign(), f0, grad::fabs(), VectorHandler::iGetSize(), VectorHandler::InnerProd(), VectorHandler::Norm(), StepIntegrator::pDM, pXCurr, pXPrimeCurr, VectorHandler::Reset(), MyVectorHandler::Reset(), Residual(), MyVectorHandler::Resize(), SavedDerState, SavedState, VectorHandler::ScalarMul(), MyVectorHandler::ScalarMul(), DataManager::Update(), Update(), and XTau.

{
        /* matrix-free product
         *
         * J(XCurr) * w = -||w|| * (Res(XCurr + sigma * Tau * w/||w||) - f0) / (sigma * Tau)
         *
         */
        if (bEvalProdCalledFirstTime) {
                XTau.Resize(w.iGetSize());
                SavedState.Resize(w.iGetSize());
                SavedDerState.Resize(w.iGetSize());
                bEvalProdCalledFirstTime = false;
        }

        SavedState = *pXCurr;
        SavedDerState = *pXPrimeCurr;

        /* if w = 0; J * w = 0 */
        ASSERT(pDM != NULL);

        doublereal nw = w.Norm();
        if (nw < std::numeric_limits<doublereal>::epsilon()) {
                z.Reset();
                return;
        }
        doublereal sigma = pXCurr->InnerProd(w);
        sigma /=  nw;
        if (fabs(sigma) > std::numeric_limits<doublereal>::epsilon()) {
                doublereal xx = (fabs( sigma) <= 1.) ? 1. : fabs(sigma);
                Tau = copysign(Tau*xx, sigma);
        }
        Tau /= nw;
#ifdef DEBUG_ITERATIVE
        std::cout << "Tau " << Tau << std::endl;
#endif /* DEBUG_ITERATIVE */

        XTau.Reset();
        z.Reset();
        XTau.ScalarMul(w, Tau);
        Update(&XTau);
#ifdef  USE_EXTERNAL
        External::SendFreeze();
#endif /* USE_EXTERNAL */
        /* deal with throwing elements: do not honor their requests while perfoming matrix free update */
        try {
                Residual(&z);
        }
        catch (DataManager::ChangedEquationStructure) {
        }
        XTau.ScalarMul(XTau, -1.);

        /* riporta tutto nelle condizioni inziali */
#if 0
        Update(&XTau);
#endif
        *pXCurr = SavedState;
        *pXPrimeCurr = SavedDerState;
        pDM->Update();
        z -= f0;
        z.ScalarMul(z, -1./Tau);
}

Here is the call graph for this function:

InverseDynamics::Order InverseDynamicsStepSolver::GetOrder

(

void

)

const

Definition at line 1516 of file stepsol.cc.

References iOrder.

Referenced by DataManager::AssConstrJac().

{
        return iOrder;
}

void InverseDynamicsStepSolver::Jacobian ( MatrixHandler *  pJac ) const

virtual

Implements NonlinearProblem.

Definition at line 1698 of file stepsol.cc.

References DataManager::AssConstrJac(), ASSERT, and StepIntegrator::pDM.

Referenced by Advance().

{
        ASSERT(pDM != NULL);
        pDM->AssConstrJac(*pJac);
}

Here is the call graph for this function:

void InverseDynamicsStepSolver::Residual ( VectorHandler *  pRes ) const

virtual

Implements NonlinearProblem.

Definition at line 1683 of file stepsol.cc.

References DataManager::AssConstrRes(), ASSERT, DataManager::AssRes(), InverseDynamics::INVERSE_DYNAMICS, iOrder, and StepIntegrator::pDM.

Referenced by Advance(), and EvalProd().

{
        ASSERT(pDM != NULL);
        switch (iOrder) {
        case InverseDynamics::INVERSE_DYNAMICS:
                pDM->AssRes(*pRes);
                break;

        default:
                pDM->AssConstrRes(*pRes, iOrder);
                break;
        }
}

Here is the call graph for this function:

void InverseDynamicsStepSolver::SetOrder

(

InverseDynamics::Order

iOrder

)

Definition at line 1510 of file stepsol.cc.

References iOrder.

Referenced by Advance().

{
        this->iOrder = iOrder;
}

doublereal InverseDynamicsStepSolver::TestScale ( const NonlinearSolverTest *  pTest, doublereal &  dCoef  ) const

virtual

Implements NonlinearProblem.

Definition at line 1471 of file stepsol.cc.

References ASSERT, bModResTest, DofOrder::DIFFERENTIAL, NonlinearSolverTest::dScaleCoef(), VectorHandler::iGetSize(), StepIntegrator::pDofs, and pXPrimeCurr.

{
        dCoef = 1.;

        if (bModResTest) {
#ifdef USE_MPI
#warning "InverseDynamicsStepSolver::TestScale() not available with Schur solution"
#endif /* USE_MPI */

                doublereal dXPr = 0.;

                DataManager::DofIterator_const CurrDof = pDofs->begin();

                for (int iCntp1 = 1; iCntp1 <= pXPrimeCurr->iGetSize();
                        iCntp1++, ++CurrDof)
                {
                        ASSERT(CurrDof != pDofs->end());

                        if (CurrDof->Order == DofOrder::DIFFERENTIAL) {
                                doublereal d = pXPrimeCurr->operator()(iCntp1);
                                doublereal d2 = d*d;

                                doublereal ds = pTest->dScaleCoef(iCntp1);
                                doublereal ds2 = ds*ds;
                                d2 *= ds2;

                                dXPr += d2;
                        }
                        /* else if ALGEBRAIC: non aggiunge nulla */
                }

                return 1./(1. + dXPr);

        } else {
                return 1.;
        }
}

Here is the call graph for this function:

void InverseDynamicsStepSolver::Update ( const VectorHandler *  pSol ) const

virtual

Implements NonlinearProblem.

Definition at line 1705 of file stepsol.cc.

References InverseDynamics::ACCELERATION, ASSERT, DEBUGCOUTFNAME, InverseDynamics::FULLY_ACTUATED_COLLOCATED, InverseDynamics::FULLY_ACTUATED_NON_COLLOCATED, DataManager::GetSolver(), InverseDynamics::INVERSE_DYNAMICS, iOrder, m_bJacobian, MBDYN_EXCEPT_ARGS, StepIntegrator::pDM, pLambdaCurr, InverseDynamics::POSITION, pXCurr, pXPrimeCurr, pXPrimePrimeCurr, InverseDynamics::UNDERDETERMINED_FULLY_ACTUATED, InverseDynamics::UNDERDETERMINED_UNDERACTUATED_COLLOCATED, DataManager::Update(), and InverseDynamics::VELOCITY.

Referenced by Advance(), and EvalProd().

{
        DEBUGCOUTFNAME("InverseDynamicsStepSolver::Update");
        ASSERT(pDM != NULL);

        switch (iOrder) {
        case InverseDynamics::POSITION:
                *pXCurr += *pSol;
                break;

        case InverseDynamics::VELOCITY:
                *pXPrimeCurr = *pSol;
                break;

        case InverseDynamics::ACCELERATION:
                *pXPrimePrimeCurr = *pSol;
                break;

        case InverseDynamics::INVERSE_DYNAMICS:
                *pLambdaCurr = *pSol;
                break;

        default:
                ASSERT(0);
                throw ErrGeneric(MBDYN_EXCEPT_ARGS);
        }

        pDM->Update(iOrder);

        // prepare m_bJacobian for next phase
        switch (dynamic_cast<const InverseSolver *>(pDM->GetSolver())->GetProblemType()) {
        case InverseDynamics::FULLY_ACTUATED_COLLOCATED:
                switch (iOrder) {
                case InverseDynamics::INVERSE_DYNAMICS:
                case InverseDynamics::POSITION:
                        m_bJacobian = true;
                        break;

                case InverseDynamics::VELOCITY:
                case InverseDynamics::ACCELERATION:
                        m_bJacobian = false;
                        break;

                default:
                        break;
                }
                break;

        case InverseDynamics::FULLY_ACTUATED_NON_COLLOCATED:
                switch (iOrder) {
                case InverseDynamics::INVERSE_DYNAMICS:
                case InverseDynamics::POSITION:
                case InverseDynamics::ACCELERATION:
                        m_bJacobian = true;
                        break;

                case InverseDynamics::VELOCITY:
                        m_bJacobian = false;
                        break;

                default:
                        break;
                }
                break;

        case InverseDynamics::UNDERDETERMINED_UNDERACTUATED_COLLOCATED:
                // TODO
                throw ErrGeneric(MBDYN_EXCEPT_ARGS);

        case InverseDynamics::UNDERDETERMINED_FULLY_ACTUATED:
                m_bJacobian = true;
                break;

        default:
                break;
        }
}

Here is the call graph for this function:

Member Data Documentation

bool InverseDynamicsStepSolver::bEvalProdCalledFirstTime

mutableprivate

Definition at line 677 of file stepsol.h.

Referenced by EvalProd().

bool InverseDynamicsStepSolver::bModResTest

protected

Definition at line 687 of file stepsol.h.

Referenced by TestScale().

InverseDynamics::Order InverseDynamicsStepSolver::iOrder

private

Definition at line 679 of file stepsol.h.

Referenced by GetOrder(), Residual(), SetOrder(), and Update().

bool InverseDynamicsStepSolver::m_bJacobian

mutableprivate

Definition at line 680 of file stepsol.h.

Referenced by bJacobian(), and Update().

VectorHandler* InverseDynamicsStepSolver::pLambdaCurr

protected

Definition at line 686 of file stepsol.h.

Referenced by Advance(), and Update().

VectorHandler* InverseDynamicsStepSolver::pXCurr

protected

Definition at line 683 of file stepsol.h.

Referenced by Advance(), EvalProd(), and Update().

VectorHandler* InverseDynamicsStepSolver::pXPrimeCurr

protected

Definition at line 684 of file stepsol.h.

Referenced by Advance(), EvalProd(), TestScale(), and Update().

VectorHandler* InverseDynamicsStepSolver::pXPrimePrimeCurr

protected

Definition at line 685 of file stepsol.h.

Referenced by Advance(), and Update().

MyVectorHandler InverseDynamicsStepSolver::SavedDerState

mutableprivate

Definition at line 676 of file stepsol.h.

Referenced by EvalProd().

MyVectorHandler InverseDynamicsStepSolver::SavedState

mutableprivate

Definition at line 673 of file stepsol.h.

Referenced by EvalProd().

MyVectorHandler InverseDynamicsStepSolver::XTau

mutableprivate

Definition at line 672 of file stepsol.h.

Referenced by EvalProd().

---

The documentation for this class was generated from the following files:

• stepsol.h
• stepsol.cc