dynamic_constraint.h

Go to the documentation of this file.

#pragma once

#include <memory>

#include <Eigen/Core>

#include <drake/drakeDynamicConstraint_export.h>
#include <drake/core/Gradient.h>
#include <drake/solvers/Constraint.h>
#include <drake/systems/System.h>

namespace drake {
namespace systems {

class DRAKEDYNAMICCONSTRAINT_EXPORT DynamicConstraint :
      public solvers::Constraint {
 public:
  DynamicConstraint(int num_states, int num_inputs);
  virtual ~DynamicConstraint();

  void eval(const Eigen::Ref<const Eigen::VectorXd>& x,
                    Eigen::VectorXd& y) const override;
  void eval(const Eigen::Ref<const Drake::TaylorVecXd>& x,
                    Drake::TaylorVecXd& y) const override;

 protected:
  virtual void dynamics(const Drake::TaylorVecXd& state,
                        const Drake::TaylorVecXd& input,
                        Drake::TaylorVecXd* xdot) const = 0;

 private:
  int num_states_;
  int num_inputs_;
};

template <typename System>
class SystemDynamicConstraint : public DynamicConstraint {
 public:
  // TODO(sam.creasey) Should this be a const bare ptr?
  explicit SystemDynamicConstraint(std::shared_ptr<System> system)
      : DynamicConstraint(Drake::getNumStates(*system),
                          Drake::getNumInputs(*system)),
        system_(system) {}

 private:
  void dynamics(const Drake::TaylorVecXd& state,
                const Drake::TaylorVecXd& input,
                Drake::TaylorVecXd* xdot) const override {
    typename System::template StateVector<Drake::TaylorVarXd> x = state;
    typename System::template InputVector<Drake::TaylorVarXd> u = input;
    Drake::TaylorVarXd t(1);
    t = 0;
    *xdot = toEigen(system_->dynamics(t, x, u));
  }

  std::shared_ptr<System> system_;
};

}  // systems
}  // drake