UpdatelessFunction.h

Go to the documentation of this file.

 
#pragma once
 
#include <tvm/function/abstract/Function.h>
#include <tvm/graph/CallGraph.h>
#include <tvm/internal/meta.h>
 
#include <algorithm>
#include <initializer_list>
 
namespace tvm
{
 
namespace utils
{
 
class TVM_DLLAPI UpdatelessFunction
{
public:
  UpdatelessFunction(FunctionPtr f);
 
  inline const VariableVector & variables() const noexcept { return f_->variables(); }
 
  inline const FunctionPtr & function() const noexcept { return f_; }
 
  template<typename... Vals>
  const Eigen::VectorXd & value(Vals &&... vals) const;
 
  template<typename... Vals>
  tvm::internal::MatrixConstRefWithProperties jacobian(const Variable & x, Vals &&... vals) const;
 
  template<typename... Vals>
  const Eigen::VectorXd & velocity(Vals &&... vals) const;
 
  template<typename... Vals>
  const Eigen::VectorXd & normalAcceleration(Vals &&... vals) const;
 
  template<typename... Vals>
  tvm::internal::MatrixConstRefWithProperties JDot(const Variable & x, Vals &&... vals) const;
 
private:
  static Eigen::VectorXd toVec(std::initializer_list<double> val);
 
  void assign(size_t i, const Eigen::VectorXd & val, bool value) const;
  void assign(Variable & x, const Eigen::VectorXd & val, bool value) const;
 
  void assign(const Eigen::VectorXd & val) const;
  void assign(const Eigen::VectorXd & val, const Eigen::VectorXd & vel) const;
 
  template<typename... Vals>
  void parseValues(const Eigen::VectorXd & v, Vals &&... vals) const;
  template<typename... Vals>
  void parseValues(std::initializer_list<double> v, Vals &&... vals) const;
  template<typename... Vals>
  void parseValues(Variable & x, Vals &&... vals) const;
 
  template<typename... Vals>
  void parseValues_(int i, const Eigen::VectorXd & v, Vals &&... vals) const;
  template<typename... Vals>
  void parseValues_(int i, std::initializer_list<double> v, Vals &&... vals) const;
  template<typename... Vals>
  void parseValues_(Variable & x, const Eigen::VectorXd & v, Vals &&... vals) const;
  template<typename... Vals>
  void parseValues_(Variable & x, std::initializer_list<double> v, Vals &&... vals) const;
 
  template<typename T>
  void parseValues_(T) const;
 
  void parseValues_(int i, const Eigen::VectorXd & v) const;
  void parseValues_(int i, std::initializer_list<double> v) const;
  void parseValues_(Variable & x, const Eigen::VectorXd & v) const;
  void parseValues_(Variable & x, std::initializer_list<double> v) const;
 
  template<typename... Vals>
  void parseValuesAndVelocities(const Eigen::VectorXd & v, Vals &&... vals) const;
  template<typename... Vals>
  void parseValuesAndVelocities(std::initializer_list<double> v, Vals &&... vals) const;
  template<typename... Vals>
  void parseValuesAndVelocities(Variable & x, Vals &&... vals) const;
 
  template<typename... Vals>
  void parseValuesAndVelocities_(int i,
                                 const Eigen::VectorXd & val,
                                 const Eigen::VectorXd & vel,
                                 Vals &&... vals) const;
  template<typename... Vals>
  void parseValuesAndVelocities_(int i,
                                 const Eigen::VectorXd & val,
                                 std::initializer_list<double> vel,
                                 Vals &&... vals) const;
  template<typename... Vals>
  void parseValuesAndVelocities_(int i,
                                 std::initializer_list<double> val,
                                 const Eigen::VectorXd & vel,
                                 Vals &&... vals) const;
  template<typename... Vals>
  void parseValuesAndVelocities_(int i,
                                 std::initializer_list<double> val,
                                 std::initializer_list<double> vel,
                                 Vals &&... vals) const;
  template<typename... Vals>
  void parseValuesAndVelocities_(Variable & x,
                                 const Eigen::VectorXd & val,
                                 const Eigen::VectorXd & vel,
                                 Vals &&... vals) const;
  template<typename... Vals>
  void parseValuesAndVelocities_(Variable & x,
                                 const Eigen::VectorXd & val,
                                 std::initializer_list<double> vel,
                                 Vals &&... vals) const;
  template<typename... Vals>
  void parseValuesAndVelocities_(Variable & x,
                                 std::initializer_list<double> val,
                                 const Eigen::VectorXd & vel,
                                 Vals &&... vals) const;
  template<typename... Vals>
  void parseValuesAndVelocities_(Variable & x,
                                 std::initializer_list<double> val,
                                 std::initializer_list<double> vel,
                                 Vals &&... vals) const;
 
  template<typename T>
  void parseValuesAndVelocities_(T) const;
  template<typename T>
  void parseValuesAndVelocities_(int, T) const;
  template<typename T, typename U>
  void parseValuesAndVelocities_(T, U) const;
 
