MotionNodes.h

Go to the documentation of this file.

#ifndef _MOTION_NODES_H_
#define _MOTION_NODES_H_

#ifdef TGT_HAS_ARMS

//************

#include "Behaviors/StateNode.h"
#include "Behaviors/Nodes/ArmNode.h"
#include "Behaviors/Nodes/HeadPointerNode.h"
#include "Behaviors/Nodes/DynamicMotionSequenceNode.h"
#include "Behaviors/Nodes/PostureNode.h"
#include "Behaviors/Nodes/SoundNode.h"
#include "Behaviors/Nodes/SpeechNode.h"

#if defined(TGT_HAS_LEGS) || defined(TGT_HAS_WHEELS)
#include "Behaviors/Nodes/WalkNode.h"
#endif

#include "DualCoding/DualCoding.h"
#include "DualCoding/VisualRoutinesStateNode.h"

#include "Behaviors/Transition.h"
#include "Behaviors/Transitions/CompareTrans.h"
#include "Behaviors/Transitions/CompletionTrans.h"
#include "Behaviors/Transitions/EventTrans.h"
#include "Behaviors/Transitions/NullTrans.h"
#include "Behaviors/Transitions/RandomTrans.h"
#include "Behaviors/Transitions/SignalTrans.h"
#include "Behaviors/Transitions/SmoothCompareTrans.h"
#include "Behaviors/Transitions/TextMsgTrans.h"
#include "Behaviors/Transitions/TimeOutTrans.h"
#include "Behaviors/Transitions/VisualTargetTrans.h"
#include <Behaviors/Transitions/TimeOutTrans.h>

#ifdef TARGET_HAS_IR
#include "Behaviors/Transitions/VisualTargetCloseTrans.h"
#endif


//************

#include <Motion/MotionManager.h>

#include <IPC/DriverMessaging.h>
#include <Motion/PlanarThreeLinkArm.h>
#include <Shared/fmatCore.h>

#include <string>
using namespace std;

typedef fmat::Row<RobotInfo::NumArmJoints> ArmConfiguration;

/*
  Nodes Defined:

  SetWrist
  SetArm
  SetJoint
  SetGround
  Twist
  Sway
  Rock
  Turn
  WalkSideways
  WalkForward
*/

extern MotionManager::MC_ID motionNodesWalkMC;

#if defined(TGT_HAS_LEGS) || defined(TGT_HAS_WHEELS)
class ResetInitialPlacementXX : public WalkNode {
 public:
  ResetInitialPlacementXX(const std::string &nodename="ResetInitialPlacementXX") : WalkNode(nodename) {}
  virtual void doStart() {
  setMC(motionNodesWalkMC);
  motman->setPriority(motionNodesWalkMC, priority);
  getMC()->start();
  postStateCompletion();
  }
};
#endif

class SetWrist : public ArmNode {
 public:
  SetWrist(const std::string &nodename, float _pitch=0, float _roll=0, float _grip=0) : ArmNode(nodename), pitch(_pitch), roll(_roll), grip(_grip) {}
  float pitch;  // cache the constructor's parameter
  float roll;  // cache the constructor's parameter
  float grip;  // cache the constructor's parameter
  virtual void doStart() {
  if (RobotInfo::NumArmJoints <= 3) {
    postStateCompletion();
    return;
  }
  cout<<"Setting wrist to ("<<pitch<<", "<<roll<<", "<<grip<<")"<<endl;
  //    getMC()->takeSnapshot();
  getMC()->setWrist(pitch, roll, grip);
  }
};

class SetArm : public ArmNode {
 public:
  SetArm(const std::string &nodename, float _shoulder=0, float _elbow=0, float _wrist=0) : ArmNode(nodename), shoulder(_shoulder), elbow(_elbow), wrist(_wrist) {}
  float shoulder;  // cache the constructor's parameter
  float elbow;  // cache the constructor's parameter
  float wrist;  // cache the constructor's parameter
  void move(float s, float e, float w) {
    cout<<"Setting arm to ("<<s<<", "<<e<<", "<<w<<")"<<endl;
    //    getMC()->takeSnapshot();
    getMC()->setJoints(s, e, w);
  }

  virtual void doStart() {
    if(const DataEvent<ArmConfiguration>* dev = dynamic_cast<const DataEvent<ArmConfiguration>*>(event)) {
      ArmConfiguration value = dev->getData();
      move(value[0], value[1], value[2]);
    } else {
      move(shoulder, elbow, wrist);
    }
  }
};

