Skip to content

Commit

Permalink
Revert to original implementation for calculating and enforcing kinem…
Browse files Browse the repository at this point in the history 
…atic constraint errors
  • Loading branch information
@nickbianco
nickbianco committed Sep 17, 2024
1 parent 93d0060 commit 4ff1d26
Show file tree
Hide file tree
Showing 6 changed files with 310 additions and 266 deletions.
79 changes: 28 additions & 51 deletions OpenSim/Moco/MocoCasADiSolver/CasOCFunction.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -241,7 +241,8 @@ VectorDM EndpointConstraint::eval(const VectorDM& args) const {
return out;
}

casadi::Sparsity MultibodySystemExplicit::get_sparsity_out(
template <bool calcKCErr>
casadi::Sparsity MultibodySystemExplicit<calcKCErr>::get_sparsity_out(
casadi_int i) {
if (i == 0) {
return casadi::Sparsity::dense(
Expand All @@ -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 <bool calcKCErr>
VectorDM MultibodySystemExplicit<calcKCErr>::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());
Expand All @@ -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<false>;
template class CasOC::MultibodySystemExplicit<true>;

template <bool calcKCErr>
casadi::Sparsity MultibodySystemImplicit<calcKCErr>::get_sparsity_out(
casadi_int i) {
if (i == 0) {
return casadi::Sparsity::dense(
m_casProblem->getNumMultibodyDynamicsEquations(), 1);
Expand All @@ -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 <bool calcKCErr>
VectorDM MultibodySystemImplicit<calcKCErr>::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());
Expand All @@ -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<false>;
template class CasOC::MultibodySystemImplicit<true>;

casadi::Sparsity VelocityCorrection::get_sparsity_in(casadi_int i) {
if (i == 0) {
Expand Down
33 changes: 4 additions & 29 deletions OpenSim/Moco/MocoCasADiSolver/CasOCFunction.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 <bool calcKCErr>
class MultibodySystemExplicit : public Function {
public:
casadi_int get_n_out() override final { return 4; }
Expand All @@ -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.");
}
}
Expand All @@ -277,14 +278,15 @@ 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 <bool calcKCErr>
class MultibodySystemImplicit : public Function {
casadi_int get_n_out() override final { return 4; }
std::string get_name_out(casadi_int i) override final {
switch (i) {
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.");
}
}
Expand All @@ -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 {
Expand Down
63 changes: 39 additions & 24 deletions OpenSim/Moco/MocoCasADiSolver/CasOCProblem.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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:
Expand Down Expand Up @@ -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,
Expand Down Expand Up @@ -465,26 +462,35 @@ class Problem {
if (m_dynamicsMode == "implicit") {
// Construct an implicit multibody system.
mutThis->m_implicitMultibodyFunc =
OpenSim::make_unique<MultibodySystemImplicit>();
OpenSim::make_unique<MultibodySystemImplicit<true>>();
mutThis->m_implicitMultibodyFunc->constructFunction(this,
"implicit_multibody_system", finiteDiffScheme,
pointsForSparsityDetection);

// Construct an implicit multibody system ignoring kinematic
// constraints.
mutThis->m_implicitMultibodyFuncIgnoringConstraints =
OpenSim::make_unique<MultibodySystemImplicit<false>>();
mutThis->m_implicitMultibodyFuncIgnoringConstraints
->constructFunction(this,
"implicit_multibody_system_ignoring_constraints",
finiteDiffScheme, pointsForSparsityDetection);
} else {
// Construct an explicit multibody system.
mutThis->m_multibodyFunc =
OpenSim::make_unique<MultibodySystemExplicit>();
OpenSim::make_unique<MultibodySystemExplicit<true>>();
mutThis->m_multibodyFunc->constructFunction(this,
"explicit_multibody_system", finiteDiffScheme,
pointsForSparsityDetection);

mutThis->m_multibodyFuncIgnoringConstraints =
OpenSim::make_unique<MultibodySystemExplicit<false>>();
mutThis->m_multibodyFuncIgnoringConstraints->constructFunction(this,
"multibody_system_ignoring_constraints", finiteDiffScheme,
pointsForSparsityDetection);
}

if (getNumKinematicConstraintEquations()) {
mutThis->m_kinematicConstraintsFunc =
OpenSim::make_unique<KinematicConstraints>();
mutThis->m_kinematicConstraintsFunc->constructFunction(this,
"kinematic_constraints", finiteDiffScheme,
pointsForSparsityDetection);

if (getEnforceConstraintDerivatives()) {
if (isKinematicConstraintMethodBordalba2023()) {
mutThis->m_stateProjectionFunc =
Expand Down Expand Up @@ -637,15 +643,17 @@ class Problem {
const std::vector<PathConstraintInfo>& 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
Expand All @@ -665,6 +673,10 @@ class Problem {
const casadi::Function& getImplicitMultibodySystem() const {
return *m_implicitMultibodyFunc;
}
const casadi::Function&
getImplicitMultibodySystemIgnoringConstraints() const {
return *m_implicitMultibodyFuncIgnoringConstraints;
}
/// @}

private:
Expand Down Expand Up @@ -700,9 +712,12 @@ class Problem {
std::vector<CostInfo> m_costInfos;
std::vector<EndpointConstraintInfo> m_endpointConstraintInfos;
std::vector<PathConstraintInfo> m_pathInfos;
std::unique_ptr<MultibodySystemExplicit> m_multibodyFunc;
std::unique_ptr<MultibodySystemImplicit> m_implicitMultibodyFunc;
std::unique_ptr<KinematicConstraints> m_kinematicConstraintsFunc;
std::unique_ptr<MultibodySystemExplicit<true>> m_multibodyFunc;
std::unique_ptr<MultibodySystemExplicit<false>>
m_multibodyFuncIgnoringConstraints;
std::unique_ptr<MultibodySystemImplicit<true>> m_implicitMultibodyFunc;
std::unique_ptr<MultibodySystemImplicit<false>>
m_implicitMultibodyFuncIgnoringConstraints;
std::unique_ptr<VelocityCorrection> m_velocityCorrectionFunc;
std::unique_ptr<StateProjection> m_stateProjectionFunc;
};
Expand Down
Loading

0 comments on commit 4ff1d26

Please sign in to comment.