MotionNodes.h

Go to the documentation of this file.

#ifndef Included_MotionNodes_h_
#define Included_MotionNodes_h_

#include "Crew/PilotNode.h"
#include "Behaviors/Nodes/HeadPointerNode.h"
#include "Behaviors/Nodes/PIDNode.h"
#include "Behaviors/Nodes/PostureNode.h"
#include "Behaviors/Nodes/PostMachine.h"
#include "Behaviors/Transitions/CompletionTrans.h"
#include "Behaviors/Transitions/TimeOutTrans.h"

class WalkForward : public PilotNode {
 public:
  WalkForward(const std::string &nodename, float _xdist=0) : PilotNode(nodename,PilotTypes::walk), xdist(_xdist) {}
  float xdist;  // cache the constructor's parameter
  virtual void preStart() {
  PilotNode::preStart();
  pilotreq.dx = xdist;
  }
};

class WalkSideways : public PilotNode {
 public:
  WalkSideways(const std::string &nodename, float _ydist=0) : PilotNode(nodename,PilotTypes::walk), ydist(_ydist) {}
  float ydist;  // cache the constructor's parameter
  virtual void preStart() {
  PilotNode::preStart();
  pilotreq.dy = ydist;
  }
};

class Turn : public PilotNode {
 public:
  Turn(const std::string &nodename, float _angle=0) : PilotNode(nodename,PilotTypes::walk), angle(_angle) {}
  float angle;  // cache the constructor's parameter
  virtual void preStart() {
  PilotNode::preStart();
  pilotreq.da = angle;
  }
};

#ifdef TGT_HAS_ARMS
class ArmPower : public PIDNode {
 public:
  ArmPower(const std::string &nodename, float _level) : PIDNode(nodename,ArmOffset,ArmOffset+NumArmJoints,_level), level(_level) {}
  float level;  // cache the constructor's parameter
};

class ResetArm : public StateNode {
 public:
  ResetArm(const std::string &nodename="ResetArm") : StateNode(nodename) {}
  virtual void setup() {
   ArmPower *armpower1 = new ArmPower("armpower1",0.0);
   addNode(armpower1);

   ArmPower *armpower2 = new ArmPower("armpower2",1.0);
   addNode(armpower2);

   PostMachineCompletion *postmachinecompletion1 = new PostMachineCompletion("postmachinecompletion1");
   addNode(postmachinecompletion1);

   startnode = armpower1;

   armpower1->addTransition(new TimeOutTrans("timeouttrans1",armpower2,500));
   armpower2->addTransition(new NullTrans("nulltrans1",postmachinecompletion1));
  }
};
#else
class ArmPower : public StateNode {
 public:
  ArmPower(const std::string &nodename, float _level) : StateNode(nodename), level(_level) {}
  float level;  // cache the constructor's parameter
};
class ResetArm : public StateNode {
 public:
  ResetArm(const std::string &nodename="ResetArm") : StateNode(nodename) {}
};
#endif


// GripperPower
#if defined(TGT_IS_CALLIOPE2) ||  defined(TGT_IS_CALLIOPE3)
class GripperPower : public PIDNode {
 public:
  GripperPower(const std::string &nodename, float _level) : PIDNode(nodename,GripperOffset,GripperOffset+1,_level), level(_level) {}
  float level;  // cache the constructor's parameter
};
#elif defined(TGT_IS_CALLIOPE5)
class GripperPower : public PIDNode {
 public:
  GripperPower(const std::string &nodename, float _level) : PIDNode(nodename,LeftFingerOffset,RightFingerOffset+1,_level), level(_level) {}
  float level;  // cache the constructor's parameter
};
#else
class GripperPower : public PostStateCompletion {
 public:
  GripperPower(const std::string &nodename, float _level) : PostStateCompletion(nodename), level(_level) {}
  float level;  // cache the constructor's parameter
};
#endif