class SetJoint : public ArmNode {
 public:
  SetJoint(const std::string &nodename, string _jointName, float _storedVal=0, float _speed=0.4f) : ArmNode(nodename), jointName(_jointName), storedVal(_storedVal), speed(_speed) {}
  string jointName;  // cache the constructor's parameter
  float storedVal;  // cache the constructor's parameter
  float speed;  // cache the constructor's parameter
  void moveJoint(float value);  
  virtual void doStart() {
    if (const DataEvent<float> *dev = dynamic_cast<const DataEvent<float>*>(event)) {
      float receivedVal = dev->getData();
      moveJoint(receivedVal);
    } else {
      moveJoint(storedVal);
    }
  }
};

class SetServoPower : public StateNode {
 public:
  SetServoPower(const std::string &nodename, string _jointName, bool _val) : StateNode(nodename), jointName(_jointName), val(_val) {}
  string jointName;  // cache the constructor's parameter
  bool val;  // cache the constructor's parameter
  virtual void doStart() {
  /*  using namespace DriverMessaging;
    if(hasListener(ToggleServoPower::NAME)) {
    ToggleServoPower msg;
    msg.jointName = jointName;
    msg.holdPower = true;
    postMessage(msg);
    }
  */
  }
};

#if TGT_HAS_LEGS

class SetGround : public WalkNode {
 public:
  SetGround(const std::string &nodename="SetGround", float _groundOffset=-10) : WalkNode(nodename), groundOffset(_groundOffset) {}
  float groundOffset;  // cache the constructor's parameter
  void setID() {
    if(motionNodesWalkMC == MotionManager::invalid_MC_ID){
      postStateCompletion();
      return;
    }
    setMC(motionNodesWalkMC);
    motman->setPriority(motionNodesWalkMC, priority);
  }
  
  void move(float offset) {
    cout<<"Setting ground plane to "<<offset<<endl;
    getMC()->groundPlane[3] = offset;
  }

  virtual void doStart() {
    setID();
    if(const DataEvent<float>* dev = dynamic_cast<const DataEvent<float>*>(event)) {
      float off = dev->getData();
      move(off);
    } else {
      move(groundOffset);
    }
  }
};

class Twist : public WalkNode {
 public:
  Twist(const std::string &nodename, float _angle=0) : WalkNode(nodename), angle(_angle) {}
  float angle;  // cache the constructor's parameter
  void setID() {
    if(motionNodesWalkMC == MotionManager::invalid_MC_ID){
      postStateCompletion();
      return;
    }
    setMC(motionNodesWalkMC);
    motman->setPriority(motionNodesWalkMC, priority);
  }
  
  void move(float howmuch) {
    cout<<"Twisting to "<<howmuch<<endl;
    getMC()->offsetA = howmuch;
  }

  virtual void doStart() {
    setID();
    if(const DataEvent<float>* dev = dynamic_cast<const DataEvent<float>*>(event)) {
      float off = dev->getData();
      move(off);
    } else {
      move(angle);
    }
  }
};

class Sway : public WalkNode {
 public:
  Sway(const std::string &nodename, float _offset=0) : WalkNode(nodename), offset(_offset) {}
  float offset;  // cache the constructor's parameter
  void setID() {
    if(motionNodesWalkMC == MotionManager::invalid_MC_ID){
      postStateCompletion();
      return;
    }
    setMC(motionNodesWalkMC);
    motman->setPriority(motionNodesWalkMC, priority);
  }
  
  void move(float howmuch) {
    cout<<"Swaying to "<<howmuch<<endl;
    getMC()->offsetY = howmuch;
  }

  virtual void doStart() {
    setID();
    if(const DataEvent<float>* dev = dynamic_cast<const DataEvent<float>*>(event)) {
      float off = dev->getData();
      move(off);
    } else {
      move(offset);
    }
  }
};

class Rock : public WalkNode {
 public:
  Rock(const std::string &nodename, float _offset=0) : WalkNode(nodename), offset(_offset) {}
  float offset;  // cache the constructor's parameter
  void setID() {
    if(motionNodesWalkMC == MotionManager::invalid_MC_ID){
      postStateCompletion();
      return;
    }
    setMC(motionNodesWalkMC);
    motman->setPriority(motionNodesWalkMC, priority);
  }
  
  void move(float howmuch) {
    cout<<"Rocking to "<<howmuch<<endl;
    getMC()->offsetX = howmuch;
  }

  virtual void doStart() {
    setID();
    if(const DataEvent<float>* dev = dynamic_cast<const DataEvent<float>*>(event)) {
      float off = dev->getData();
      move(off);
    } else {
      move(offset);
    }
  }
};

