StanceCSpace.h

#ifndef ROBOT_STANCE_CSPACE_H
#define ROBOT_STANCE_CSPACE_H

#include "ContactCSpace.h"
#include <Klampt/Contact/Stance.h>
#include <KrisLibrary/robotics/Stability.h>
#include <KrisLibrary/robotics/TorqueSolver.h>

namespace Klampt {

class StanceCSpace : public ContactCSpace
{
 public:
  StanceCSpace(WorldModel& world,int index,
                WorldPlannerSettings* settings);
  StanceCSpace(const SingleRobotCSpace& space);
  StanceCSpace(const StanceCSpace& space);
  virtual ~StanceCSpace() {}
  
  void SetStance(const Stance& s);
  void SetHold(const Hold& h);
  void CalculateSP();
  void SetSPMargin(Real margin);
  void InitTorqueSolver();

  bool CheckRBStability(const Config& x);
  bool CheckTorqueStability(const Config& x);

  Stance stance;
  Vector3 gravity;
  int numFCEdges;
  bool spCalculated;
  Real spMargin;
  SupportPolygon sp;
  ContactFormation formation;
  TorqueSolver torqueSolver;
};

} // namespace Klampt;

#endif