Pendulum.h

Go to the documentation of this file.

/*
 * Copyright 2015-2019 CNRS-UM LIRMM, CNRS-AIST JRL
 *
 * This file is inspired by Stephane's Caron original implementation as part of
 * lipm_walking_controller <https://github.com/stephane-caron/lipm_walking_controller>
 */
 
#pragma once
#include <mc_planning/api.h>
#include <SpaceVecAlg/SpaceVecAlg>
 
namespace mc_planning
{
struct MC_PLANNING_DLLAPI Pendulum
{
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
  Pendulum();
 
  Pendulum(double lamda,
           const Eigen::Vector3d & com = Eigen::Vector3d::Zero(),
           const Eigen::Vector3d & comd = Eigen::Vector3d::Zero(),
           const Eigen::Vector3d & comdd = Eigen::Vector3d::Zero());
 
  void completeIPM(const Eigen::Vector3d & p, const Eigen::Vector3d & n);
 
  void integrateCoMJerk(const Eigen::Vector3d & comddd, double dt);
 
  void integrateIPM(Eigen::Vector3d zmp, double lambda, double dt);
 
  void reset(double lambda,
             const Eigen::Vector3d & com,
             const Eigen::Vector3d & comd = Eigen::Vector3d::Zero(),
             const Eigen::Vector3d & comdd = Eigen::Vector3d::Zero());
 
  void resetCoMHeight(double height, const Eigen::Vector3d & p, const Eigen::Vector3d & n);
 
  const Eigen::Vector3d & com() const { return com_; }
 
  const Eigen::Vector3d & comd() const { return comd_; }
 
  const Eigen::Vector3d & comdd() const { return comdd_; }
 
  Eigen::Vector3d dcm() const { return com_ + comd_ / omega_; }
 
  double omega() const { return omega_; }
 
  const Eigen::Vector3d & zmp() const { return zmp_; }
 
  const Eigen::Vector3d & zmpd() const { return zmpd_; }
 
protected:
  Eigen::Vector3d com_ = Eigen::Vector3d::Zero(); 
  Eigen::Vector3d comd_ = Eigen::Vector3d::Zero(); 
  Eigen::Vector3d comdd_ = Eigen::Vector3d::Zero(); 
  Eigen::Vector3d comddd_ = Eigen::Vector3d::Zero(); 
  Eigen::Vector3d zmp_ = Eigen::Vector3d::Zero(); 
  Eigen::Vector3d zmpd_ = Eigen::Vector3d::Zero(); 
  double omega_ = 0; 
};
} // namespace mc_planning