ArmController.cc

Go to the documentation of this file.

#include "Shared/RobotInfo.h"
#include "Behaviors/Controller.h"

#ifndef TGT_HAS_ARMS
class ArmController : public BehaviorBase {
  virtual void doStart() {
    // This will popup a message in the ControllerGUI, if one is connected
    std::vector<std::string> errmsg;
    errmsg.push_back("The selected robot doesn't have an arm...");
    Controller::loadGUI("org.tekkotsu.mon.ControllerErr","",0,errmsg);
    stop();
  }
};
#else

#include "ArmController.h"
#include "Motion/MMAccessor.h"
#include "Motion/ArmMC.h"
#include "Shared/RobotInfo.h"
#include "Motion/PIDMC.h"

ArmController* ArmController::theOne = NULL;

void ArmController::doStart() {
  // Set up ArmMC and PIDMC for controlling and relaxing the arm
  SharedObject<ArmMC> arm;
  arm->setHold(false);
  arm_id = motman->addPersistentMotion(arm);
  SharedObject<PIDMC> pid;
  pidMCID = motman->addPersistentMotion(pid,MotionManager::kHighPriority);
  
  // Identify the initial sequence of pitch and yaw joints using alpha, theta, and jointType
  gripperFrameKJ->getRoot()->buildChildMap(KJjoints, ArmOffset, NumArmJoints);
  
  int counter = 0;
  float alphaCum = 0;
  for (unsigned int i = 0; i < NumArmJoints; i++) {
    if (KJjoints[i] == NULL) break;
    if ( counter > 0    // don't worry about any rotation from base frame to first arm joint
      && KJjoints[i]->theta != 0 ) // we're leaving the domain of pure yaw and pitch joints: punt
      break;
    
    alphaCum += KJjoints[i]->alpha;
    if ( std::abs(alphaCum) < 1e-5  && KJjoints[i]->jointType.get() == "revolute" ) {
      numYawJoints++;
      armConfig[counter++] = 'H';
    }
    else if ( std::abs(alphaCum) > 0.99*(M_PI/2) && KJjoints[i]->jointType.get() == "revolute" ) {
      numPitchJoints++;
      armConfig[counter++] = 'V';
    }
  }
  linksToDisplay = numPitchJoints + numYawJoints;
  
#ifdef TGT_LYNXARM6
  horScale = 20;
#endif

  // Find the dimensions of the arm.  Must work from the gripper
  // backward because the kinematic chain may branch going forward (as in
  // Calliope2 with its four-bar linkage).
  fmat::Column<3> armDims;
  fmat::Transform gripperToArm;
  const KinematicJoint* armJ = kine->getKinematicJoint(ArmOffset);
  const KinematicJoint* tmpJ = kine->getKinematicJoint(GripperFrameOffset);
  if ( armJ == NULL || tmpJ == NULL )
    std::cout << "Error in Arm Controller: can't find arm or gripper frame!\n";
  else
    while (tmpJ != armJ && tmpJ != NULL) {
        tmpJ = tmpJ->getParent();
        gripperToArm *= tmpJ->getTo().rigidInverse();
    }
  gripperToArm = gripperToArm.rigidInverse();
  armDims = gripperToArm.translation();
  horScale = verScale = armDims.norm();
  horScale /= 0.85;
  
  // determine transformation from first horizontal and vertical joint to base, for IK
  int horIndex = 0, verIndex = 0;
  fmat::Transform horT, verT;
  if (numYawJoints != 0) {
    while (armConfig[horIndex] != 'H')
      horIndex++;
    while (horIndex) {
      horT *= KJjoints[horIndex--]->getTo();
    }
  }
  if (numPitchJoints != 0) {
    while (armConfig[verIndex] != 'V')
      verIndex++;
    while (verIndex) {
      verT *= KJjoints[verIndex--]->getTo();
    }
  }
  horToBase = horT.translation();
  verToBase = verT.translation();
  
  // Turn on wireless
  theLastOne=theOne;
  theOne=this;
  cmdsock=wireless->socket(Socket::SOCK_STREAM, 2048, 2048);
  wireless->setReceiver(cmdsock->sock, mechacmd_callback);
  wireless->setDaemon(cmdsock,true);  
  wireless->listen(cmdsock->sock, config->main.armControl_port);  
  // Open the ArmGUI on the desktop 
  Controller::loadGUI("org.tekkotsu.mon.ArmGUI","ArmGUI",config->main.armControl_port);
}

