Skip to content

Commit

Permalink
Renames "compliant" models to "continuous" models and "time stepping"…
Browse files Browse the repository at this point in the history
… models to "discretized" models. (RobotLocomotion#8551)
  • Loading branch information
edrumwri authored Apr 13, 2018
1 parent e0f2b67 commit fc428c0
Show file tree
Hide file tree
Showing 16 changed files with 124 additions and 660 deletions.
10 changes: 5 additions & 5 deletions examples/contact_model/bowling_ball.cc
Original file line number Diff line number Diff line change
Expand Up @@ -71,11 +71,11 @@ DEFINE_double(contact_radius, 1e-3,
"The characteristic scale of radius (m) of the contact area");
DEFINE_double(sim_duration, 3, "The simulation duration (s)");
DEFINE_int32(pin_count, 10, "The number of pins -- in the range [0, 10]");
DEFINE_string(simulation_type, "compliant", "The type of simulation to use: "
"'compliant' or 'timestepping'");
DEFINE_string(system_type, "continuous", "The type of system to use: "
"'continuous' or 'discretized'");
DEFINE_double(dt, 1e-3, "The step size to use for "
"'simulation_type=timestepping' (ignored for "
"'simulation_type=compliant'");
"'system_type=discretized' (ignored for "
"'system_type=continuous'");

