Door hinge example for #12697

door_hinge.cc

#include "sim/dish/door_hinge.h"

using drake::AutoDiffXd;
using drake::systems::Context;
using drake::multibody::internal::MultibodyTree;
using drake::multibody::internal::PositionKinematicsCache;
using drake::multibody::internal::VelocityKinematicsCache;
using drake::multibody::ForceElement;
using drake::multibody::Joint;
using drake::multibody::MultibodyForces;
using drake::multibody::RevoluteJoint;
using drake::symbolic::Expression;

namespace anzu {
namespace sim {
namespace dish {

namespace {
// Approximates {x<0: -1 ; x>0: 1} outside of -threshold < x < threshold.
// That is, this is a step.
template <typename T>
T sigmoid(T x, T threshold) {
  return tanh(x / threshold);
}

// First derivative of the sigmoid -- a hump at 0 that integrates to 2.
template <typename T>
T singlet(T x, T threshold) {
  return 1 - sigmoid(x, threshold) * sigmoid(x, threshold);
}

// Second derivative of the sigmoid -- a lump at negative x that integrates to
// 1 and a lump at positive x that integrates to -1.
template <typename T>
T doublet(T x, T threshold) {
  return 2 * sigmoid(x, threshold) * singlet(x, threshold);
}
}  // namespace

namespace internal {
// Free functions to compute the dynamic properties of the hinge in
// angle/torque terms, separate from Drake idioms.

template <typename T>
T hinge_frictional_torque(T angle, T angular_velocity,
                          const DoorHingeConfig& config) {
  T torque = 0.;
  torque -= (config.dynamic_friction_torque *
             sigmoid(angular_velocity, T(config.motion_threshold)));
  torque -= (config.static_friction_torque *
             doublet(angular_velocity, T(config.motion_threshold)));
  torque -= angular_velocity * config.viscous_friction;
  return torque;
}

template <typename T>
T hinge_spring_torque(T angle, T angular_velocity,
                      const DoorHingeConfig& config) {
  T torque = 0.;
  const T catch_center = config.catch_width / 2;
  torque += doublet(angle - catch_center, catch_center) * config.catch_torque;
  torque -= (angle - config.spring_zero_angle_rad) * config.spring_constant;
  return torque;
}

template <typename T>
T hinge_torque(T angle, T angular_velocity,
               const DoorHingeConfig& config) {
  T result = hinge_frictional_torque(angle, angular_velocity, config) +
      hinge_spring_torque(angle, angular_velocity, config);
  return result;
}

template <typename T>
T hinge_conservative_power(T angle, T angular_velocity,
                           const DoorHingeConfig& config) {
  return angular_velocity *
      hinge_spring_torque(angle, angular_velocity, config);
}

template <typename T>
T hinge_nonconservative_power(T angle, T angular_velocity,
                              const DoorHingeConfig& config) {
  return angular_velocity *
      hinge_frictional_torque(angle, angular_velocity, config);
}

template <typename T>
T hinge_stored_energy(T angle, T angular_velocity,
                      const DoorHingeConfig& config) {
  T energy = 0.;
  const T catch_center = config.catch_width / 2;
  energy += (singlet(angle - catch_center, catch_center) * config.catch_torque
             / config.catch_torque);
  const T spring_offset = (angle - config.spring_zero_angle_rad);
  energy += 0.5 * spring_offset * spring_offset * config.spring_constant;
  return energy;
}
}  // namespace internal

template <typename T>
T DoorHinge<T>::CalcPotentialEnergy(
    const Context<T>& context,
    const PositionKinematicsCache<T>&) const {
  const Joint<T>& joint = tree_->get_joint(joint_id_);
  const T angle = joint.GetOnePosition(context);
  const T angular_velocity = joint.GetOneVelocity(context);
  return internal::hinge_stored_energy(angle, angular_velocity, config_);
}

template <typename T>
T DoorHinge<T>::CalcConservativePower(
    const Context<T>& context,
    const PositionKinematicsCache<T>&,
    const VelocityKinematicsCache<T>&) const {
  const Joint<T>& joint = tree_->get_joint(joint_id_);
  const T angle = joint.GetOnePosition(context);
  const T angular_velocity = joint.GetOneVelocity(context);
  return internal::hinge_conservative_power(angle, angular_velocity, config_);
}

template <typename T>
T DoorHinge<T>::CalcNonConservativePower(
    const Context<T>& context,
    const PositionKinematicsCache<T>&,
    const VelocityKinematicsCache<T>&) const {
  const Joint<T>& joint = tree_->get_joint(joint_id_);
  const T angle = joint.GetOnePosition(context);
  const T angular_velocity = joint.GetOneVelocity(context);
  return internal::hinge_nonconservative_power(
      angle, angular_velocity, config_);
}

template <typename T>
void DoorHinge<T>::DoCalcAndAddForceContribution(
    const Context<T>& context,
    const PositionKinematicsCache<T>&,
    const VelocityKinematicsCache<T>&,
    MultibodyForces<T>* forces) const {
  const RevoluteJoint<T>& joint =
      dynamic_cast<const RevoluteJoint<T>&>(tree_->get_joint(joint_id_));
  const T angle = joint.GetOnePosition(context);
  const T angular_velocity = joint.GetOneVelocity(context);
  const T torque = internal::hinge_torque(angle, angular_velocity, config_);
  joint.AddInTorque(context, torque, forces);
}

// Drake template boilerplate:
template <typename T>
template <typename ToScalar>
std::unique_ptr<ForceElement<ToScalar>>
DoorHinge<T>::TemplatedClone(const MultibodyTree<ToScalar>& tree_clone) const {
  return std::make_unique<DoorHinge<ToScalar>>(
      &tree_clone, model_instance_id_, joint_id_, config_);
}

template <typename T>
std::unique_ptr<ForceElement<double>>
DoorHinge<T>::DoCloneToScalar(const MultibodyTree<double>& tree_clone) const {
  return TemplatedClone(tree_clone);
}

template <typename T>
std::unique_ptr<ForceElement<AutoDiffXd>>
DoorHinge<T>::DoCloneToScalar(
    const MultibodyTree<AutoDiffXd>& tree_clone) const {
  return TemplatedClone(tree_clone);
}

template <typename T>
std::unique_ptr<ForceElement<Expression>>
DoorHinge<T>::DoCloneToScalar(
    const MultibodyTree<Expression>& tree_clone) const {
  return TemplatedClone(tree_clone);
}

DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS(class DoorHinge)

}  // namespace dish
}  // namespace sim
}  // namespace anzu

