kernel/MotionControls.h

Go to the documentation of this file.

#ifndef MOTIONCONTROLS_H
#define MOTIONCONTROLS_H

class PartControl : public controlable {
public:
  PartControl() : partFileType(0), static_(0), rigid_(1) {}

  virtual ~PartControl() {}

  virtual void register_at(ControlDevice& Ctrl) {}
  virtual void register_at(ControlDevice& Ctrl, const std::string& prefix) {
    PLocCtrl = Ctrl.getSubDevice(prefix);
    RegisterAt(PLocCtrl,"Name",partName);
    RegisterAt(PLocCtrl,"FileName",partFileName);
    RegisterAt(PLocCtrl,"FileType",partFileType);
    RegisterAt(PLocCtrl,"Static",static_);
    RegisterAt(PLocCtrl,"Rigid",rigid_);
  }

public:

  std::string partName, partFileName;
  int partFileType;
  bool static_, rigid_;
  ControlDevice PLocCtrl;
};

class AssemblyControl : public controlable {
public:
  AssemblyControl() : numParts(1), numSubAssemblies(0) {}

  virtual ~AssemblyControl() {}

  virtual void register_at(ControlDevice& Ctrl) {}
  virtual void register_at(ControlDevice& Ctrl, const std::string& prefix) {
    ALocCtrl = Ctrl.getSubDevice(prefix);
    RegisterAt(ALocCtrl,"Name",assemblyName);
    RegisterAt(ALocCtrl,"NumParts",numParts);
    RegisterAt(ALocCtrl,"NumSubAssemblies",numSubAssemblies);

    for (register int j=0; j < maxParts; j++) {
      char VariableName1[16];
      std::sprintf(VariableName1,"Part(%02d)",j+1);
      PartCtrl[j].register_at(ALocCtrl,std::string(VariableName1));
    }
  }

public:

  std::string assemblyName;
  int numParts, numSubAssemblies;
  PartControl PartCtrl[maxParts];
  AssemblyControl * AssemblyCtrl[maxSubAssemblies];
  ControlDevice ALocCtrl;
};


class JointControl : public controlable {
public:
  JointControl() : static_(0) {
    origin(0.);
    zaxis(0.);
    xaxis(0.);
    cfId.resize(10,-1);
  }

  virtual ~JointControl() {}

  virtual void register_at(ControlDevice& Ctrl) {}
  virtual void register_at(ControlDevice& Ctrl, const std::string& prefix) {
    JLocCtrl = Ctrl.getSubDevice(prefix);
    RegisterAt(JLocCtrl,"Type",jointType);
    RegisterAt(JLocCtrl,"Static",static_);
    RegisterAt(JLocCtrl,"o(0)",origin(0));
    RegisterAt(JLocCtrl,"o(1)",origin(1));
    RegisterAt(JLocCtrl,"o(2)",origin(2));
    RegisterAt(JLocCtrl,"z(0)",zaxis(0));
    RegisterAt(JLocCtrl,"z(1)",zaxis(1));
    RegisterAt(JLocCtrl,"z(2)",zaxis(2));
    RegisterAt(JLocCtrl,"x(0)",xaxis(0));
    RegisterAt(JLocCtrl,"x(1)",xaxis(1));
    RegisterAt(JLocCtrl,"x(2)",xaxis(2));

    for (register int i = 0; i < 10; i++) {
      char VariableName1[16];
      std::sprintf(VariableName1,"Id(%02d)",i+1);
      RegisterAt(JLocCtrl,std::string(VariableName1),cfId[i]);
    }
  }

public:

  std::string partName, partFileName;
  int jointType;
  PType origin, zaxis, xaxis;
  bool static_;
  std::vector<int> cfId;
  ControlDevice JLocCtrl;
};

class LinkControl : public controlable {
public:
  LinkControl() : numJoints(2), r(0.), a(0.), d(0.), t(0.) {}

  virtual ~LinkControl() {}

  virtual void register_at(ControlDevice& Ctrl) {}
  virtual void register_at(ControlDevice& Ctrl, const std::string& prefix) {
    LLocCtrl = Ctrl.getSubDevice(prefix);
    RegisterAt(LLocCtrl,"NumJoints",numJoints);
    RegisterAt(LLocCtrl,"Parent",JointIds[0]);
    RegisterAt(LLocCtrl,"Child",JointIds[1]);
    RegisterAt(LLocCtrl,"Radius",r);
    RegisterAt(LLocCtrl,"Alpha",a);
    RegisterAt(LLocCtrl,"Depth",d);
    RegisterAt(LLocCtrl,"Theta",t);
  }

public:

  std::string assemblyName;
  int numJoints;
  int JointIds[maxJoints];
  DataType r, a, d, t;
  ControlDevice LLocCtrl;
};


class ChainLinkControl : public controlable {
public:
  ChainLinkControl() :  numJoints(2), q_pre(0.), q_dot_pre(0.)  {}

  virtual ~ChainLinkControl() {}