class ResetGripper : public StateNode {
 public:
  ResetGripper(const std::string &nodename="ResetGripper") : StateNode(nodename) {}
#if defined(TGT_IS_CALLIOPE2) || defined(TGT_IS_CALLIOPE3) || defined(TGT_IS_CALLIOPE5)
  virtual void setup() {
   GripperPower *gripperpower1 = new GripperPower("gripperpower1",0.0);
   addNode(gripperpower1);

   GripperPower *gripperpower2 = new GripperPower("gripperpower2",1.0);
   addNode(gripperpower2);

   PostMachineCompletion *postmachinecompletion2 = new PostMachineCompletion("postmachinecompletion2");
   addNode(postmachinecompletion2);

   startnode = gripperpower1;

   gripperpower1->addTransition(new TimeOutTrans("timeouttrans2",gripperpower2,500));
   gripperpower2->addTransition(new CompletionTrans("completiontrans1",postmachinecompletion2));
  }
#endif
};

#ifdef TGT_HAS_GRIPPER
class OpenGripper : public ArmNode {
 public:
  OpenGripper(const std::string &nodename="OpenGripper", float _percentage=0.5) : ArmNode(nodename), percentage(_percentage) {}
  float percentage;  // cache the constructor's parameter
  virtual void start() {
  ArmNode::start();
#if defined(TGT_IS_CALLIOPE) || defined(TGT_IS_CALLIOPE3)
  getMC()->setMaxSpeed(0.2);
  getMC()->openGripper(percentage);
#endif
  }
};
#else
class OpenGripper : public PostStateCompletion {
 public:
  OpenGripper(const std::string &nodename="OpenGripper", float _percentage=0.5) : PostStateCompletion(nodename), percentage(_percentage) {}
  float percentage;  // cache the constructor's parameter
};
#endif

#if defined(TGT_IS_CALLIOPE2) || defined(TGT_IS_CALLIOPE3) || defined(TGT_IS_CALLIOPE5)
class ParkArm : public StateNode {
 public:
  ParkArm(const std::string &nodename="ParkArm") : StateNode(nodename) {}
  virtual void setup() {
   PostureNode *posturenode1 = new PostureNode("posturenode1","park1.pos");
   addNode(posturenode1);

   PostureNode *posturenode2 = new PostureNode("posturenode2","park2.pos");
   addNode(posturenode2);

   ArmPower *armpower3 = new ArmPower("armpower3",0.0);
   addNode(armpower3);

   PostMachineCompletion *postmachinecompletion3 = new PostMachineCompletion("postmachinecompletion3");
   addNode(postmachinecompletion3);

   startnode = posturenode1;

   posturenode1->addTransition(new CompletionTrans("completiontrans2",posturenode2));
   posturenode2->addTransition(new CompletionTrans("completiontrans3",armpower3));
   armpower3->addTransition(new CompletionTrans("completiontrans4",postmachinecompletion3));
  }
};
#else
class ParkArm : public PostStateCompletion {
 public:
  ParkArm(const std::string &nodename="ParkArm") : PostStateCompletion(nodename) {}
};
#endif

// LookAtGripper
#if defined(TGT_HAS_HEAD) && defined(TGT_HAS_GRIPPER)
class LookAtGripper : public HeadPointerNode {
 public:
  LookAtGripper(const std::string &nodename, float _xoffset=0) : HeadPointerNode(nodename), xoffset(_xoffset) {}
  float xoffset;  // cache the constructor's parameter
  virtual void preStart() {
  HeadPointerNode::preStart();
  fmat::Column<3> gripperPos = kine->linkToBase(GripperFrameOffset).translation();
  getMC()->lookAtPoint(gripperPos[0]+xoffset, gripperPos[1], gripperPos[2]);
  }
};
#else
class LookAtGripper : public PostStateCompletion {
 public:
  LookAtGripper(const std::string &nodename, float _xoffset=0) : PostStateCompletion(nodename), xoffset(_xoffset) {}
  float xoffset;  // cache the constructor's parameter
};
#endif

#endif