door_hinge.h

pragma once

#include <memory>
#include <vector>

#include "drake/common/default_scalars.h"
#include "drake/common/drake_copyable.h"
#include "drake/multibody/tree/force_element.h"
#include "drake/multibody/tree/multibody_tree.h"
#include "drake/multibody/tree/revolute_joint.h"

namespace anzu {
namespace sim {
namespace dish {

/// Configuration structure for the door hinge.
struct DoorHingeConfig {
  double spring_zero_angle_rad;    //< radians (outward from closed)
  double spring_constant;          //< Nm/rad (toward `spring_zero_angle_rad`)
  double dynamic_friction_torque;  //< Nm (opposite direction of motion)
  double static_friction_torque;   //< Nm (opposite direction of motion)
  double viscous_friction;         //< Nms/rad (Nm per rad/s) (opp. motion)
  double catch_width;              //< radians (from closed (θ=0) position)
  double catch_torque;             //< Nm (applied over `catch_width`)

  /// Realistic frictional force is very stiff, reversing entirely over zero
  /// change in position or velocity, which kills integrators.  We approximate
  /// it with a continuous function.  This constant is the scaling factor on
  /// that function -- very approximately the rad/s at which half of the full
  /// frictional force is applied.  This number is nonphysical; make it small
  /// but not so small that the simulation vibrates or explodes.
  double motion_threshold;        //< rad/s

  /// Initialize to empirically reasonable values measured approximately by
  /// banging on the door of Harvard's dishwasher with a force gauge.
  DoorHingeConfig()
      // TODO(ggould) Should these be gflags?  The old spring values were...
      : spring_zero_angle_rad(0.8),
        spring_constant(0),
        dynamic_friction_torque(5),
        static_friction_torque(10),
        viscous_friction(0),
        catch_width(0.02),
        catch_torque(25),
        motion_threshold(0.01) {}
};


/// Our dishwasher door has a frictional torque sufficient for it to rest
/// motionless at any angle, a catch at the top to hold it in place, a dashpot
/// (viscous friction source) to prevent it from swinging too fast, and a
/// spring to counteract some of its mass.  This class implements a "christmas
/// tree" accumulation of these different forces in an empirical and
/// unprincipled way.
///
/// The door is assumed to be closed at θ=0, opening in the positive-θ
/// direction.  This class applies all hinge-originating forces, so it can be
/// used instead of / interchangeably with SDF viscous damping.
template <typename T>
class DoorHinge : public drake::multibody::ForceElement<T> {
 public:
  DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(DoorHinge)

