ArmController.h

Go to the documentation of this file.

//-*-c++-*-
#ifndef INCLUDED_ArmController_h_
#define INCLUDED_ArmController_h_

#include <iostream>
#include "Wireless/Wireless.h"
#include "Behaviors/BehaviorBase.h"
#include "Motion/MotionManager.h"
#include "Events/EventRouter.h"
#include "Events/EventBase.h"
#include "Shared/Config.h"
#include "Motion/PlanarThreeLinkArm.h"
#include "Motion/HandEyeGripper.h"
#include "Motion/PIDMC.h"
#include "IPC/SharedObject.h"
#include "Events/EventRouter.h"
#include "Motion/KinematicJoint.h"

//! Listens to control commands coming in from the command port for remotely controlling the arm.
/*! The communication protocol is a very simple binary format, shared
 *  with WalkControllerBehavior.  Each command is sent as a 5-byte
 *  group.  The first byte is a command selector, and the following 4
 *  bytes are a floating point argument:
 *
 *  - <@c char: command indicator>
 *  - <@c float: value>
 *  
 *  The valid values for command indicator are given by #CMD_shoulder,
 *  #CMD_elbow, or #CMD_wrist ('s', 'e', or 'w' respectively).
 */  
class ArmController : public BehaviorBase {
  
public: 
  //! Points to the one ArmController object that the input
  //! command stream is talking to. A kludge. Dunno how you're gonna
  //! make sure you're not using this uninitialized.
  static ArmController * theOne;
  static int mechacmd_callback(char *buf, int bytes); //!< called by wireless when there's new data
  
protected:
  MotionManager::MC_ID arm_id;   //!< the ArmMC to use
  MotionManager::MC_ID pidMCID;
  KinematicJoint* gripperFrameKJ;
  KinematicJoint* KJjoints[NumArmJoints];
  float horScale, verScale; 
  
  enum DisplayMode_t {
    pitchAndYaw = 0,
    yawOnly,
    pitchOnly
  } displayMode;
  
  //! @name Command Bytes        
  static const char cmdUnrelax  = 'u';
  static const char cmdRelax  = 'v';
  static const char cmdGripper  = 'w';
  static const char cmdPoint  = 'x';
  static const char cmdSpeed  = 'y';
  static const char cmdConnect  = 'z';
  static const char cmdOrientation = 'o';
  
  //! Angle between base horizontal joint and gripper, as from a bird's eye view
  /*! To be kept constant between point picks. */
  float theta;
  
  //! Height of point picked
  /*! To be kept constant between point picks. */
  float z;
  
  //! First horizontal joint location, relative to base
  fmat::Column<3> horToBase;
  
  //! First vertical joint location, relative to base
  fmat::Column<3> verToBase;
  
  //! Maximum speed for every joint
  float speed;
  
  //! Target arm orientations for IKSolver
  fmat::Quaternion orientation[2];
  
  //! Currently selected orientation (0 - side, 1 - overhead)
  int orientationIndex;
  
  //! Number of yaw joints
  unsigned int numYawJoints;
  
  //! Number of pitch joints
  unsigned int numPitchJoints;
  
  //! Total number of links to display (@numYawJoints + @numPitchJoints)
  unsigned int linksToDisplay;
  
  //Joint configuration info
  char armConfig[NumArmJoints];
  float yawCoords[NumArmJoints][2];
  float pitchCoords[NumArmJoints][2];
  
  //! The last HPCB object that was theOne, so we can restore it
  //! to prominence when we die. This is a nice gesture, but it doesn't
  //! really make sense since we're all using the same port. But just
  //! in case something changes and we don't do that, this mechanism
  //! is in place.
  ArmController *theLastOne;
  
  //! The input command stream socket
  Socket *cmdsock;
  
  //! Executes a command. Called by mechacmd_callback.
  void runCommand(unsigned char *command);
  void doEvent();
  
  // Possible commands from ArmGUI
  void connect();
  void pointPicked(float param, float param2, float param3, int cmdno);
  void gripper(float param, int cmdno);
  void relax();
  void unrelax();
  void setOrientation(int index);
  void setJoint(unsigned int joint, float param);
  
  ArmController(const ArmController&); //!< don't call
  ArmController operator=(const ArmController&); //!< don't call
  
public:
  //! constructor
  ArmController() : BehaviorBase("ArmController"),
  arm_id(MotionManager::invalid_MC_ID),
  pidMCID(MotionManager::invalid_MC_ID),
  gripperFrameKJ(NULL),
  KJjoints(),
  horScale(0),
  verScale(0),
  displayMode(),
  theta(0),
  z(0),
  horToBase(),
  verToBase(),
  speed(0.4f),
  orientation(),
  orientationIndex(0),
  numYawJoints(0),
  numPitchJoints(0),
  linksToDisplay(0),
  yawCoords(),
  pitchCoords(),
  theLastOne(theOne), /* Set the default to the robot last joint before the gripper */
  cmdsock(NULL) {
    for(unsigned int i =0; i < (NumArmJoints+1); i++)
      armConfig[i] = '\0';
    gripperFrameKJ = kine->getKinematicJoint(GripperFrameOffset)->cloneBranch();
    
    orientation[0] = fmat::Quaternion();
    orientation[1] = fmat::Quaternion::fromMatrix(fmat::rotationY(-M_PI_2));
  }
  
  //! destructor
  virtual ~ArmController() {
    theOne = theLastOne;
    delete gripperFrameKJ;
    gripperFrameKJ = NULL;
  }
  
  //! Setup the scale and the joints quantity to display
  virtual void doStart();
  
  virtual void doStop();
  
  void computeCoords();
  void sendCoords();  
  
  static std::string getClassDescription() {
    char tmp[20];
    sprintf(tmp,"%d",*config->main.armControl_port);
    return std::string("Listens to arm control commands coming in from port ")+tmp;
  }
  
  virtual std::string getDescription() const { return getClassDescription(); }
};

/*! @file
 * @brief Describes ArmController, listens to control commands coming in from the command port for remotely controlling the head
 * @author tss (Creator)
 */

#endif