class Turn : public WalkNode {
 public:
  Turn(const std::string &nodename, float _myangle=0) : WalkNode(nodename), myangle(_myangle) {}
  float myangle;  // cache the constructor's parameter
  void setID() {
    if(motionNodesWalkMC == MotionManager::invalid_MC_ID){
      postStateCompletion();
      return;
    }
    setMC(motionNodesWalkMC);
    motman->setPriority(motionNodesWalkMC, priority);
  }
  
  void move(float ang) {
    cout<<"Turning by "<<ang<<endl;
    getMC()->setTargetVelocity(0, 0, 3*(float)M_PI/180);
    getMC()->setTargetDisplacement(0,0, ang);
  }

  virtual void doStart() {
    setID();
    if(const DataEvent<float>* dev = dynamic_cast<const DataEvent<float>*>(event)) {
      float val = dev->getData();
      move(val);
    } else {
      move(myangle);
    }
  }
};

class WalkSideways : public WalkNode {
 public:
  WalkSideways(const std::string &nodename, float _distance=0) : WalkNode(nodename), distance(_distance) {}
  float distance;  // cache the constructor's parameter
  void setID() {
    if(motionNodesWalkMC == MotionManager::invalid_MC_ID){
      postStateCompletion();
      return;
    }
    setMC(motionNodesWalkMC);
    motman->setPriority(motionNodesWalkMC, priority);
  }
  
  void move(float dist) {
    cout<<"Walking sideways "<<dist<<endl;
    getMC()->setTargetVelocity(0, 10, 0);
    getMC()->setTargetDisplacement(0, dist, 0);
  }

  virtual void doStart() {
    setID();
    if(const DataEvent<float>* dev = dynamic_cast<const DataEvent<float>*>(event)) {
      float dist = dev->getData();
      move(dist);
    } else {
      move(distance);
    }
  }
};

class WalkForward : public WalkNode {
 public:
  WalkForward(const std::string &nodename, float _distance=0) : WalkNode(nodename), distance(_distance) {}
  float distance;  // cache the constructor's parameter
  void setID() {
    if(motionNodesWalkMC == MotionManager::invalid_MC_ID){
      postStateCompletion();
      return;
    }
    setMC(motionNodesWalkMC);
    motman->setPriority(motionNodesWalkMC, priority);
  }
  
  void move(float dist) {
    cout<<"Walking forward by "<<dist<<endl;
    getMC()->setTargetVelocity(10, 0, 0);
    getMC()->setTargetDisplacement(dist, 0, 0);
  }

  virtual void doStart() {
    setID();
    if(const DataEvent<float>* dev = dynamic_cast<const DataEvent<float>*>(event)) {
      float dist = dev->getData();
      move(dist);
    } else {
      move(distance);
    }
  }
};

#else //TGT_HAS_LEGS

class SetGround : public StateNode {
 public:
  SetGround(const std::string &nodename="SetGround", float _a=-10) : StateNode(nodename), a(_a) {}
  float a;  // cache the constructor's parameter
  virtual void doStart() {
  postStateCompletion();
  }
};
  
class Twist : public StateNode {
 public:
  Twist(const std::string &nodename, float _a=0) : StateNode(nodename), a(_a) {}
  float a;  // cache the constructor's parameter
  virtual void doStart() {
  postStateCompletion();
  }
};
  
class Sway : public StateNode {
 public:
  Sway(const std::string &nodename, float _a=0) : StateNode(nodename), a(_a) {}
  float a;  // cache the constructor's parameter
  virtual void doStart() {
  postStateCompletion();
  }
};
  
class Rock : public StateNode {
 public:
  Rock(const std::string &nodename, float _a=0) : StateNode(nodename), a(_a) {}
  float a;  // cache the constructor's parameter
  virtual void doStart() {
  postStateCompletion();
  }
};
  
class Turn : public StateNode {
 public:
  Turn(const std::string &nodename, float _a=0) : StateNode(nodename), a(_a) {}
  float a;  // cache the constructor's parameter
  virtual void doStart() {
  postStateCompletion();
  }
};
  
class WalkSideways : public StateNode {
 public:
  WalkSideways(const std::string &nodename, float _a=0) : StateNode(nodename), a(_a) {}
  float a;  // cache the constructor's parameter
  virtual void doStart() {
  postStateCompletion();
  }
};
  
class WalkForward : public StateNode {
 public:
  WalkForward(const std::string &nodename, float _a=0) : StateNode(nodename), a(_a) {}
  float a;  // cache the constructor's parameter
  virtual void doStart() {
  postStateCompletion();
  }
};
  
#endif //TGT_HAS_LEGS

#else // not TGT_HAS_ARMS
#include "Crew/PilotNode.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 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;
  }
};

#endif  //not TGT_HAS_ARMS

#endif