Skip to content

Commit

Permalink
Unused constraint equations removal from system framework (RobotLocom…
Browse files Browse the repository at this point in the history 
…otion#12553)
  • Loading branch information
@edrumwri @jwnimmer-tri
edrumwri authored and jwnimmer-tri committed Jan 14, 2020
1 parent 3cfc130 commit 21d099a
Show file tree
Hide file tree
Showing 3 changed files with 36 additions and 188 deletions.
22 changes: 5 additions & 17 deletions examples/bead_on_a_wire/bead_on_a_wire-inl.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -53,11 +53,8 @@ typename BeadOnAWire<T>::ArcLength
return atan2(v(1), v(0));
}

/// Evaluates the constraint equations for a bead represented in absolute
/// coordinates (no constraint equations are used for a bead represented in
/// minimal coordinates).
template <class T>
Eigen::VectorXd BeadOnAWire<T>::DoEvalConstraintEquations(
Eigen::VectorXd BeadOnAWire<T>::EvalConstraintEquations(
const systems::Context<T>& context) const {
// Return a zero vector if this system is in minimal coordinates.
if (coordinate_type_ == BeadOnAWire<T>::kMinimalCoordinates)
Expand Down Expand Up @@ -88,10 +85,8 @@ Eigen::VectorXd BeadOnAWire<T>::DoEvalConstraintEquations(
return xprime;
}

/// Computes the time derivative of the constraint equations, evaluated at
/// the current generalized coordinates and generalized velocity.
template <class T>
Eigen::VectorXd BeadOnAWire<T>::DoEvalConstraintEquationsDot(
Eigen::VectorXd BeadOnAWire<T>::EvalConstraintEquationsDot(
const systems::Context<T>& context) const {
constexpr int three_d = 3;

Expand Down Expand Up @@ -149,29 +144,22 @@ void BeadOnAWire<T>::CopyStateOut(const systems::Context<T>& context,
context.get_continuous_state().CopyToVector();
}

/// Gets the number of constraint equations used for dynamics.
template <class T>
int BeadOnAWire<T>::do_get_num_constraint_equations(
int BeadOnAWire<T>::get_num_constraint_equations(
const systems::Context<T>&) const {
return (coordinate_type_ == kAbsoluteCoordinates) ? 3 : 0;
}

/// Computes the change in generalized velocity from applying constraint
/// impulses @p lambda.
/// @param context The current state of the system.
/// @param lambda The vector of constraint forces.
/// @returns a `n` dimensional vector, where `n` is the dimension of the
/// quasi-coordinates.
template <class T>
Eigen::VectorXd BeadOnAWire<T>::DoCalcVelocityChangeFromConstraintImpulses(
Eigen::VectorXd BeadOnAWire<T>::CalcVelocityChangeFromConstraintImpulses(
const systems::Context<T>&, const Eigen::MatrixXd& J,
const Eigen::VectorXd& lambda) const {

// TODO(edrumwri): Test this method as soon as DAE solver is available,
// (necessarily removing abort() first).
if (true) {
throw std::logic_error(
"DoCalcVelocityChangeFromConstraintImpulses requires testing.");
"CalcVelocityChangeFromConstraintImpulses requires testing.");
}

// The bead on the wire is unit mass, so the velocity change is equal to
Expand Down
51 changes: 31 additions & 20 deletions examples/bead_on_a_wire/bead_on_a_wire.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -187,37 +187,48 @@ class BeadOnAWire : public systems::LeafSystem<T> {
/// @param m the output from the inverse parametric wire function.
static double get_inv_pfunction_second_derivative(const ArcLength& m);

/// Gets the number of constraint equations used for dynamics.
int get_num_constraint_equations(const systems::Context<T>& context) const;

/// Evaluates the constraint equations for a bead represented in absolute
/// coordinates (no constraint equations are used for a bead represented in
/// minimal coordinates).
Eigen::VectorXd EvalConstraintEquations(
const systems::Context<T>& context) const;

/// Computes the time derivative of the constraint equations, evaluated at
/// the current generalized coordinates and generalized velocity.
Eigen::VectorXd EvalConstraintEquationsDot(
const systems::Context<T>& context) const;

/// Computes the change in generalized velocity from applying constraint
/// impulses @p lambda.
/// @param context The current state of the system.
/// @param lambda The vector of constraint forces.
/// @returns a `n` dimensional vector, where `n` is the dimension of the
/// quasi-coordinates.
Eigen::VectorXd CalcVelocityChangeFromConstraintImpulses(
const systems::Context<T>& context, const Eigen::MatrixXd& J,
const Eigen::VectorXd& lambda) const;

protected:
void CopyStateOut(const systems::Context<T> &context,
void CopyStateOut(const systems::Context<T>& context,
systems::BasicVector<T>* output) const;

void DoCalcTimeDerivatives(
const systems::Context<T> &context,
systems::ContinuousState<T> *derivatives) const override;

void SetDefaultState(const systems::Context<T> &context,
systems::State<T> *state) const override;

int do_get_num_constraint_equations(const systems::Context<T> &context) const
override;

Eigen::VectorXd DoEvalConstraintEquations(
const systems::Context<T> &context) const override;

Eigen::VectorXd DoEvalConstraintEquationsDot(
const systems::Context<T> &context) const override;
const systems::Context<T>& context,
systems::ContinuousState<T>* derivatives) const override;

Eigen::VectorXd DoCalcVelocityChangeFromConstraintImpulses(
const systems::Context<T> &context, const Eigen::MatrixXd &J,
const Eigen::VectorXd &lambda) const override;
void SetDefaultState(const systems::Context<T>& context,
systems::State<T>* state) const override;

private:
// The coordinate representation used for kinematics and dynamics.
CoordinateType coordinate_type_;

// The wire parameter function. See set_wire_parameter_function() for more
// information. This pointer is expected to never be null.
std::function<Eigen::Matrix<ArcLength, 3, 1>(const ArcLength &)> f_{
std::function<Eigen::Matrix<ArcLength, 3, 1>(const ArcLength&)> f_{
&helix_function};

// The inverse of the wire parameter function.
Expand All @@ -228,7 +239,7 @@ class BeadOnAWire : public systems::LeafSystem<T> {
// function is null, EvaluateConstraintEquations() will use generic,
// presumably less efficient methods instead. This pointer is expected to
// never be null.
std::function<ArcLength(const Eigen::Matrix<ArcLength, 3, 1> &)> inv_f_{
std::function<ArcLength(const Eigen::Matrix<ArcLength, 3, 1>&)> inv_f_{
&inverse_helix_function};

// Signed acceleration due to gravity.
Expand Down
151 changes: 0 additions & 151 deletions systems/framework/system.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -572,81 +572,6 @@ class System : public SystemBase {
/// @name Constraint-related functions
//@{

/// Gets the number of constraint equations for this system using the given
/// context (useful in case the number of constraints is dependent upon the
/// current state (as might be the case with a system modeled using piecewise
/// differential algebraic equations).
int num_constraint_equations(const Context<T>& context) const {
return do_get_num_constraint_equations(context);
}

/// Evaluates the constraint equations for the system at the generalized
/// coordinates and generalized velocity specified by the context. The context
/// allows the set of constraints to be dependent upon the current system
/// state (as might be the case with a system modeled using piecewise
/// differential algebraic equations).
/// @returns a vector of dimension num_constraint_equations(); the
/// zero vector indicates that the algebraic constraints are all
/// satisfied.
Eigen::VectorXd EvalConstraintEquations(const Context<T>& context) const {
return DoEvalConstraintEquations(context);
}

/// Computes the time derivative of each constraint equation, evaluated at
/// the generalized coordinates and generalized velocity specified by the
/// context. The context allows the set of constraints to be dependent upon
/// the current system state (as might be the case with a system modeled using
/// piecewise differential algebraic equations).
/// @returns a vector of dimension num_constraint_equations().
Eigen::VectorXd EvalConstraintEquationsDot(const Context<T>& context) const {
return DoEvalConstraintEquationsDot(context);
}

/// Computes the change in velocity from applying the given constraint forces
/// to the system at the given context.
/// @param context the current system state, provision of which also yields
/// the ability of the constraints to be dependent upon the current
/// system state (as might be the case with a piecewise differential
/// algebraic equation).
/// @param J a m × n constraint Jacobian matrix of the `m` constraint
/// equations `g()` differentiated with respect to the `n`
/// configuration variables `q` (i.e., `J` should be `∂g/∂q`). If
/// the time derivatives of the generalized coordinates of the system
/// are not identical to the generalized velocity (in general they
/// need not be, e.g., if generalized coordinates use unit
/// unit quaternions to represent 3D orientation), `J` should instead
/// be defined as `∂g/∂q⋅N`, where `N ≡ ∂q/∂ꝗ` is the Jacobian matrix
/// (dependent on `q`) of the generalized coordinates with respect
/// to the quasi-coordinates (ꝗ, pronounced "qbar", where dꝗ/dt are
/// the generalized velocities).
/// @param lambda the vector of constraint forces (of same dimension as the
/// number of rows in the Jacobian matrix, @p J)
/// @returns a `n` dimensional vector, where `n` is the dimension of the
/// quasi-coordinates.
Eigen::VectorXd CalcVelocityChangeFromConstraintImpulses(
const Context<T>& context, const Eigen::MatrixXd& J,
const Eigen::VectorXd& lambda) const {
DRAKE_ASSERT(lambda.size() == num_constraint_equations(context));
DRAKE_ASSERT(J.rows() == num_constraint_equations(context));
DRAKE_ASSERT(
J.cols() ==
context.get_continuous_state().get_generalized_velocity().size());
return DoCalcVelocityChangeFromConstraintImpulses(context, J, lambda);
}

/// Computes the norm on constraint error (used as a metric for comparing
/// errors between the outputs of algebraic equations applied to two
/// different state variable instances). This norm need be neither continuous
/// nor differentiable.
/// @throws std::logic_error if the dimension of @p err is not equivalent to
/// the output of num_constraint_equations().
double CalcConstraintErrorNorm(const Context<T>& context,
const Eigen::VectorXd& error) const {
if (error.size() != num_constraint_equations(context))
throw std::logic_error("Error vector is mis-sized.");
return DoCalcConstraintErrorNorm(context, error);
}

/// Adds an "external" constraint to this System.
///
/// This method is intended for use by applications that are examining this
Expand Down Expand Up @@ -2164,85 +2089,9 @@ class System : public SystemBase {
}
//@}

//----------------------------------------------------------------------------
/// @name Constraint-related functions (protected).
//@{

/// Totals the number of continuous state variables in this System or Diagram.
virtual int do_get_num_continuous_states() const = 0;

/// Gets the number of constraint equations for this system from the given
/// context. The context is supplied in case the number of constraints is
/// dependent upon the current state (as might be the case with a piecewise
/// differential algebraic equation). Derived classes can override this
/// function, which is called by num_constraint_equations().
/// @sa num_constraint_equations() for parameter documentation.
/// @returns zero by default
virtual int do_get_num_constraint_equations(const Context<T>& context) const {
unused(context);
return 0;
}

/// Evaluates the constraint equations for the system at the generalized
/// coordinates and generalized velocity specified by the context. The context
/// allows the set of constraints to be dependent upon the current
/// system state (as might be the case with a piecewise differential algebraic
/// equation). The default implementation of this function returns a
/// zero-dimensional vector. Derived classes can override this function,
/// which is called by EvalConstraintEquations().
/// @sa EvalConstraintEquations() for parameter documentation.
/// @returns a vector of dimension num_constraint_equations(); the
/// zero vector indicates that the algebraic constraints are all
/// satisfied.
virtual Eigen::VectorXd DoEvalConstraintEquations(
const Context<T>& context) const {
DRAKE_DEMAND(num_constraint_equations(context) == 0);
return Eigen::VectorXd();
}

/// Computes the time derivative of each constraint equation, evaluated at
/// the generalized coordinates and generalized velocity specified by the
/// context. The context allows the set of constraints to be dependent upon
/// the current system state (as might be the case with a piecewise
/// differential algebraic equation). The default implementation of this
/// function returns a zero-dimensional vector. Derived classes can override
/// this function, which is called by EvalConstraintEquationsDot().
/// @returns a vector of dimension num_constraint_equations().
/// @sa EvalConstraintEquationsDot() for parameter documentation.
virtual Eigen::VectorXd DoEvalConstraintEquationsDot(
const Context<T>& context) const {
DRAKE_DEMAND(num_constraint_equations(context) == 0);
return Eigen::VectorXd();
}

/// Computes the change in velocity from applying the given constraint forces
/// to the system at the given context. Derived classes can override this
/// function, which is called by CalcVelocityChangeFromConstraintImpulses().
/// @returns the zero vector of dimension of the dimension of the
/// quasi-coordinates, by default.
/// @sa CalcVelocityChangeFromConstraintImpulses() for parameter
/// documentation.
virtual Eigen::VectorXd DoCalcVelocityChangeFromConstraintImpulses(
const Context<T>& context, const Eigen::MatrixXd& J,
const Eigen::VectorXd& lambda) const {
unused(J, lambda);
DRAKE_DEMAND(num_constraint_equations(context) == 0);
const auto& gv = context.get_continuous_state().get_generalized_velocity();
return Eigen::VectorXd::Zero(gv.size());
}

/// Computes the norm of the constraint error. This default implementation
/// computes a Euclidean norm of the error. Derived classes can override this
/// function, which is called by CalcConstraintErrorNorm(). This norm need be
/// neither continuous nor differentiable.
/// @sa CalcConstraintErrorNorm() for parameter documentation.
virtual double DoCalcConstraintErrorNorm(const Context<T>& context,
const Eigen::VectorXd& error) const {
unused(context);
return error.norm();
}
//@}

//----------------------------------------------------------------------------
/// @name Utility methods (protected)
//@{
Expand Down

0 comments on commit 21d099a

Please sign in to comment.