void ArmController::doStop() {
  Controller::closeGUI("ArmGUI");
  erouter->removeListener(this);
  wireless->setDaemon(cmdsock,false);
  wireless->close(cmdsock);
  theOne=theLastOne;
  motman->removeMotion(arm_id);
  motman->removeMotion(pidMCID);
}

void ArmController::doEvent() {
  if(event->getGeneratorID()==EventBase::timerEGID) {
    float normJointVal, jointAngle;
    for(unsigned int joint = 0; joint < NumArmJoints; joint++) {
      jointAngle = state->outputs[ArmOffset + joint]; //When using mirage
      //  % = (Max - Val) / (Max - Min)  //
      normJointVal = (outputRanges[ArmOffset + joint][MaxRange] - jointAngle)/(outputRanges[ArmOffset + joint][MaxRange] - outputRanges[ArmOffset + joint][MinRange]);
      cmdsock->printf( "Value %d %d %f\n", -1, joint, normJointVal );
    }
    
    computeCoords();
    sendCoords();
    
    //RValue set the red point in the plane #1 & plane #2 when the joints are reset to their original values
    if (displayMode == pitchAndYaw) {
      cmdsock->printf("RValue %f %f\n", -yawCoords[numYawJoints -1][0] / horScale, yawCoords[numYawJoints -1][1] / horScale);
      cmdsock->printf("RValue1 %f %f\n",  pitchCoords[numPitchJoints - 1][0] / verScale, pitchCoords[numPitchJoints - 1][1] / verScale);
    }
    
    //RValue set the red point in the plane #1 when the joints are reset to their original values
    if (displayMode == yawOnly) {
      cmdsock->printf("RValue %f %f\n", -yawCoords[numYawJoints -1][0] / horScale, yawCoords[numYawJoints -1][1] / horScale);
    }
    
    //RValue set the red point in the plane #2 when the joints are reset to their original values
    if (displayMode == pitchOnly) {
      cmdsock->printf("Rvalue1 %f %f\n",  pitchCoords[numPitchJoints - 1][0] / verScale, pitchCoords[numPitchJoints - 1][1] / verScale);
    }
  }
}

// The command packet reassembly mechanism
int ArmController::mechacmd_callback(char *buf, int bytes) {
  static char cb_buf[19];
  static int cb_buf_filled;
  
  // If there's an incomplete command in the command buffer, fill
  // up as much of the command buffer as we can and then execute it
  // if possible
  if(cb_buf_filled) {
    while((cb_buf_filled < 19) && bytes) {
      cb_buf[cb_buf_filled++] = *buf++; // copy incoming buffer byte
      --bytes;        // decrement remaining byte ct.
    }
    // did we fill it? if so, execute! and mark buffer empty.
    if(cb_buf_filled == 9 && ((unsigned char*)cb_buf)[0] != 'x') {
      if(ArmController::theOne) ArmController::theOne->runCommand((unsigned char*) cb_buf);
      cb_buf_filled = 0;
    }
    
    if(cb_buf_filled == 19) {
      if(ArmController::theOne) ArmController::theOne->runCommand((unsigned char*) cb_buf);
      cb_buf_filled = 0;
    }
  }
  
  // now execute all complete bytes in the incoming buffer
  while(bytes >= 9) {
    if (((unsigned char*)buf)[0] != 'x') {
      if(ArmController::theOne) ArmController::theOne->runCommand((unsigned char *) buf);
      bytes -= 9;
      buf += 9;
    } else if (((unsigned char*)buf)[0] == 'x') {
      if(ArmController::theOne) ArmController::theOne->runCommand((unsigned char *) buf);
      bytes -= 19;
      buf += 19;
    }
  }
  
  // finally, store all remaining bytes in the command buffer
  while(bytes) {
    cb_buf[cb_buf_filled++] = *buf++;
    --bytes;
  }
  
  return 0;
}

