NewtonEuler.h

#ifndef NEWTON_EULER_H
#define NEWTON_EULER_H

#include "RobotDynamics3D.h"
#include "Wrench.h"


class NewtonEulerSolver
{
public:
  NewtonEulerSolver(RobotDynamics3D& robot);
  //sets the external wrenches equal to gravity
  void SetGravityWrenches(const Vector3& gravity);
  //forward and inverse dynamics, assuming the current state of the robot is updated
  void CalcTorques(const Vector& ddq,Vector& t);
  void CalcAccel(const Vector& t,Vector& ddq);
  void CalcKineticEnergyMatrix(Matrix& B);
  void CalcKineticEnergyMatrixInverse(Matrix& Binv);
  void CalcResidualTorques(Vector& CG);
  void CalcResidualAccel(Vector& ddq0);

  //helpers (also assume current state of robot has been updated)
  void MulKineticEnergyMatrix(const Vector& x,Vector& Bx);
  void MulKineticEnergyMatrix(const Matrix& A,Matrix& BA);
  void MulKineticEnergyMatrixInverse(const Vector& x,Vector& Binvx);
  void MulKineticEnergyMatrixInverse(const Matrix& A,Matrix& BinvA);
  void CalcVelocities();
  void CalcLinkAccel(const Vector& ddq);
  void SelfTest();

  RobotDynamics3D& robot;
  std::vector<Wrench> externalWrenches;  

  //temporary/output
  std::vector<std::vector<int> > children;
  std::vector<RigidBodyVelocity> velocities;
  std::vector<RigidBodyVelocity> accelerations;
  std::vector<Wrench> jointWrenches;  
  std::vector<SpatialMatrix> inertiaMatrices;  
  std::vector<SpatialVector> biasingForces;     
};

#endif