// Bowling ball rolled down a conceptual lane to strike pins.
int main() {
Expand Down Expand Up @@ -115,7 +115,7 @@ int main() {
multibody::AddFlatTerrainToWorld(tree_ptr.get(), 100., 10.);

// Instantiate a RigidBodyPlant from the RigidBodyTree.
if (FLAGS_simulation_type != "timestepping")
if (FLAGS_system_type != "discretized")
FLAGS_dt = 0.0;
auto& plant = *builder.AddSystem<RigidBodyPlant<double>>(
move(tree_ptr), FLAGS_dt);
Expand Down
10 changes: 5 additions & 5 deletions examples/contact_model/rigid_gripper.cc
Original file line number Diff line number Diff line change
Expand Up @@ -48,11 +48,11 @@ DEFINE_double(contact_radius, 1e-4,
DEFINE_double(sim_duration, 5, "Amount of time to simulate (s)");
DEFINE_bool(playback, true,
"If true, simulation begins looping playback when complete");
DEFINE_string(simulation_type, "compliant", "The type of simulation to use: "
"'compliant' or 'timestepping'");
DEFINE_string(system_type, "continuous", "The type of system to use: "
"'continuous' or 'discretized'");
DEFINE_double(dt, 1e-3, "The step size to use for "
"'simulation_type=timestepping' (ignored for "
"'simulation_type=compliant'");
"'system_type=discretized' (ignored for "
"'system_type=continuous'");

namespace drake {
namespace examples {
Expand Down Expand Up @@ -90,7 +90,7 @@ std::unique_ptr<RigidBodyTreed> BuildTestTree() {
int main() {
systems::DiagramBuilder<double> builder;

if (FLAGS_simulation_type != "timestepping")
if (FLAGS_system_type != "discretized")
FLAGS_dt = 0.0;
systems::RigidBodyPlant<double>* plant =
builder.AddSystem<systems::RigidBodyPlant<double>>(BuildTestTree(),
Expand Down
10 changes: 5 additions & 5 deletions examples/contact_model/sliding_bricks.cc
Original file line number Diff line number Diff line change
Expand Up @@ -50,11 +50,11 @@ DEFINE_double(contact_radius, 1e-3,
DEFINE_double(sim_duration, 3, "The simulation duration (s)");
DEFINE_bool(playback, true,
"If true, enters looping playback after sim finished");
DEFINE_string(simulation_type, "compliant", "The type of simulation to use: "
"'compliant' or 'timestepping'");
DEFINE_string(system_type, "continuous", "The type of system to use: "
"'continuous' or 'discretized'");
DEFINE_double(dt, 1e-3, "The step size to use for "
"'simulation_type=timestepping' (ignored for "
"'simulation_type=compliant'");
"'system_type=discretized' (ignored for "
"'system_type=continuous'");

namespace {
const char* kSlidingBrickUrdf =
Expand Down Expand Up @@ -89,7 +89,7 @@ int main() {
multibody::AddFlatTerrainToWorld(tree_ptr.get(), 100., 10.);

// Instantiate a RigidBodyPlant from the RigidBodyTree.
if (FLAGS_simulation_type != "timestepping")
if (FLAGS_system_type != "discretized")
FLAGS_dt = 0.0;
auto& plant = *builder.AddSystem<RigidBodyPlant<double>>(
move(tree_ptr), FLAGS_dt);
Expand Down
40 changes: 20 additions & 20 deletions examples/rod2d/rod2d-inl.h
Original file line number Diff line number Diff line change
Expand Up @@ -27,17 +27,17 @@ namespace examples {
namespace rod2d {

template <typename T>
Rod2D<T>::Rod2D(SimulationType simulation_type, double dt)
: simulation_type_(simulation_type), dt_(dt) {
Rod2D<T>::Rod2D(SystemType system_type, double dt)
: system_type_(system_type), dt_(dt) {
// Verify that the simulation approach is either piecewise DAE or
// compliant ODE.
if (simulation_type == SimulationType::kTimeStepping) {
if (system_type == SystemType::kDiscretized) {
if (dt <= 0.0)
throw std::logic_error(
"Time stepping approach must be constructed using"
"Discretization approach must be constructed using"
" strictly positive step size.");

// Time stepping approach requires three position variables and
// Discretization approach requires three position variables and
// three velocity variables, all discrete, and periodic update.
this->DeclarePeriodicDiscreteUpdate(dt);
this->DeclareDiscreteState(6);
Expand Down Expand Up @@ -79,7 +79,7 @@ template <class T>
int Rod2D<T>::DetermineNumWitnessFunctions(
const systems::Context<T>&) const {
// No witness functions if this is not a piecewise DAE model.
if (simulation_type_ != SimulationType::kPiecewiseDAE)
if (system_type_ != SystemType::kPiecewiseDAE)
return 0;

// TODO(edrumwri): Flesh out this stub.
Expand All @@ -89,15 +89,15 @@ int Rod2D<T>::DetermineNumWitnessFunctions(

/// Gets the integer variable 'k' used to determine the point of contact
/// indicated by the current mode.
/// @throws std::logic_error if this is a time-stepping system (implying that
/// @throws std::logic_error if this is a discretized system (implying that
/// modes are unused).
/// @returns the value -1 to indicate the bottom of the rod (when theta = pi/2),
/// +1 to indicate the top of the rod (when theta = pi/2), or 0 to
/// indicate the mode where both endpoints of the rod are contacting
/// the halfspace.
template <class T>
int Rod2D<T>::get_k(const systems::Context<T>& context) const {
if (simulation_type_ != SimulationType::kPiecewiseDAE)
if (system_type_ != SystemType::kPiecewiseDAE)
throw std::logic_error("'k' is only valid for piecewise DAE approach.");
const int k = context.template get_abstract_state<int>(1);
DRAKE_DEMAND(std::abs(k) <= 1);
Expand Down Expand Up @@ -492,7 +492,7 @@ template <typename T>
void Rod2D<T>::CopyStateOut(const systems::Context<T>& context,
systems::BasicVector<T>* state_port_value) const {
// Output port value is just the continuous or discrete state.
const VectorX<T> state = (simulation_type_ == SimulationType::kTimeStepping)
const VectorX<T> state = (system_type_ == SystemType::kDiscretized)
? context.get_discrete_state(0).CopyToVector()
: context.get_continuous_state().CopyToVector();
state_port_value->SetFromVector(state);
Expand All @@ -502,14 +502,14 @@ template <typename T>
void Rod2D<T>::CopyPoseOut(
const systems::Context<T>& context,
systems::rendering::PoseVector<T>* pose_port_value) const {
const VectorX<T> state = (simulation_type_ == SimulationType::kTimeStepping)
const VectorX<T> state = (system_type_ == SystemType::kDiscretized)
? context.get_discrete_state(0).CopyToVector()
: context.get_continuous_state().CopyToVector();
ConvertStateToPose(state, pose_port_value);
}

/// Integrates the Rod 2D example forward in time using a
/// half-explicit time stepping scheme.
/// half-explicit discretization scheme.
template <class T>
void Rod2D<T>::DoCalcDiscreteVariableUpdates(
const systems::Context<T>& context,
Expand All @@ -536,7 +536,7 @@ void Rod2D<T>::DoCalcDiscreteVariableUpdates(

// Two contact points, corresponding to the two rod endpoints, are always
// used, regardless of whether any part of the rod is in contact with the
// halfspace. This practice is standard in time stepping approaches with
// halfspace. This practice is standard in discretization approaches with
// constraint stabilization. See:
// M. Anitescu and G. Hart. A Constraint-Stabilized Time-Stepping Approach
// for Rigid Multibody Dynamics with Joints, Contact, and Friction. Intl.
Expand Down Expand Up @@ -903,7 +903,7 @@ template <class T>
Vector3<T> Rod2D<T>::CalcCompliantContactForces(
const systems::Context<T>& context) const {
// Depends on continuous state being available.
DRAKE_DEMAND(simulation_type_ == SimulationType::kCompliant);
DRAKE_DEMAND(system_type_ == SystemType::kContinuous);

using std::abs;
using std::max;
Expand Down Expand Up @@ -1016,8 +1016,8 @@ void Rod2D<T>::DoCalcTimeDerivatives(
using std::cos;
using std::abs;

// Don't compute any derivatives if this is the time stepping system.
if (simulation_type_ == SimulationType::kTimeStepping) {
// Don't compute any derivatives if this is the discretized system.
if (system_type_ == SystemType::kDiscretized) {
DRAKE_ASSERT(derivatives->size() == 0);
return;
}
Expand All @@ -1037,7 +1037,7 @@ void Rod2D<T>::DoCalcTimeDerivatives(
f.SetAtIndex(2, thetadot);

// Compute the velocity derivatives (accelerations).
if (simulation_type_ == SimulationType::kCompliant) {
if (system_type_ == SystemType::kContinuous) {
return CalcAccelerationsCompliantContactAndBallistic(context, derivatives);
} else {
// TODO(edrumwri): Implement the piecewise DAE approach.
Expand All @@ -1049,11 +1049,11 @@ void Rod2D<T>::DoCalcTimeDerivatives(
template <typename T>
std::unique_ptr<systems::AbstractValues> Rod2D<T>::AllocateAbstractState()
const {
if (simulation_type_ == SimulationType::kPiecewiseDAE) {
if (system_type_ == SystemType::kPiecewiseDAE) {
// TODO(edrumwri): Allocate the abstract mode variables.
return std::make_unique<systems::AbstractValues>();
} else {
// Time stepping and compliant approaches need no abstract variables.
// Discretized and continuous approaches need no abstract variables.
return std::make_unique<systems::AbstractValues>();
}
}
Expand All @@ -1072,15 +1072,15 @@ void Rod2D<T>::SetDefaultState(const systems::Context<T>&,
VectorX<T> x0(6);
const double r22 = sqrt(2) / 2;
x0 << half_len * r22, half_len * r22, M_PI / 4.0, -1, 0, 0; // Initial state.
if (simulation_type_ == SimulationType::kTimeStepping) {
if (system_type_ == SystemType::kDiscretized) {
state->get_mutable_discrete_state().get_mutable_vector(0)
.SetFromVector(x0);
} else {
// Continuous variables.
state->get_mutable_continuous_state().SetFromVector(x0);

// Set abstract variables for piecewise DAE approach.
if (simulation_type_ == SimulationType::kPiecewiseDAE) {
if (system_type_ == SystemType::kPiecewiseDAE) {
// TODO(edrumwri): Indicate that the rod is in the single contact
// sliding mode.

Expand Down
66 changes: 35 additions & 31 deletions examples/rod2d/rod2d.h
Original file line number Diff line number Diff line change
Expand Up @@ -109,13 +109,13 @@ h, and "left" and "right" endpoints `Rl=Ro-h*Rx` and `Rr=Ro+h*Rx` at which
it can contact the halfspace whose surface is at Wy=0.
This system can be simulated using one of three models:
- a compliant contact model (the rod is rigid, but contact between
the rod and the half-space is modeled as compliant) simulated using
- continuously, using a compliant contact model (the rod is rigid, but contact
between the rod and the half-space is modeled as compliant) simulated using
ordinary differential equations (ODEs),
- a fully rigid model simulated with piecewise differential algebraic
equations (DAEs), and
- a fully rigid model simulated as a discrete system using a first-order
time stepping approach.
discretization approach.
The rod state is initialized to the configuration that corresponds to the
Painlevé Paradox problem, described in [Stewart 2000]. The paradox consists
Expand Down Expand Up @@ -156,30 +156,33 @@ class Rod2D : public systems::LeafSystem<T> {
public:
~Rod2D() override {}

/// Simulation model and approach for the system.
enum class SimulationType {
/// For simulating the system using rigid contact, Coulomb friction, and
/// piecewise differential algebraic equations.
/// System model and approach for simulating the system.
enum class SystemType {
/// For modeling the system using rigid contact, Coulomb friction, and
/// hybrid mode variables and simulating the system through piecewise
/// solutions of differential algebraic equations.
kPiecewiseDAE,

/// For simulating the system using rigid contact, Coulomb friction, and
/// a first-order time stepping approach.
kTimeStepping,
/// For modeling the system using either rigid or compliant contact,
/// Coulomb friction, and a first-order time discretization (which can
/// be applied to simulating the system without an integrator).
kDiscretized,

/// For simulating the system using compliant contact, Coulomb friction,
/// and ordinary differential equations.
kCompliant
/// For modeling the system using compliant contact, Coulomb friction,
/// and ordinary differential equations and simulating the system
/// through standard algorithms for solving initial value problems.
kContinuous
};

/// Constructor for the 2D rod system using the piecewise DAE (differential
/// algebraic equation) based approach, the time stepping approach, or the
/// compliant ordinary differential equation based approach.
/// algebraic equation) based approach, the discretization approach, or the
/// continuous ordinary differential equation based approach.
/// @param dt The integration step size. This step size cannot be reset
/// after construction.
/// @throws std::logic_error if @p dt is not positive and simulation_type is
/// kTimeStepping or @p dt is not zero and simulation_type is
/// kPiecewiseDAE or kCompliant.
explicit Rod2D(SimulationType simulation_type, double dt);
/// @throws std::logic_error if @p dt is not positive and system_type is
/// kDiscretized or @p dt is not zero and system_type is
/// kPiecewiseDAE or kContinuous.
explicit Rod2D(SystemType system_type, double dt);

static const Rod2dStateVector<T>& get_state(
const systems::ContinuousState<T>& cstate) {
Expand Down Expand Up @@ -227,15 +230,15 @@ class Rod2D : public systems::LeafSystem<T> {
half_length_);
}

/// Gets the constraint force mixing parameter (CFM, used for time stepping
/// Gets the constraint force mixing parameter (CFM, used for discretized
/// systems only), which should lie in the interval [0, infinity].
double get_cfm() const {
return 1.0 /
(stiffness_ * dt_ + TransformDissipationToDampingAboutDeformation(
kCharacteristicDeformation));
}

/// Gets the error reduction parameter (ERP, used for time stepping
/// Gets the error reduction parameter (ERP, used for discretized
/// systems only), which should lie in the interval [0, 1].
double get_erp() const {
return dt_ * stiffness_ / (stiffness_ * dt_ +
Expand Down Expand Up @@ -312,7 +315,7 @@ class Rod2D : public systems::LeafSystem<T> {
}

/// Sets stiffness and dissipation for the rod from cfm and erp values (used
/// for time stepping implementations).
/// for discretized system implementations).
void SetStiffnessAndDissipation(double cfm, double erp) {
// These values were determined by solving the equations:
// cfm = 1 / (dt * stiffness + damping)
Expand All @@ -330,7 +333,7 @@ class Rod2D : public systems::LeafSystem<T> {
double get_mu_static() const { return mu_s_; }

/// Set contact stiction coefficient (>= mu_coulomb). This has no
/// effect if the rod model is time stepping.
/// effect if the rod model is discretized.
void set_mu_static(double mu_static) {
DRAKE_DEMAND(mu_static >= mu_);
mu_s_ = mu_static;
Expand Down Expand Up @@ -378,18 +381,19 @@ class Rod2D : public systems::LeafSystem<T> {
/// this method returns `false`.
bool IsImpacting(const systems::Context<T>& context) const;

/// Gets the integration step size for the time stepping system.
/// Gets the integration step size for the discretized system.
/// @returns 0 if this is a DAE-based system.
double get_integration_step_size() const { return dt_; }

/// Gets the model and simulation type for this system.
SimulationType get_simulation_type() const { return simulation_type_; }
SystemType get_system_type() const { return system_type_; }

/// Return net contact forces as a spatial force F_Ro_W=(fx,fy,τ) where
/// translational force f_Ro_W=(fx,fy) is applied at the rod origin Ro,
/// and torque t_R=τ is the moment due to the contact forces actually being
/// applied elsewhere. The returned spatial force may be the resultant of
/// multiple active contact points. Only valid for simulation type kCompliant.
/// multiple active contact points. Only valid for simulation type
/// kContinuous.
Vector3<T> CalcCompliantContactForces(
const systems::Context<T>& context) const;

Expand Down Expand Up @@ -542,21 +546,21 @@ class Rod2D : public systems::LeafSystem<T> {
// Quintic step function approximation used by Stribeck friction model.
static T step5(const T& x);

// Friction model used in compliant contact.
// Friction model used in the continuous model.
static T CalcMuStribeck(const T& us, const T& ud, const T& v);

// The constraint solver.
multibody::constraint::ConstraintSolver<T> solver_;

// Solves linear complementarity problems for time stepping.
// Solves linear complementarity problems for the discretized system.
solvers::MobyLCPSolver<T> lcp_;

// The simulation type, unable to be changed after object construction.
const SimulationType simulation_type_;
// The system type, unable to be changed after object construction.
const SystemType system_type_;

// TODO(edrumwri,sherm1) Document these defaults once they stabilize.

double dt_{0.}; // Integration step-size for time stepping approach.
double dt_{0.}; // Step-size for the discretization approach.
double mass_{1.}; // The mass of the rod (kg).
double half_length_{1.}; // The length of the rod (m).
double mu_{1000.}; // The (dynamic) coefficient of friction.
Expand Down
Loading

0 comments on commit fc428c0

Please sign in to comment.