void ArmController::runCommand(unsigned char *command) {
  // Extract the command parameter
  float param;
  float param2;
  float param3;
  int cmdno;
  unsigned char *paramp = (unsigned char *) &param;
  unsigned char *paramp2 = (unsigned char *) &param2;
  unsigned char *paramp3 = (unsigned char *) &param3;
  unsigned char *cmdnop = (unsigned char *) &cmdno;
  
#if defined(BYTE_ORDER) && BYTE_ORDER==BIG_ENDIAN
  paramp[0] = command[4];
  paramp[1] = command[3];
  paramp[2] = command[2];
  paramp[3] = command[1];
  
  if (command[0] == 'x') {
    paramp2[0] = command[9];
    paramp2[1] = command[8];
    paramp2[2] = command[7];
    paramp2[3] = command[6];
    
    paramp3[0] = command[14];
    paramp3[1] = command[13];
    paramp3[2] = command[12];
    paramp3[3] = command[11];
    
    cmdnop[0] = command[18];
    cmdnop[1] = command[17];
    cmdnop[2] = command[16];
    cmdnop[3] = command[15];
  } else {
    cmdnop[0] = command[8];
    cmdnop[1] = command[7];
    cmdnop[2] = command[6];
    cmdnop[3] = command[5];
  }
#else
  paramp[0] = command[1];
  paramp[1] = command[2];
  paramp[2] = command[3];
  paramp[3] = command[4];
  
  if (command[0] == 'x') {
    paramp2[0] = command[6];
    paramp2[1] = command[7];
    paramp2[2] = command[8];
    paramp2[3] = command[9];
    
    paramp3[0] = command[11];
    paramp3[1] = command[12];
    paramp3[2] = command[13];
    paramp3[3] = command[14];
    
    cmdnop[0] = command[15];
    cmdnop[1] = command[16];
    cmdnop[2] = command[17];
    cmdnop[3] = command[18];
  } else {
    cmdnop[0] = command[5];
    cmdnop[1] = command[6];
    cmdnop[2] = command[7];
    cmdnop[3] = command[8];
  }
  
#endif   // byte order
  
  // Find out what type of command this is
  switch(command[0]) {
    case CMD_connect: // Connect to ArmGUI -> z command 
      cmdConnect();
      break;  
    case CMD_point:   // Point Picker plane #1 & plane#2-> x command
      cmdPointPicker(param, param2, param3, cmdno); 
      break;
    case CMD_gripper: // Gripper -> w command 
      cmdGripper(param, cmdno);
      break;    
    case CMD_speed:   // Speed parameter -> y command 
      speed = param;
      break;
    case CMD_relax:   // Relax Button -> v command 
      cmdRelax();
      break;
    case CMD_unrelax: // Unrelax Button -> u command 
      cmdUnrelax();
      break;
    default: {
      setJoint( command[0], param );
    }
  }
}

