ZMP.h

#ifndef KLAMPT_PLANNING_ZMP_H
#define KLAMPT_PLANNING_ZMP_H

#include <Klampt/Modeling/Robot.h>
#include <Klampt/Modeling/Paths.h>
#include <KrisLibrary/planning/GeneralizedBezierCurve.h>
class NewtonEulerSolver;

void GetCOMDerivs(Robot& robot,const Config& q,const Vector& dq,const Vector& ddq,Vector3& cm,Vector3& dcm,Vector3& ddcm,NewtonEulerSolver& ne);

void GetCOMDerivs(Robot& robot,const Config& q,const Vector& dq,const Vector& ddq,Vector3& cm,Vector3& dcm,Vector3& ddcm);

Vector2 GetZMP(Robot& robot,const Config& q,const Vector& dq,const Vector& ddq,NewtonEulerSolver& ne,Real groundHeight=0,Real g=9.8);

Vector2 GetZMP(Robot& robot,const Config& q,const Vector& dq,const Vector& ddq,Real groundHeight=0,Real g=9.8);

std::vector<Vector2> GetZMPTrajectory(Robot& robot,const GeneralizedCubicBezierCurve& path,Real dt,Real groundHeight=0,Real g=9.8);

std::vector<Vector2> GetZMPTrajectory(Robot& robot,const Spline::PiecewisePolynomialND& path,Real dt,Real groundHeight=0,Real g=9.8);

#endif