  virtual void register_at(ControlDevice& Ctrl) {}
  virtual void register_at(ControlDevice& Ctrl, const std::string& prefix) {
    CLLocCtrl = Ctrl.getSubDevice(prefix);
    RegisterAt(CLLocCtrl,"Id",id);
    RegisterAt(CLLocCtrl,"NumJoints",numJoints);
    RegisterAt(CLLocCtrl,"ConstantPosition",constPos);
    RegisterAt(CLLocCtrl,"Position",q_pre);
    RegisterAt(CLLocCtrl,"ConstantVelocity",constVel);
    RegisterAt(CLLocCtrl,"Velocity",q_dot_pre);

  }

public:
  std::string assemblyName;
  int numJoints, id;
  int constPos, constVel;
  DataType q_pre, q_dot_pre;
  ControlDevice CLLocCtrl;
};

class ChainTernLinkControl : public controlable {
public:
  ChainTernLinkControl() : numJoints(2), q_pre(0.), q_dot_pre(0.) {}

  virtual ~ChainTernLinkControl() {}

  virtual void register_at(ControlDevice& Ctrl) {}
  virtual void register_at(ControlDevice& Ctrl, const std::string& prefix) {
    CLLocCtrl = Ctrl.getSubDevice(prefix);
    RegisterAt(CLLocCtrl,"Id",id);
    RegisterAt(CLLocCtrl,"NumJoints",numJoints);
    RegisterAt(CLLocCtrl,"CLink",cLink);
    RegisterAt(CLLocCtrl,"NLink",nLink);
    RegisterAt(CLLocCtrl,"ConstantPosition",constPos);
    RegisterAt(CLLocCtrl,"Position",q_pre);
    RegisterAt(CLLocCtrl,"ConstantVelocity",constVel);
    RegisterAt(CLLocCtrl,"Velocity",q_dot_pre);
  }

public:

  std::string assemblyName;
  int numJoints, id, cLink, nLink;
  int constPos, constVel;
  DataType q_pre, q_dot_pre;
  ControlDevice CLLocCtrl;
};

class ChainNativeLinkControl : public controlable {
public:
  ChainNativeLinkControl() : numJoints(2), q_pre(0.), q_dot_pre(0.)  {}

  virtual ~ChainNativeLinkControl() {}

  virtual void register_at(ControlDevice& Ctrl) {}
  virtual void register_at(ControlDevice& Ctrl, const std::string& prefix) {
    CLLocCtrl = Ctrl.getSubDevice(prefix);
    RegisterAt(CLLocCtrl,"Id",id);
    RegisterAt(CLLocCtrl,"NumJoints",numJoints);
    RegisterAt(CLLocCtrl,"ConstantPosition",constPos);
    RegisterAt(CLLocCtrl,"Position",q_pre);
    RegisterAt(CLLocCtrl,"ConstantVelocity",constVel);
    RegisterAt(CLLocCtrl,"Velocity",q_dot_pre);
  }

public:

  std::string assemblyName;
  int numJoints, id;
  int constPos, constVel;
  DataType q_pre, q_dot_pre;
  ControlDevice CLLocCtrl;
};

class DHChainControl : public controlable {
public:
  DHChainControl() : numSubChains(0) {}

  virtual ~DHChainControl() {}

  virtual void register_at(ControlDevice& Ctrl) {}
  virtual void register_at(ControlDevice& Ctrl, const std::string& prefix) {
    CLocCtrl = Ctrl.getSubDevice(prefix);
    RegisterAt(CLocCtrl,"Name",chainName);
    RegisterAt(CLocCtrl,"NumLinks",numLinks);
    RegisterAt(CLocCtrl,"NumTernaryLinks",numTernaryLinks);
    RegisterAt(CLocCtrl,"NumNativeLinks",numNativeLinks);
    RegisterAt(CLocCtrl,"NumSubChains",numSubChains);
    RegisterAt(CLocCtrl,"ParentChainId",parentChainId);

    for (register int j=0; j < maxLinks; j++) {
      char VariableName1[16];
      std::sprintf(VariableName1,"Link(%02d)",j+1);
      CLLinkCtrls[j].register_at(CLocCtrl,std::string(VariableName1));
      std::sprintf(VariableName1,"NativeLink(%02d)",j+1);
      CNLinkCtrls[j].register_at(CLocCtrl,std::string(VariableName1));
      std::sprintf(VariableName1,"TernaryLink(%02d)",j+1);
      CTLinkCtrls[j].register_at(CLocCtrl,std::string(VariableName1));
    }
  }

public:
  std::string chainName;
  int numParts, numLinks, numTernaryLinks, numNativeLinks, numSubChains, parentChainId;
  int DHChainCtrls[maxSubChains];
  ChainLinkControl CLLinkCtrls[maxLinks];
  ChainNativeLinkControl CNLinkCtrls[maxLinks];
  ChainTernLinkControl CTLinkCtrls[maxLinks];
  ControlDevice CLocCtrl;
};

#endif // MOTIONCONTROLS_H

---