//Setup function
void ArmController::cmdConnect() {
  
  //Code that validates the plane to be used in the interface 
  if(numYawJoints > 0 && numPitchJoints > 0)
    displayMode = pitchAndYaw; //create both planes 
  else if(numYawJoints > 0 && numPitchJoints == 0)
    displayMode = yawOnly; //create just the horizontal plane
  else
    displayMode = pitchOnly; //create just the vertical plane
  
  // Send out the number of joints on the arm, and the display mode
  cmdsock->printf("NOJ %d %d\n", NumArmJoints, displayMode);
  
  //Send the joint config
  for(unsigned int i = 0; i < NumArmJoints; i++)
    cmdsock->printf("Config %u %c\n", i, armConfig[i]); 
  
  // Must find out what OriJoint is for.
  OriJoint = linksToDisplay - 1;
  
  //Send out which joint is the one that control the orientation
  cmdsock->printf("OriJoint %d %d %d\n", OriJoint, numYawJoints, numPitchJoints);
  
  //Send Names, Max Ouput Ranges, Min output Ranges, Current Value
  float jointVal, normJointVal;
  
  for(unsigned int joint = 0; joint < NumArmJoints; joint++) {
    jointVal = state->outputs[ArmOffset + joint];
    normJointVal = (outputRanges[ArmOffset + joint][MaxRange] - jointVal) /
      (outputRanges[ArmOffset + joint][MaxRange] - outputRanges[ArmOffset + joint][MinRange]);
    cmdsock->printf("JointParam %s %f %f %f\n", outputNames[ArmOffset + joint], 
            outputRanges[ArmOffset + joint][MaxRange], outputRanges[ArmOffset + joint][MinRange], normJointVal);
  }
  
  //Set the red point in the plane 
  if(numYawJoints > 0)
    cmdsock->printf("IRGValue %f %f\n", -yawCoords[numYawJoints - 1][0] / horScale, yawCoords[numYawJoints - 1][1] / horScale);
  if(numPitchJoints > 0)
    cmdsock->printf("IRGValue1 %f %f\n", pitchCoords[numPitchJoints - 1][0] / verScale, pitchCoords[numPitchJoints - 1][1] / verScale);
  
  // Compute / send
  computeCoords();
  sendCoords();
  
}

// Command to the Point Picker
void ArmController::cmdPointPicker(float param, float param2, float param3, int cmdno) {
#if TGT_TENTACLE
  //== Setup (x,y,z) so they are relative to 'ARM:5' ==//
#else
  fmat::Column<3> tgtPoint;
  if (param3 == 1.0) { // horizontal plane
    theta = std::atan2(-param,param2);
    tgtPoint[0] = param2 * horScale + horToBase[0];
    tgtPoint[1] = -param * horScale + horToBase[1];
    tgtPoint[2] = verZ * verScale;
  }
  else if (param3 == 2.0) { // vertical plane
    verZ = param2;
    tgtPoint[0] = (param * verScale + verToBase[0]) * std::cos(theta);
    tgtPoint[1] = tgtPoint[0] * std::tan(theta);
    tgtPoint[2] = param2 * verScale + verToBase[2];
  }
  
  const KinematicJoint* tmpKJ = KJjoints[0];
  // transform to base
  while (tmpKJ->getParent() != NULL) {
    tgtPoint += tmpKJ->getTo().translation();
    tmpKJ = tmpKJ->getParent();
  }
  
  // perform IK
  gripperFrameKJ->getIK().solve(fmat::Column<3>(), *gripperFrameKJ, IKSolver::Point(tgtPoint));
  
  for(int joint = 0; joint < linksToDisplay; joint++) {
    // angle, mapped from 0 - 1, based on joint limits
    float normJointVal;
    
    // set the new angles
    float q = KJjoints[joint]->getQ();
    // we can't be outside the limits
    if (q > outputRanges[ArmOffset + joint][MaxRange]) {
      q = outputRanges[ArmOffset + joint][MaxRange];
      normJointVal = 0;
    }
    else if (q < outputRanges[ArmOffset + joint][MinRange]) {
      q = outputRanges[ArmOffset + joint][MinRange];
      normJointVal = 1;
    }
    else {
      //  % = (Max - Val) / (Max - Min)
      normJointVal = (outputRanges[ArmOffset + joint][MaxRange] - q)/(outputRanges[ArmOffset + joint][MaxRange] - outputRanges[ArmOffset + joint][MinRange]);
    }
    
    if(!std::isfinite(normJointVal))
      normJointVal = state->outputs[ArmOffset + joint]; // TODO: will this work?
    cmdsock->printf("CValue %d %d %f\n", cmdno, joint, normJointVal);
  }
#endif
}

