Grasp.h

#ifndef GRASP_H
#define GRASP_H

#include <KrisLibrary/robotics/Contact.h>
#include <KrisLibrary/robotics/IK.h>
#include "Hold.h"
#include "Stance.h"
#include <vector>
class TiXmlElement;

class Grasp
{
 public:
  Grasp();

  inline size_t NumConstraints() const { return constraints.size(); }

  inline bool IsSimple() const
  {
    return constraints.size()==1 && IKGoal::NumDims(constraints[0].posConstraint)+IKGoal::NumDims(constraints[0].rotConstraint) == 6;
  }

  void Add(const Grasp& grasp);
  void SetHold(const Hold& h);
  void SetStance(const Stance& h);
  void GetHold(Hold& h) const;
  void GetStance(Stance& s) const;
  void GetContactFormation(ContactFormation& s) const;
  void Transform(const RigidTransform& T);
  void SetFixed(Vector& q) const;

  bool Load(TiXmlElement* e);
  bool Save(TiXmlElement* e);

  int objectIndex,robotIndex;

  vector<IKGoal> constraints;
  vector<Real> fixedValues;
  vector<int> fixedDofs;

  vector<ContactPoint> contacts;
  vector<int> contactLinks;
  vector<Vector3> forces;
};

#endif