diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCFunction.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCFunction.cpp index 0a5adfdb0b..5ef7dbdc4e 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCFunction.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCFunction.cpp @@ -241,7 +241,8 @@ VectorDM EndpointConstraint::eval(const VectorDM& args) const { return out; } -casadi::Sparsity MultibodySystemExplicit::get_sparsity_out( +template +casadi::Sparsity MultibodySystemExplicit::get_sparsity_out( casadi_int i) { if (i == 0) { return casadi::Sparsity::dense( @@ -253,13 +254,19 @@ casadi::Sparsity MultibodySystemExplicit::get_sparsity_out( return casadi::Sparsity::dense( m_casProblem->getNumAuxiliaryResidualEquations(), 1); } else if (i == 3) { - return casadi::Sparsity::dense(m_casProblem->getNumUDotErr(), 1); + if (calcKCErr) { + return casadi::Sparsity::dense( + m_casProblem->getNumKinematicConstraintEquations(), 1); + } else { + return casadi::Sparsity(0, 0); + } } else { return casadi::Sparsity(0, 0); } } -VectorDM MultibodySystemExplicit::eval(const VectorDM& args) const { +template +VectorDM MultibodySystemExplicit::eval(const VectorDM& args) const { Problem::ContinuousInput input{args.at(0).scalar(), args.at(1), args.at(2), args.at(3), args.at(4), args.at(5)}; VectorDM out((int)n_out()); @@ -268,11 +275,16 @@ VectorDM MultibodySystemExplicit::eval(const VectorDM& args) const { } Problem::MultibodySystemExplicitOutput output{out[0], out[1], out[2], out[3]}; - m_casProblem->calcMultibodySystemExplicit(input, output); + m_casProblem->calcMultibodySystemExplicit(input, calcKCErr, output); return out; } -casadi::Sparsity MultibodySystemImplicit::get_sparsity_out(casadi_int i) { +template class CasOC::MultibodySystemExplicit; +template class CasOC::MultibodySystemExplicit; + +template +casadi::Sparsity MultibodySystemImplicit::get_sparsity_out( + casadi_int i) { if (i == 0) { return casadi::Sparsity::dense( m_casProblem->getNumMultibodyDynamicsEquations(), 1); @@ -283,13 +295,19 @@ casadi::Sparsity MultibodySystemImplicit::get_sparsity_out(casadi_int i) { return casadi::Sparsity::dense( m_casProblem->getNumAuxiliaryResidualEquations(), 1); } else if (i == 3) { - return casadi::Sparsity::dense(m_casProblem->getNumUDotErr(), 1); + if (calcKCErr) { + return casadi::Sparsity::dense( + m_casProblem->getNumKinematicConstraintEquations(), 1); + } else { + return casadi::Sparsity(0, 0); + } } else { return casadi::Sparsity(0, 0); } } -VectorDM MultibodySystemImplicit::eval(const VectorDM& args) const { +template +VectorDM MultibodySystemImplicit::eval(const VectorDM& args) const { Problem::ContinuousInput input{args.at(0).scalar(), args.at(1), args.at(2), args.at(3), args.at(4), args.at(5)}; VectorDM out((int)n_out()); @@ -299,53 +317,12 @@ VectorDM MultibodySystemImplicit::eval(const VectorDM& args) const { Problem::MultibodySystemImplicitOutput output{out[0], out[1], out[2], out[3]}; - m_casProblem->calcMultibodySystemImplicit(input, output); - return out; -} - -casadi::Sparsity KinematicConstraints::get_sparsity_in(casadi_int i) { - if (i == 0) { - return casadi::Sparsity::dense(1, 1); - } else if (i == 1) { - return casadi::Sparsity::dense(m_casProblem->getNumMultibodyStates(), 1); - } else if (i == 2) { - return casadi::Sparsity::dense(m_casProblem->getNumParameters(), 1); - } else { - return casadi::Sparsity(0, 0); - } -} - -casadi::Sparsity KinematicConstraints::get_sparsity_out(casadi_int i) { - if (i == 0) { - return casadi::Sparsity::dense( - m_casProblem->getNumQErr() + m_casProblem->getNumUErr(), 1); - } else { - return casadi::Sparsity(0, 0); - } -} - -VectorDM KinematicConstraints::eval(const VectorDM& args) const { - VectorDM out{casadi::DM(sparsity_out(0))}; - m_casProblem->calcKinematicConstraintErrors( - args.at(0).scalar(), args.at(1), args.at(2), out[0]); + m_casProblem->calcMultibodySystemImplicit(input, calcKCErr, output); return out; } -casadi::DM KinematicConstraints::getSubsetPoint( - const VariablesDM& fullPoint, casadi_int i) const { - int itime = 0; - using casadi::Slice; - const int NMBS = m_casProblem->getNumMultibodyStates(); - if (i == 0) { - return fullPoint.at(initial_time)(itime); - } else if (i == 1) { - return fullPoint.at(states)(Slice(0, NMBS), itime); - } else if (i == 3) { - return fullPoint.at(parameters)(Slice(), itime); - } else { - return casadi::DM(); - } -} +template class CasOC::MultibodySystemImplicit; +template class CasOC::MultibodySystemImplicit; casadi::Sparsity VelocityCorrection::get_sparsity_in(casadi_int i) { if (i == 0) { diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCFunction.h b/OpenSim/Moco/MocoCasADiSolver/CasOCFunction.h index 59afe6915c..f459469b5a 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCFunction.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCFunction.h @@ -257,6 +257,7 @@ class EndpointConstraint : public Endpoint { /// This function should compute forward dynamics (explicit multibody dynamics), /// auxiliary dynamics (explicit and implicit), and the errors for the /// acceleration-level kinematic constraints. +template class MultibodySystemExplicit : public Function { public: casadi_int get_n_out() override final { return 4; } @@ -265,7 +266,7 @@ class MultibodySystemExplicit : public Function { case 0: return "multibody_derivatives"; case 1: return "auxiliary_derivatives"; case 2: return "auxiliary_residuals"; - case 3: return "kinematic_constraint_udot_errors"; + case 3: return "kinematic_constraint_errors"; default: OPENSIM_THROW(OpenSim::Exception, "Internal error."); } } @@ -277,6 +278,7 @@ class MultibodySystemExplicit : public Function { /// This function should compute implicit multibody dynamics, auxiliary dynamics /// (explicit and implicit), and the errors for the acceleration-level kinematic /// constraints. +template class MultibodySystemImplicit : public Function { casadi_int get_n_out() override final { return 4; } std::string get_name_out(casadi_int i) override final { @@ -284,7 +286,7 @@ class MultibodySystemImplicit : public Function { case 0: return "multibody_residuals"; case 1: return "auxiliary_derivatives"; case 2: return "auxiliary_residuals"; - case 3: return "kinematic_constraint_udot_errors"; + case 3: return "kinematic_constraint_errors"; default: OPENSIM_THROW(OpenSim::Exception, "Internal error."); } } @@ -293,33 +295,6 @@ class MultibodySystemImplicit : public Function { VectorDM eval(const VectorDM& args) const override; }; -/// This function should compute the errors for the position- and velocity-level -/// kinematic constraints. -class KinematicConstraints : public Function { -public: - casadi_int get_n_in() override final { return 3; } - casadi_int get_n_out() override final { return 1; } - std::string get_name_in(casadi_int i) override final { - switch (i) { - case 0: return "time"; - case 1: return "multibody_states"; - case 2: return "parameters"; - default: OPENSIM_THROW(OpenSim::Exception, "Internal error."); - } - } - std::string get_name_out(casadi_int i) override final { - switch (i) { - case 0: return "kinematic_constraint_errors"; - default: OPENSIM_THROW(OpenSim::Exception, "Internal error."); - } - } - casadi::Sparsity get_sparsity_in(casadi_int i) override final; - casadi::Sparsity get_sparsity_out(casadi_int i) override final; - VectorDM eval(const VectorDM& args) const override; - casadi::DM getSubsetPoint(const VariablesDM& fullPoint, - casadi_int i) const override; -}; - /// This function should compute a velocity correction term to make feasible /// problems that enforce kinematic constraints and their derivatives. class VelocityCorrection : public Function { diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCProblem.h b/OpenSim/Moco/MocoCasADiSolver/CasOCProblem.h index f118928540..364d24966f 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCProblem.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCProblem.h @@ -154,13 +154,13 @@ class Problem { casadi::DM& multibody_derivatives; casadi::DM& auxiliary_derivatives; casadi::DM& auxiliary_residuals; - casadi::DM& kinematic_constraint_udot_errors; + casadi::DM& kinematic_constraint_errors; }; struct MultibodySystemImplicitOutput { casadi::DM& multibody_residuals; casadi::DM& auxiliary_derivatives; casadi::DM& auxiliary_residuals; - casadi::DM& kinematic_constraint_udot_errors; + casadi::DM& kinematic_constraint_errors; }; protected: @@ -319,12 +319,9 @@ class Problem { /// - first derivative of velocity-level constraints /// - acceleration-level constraints virtual void calcMultibodySystemExplicit(const ContinuousInput& input, - MultibodySystemExplicitOutput& output) const = 0; + bool calcKCErrors, MultibodySystemExplicitOutput& output) const = 0; virtual void calcMultibodySystemImplicit(const ContinuousInput& input, - MultibodySystemImplicitOutput& output) const = 0; - virtual void calcKinematicConstraintErrors(const double& time, - const casadi::DM& multibody_states, const casadi::DM& parameters, - casadi::DM& kinematic_constraint_errors) const = 0; + bool calcKCErrors, MultibodySystemImplicitOutput& output) const = 0; virtual void calcVelocityCorrection(const double& time, const casadi::DM& multibody_states, const casadi::DM& slacks, const casadi::DM& parameters, @@ -465,26 +462,35 @@ class Problem { if (m_dynamicsMode == "implicit") { // Construct an implicit multibody system. mutThis->m_implicitMultibodyFunc = - OpenSim::make_unique(); + OpenSim::make_unique>(); mutThis->m_implicitMultibodyFunc->constructFunction(this, "implicit_multibody_system", finiteDiffScheme, pointsForSparsityDetection); + + // Construct an implicit multibody system ignoring kinematic + // constraints. + mutThis->m_implicitMultibodyFuncIgnoringConstraints = + OpenSim::make_unique>(); + mutThis->m_implicitMultibodyFuncIgnoringConstraints + ->constructFunction(this, + "implicit_multibody_system_ignoring_constraints", + finiteDiffScheme, pointsForSparsityDetection); } else { // Construct an explicit multibody system. mutThis->m_multibodyFunc = - OpenSim::make_unique(); + OpenSim::make_unique>(); mutThis->m_multibodyFunc->constructFunction(this, "explicit_multibody_system", finiteDiffScheme, pointsForSparsityDetection); + + mutThis->m_multibodyFuncIgnoringConstraints = + OpenSim::make_unique>(); + mutThis->m_multibodyFuncIgnoringConstraints->constructFunction(this, + "multibody_system_ignoring_constraints", finiteDiffScheme, + pointsForSparsityDetection); } if (getNumKinematicConstraintEquations()) { - mutThis->m_kinematicConstraintsFunc = - OpenSim::make_unique(); - mutThis->m_kinematicConstraintsFunc->constructFunction(this, - "kinematic_constraints", finiteDiffScheme, - pointsForSparsityDetection); - if (getEnforceConstraintDerivatives()) { if (isKinematicConstraintMethodBordalba2023()) { mutThis->m_stateProjectionFunc = @@ -637,15 +643,17 @@ class Problem { const std::vector& getPathConstraintInfos() const { return m_pathInfos; } - /// Get a function to the full explicit multibody system (i.e., including - /// acceleration-level kinematic constraint errors). + /// Get a function to the full multibody system (i.e. including kinematic + /// constraints errors). const casadi::Function& getMultibodySystem() const { return *m_multibodyFunc; } - /// Get a function to compute the position- and velocity-level kinematic - /// constraint errors. - const casadi::Function& getKinematicConstraints() const { - return *m_kinematicConstraintsFunc; + /// Get a function to the multibody system that does *not* compute kinematic + /// constraint errors (if they exist). This may be necessary for computing + /// state derivatives at grid points where we do not want to enforce + /// kinematic constraint errors. + const casadi::Function& getMultibodySystemIgnoringConstraints() const { + return *m_multibodyFuncIgnoringConstraints; } /// Get a function to compute the velocity correction to qdot when enforcing /// kinematic constraints and their derivatives using the method by @@ -665,6 +673,10 @@ class Problem { const casadi::Function& getImplicitMultibodySystem() const { return *m_implicitMultibodyFunc; } + const casadi::Function& + getImplicitMultibodySystemIgnoringConstraints() const { + return *m_implicitMultibodyFuncIgnoringConstraints; + } /// @} private: @@ -700,9 +712,12 @@ class Problem { std::vector m_costInfos; std::vector m_endpointConstraintInfos; std::vector m_pathInfos; - std::unique_ptr m_multibodyFunc; - std::unique_ptr m_implicitMultibodyFunc; - std::unique_ptr m_kinematicConstraintsFunc; + std::unique_ptr> m_multibodyFunc; + std::unique_ptr> + m_multibodyFuncIgnoringConstraints; + std::unique_ptr> m_implicitMultibodyFunc; + std::unique_ptr> + m_implicitMultibodyFuncIgnoringConstraints; std::unique_ptr m_velocityCorrectionFunc; std::unique_ptr m_stateProjectionFunc; }; diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp index 8ead42003a..25c5c38c31 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp @@ -319,11 +319,12 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, initializeScalingDM(m_shift); initializeScalingDM(m_scale); - Bounds infBounds = {-casadi::inf, casadi::inf}; + Bounds timeBounds = {m_problem.getTimeInitialBounds().lower, + m_problem.getTimeFinalBounds().upper}; setVariableBounds(initial_time, 0, 0, m_problem.getTimeInitialBounds()); setVariableBounds(final_time, 0, 0, m_problem.getTimeFinalBounds()); - setVariableBounds(initial_time, 0, Slice(1, m_numMeshPoints), infBounds); - setVariableBounds(final_time, 0, Slice(1, m_numMeshPoints), infBounds); + setVariableBounds(initial_time, 0, Slice(1, m_numMeshPoints), timeBounds); + setVariableBounds(final_time, 0, Slice(1, m_numMeshPoints), timeBounds); setVariableScaling(initial_time, 0, Slice(), m_problem.getTimeInitialBounds()); @@ -526,21 +527,31 @@ void Transcription::transcribe() { int nqerr = m_problem.getNumQErr(); int nuerr = m_problem.getNumUErr(); int nudoterr = m_problem.getNumUDotErr(); + int nkc = m_problem.getNumKinematicConstraintEquations(); const auto& kcBounds = m_problem.getKinematicConstraintBounds(); - m_constraints.kinematic = MX(casadi::Sparsity::dense( - nqerr + nuerr, m_numMeshPoints)); - m_constraintsLowerBounds.kinematic = casadi::DM::repmat( - kcBounds.lower, nqerr + nuerr, m_numMeshPoints); - m_constraintsUpperBounds.kinematic = casadi::DM::repmat( - kcBounds.upper, nqerr + nuerr, m_numMeshPoints); - - m_constraints.kinematic_udoterr = MX(casadi::Sparsity::dense( - nudoterr, m_numUDotErrorPoints)); - m_constraintsLowerBounds.kinematic_udoterr = casadi::DM::repmat( - kcBounds.lower, nudoterr, m_numUDotErrorPoints); - m_constraintsUpperBounds.kinematic_udoterr = casadi::DM::repmat( - kcBounds.upper, nudoterr, m_numUDotErrorPoints); + if (m_problem.isKinematicConstraintMethodBordalba2023()) { + m_constraints.kinematic = MX(casadi::Sparsity::dense( + nqerr + nuerr, m_numMeshPoints)); + m_constraintsLowerBounds.kinematic = casadi::DM::repmat( + kcBounds.lower, nqerr + nuerr, m_numMeshPoints); + m_constraintsUpperBounds.kinematic = casadi::DM::repmat( + kcBounds.upper, nqerr + nuerr, m_numMeshPoints); + + m_constraints.kinematic_udoterr = MX(casadi::Sparsity::dense( + nudoterr, m_numUDotErrorPoints)); + m_constraintsLowerBounds.kinematic_udoterr = casadi::DM::repmat( + kcBounds.lower, nudoterr, m_numUDotErrorPoints); + m_constraintsUpperBounds.kinematic_udoterr = casadi::DM::repmat( + kcBounds.upper, nudoterr, m_numUDotErrorPoints); + } else { + m_constraints.kinematic = MX(casadi::Sparsity::dense( + nkc, m_numMeshPoints)); + m_constraintsLowerBounds.kinematic = casadi::DM::repmat( + kcBounds.lower, nkc, m_numMeshPoints); + m_constraintsUpperBounds.kinematic = casadi::DM::repmat( + kcBounds.upper, nkc, m_numMeshPoints); + } // Initialize memory for projection constraints. // --------------------------------------------- @@ -563,10 +574,11 @@ void Transcription::transcribe() { m_xdot_projection(Slice(0, NQ), Slice()) = u_projection; } - // kinematic constraints - // --------------------- + // projection constraints + // ---------------------- if (m_problem.getNumKinematicConstraintEquations() && - !m_problem.isPrescribedKinematics()) { + !m_problem.isPrescribedKinematics() && + m_problem.getEnforceConstraintDerivatives()) { OPENSIM_THROW_IF(!m_problem.isKinematicConstraintMethodBordalba2023() && m_solver.getTranscriptionScheme() != "hermite-simpson", @@ -576,17 +588,10 @@ void Transcription::transcribe() { "but the '{}' scheme was selected.", m_solver.getTranscriptionScheme()); - // qerr, uerr - // ---------- - const auto out = evalOnTrajectory(m_problem.getKinematicConstraints(), - {multibody_states}, m_meshIndices); - m_constraints.kinematic = out.at(0); - // velocity correction // ------------------- if (m_solver.getTranscriptionScheme() == "hermite-simpson" && - !m_problem.isKinematicConstraintMethodBordalba2023() && - m_problem.getEnforceConstraintDerivatives()) { + !m_problem.isKinematicConstraintMethodBordalba2023()) { // In Hermite-Simpson, we must compute a velocity correction at all // mesh interval midpoints and update qdot. // See MocoCasADiVelocityCorrection for more details. This function @@ -600,8 +605,8 @@ void Transcription::transcribe() { m_xdot(Slice(0, NQ), m_meshInteriorIndices) += u_correction; } - // projection constraints - // ---------------------- + // projection method + // ----------------- if (m_numProjectionStates) { const auto projectionOut = evalOnTrajectory( m_problem.getStateProjection(), @@ -618,8 +623,8 @@ void Transcription::transcribe() { } } - // udot, zdot, residual, udoterr - // ----------------------------- + // udot, zdot, residual, kcerr + // --------------------------- if (m_problem.isDynamicsModeImplicit()) { // udot. const MX w = m_unscaledVars[derivatives] @@ -631,17 +636,61 @@ void Transcription::transcribe() { m_xdot_projection(Slice(NQ, NQ + NU), Slice()) = w_projection; } - // Evaluate the implicit multibody system function and get multibody - // residuals, auxiliary residuals, zdot (auxiliary derivatives), and - // udoterr (acceleration-level kinematic constraint errors). + // When the model has kinematic constraints, we must treat grid points + // differently, as kinematic constraints are computed for only some + // grid points. When the model does *not* have kinematic constraints, + // the DAE is the same for all grid points, but the evaluation is still + // done separately to keep implementation general. std::vector inputs{states, controls, multipliers, derivatives}; - const auto out = evalOnTrajectory(m_problem.getImplicitMultibodySystem(), - inputs, m_gridIndices); - m_constraints.multibody_residuals = out.at(0)(Slice(), m_controlIndices); - // zdot. - m_xdot(Slice(NQ + NU, NS), Slice()) = out.at(1); - m_constraints.auxiliary_residuals = out.at(2)(Slice(), m_controlIndices); - m_constraints.kinematic_udoterr = out.at(3)(Slice(), m_udotErrIndices); + + // residual, zdot, kcerr + // Points where we compute algebraic constraints. + { + const auto out = + evalOnTrajectory(m_problem.getImplicitMultibodySystem(), + inputs, m_meshIndices); + m_constraints.multibody_residuals(Slice(), m_meshIndices) = + out.at(0); + // zdot. + m_xdot(Slice(NQ + NU, NS), m_meshIndices) = out.at(1); + m_constraints.auxiliary_residuals(Slice(), m_meshIndices) = + out.at(2); + if (m_problem.isKinematicConstraintMethodBordalba2023()) { + m_constraints.kinematic(Slice(0, nqerr), Slice()) = out.at(3)( + Slice(0, nqerr), Slice()); + m_constraints.kinematic(Slice(nqerr, nqerr + nuerr), Slice()) + = out.at(3)(Slice(nqerr, nqerr + nuerr), Slice()); + m_constraints.kinematic_udoterr(Slice(), m_meshIndices) = + out.at(3)( + Slice(nqerr + nuerr, nqerr + nuerr + nudoterr), + Slice()); + } else { + m_constraints.kinematic = out.at(3); + } + } + + // Points where we ignore algebraic constraints. + if (m_numMeshInteriorPoints) { + const casadi::Function& implicitMultibodyFunction = + m_problem.isKinematicConstraintMethodBordalba2023() ? + m_problem.getImplicitMultibodySystem() : + m_problem.getImplicitMultibodySystemIgnoringConstraints(); + const auto out = evalOnTrajectory(implicitMultibodyFunction, inputs, + m_meshInteriorIndices); + m_constraints.multibody_residuals(Slice(), m_meshInteriorIndices) = + out.at(0); + // zdot. + m_xdot(Slice(NQ + NU, NS), m_meshInteriorIndices) = + out.at(1); + m_constraints.auxiliary_residuals(Slice(), m_meshInteriorIndices) = + out.at(2); + if (m_problem.isKinematicConstraintMethodBordalba2023()) { + m_constraints.kinematic_udoterr(Slice(), m_meshInteriorIndices) + = out.at(3)( + Slice(nqerr + nuerr, nqerr + nuerr + nudoterr), + Slice()); + } + } // Points where the multibody residuals and udoterr depend on the // projection states. @@ -659,27 +708,65 @@ void Transcription::transcribe() { // the projection states overlap with all the mesh indices except // the first mesh point). This also keeps the implementation above // general when we do not have projection states. - m_constraints.multibody_residuals(Slice(), - m_projectionStateIndicesForControlIndices) = out.at(0); - m_constraints.kinematic_udoterr(Slice(), - m_projectionStateIndicesForControlIndices) = out.at(3); + m_constraints.multibody_residuals( + Slice(), m_projectionStateIndices) = out.at(0); + m_constraints.kinematic_udoterr(Slice(), m_projectionStateIndices) + = out.at(3)(Slice(nqerr + nuerr, nqerr + nuerr + nudoterr), + Slice()); } } else { // Explicit dynamics mode. - - // udot, zdot, udoterr - // ------------------- std::vector inputs{states, controls, multipliers, derivatives}; - // Evaluate the explicit multibody system function and get udot - // (speed derivatives), zdot (auxiliary derivatives), and udoterr - // (acceleration-level kinematic constraint errors). - const auto out = evalOnTrajectory(m_problem.getMultibodySystem(), - inputs, m_gridIndices); - m_xdot(Slice(NQ, NQ + NU), Slice()) = out.at(0); - m_xdot(Slice(NQ + NU, NS), Slice()) = out.at(1); - m_constraints.auxiliary_residuals = out.at(2)(Slice(), m_controlIndices); - m_constraints.kinematic_udoterr = out.at(3)(Slice(), m_udotErrIndices); + // udot, zdot, kcerr. + // Points where we compute algebraic constraints. + { + // Evaluate the multibody system function and get udot + // (speed derivatives) and zdot (auxiliary derivatives). + const auto out = evalOnTrajectory( + m_problem.getMultibodySystem(), inputs, m_meshIndices); + m_xdot(Slice(NQ, NQ + NU), m_meshIndices) = out.at(0); + m_xdot(Slice(NQ + NU, NS), m_meshIndices) = out.at(1); + m_constraints.auxiliary_residuals(Slice(), m_meshIndices) = + out.at(2); + if (m_problem.isKinematicConstraintMethodBordalba2023()) { + m_constraints.kinematic(Slice(0, nqerr), Slice()) = out.at(3)( + Slice(0, nqerr), Slice()); + m_constraints.kinematic(Slice(nqerr, nqerr + nuerr), Slice()) + = out.at(3)(Slice(nqerr, nqerr + nuerr), Slice()); + m_constraints.kinematic_udoterr(Slice(), m_meshIndices) = + out.at(3)( + Slice(nqerr + nuerr, nqerr + nuerr + nudoterr), + Slice()); + } else { + m_constraints.kinematic = out.at(3); + } + } + + // Points where we ignore algebraic constraints. + if (m_numMeshInteriorPoints) { + // In the Bordalba et al. 2023 method, we need to enforce the + // acceleration-level kinematic constraints at the mesh interval + // interior points. + const casadi::Function& multibodyFunction = + m_problem.isKinematicConstraintMethodBordalba2023() ? + m_problem.getMultibodySystem() : + m_problem.getMultibodySystemIgnoringConstraints(); + const auto out = evalOnTrajectory(multibodyFunction, inputs, + m_meshInteriorIndices); + m_xdot(Slice(NQ, NQ + NU), m_meshInteriorIndices) = + out.at(0); + m_xdot(Slice(NQ + NU, NS), m_meshInteriorIndices) = + out.at(1); + m_constraints.auxiliary_residuals(Slice(), m_meshInteriorIndices) = + out.at(2); + if (m_problem.isKinematicConstraintMethodBordalba2023()) { + m_constraints.kinematic_udoterr(Slice(), m_meshInteriorIndices) + = out.at(3)( + Slice(nqerr + nuerr, nqerr + nuerr + nudoterr), + Slice()); + } + } // Points where the state derivatives and udoterr depend on the // projection states. @@ -702,8 +789,9 @@ void Transcription::transcribe() { // overlap with all the mesh indices except the first mesh point). // This also keeps the implementation above general when we do not // have projection states. - m_constraints.kinematic_udoterr(Slice(), - m_projectionStateIndicesForControlIndices) = out.at(3); + m_constraints.kinematic_udoterr(Slice(), m_projectionStateIndices) + = out.at(3)(Slice(nqerr + nuerr, nqerr + nuerr + nudoterr), + Slice()); } } @@ -1420,6 +1508,8 @@ void Transcription::printConstraintValues(const Iterate& it, int numQErr = m_problem.getNumQErr(); int numUErr = m_problem.getNumUErr(); int numUDotErr = m_problem.getNumUDotErr(); + int numKC = m_problem.isKinematicConstraintMethodBordalba2023() ? + numQErr + numUErr : numQErr + numUErr + numUDotErr; if (kinconNames.empty()) { ss << " none" << std::endl; } else { @@ -1432,7 +1522,7 @@ void Transcription::printConstraintValues(const Iterate& it, row.resize(1, m_numMeshPoints); { int ikc = 0; - for (int i = 0; i < numQErr + numUErr; ++i) { + for (int i = 0; i < numKC; ++i) { row = constraints.kinematic(i, Slice()); const double L2 = casadi::DM::norm_2(row).scalar(); int argmax; @@ -1449,23 +1539,26 @@ void Transcription::printConstraintValues(const Iterate& it, << std::endl; ++ikc; } - for (int iudot = 0; iudot < numUDotErr; ++iudot) { - row = constraints.kinematic_udoterr(iudot, Slice()); - const double L2 = casadi::DM::norm_2(row).scalar(); - int argmax; - double max = calcL1Norm(row, argmax); - const double L1 = max; - const double time_of_max = it.times(argmax).scalar(); + if (m_problem.isKinematicConstraintMethodBordalba2023()) { + for (int iudot = 0; iudot < numUDotErr; ++iudot) { + row = constraints.kinematic_udoterr(iudot, Slice()); + const double L2 = casadi::DM::norm_2(row).scalar(); + int argmax; + double max = calcL1Norm(row, argmax); + const double L1 = max; + const double time_of_max = it.times(argmax).scalar(); - std::string label = kinconNames.at(ikc); - ss << std::setfill('0') << std::setw(2) << ikc << ":" - << std::setfill(' ') << std::setw(maxNameLength) << label - << spacer << std::setprecision(2) << std::scientific - << std::setw(9) << L2 << spacer << L1 << spacer - << std::setprecision(6) << std::fixed << time_of_max - << std::endl; - ++ikc; + std::string label = kinconNames.at(ikc); + ss << std::setfill('0') << std::setw(2) << ikc << ":" + << std::setfill(' ') << std::setw(maxNameLength) << label + << spacer << std::setprecision(2) << std::scientific + << std::setw(9) << L2 << spacer << L1 << spacer + << std::setprecision(6) << std::fixed << time_of_max + << std::endl; + ++ikc; + } } + } // TODO fix this to print kinematic values correctly ss << "Kinematic constraint values at each mesh point:" @@ -1479,17 +1572,19 @@ void Transcription::printConstraintValues(const Iterate& it, ss << std::setfill('0') << std::setw(3) << imesh << " "; ss.fill(' '); ss << std::setw(9) << it.times(imesh).scalar() << " "; - for (int i = 0; i < numQErr + numUErr; ++i) { + for (int i = 0; i < numKC; ++i) { const auto& value = constraints.kinematic(i, imesh).scalar(); ss << std::setprecision(2) << std::scientific << std::setw(9) << value << " "; } - for (int iudot = 0; iudot < numUDotErr; ++iudot) { - const auto& value = - constraints.kinematic_udoterr(iudot, imesh).scalar(); - ss << std::setprecision(2) << std::scientific - << std::setw(9) << value << " "; + if (m_problem.isKinematicConstraintMethodBordalba2023()) { + for (int iudot = 0; iudot < numUDotErr; ++iudot) { + const auto& value = + constraints.kinematic_udoterr(iudot, imesh).scalar(); + ss << std::setprecision(2) << std::scientific + << std::setw(9) << value << " "; + } } ss << std::endl; } diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h index 3459b97de7..bbc8af2be6 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h @@ -485,10 +485,6 @@ class Transcription { // Kinematic constraints. copyColumn(constraints.kinematic, imesh); ng += constraints.kinematic.rows(); - if (!m_problem.isKinematicConstraintMethodBordalba2023()) { - copyColumn(constraints.kinematic_udoterr, imesh); - ng += constraints.kinematic_udoterr.rows(); - } // Multibody and auxiliary residuals. for (int i = 0; i < N; ++i) { @@ -532,10 +528,6 @@ class Transcription { // Final grid point. copyColumn(constraints.kinematic, m_numMeshPoints - 1); ng += constraints.kinematic.rows(); - if (!m_problem.isKinematicConstraintMethodBordalba2023()) { - copyColumn(constraints.kinematic_udoterr, m_numMeshPoints - 1); - ng += constraints.kinematic_udoterr.rows(); - } if (dynamicsIndices(m_numGridPoints - 1).scalar() == 1) { if (m_problem.isKinematicConstraintMethodBordalba2023()) { copyColumn(constraints.kinematic_udoterr, idyn); @@ -580,8 +572,12 @@ class Transcription { int numQErr = m_problem.getNumQErr(); int numUErr = m_problem.getNumUErr(); int numUDotErr = m_problem.getNumUDotErr(); - out.kinematic = init(numQErr + numUErr, m_numMeshPoints); - out.kinematic_udoterr = init(numUDotErr, m_numUDotErrorPoints); + int numKC = m_problem.isKinematicConstraintMethodBordalba2023() ? + numQErr + numUErr : numQErr + numUErr + numUDotErr; + out.kinematic = init(numKC, m_numMeshPoints); + if (m_problem.isKinematicConstraintMethodBordalba2023()) { + out.kinematic_udoterr = init(numUDotErr, m_numUDotErrorPoints); + } out.projection = init(m_problem.getNumProjectionConstraintEquations(), m_numMeshIntervals); out.endpoint.resize(m_problem.getEndpointConstraintInfos().size()); @@ -629,9 +625,6 @@ class Transcription { // Kinematic constraints. copyColumn(out.kinematic, imesh); - if (!m_problem.isKinematicConstraintMethodBordalba2023()) { - copyColumn(out.kinematic_udoterr, imesh); - } // Multibody and auxiliary residuals. for (int i = 0; i < N; ++i) { @@ -664,9 +657,6 @@ class Transcription { // Final grid point. copyColumn(out.kinematic, m_numMeshPoints - 1); - if (!m_problem.isKinematicConstraintMethodBordalba2023()) { - copyColumn(out.kinematic_udoterr, m_numMeshPoints - 1); - } if (dynamicsIndices(m_numGridPoints - 1).scalar() == 1) { if (m_problem.isKinematicConstraintMethodBordalba2023()) { copyColumn(out.kinematic_udoterr, idyn); diff --git a/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.h b/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.h index 1ec334464e..d98c05c4c7 100644 --- a/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.h +++ b/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.h @@ -324,6 +324,7 @@ class MocoCasOCProblem : public CasOC::Problem { private: void calcMultibodySystemExplicit(const ContinuousInput& input, + bool calcKCErrors, MultibodySystemExplicitOutput& output) const override { auto mocoProblemRep = m_jar->take(); @@ -344,23 +345,11 @@ class MocoCasOCProblem : public CasOC::Problem { simtkStateDisabledConstraints); // Compute kinematic constraint errors. - if (getNumMultipliers()) { - // Calculate udoterr. We cannot use State::getUDotErr() - // because that uses Simbody's multipliers and UDot, - // whereas we have our own multipliers and UDot. Here, we use - // the udot computed from the model with disabled constraints - // since we cannot use (nor do we have available) udot computed - // from the original model. - if (getEnforceConstraintDerivatives() || - getNumAccelerationConstraintEquations()) { - const auto& matter = modelBase.getMatterSubsystem(); - matter.calcConstraintAccelerationErrors(simtkStateBase, - simtkStateDisabledConstraints.getUDot(), m_pvaerr); - } else { - m_pvaerr = SimTK::NaN; - } - std::copy_n(m_pvaerr.getContiguousScalarData() + m_udoterrOffset, - m_udoterrSize, output.kinematic_constraint_udot_errors.ptr()); + if (getNumMultipliers() && calcKCErrors) { + calcKinematicConstraintErrors( + modelBase, simtkStateBase, + simtkStateDisabledConstraints, + output.kinematic_constraint_errors); } // Copy state derivative values to output. @@ -378,6 +367,7 @@ class MocoCasOCProblem : public CasOC::Problem { m_jar->leave(std::move(mocoProblemRep)); } void calcMultibodySystemImplicit(const ContinuousInput& input, + bool calcKCErrors, MultibodySystemImplicitOutput& output) const override { auto mocoProblemRep = m_jar->take(); @@ -401,23 +391,11 @@ class MocoCasOCProblem : public CasOC::Problem { simtkStateDisabledConstraints); // Compute kinematic constraint errors. - if (getNumMultipliers()) { - // Calculate udoterr. We cannot use State::getUDotErr() - // because that uses Simbody's multipliers and UDot, - // whereas we have our own multipliers and UDot. Here, we use - // the udot computed from the model with disabled constraints - // since we cannot use (nor do we have available) udot computed - // from the original model. - if (getEnforceConstraintDerivatives() || - getNumAccelerationConstraintEquations()) { - const auto& matter = modelBase.getMatterSubsystem(); - matter.calcConstraintAccelerationErrors(simtkStateBase, - simtkStateDisabledConstraints.getUDot(), m_pvaerr); - } else { - m_pvaerr = SimTK::NaN; - } - std::copy_n(m_pvaerr.getContiguousScalarData() + m_udoterrOffset, - m_udoterrSize, output.kinematic_constraint_udot_errors.ptr()); + if (getNumMultipliers() && calcKCErrors) { + calcKinematicConstraintErrors( + modelBase, simtkStateBase, + simtkStateDisabledConstraints, + output.kinematic_constraint_errors); } const SimTK::SimbodyMatterSubsystem& matterDisabledConstraints = @@ -438,37 +416,6 @@ class MocoCasOCProblem : public CasOC::Problem { m_jar->leave(std::move(mocoProblemRep)); } - void calcKinematicConstraintErrors(const double& time, - const casadi::DM& multibody_states, const casadi::DM& parameters, - casadi::DM& kinematic_constraint_errors) const override { - - if (isPrescribedKinematics()) return; - auto mocoProblemRep = m_jar->take(); - - const auto& modelBase = mocoProblemRep->getModelBase(); - auto& simtkStateBase = mocoProblemRep->updStateBase(); - - // Update the model and state. - applyParametersToModelProperties(parameters, *mocoProblemRep); - convertStatesToSimTKState( - SimTK::Stage::Velocity, time, multibody_states, - modelBase, simtkStateBase, false); - modelBase.realizeVelocity(simtkStateBase); - - // Position-level errors. - const auto& qerr = simtkStateBase.getQErr(); - // Velocity-level errors. - const auto& uerr = simtkStateBase.getUErr(); - - // This way of copying the data avoids a threadsafety issue in - // CasADi related to cached Sparsity objects. - std::copy_n(qerr.getContiguousScalarData(), qerr.size(), - kinematic_constraint_errors.ptr()); - std::copy_n(uerr.getContiguousScalarData() + m_uerrOffset, m_uerrSize, - kinematic_constraint_errors.ptr() + qerr.size()); - - m_jar->leave(std::move(mocoProblemRep)); - } void calcVelocityCorrection(const double& time, const casadi::DM& multibody_states, const casadi::DM& slacks, const casadi::DM& parameters, @@ -875,6 +822,51 @@ class MocoCasOCProblem : public CasOC::Problem { m_constraintMobilityForces, m_constraintBodyForces); } + void calcKinematicConstraintErrors( + const Model& modelBase, + const SimTK::State& stateBase, + const SimTK::State& simtkStateDisabledConstraints, + casadi::DM& kinematic_constraint_errors) const { + + // If all kinematics are prescribed, we assume that the prescribed + // kinematics obey any kinematic constraints. Therefore, the kinematic + // constraints would be redundant, and we need not enforce them. + if (isPrescribedKinematics()) return; + + // Calculate udoterr. We cannot use State::getUDotErr() + // because that uses Simbody's multipliers and UDot, + // whereas we have our own multipliers and UDot. Here, we use + // the udot computed from the model with disabled constraints + // since we cannot use (nor do we have available) udot computed + // from the original model. + if (getEnforceConstraintDerivatives() || + getNumAccelerationConstraintEquations()) { + const auto& matter = modelBase.getMatterSubsystem(); + matter.calcConstraintAccelerationErrors(stateBase, + simtkStateDisabledConstraints.getUDot(), m_pvaerr); + } else { + m_pvaerr = SimTK::NaN; + } + + // Position-level errors. + const auto& qerr = stateBase.getQErr(); + // Velocity-level errors. + const auto& uerr = stateBase.getUErr(); + // Acceleration-level errors. + const auto& udoterr = m_pvaerr; + + // This way of copying the data avoids a threadsafety issue in + // CasADi related to cached Sparsity objects. + std::copy_n(qerr.getContiguousScalarData(), qerr.size(), + kinematic_constraint_errors.ptr()); + std::copy_n(uerr.getContiguousScalarData() + m_uerrOffset, m_uerrSize, + kinematic_constraint_errors.ptr() + qerr.size()); + std::copy_n(udoterr.getContiguousScalarData() + m_udoterrOffset, + m_udoterrSize, + kinematic_constraint_errors.ptr() + qerr.size() + m_uerrSize); + + } + void copyImplicitResidualsToOutput(const MocoProblemRep& mocoProblemRep, const SimTK::State& state, casadi::DM& auxiliary_residuals) const { if (getNumAuxiliaryResidualEquations()) {