void ArmController::cmdGripper(float param, int cmdno) {
#ifdef TGT_HAS_GRIPPER
  float gripper = (1.0f - param) / 2.0f;
#  ifdef TGT_HAS_FINGERS
  float right = gripper;
  float left = 1.0f - gripper;
  
  cmdsock->printf("CValue %d %d %f\n", cmdno, RightFingerOffset - ArmOffset, right);
  cmdsock->printf("CValue %d %d %f\n", cmdno, LeftFingerOffset - ArmOffset, left);
#  else
  cmdsock->printf("CValue %d %d %f\n", cmdno, GripperOffset - ArmOffset, gripper);
#  endif
#endif
}

void ArmController::cmdRelax() {
  MMAccessor<PIDMC> relaxer(pidMCID);
  for(unsigned int joint = 0; joint < NumArmJoints; joint++)
    relaxer->setJointPowerLevel(ArmOffset + joint, 0);
  processEvent(EventBase(EventBase::timerEGID,1,EventBase::statusETID,0));
  erouter->addTimer(this,0,500);
}

void ArmController::cmdUnrelax() {
  erouter->removeTimer(this);
  MMAccessor<PIDMC> unrelaxer(pidMCID);
  for(unsigned int joint = 0; joint < NumArmJoints; joint++) 
    unrelaxer->setJointPowerLevel(ArmOffset + joint, 1.0);
}

void ArmController::setJoint(unsigned int joint, float param) {
  unsigned int jointNo = joint < NumArmJoints ? joint : joint - NumArmJoints;
  if (jointNo >= NumArmJoints) {
    std::cerr << "Illegal command to ArmController " << joint << " " << jointNo << " "  << NumArmJoints << std::endl;
    return;
  }
  
  MMAccessor<ArmMC> arm(arm_id);
  arm->setMaxSpeed(jointNo, speed);
  // Val = -(% * (Max - Min)) + Max 
  float jointVal = -( param * (outputRanges[ArmOffset + jointNo][MaxRange] - outputRanges[ArmOffset + jointNo][MinRange]) ) + outputRanges[ArmOffset + jointNo][MaxRange];
  arm->setJointValue(jointNo, jointVal); //change the value of the arm physically
  
  if(KJjoints[jointNo] != NULL)
    KJjoints[jointNo]->setQ(jointVal);
  
  computeCoords();
  sendCoords();
  
  if ( joint < NumArmJoints ) {
    switch (displayMode) {
      case pitchAndYaw:
        //RValue set the red point in the plane #1 & plane #2 when the joints are reset to their original values
        cmdsock->printf("RValue %f %f\n", -yawCoords[numYawJoints -1][0] / horScale, yawCoords[numYawJoints -1][1] / horScale);
        cmdsock->printf("RValue1 %f %f\n",  pitchCoords[numPitchJoints - 1][0] / verScale, pitchCoords[numPitchJoints - 1][1] / verScale);
        break;
      case yawOnly:
        //RValue set the red point in the plane #1 when the joints are reset to their original values
        cmdsock->printf("RValue %f %f\n", -yawCoords[numYawJoints -1][0] / horScale, yawCoords[numYawJoints -1][1] / horScale);
        break;
      case pitchOnly:
        //RValue set the red point in the plane #2 when the joints are reset to their original values
        cmdsock->printf("Rvalue1 %f %f\n",  pitchCoords[numPitchJoints - 1][0] / verScale, pitchCoords[numPitchJoints - 1][1] / verScale);
        break;
    }
  }
}