  /// Construct a hinge force element with parameters @p config applied to the
  /// specified @p joint.
  ///
  /// Some minimal sanity checking is asserted on the supplied config.
  ///
  /// @p joint is aliased and must remain valid for the lifetime of this
  /// object and its clones.
  DoorHinge(const drake::multibody::internal::MultibodyTree<T>* tree,
            drake::multibody::ModelInstanceIndex model_instance_id,
            drake::multibody::JointIndex joint_id,
            const DoorHingeConfig& config)
      : drake::multibody::ForceElement<T>(model_instance_id),
        tree_(tree),
        model_instance_id_(model_instance_id),
        joint_id_(joint_id),
        config_(config) {
    assert(config_.spring_constant >= 0);
    assert(config_.dynamic_friction_torque >= 0);
    assert(config_.static_friction_torque >= 0);
    assert(config_.viscous_friction_torque >= 0);
    assert(config_.catch_width > 0);
    assert(config_.motion_threshold > 0);
  }

  T CalcPotentialEnergy(
      const drake::systems::Context<T>& context,
      const drake::multibody::internal::PositionKinematicsCache<T>&)
      const override;

  T CalcConservativePower(
      const drake::systems::Context<T>& context,
      const drake::multibody::internal::PositionKinematicsCache<T>&,
      const drake::multibody::internal::VelocityKinematicsCache<T>&)
      const override;

  T CalcNonConservativePower(
      const drake::systems::Context<T>& context,
      const drake::multibody::internal::PositionKinematicsCache<T>&,
      const drake::multibody::internal::VelocityKinematicsCache<T>&)
      const override;

 protected:
  void DoCalcAndAddForceContribution(
      const drake::systems::Context<T>& context,
      const drake::multibody::internal::PositionKinematicsCache<T>&,
      const drake::multibody::internal::VelocityKinematicsCache<T>&,
      drake::multibody::MultibodyForces<T>* forces) const override;

  std::unique_ptr<drake::multibody::ForceElement<double>>
  DoCloneToScalar(
      const drake::multibody::internal::MultibodyTree<double>& tree_clone)
      const override;

  std::unique_ptr<drake::multibody::ForceElement<drake::AutoDiffXd>>
  DoCloneToScalar(
      const drake::multibody::internal::MultibodyTree
      <drake::AutoDiffXd>& tree_clone)
      const override;

  std::unique_ptr<drake::multibody::ForceElement<drake::symbolic::Expression>>
  DoCloneToScalar(
      const drake::multibody::internal::MultibodyTree
      <drake::symbolic::Expression>& tree_clone)
      const override;

 private:
  template <typename ToScalar>
  std::unique_ptr<drake::multibody::ForceElement<ToScalar>>
  TemplatedClone(
      const drake::multibody::internal::MultibodyTree<ToScalar>&) const;

  const drake::multibody::internal::MultibodyTree<T>* tree_;
  const drake::multibody::ModelInstanceIndex model_instance_id_;
  const drake::multibody::JointIndex joint_id_;
  DoorHingeConfig config_;
};

// Drake-less math exposed for easier testing.
namespace internal {
template <typename T>
T hinge_frictional_torque(T angle, T angular_velocity,
                          const DoorHingeConfig& config);

template <typename T>
T hinge_spring_torque(T angle, T angular_velocity,
                      const DoorHingeConfig& config);

template <typename T>
T hinge_torque(T angle, T angular_velocity,
               const DoorHingeConfig& config);

template <typename T>
T hinge_conservative_power(T angle, T angular_velocity,
                           const DoorHingeConfig& config);

template <typename T>
T hinge_nonconservative_power(T angle, T angular_velocity,
                              const DoorHingeConfig& config);

template <typename T>
T hinge_stored_energy(T angle, T angular_velocity,
                      const DoorHingeConfig& config);
}  // namespace internal

}  // namespace dish
}  // namespace sim
}  // namespace anzu