Hold.h

Go to the documentation of this file.

#ifndef HOLD_H
#define HOLD_H

#include <KrisLibrary/robotics/Contact.h>
#include <KrisLibrary/robotics/IK.h>
using namespace std;

struct Hold
{
  void Transform(const RigidTransform& T);
  inline void GetCentroid(Vector3& c) const 
  {
    c.setZero();
    for(size_t i=0;i<contacts.size();i++)
      c += contacts[i].x;
    c/=(Real)contacts.size();
  }

  //Returns the average normal of the contacts
  inline void GetNormal(Vector3& n) const 
  {
    n.setZero();
    for(size_t j=0;j<contacts.size();j++)
      n += contacts[j].n;
    n.inplaceNormalize();
  }

  inline Vector3 GetCentroid() const {  Vector3 c; GetCentroid(c); return c; }
  inline Vector3 GetNormal() const { Vector3 n; GetNormal(n); return n; }

  void SetupIKConstraint(const Vector3& localPos0, const Vector3& rot);
  void SetupFixedIKConstraint( const Vector3& localPos0, const Matrix3& rot);
  void SetupPointIKConstraint(const Vector3& localPos0);
  void SetupAxisIKConstraint( const Vector3& localPos0, const Vector3& axis, const Vector3& axis_w);

  int link;
  vector<ContactPoint> contacts;
  IKGoal ikConstraint;
};

bool operator == (const Hold& a,const Hold& b);
bool operator < (const Hold& a,const Hold& b);
bool operator > (const Hold& a,const Hold& b);
ostream& operator << (ostream&, const Hold&);
istream& operator >> (istream&, Hold&);

#endif