// Validate this function with the projection of joint
void ArmController::computeCoords() {
  // Works for the Calliope, Chiara, Chiara2 and Handeye and almost for the Lynxarm. 
  fmat::Column<3> baseHorizontal;
  fmat::Column<3> baseVertical;
  
  // Looking for the first horizontal joint 
  for(int i = 0; i < linksToDisplay; i++){
    if(armConfig[i] == 'H'){
      baseHorizontal = KJjoints[i]->getWorldPosition();
      break;
    }
  }
  // Looking for the first vertical joint 
  for(int i = 0; i < linksToDisplay; i++){
    if(armConfig[i] == 'V'){
      baseVertical = KJjoints[i]->getWorldPosition();
      break;
    }
  }
  
  int yawCount = 0, pitchCount = 0;
  
  // Calculate the coordinates without projections
  for (int links = 0; links < linksToDisplay; links++){
    if (armConfig[links] == 'H'){
      KinematicJoint* relPosJoint;
      if (yawCount == numYawJoints - 1)
        relPosJoint = gripperFrameKJ;
      else
        relPosJoint = KJjoints[links+1];
      fmat::Column<3> relPos = relPosJoint->getWorldPosition() - baseHorizontal;
      yawCoords[yawCount][0] = relPos[1]; // y-axis 
      yawCoords[yawCount][1] = relPos[0]; // x-axis
      yawCount++;
    }
    else if (armConfig[links] == 'V'){
      KinematicJoint* relPosJoint;
      if (pitchCount == numPitchJoints - 1)
        relPosJoint = gripperFrameKJ;
      else
        relPosJoint = KJjoints[links+1];
      fmat::Column<3> relPos = relPosJoint->getWorldPosition() - baseVertical;
      std::cout << relPos << " = " << relPosJoint->getWorldPosition() << " - " << baseVertical << std::endl;
      pitchCoords[pitchCount][0] = fmat::SubVector<2>(relPos).norm() * (yawCoords[yawCount-1][1] * relPos[0] > 0 ? 1 : -1); // x-axis
      pitchCoords[pitchCount][1] = relPos[2]; // z-axis
      pitchCount++;
    }
  }
}

void ArmController::sendCoords() {
  //Send out the coordinates of the links to planes 
  switch (displayMode) {
    case pitchAndYaw:
      for(int links = 0; links < numYawJoints; links++)
        cmdsock->printf("Coord %d %f %f\n", links, -yawCoords[links][0]/horScale, yawCoords[links][1]/horScale);
      for(int links = 0; links < numPitchJoints; links++)
        cmdsock->printf("Coord1 %d %f %f\n", links, pitchCoords[links][0] / verScale, pitchCoords[links][1] / verScale);
      //GValue set the green point in the plane #1
      cmdsock->printf("GValue %f %f\n", -yawCoords[numYawJoints - 1][0] / horScale, yawCoords[numYawJoints - 1][1] / horScale);
      
      //GValue1 set the green point in the plane #2
      cmdsock->printf("GValue1 %f %f\n", pitchCoords[numPitchJoints - 1][0] / verScale, pitchCoords[numPitchJoints - 1][1] / verScale);
      break;
    case yawOnly:
      for(int links = 0; links < numYawJoints; links++)
        cmdsock->printf("Coord %d %f %f\n", links, -yawCoords[links][0]/horScale, yawCoords[links][1]/horScale);
      //GValue set the green point in the plane #1
      cmdsock->printf("GValue %f %f\n", -yawCoords[numYawJoints - 1][0] / horScale, yawCoords[numYawJoints - 1][1] / horScale);
      break;
    case pitchOnly:
      for(int links = 0; links < numPitchJoints; links++)
        cmdsock->printf("Coord1 %d %f %f\n", links, pitchCoords[links][0] / verScale, pitchCoords[links][1] / verScale);
      //GValue1 set the green point in the plane #2
      cmdsock->printf("GValue1 %f %f\n", pitchCoords[numPitchJoints - 1][0] / verScale, pitchCoords[numPitchJoints - 1][1] / verScale);
  }
}

#endif // TGT_HAS_ARMS

REGISTER_BEHAVIOR_MENU_OPT(ArmController,"TekkotsuMon",BEH_NONEXCLUSIVE);

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