  void parseValuesAndVelocities_(int i, const Eigen::VectorXd & val, const Eigen::VectorXd & vel) const;
  void parseValuesAndVelocities_(int i, const Eigen::VectorXd & val, std::initializer_list<double> vel) const;
  void parseValuesAndVelocities_(int i, std::initializer_list<double> val, const Eigen::VectorXd & vel) const;
  void parseValuesAndVelocities_(int i, std::initializer_list<double> val, std::initializer_list<double> vel) const;
  void parseValuesAndVelocities_(Variable & x, const Eigen::VectorXd & val, const Eigen::VectorXd & vel) const;
  void parseValuesAndVelocities_(Variable & x, const Eigen::VectorXd & val, std::initializer_list<double> vel) const;
  void parseValuesAndVelocities_(Variable & x, std::initializer_list<double> val, const Eigen::VectorXd & vel) const;
  void parseValuesAndVelocities_(Variable & x,
                                 std::initializer_list<double> val,
                                 std::initializer_list<double> vel) const;
 
  FunctionPtr f_;
  tvm::graph::CallGraph valueGraph_;
  tvm::graph::CallGraph jacobianGraph_;
  tvm::graph::CallGraph velocityGraph_;
  tvm::graph::CallGraph normalAccelerationGraph_;
  tvm::graph::CallGraph JDotGraph_;
  // ensure that derivative will exist.
  std::vector<VariablePtr> dx_;
};
 
template<typename... Vals>
inline const Eigen::VectorXd & UpdatelessFunction::value(Vals &&... vals) const
{
  using Output = tvm::function::abstract::Function::Output;
  if(f_->isOutputEnabled(Output::Value))
  {
    parseValues(std::forward<Vals>(vals)...);
    valueGraph_.execute();
    return f_->value();
  }
  else
  {
    throw std::runtime_error("Underlying function does not provide a value output.");
  }
}
 
template<typename... Vals>
inline tvm::internal::MatrixConstRefWithProperties UpdatelessFunction::jacobian(const Variable & x,
                                                                                Vals &&... vals) const
{
  using Output = tvm::function::abstract::Function::Output;
  if(f_->isOutputEnabled(Output::Jacobian))
  {
    parseValues(std::forward<Vals>(vals)...);
    jacobianGraph_.execute();
    return f_->jacobian(x);
  }
  else
  {
    throw std::runtime_error("Underlying function does not provide a jacobian output.");
  }
}
 
template<typename... Vals>
inline const Eigen::VectorXd & UpdatelessFunction::velocity(Vals &&... vals) const
{
  using Output = tvm::function::abstract::Function::Output;
  if(f_->isOutputEnabled(Output::Velocity))
  {
    parseValuesAndVelocities(std::forward<Vals>(vals)...);
    velocityGraph_.execute();
    return f_->velocity();
  }
  else
  {
    throw std::runtime_error("Underlying function does not provide a velocity output.");
  }
}
 
template<typename... Vals>
inline const Eigen::VectorXd & UpdatelessFunction::normalAcceleration(Vals &&... vals) const
{
  using Output = tvm::function::abstract::Function::Output;
  if(f_->isOutputEnabled(Output::NormalAcceleration))
  {
    parseValuesAndVelocities(std::forward<Vals>(vals)...);
    normalAccelerationGraph_.execute();
    return f_->normalAcceleration();
  }
  else
  {
    throw std::runtime_error("Underlying function does not provide a normalAcceleration output.");
  }
}
 
template<typename... Vals>
inline tvm::internal::MatrixConstRefWithProperties UpdatelessFunction::JDot(const Variable & x, Vals &&... vals) const
{
  using Output = tvm::function::abstract::Function::Output;
  if(f_->isOutputEnabled(Output::JDot))
  {
    parseValuesAndVelocities(std::forward<Vals>(vals)...);
    JDotGraph_.execute();
    return f_->JDot(x);
  }
  else
  {
    throw std::runtime_error("Underlying function does not provide a JDot output.");
  }
}
 
template<typename... Vals>
inline void UpdatelessFunction::parseValues(const Eigen::VectorXd & v, Vals &&... vals) const
{ parseValues_(0, v, std::forward<Vals>(vals)...); }
 
template<typename... Vals>
inline void UpdatelessFunction::parseValues(std::initializer_list<double> v, Vals &&... vals) const
{ parseValues_(0, v, std::forward<Vals>(vals)...); }
 
template<typename... Vals>
inline void UpdatelessFunction::parseValues(Variable & x, Vals &&... vals) const
{ parseValues_(x, std::forward<Vals>(vals)...); }
 
template<typename... Vals>
inline void UpdatelessFunction::parseValues_(int i, const Eigen::VectorXd & v, Vals &&... vals) const
{
  assign(i, v, true);
  parseValues_(i + 1, std::forward<Vals>(vals)...);
}
 
template<typename... Vals>
inline void UpdatelessFunction::parseValues_(int i, std::initializer_list<double> v, Vals &&... vals) const
{ parseValues_(i, toVec(v), std::forward<Vals>(vals)...); }
 
template<typename... Vals>
inline void UpdatelessFunction::parseValues_(Variable & x, const Eigen::VectorXd & v, Vals &&... vals) const
{
  assign(x, v, true);
  parseValues_(std::forward<Vals>(vals)...);
}
 
template<typename... Vals>
inline void UpdatelessFunction::parseValues_(Variable & x, std::initializer_list<double> v, Vals &&... vals) const
{ parseValues_(x, toVec(v), std::forward<Vals>(vals)...); }
 
template<typename T>
inline void UpdatelessFunction::parseValues_(T) const
{
  static_assert(
      ::tvm::internal::always_false<T>::value,
      "Incorrect number of argument. You likely did not observe the alternation between variables and values.");
}
 
template<typename... Vals>
inline void UpdatelessFunction::parseValuesAndVelocities(const Eigen::VectorXd & v, Vals &&... vals) const
{ parseValuesAndVelocities_(0, v, std::forward<Vals>(vals)...); }
 
template<typename... Vals>
inline void UpdatelessFunction::parseValuesAndVelocities(std::initializer_list<double> v, Vals &&... vals) const
{ parseValuesAndVelocities_(0, v, std::forward<Vals>(vals)...); }
 
template<typename... Vals>
inline void UpdatelessFunction::parseValuesAndVelocities(Variable & x, Vals &&... vals) const
{ parseValuesAndVelocities_(x, std::forward<Vals>(vals)...); }
 
template<typename... Vals>
inline void UpdatelessFunction::parseValuesAndVelocities_(int i,
                                                          const Eigen::VectorXd & val,
                                                          const Eigen::VectorXd & vel,
                                                          Vals &&... vals) const
{
  assign(i, val, true);
  assign(i, vel, false);
  parseValuesAndVelocities_(i + 1, std::forward<Vals>(vals)...);
}
 
template<typename... Vals>
inline void UpdatelessFunction::parseValuesAndVelocities_(int i,
                                                          const Eigen::VectorXd & val,
                                                          std::initializer_list<double> vel,
                                                          Vals &&... vals) const
{ parseValuesAndVelocities_(i, val, toVec(vel), std::forward<Vals>(vals)...); }
 
template<typename... Vals>
inline void UpdatelessFunction::parseValuesAndVelocities_(int i,
                                                          std::initializer_list<double> val,
                                                          const Eigen::VectorXd & vel,
                                                          Vals &&... vals) const
{ parseValuesAndVelocities_(i, toVec(val), vel, std::forward<Vals>(vals)...); }
 
template<typename... Vals>
inline void UpdatelessFunction::parseValuesAndVelocities_(int i,
                                                          std::initializer_list<double> val,
                                                          std::initializer_list<double> vel,
                                                          Vals &&... vals) const
{ parseValuesAndVelocities_(i, toVec(val), toVec(vel), std::forward<Vals>(vals)...); }
 
template<typename... Vals>
inline void UpdatelessFunction::parseValuesAndVelocities_(Variable & x,
                                                          const Eigen::VectorXd & val,
                                                          const Eigen::VectorXd & vel,
                                                          Vals &&... vals) const
{
  assign(x, val, true);
  assign(x, vel, false);
  parseValuesAndVelocities_(std::forward<Vals>(vals)...);
}
 
template<typename... Vals>
inline void UpdatelessFunction::parseValuesAndVelocities_(Variable & x,
                                                          const Eigen::VectorXd & val,
                                                          std::initializer_list<double> vel,
                                                          Vals &&... vals) const
{ parseValuesAndVelocities_(x, val, toVec(vel), std::forward<Vals>(vals)...); }
 
template<typename... Vals>
inline void UpdatelessFunction::parseValuesAndVelocities_(Variable & x,
                                                          std::initializer_list<double> val,
                                                          const Eigen::VectorXd & vel,
                                                          Vals &&... vals) const
{ parseValuesAndVelocities_(x, toVec(val), vel, std::forward<Vals>(vals)...); }
 
template<typename... Vals>
inline void UpdatelessFunction::parseValuesAndVelocities_(Variable & x,
                                                          std::initializer_list<double> val,
                                                          std::initializer_list<double> vel,
                                                          Vals &&... vals) const
{ parseValuesAndVelocities_(x, toVec(val), toVec(vel), std::forward<Vals>(vals)...); }
 
template<typename T>
inline void UpdatelessFunction::parseValuesAndVelocities_(T) const
{
  static_assert(::tvm::internal::always_false<T>::value, "Incorrect number of argument. You likely did not observe the "
                                                         "alternation between variables, values and velocities.");
}
 
template<typename T>
inline void UpdatelessFunction::parseValuesAndVelocities_(int, T) const
{
  static_assert(::tvm::internal::always_false<T>::value,
                "Incorrect number of argument. You likely forgot a value or velocity.");
}
 
template<typename T, typename U>
inline void UpdatelessFunction::parseValuesAndVelocities_(T, U) const
{
  static_assert(::tvm::internal::always_false<T>::value, "Incorrect number of argument. You likely did not observe the "
                                                         "alternation between variables, values and velocities.");
}
 
} // namespace utils
 
} // namespace tvm