ArmMC.cc

Go to the documentation of this file.

#include "ArmMC.h"
#include "Kinematics.h"
#include "IKSolver.h"
#include "Shared/debuget.h"
#include "Shared/WorldState.h"
#include "MotionManager.h"
#include "Shared/Config.h"
#include "Wireless/Socket.h"
#include "Shared/get_time.h"
#include "Events/EventBase.h"

#include <memory>
#include <cmath>
using namespace std;

ArmMC::ArmMC()
  : MotionCommand(), dirty(true), hold(true), tolerance(.05f),
    completionReported(true), targetTimestamp(0), timeout(2000)
{
  // Initialize armTargets[] by calling takeSnapShot() so we
  // don't have astronomical garbage values in there, which
  // could cause the motion command to fail to complete even if
  // the joint weights are zero.
  takeSnapshot();
  setWeight(0);
  defaultMaxSpeed();
}

void ArmMC::freezeMotion() {
#ifdef TGT_HAS_ARMS
  for(unsigned int i=0; i<NumArmJoints; i++)
    armTargets[i]=armCmds[i].value;
  dirty=false;
#endif
}

void ArmMC::takeSnapshot() {
#ifdef TGT_HAS_ARMS
  for(unsigned int i=0; i<NumArmJoints; i++)
    armTargets[i]=armCmds[i].value=state->outputs[ArmOffset+i];
  dirty=true;
#endif
}

float ArmMC::armJointValue(unsigned int i)  {
#ifdef TGT_HAS_ARMS
  return armTargets[i];
#else
  serr->printf("WARNING: No Arm");
  return -1000.0f;
#endif
}

void ArmMC::defaultMaxSpeed(float x) {
#ifdef TGT_HAS_ARMS
  for(unsigned int i = 0; i<NumArmJoints; i++)
    maxSpeed[i] = 0.3f; // **hack** hard coded for now
#endif
}

void ArmMC::setWeight(float w) {
#ifdef TGT_HAS_ARMS
  for(unsigned int i=0; i<NumArmJoints; i++)
    armCmds[i].weight=w;
  setDirty();
#endif
}

void ArmMC::setWeight(int i, float w) {
#ifdef TGT_HAS_ARMS
  armCmds[i].weight=w;
  setDirty();
#endif
}

void ArmMC::setJoints(float shoulder, float elbow, float wrist){   
#ifdef TGT_HAS_ARMS
  setMaxSpeed(0, 0.4f);    
  setMaxSpeed(1, 0.4f);    
  setMaxSpeed(2, 0.4f);    
  setJointValue(0, shoulder);    
  setJointValue(1, elbow);   
  setJointValue(2, wrist);   
#endif   
}    
       
       
void ArmMC::setJoints(float shoulder, float elbow, float yaw, float pitch, float roll, float gripper) {    
#ifdef TGT_HAS_ARMS 
  setMaxSpeed(0, 0.4f);    
  setMaxSpeed(1, 0.4f);    
  setMaxSpeed(2, 0.4f);    
  setMaxSpeed(3, 0.4f);    
  setMaxSpeed(4, 0.4f);    
  setMaxSpeed(5, 0.4f);    
  setJointValue(0, shoulder);    
  setJointValue(1, elbow);   
  setJointValue(2, yaw);   
  setJointValue(3, pitch);   
  setJointValue(4, roll);    
  setJointValue(5, gripper);   
#endif   
}    

void ArmMC::setWrist(float pitch, float roll, float gripper) {
#ifdef TGT_HAS_ARMS
  setMaxSpeed(3, 0.4f);    
  setMaxSpeed(4, 0.4f);    
  setMaxSpeed(5, 0.4f);  
  setJointValue(3, pitch);   
  setJointValue(4, roll);    
  setJointValue(5, gripper);
#endif
}

bool ArmMC::moveOffsetToPoint(const fmat::Column<3>& offset, const fmat::Column<3>& tgt) {
#ifndef TGT_HAS_ARMS
  return false;
#else
  const KinematicJoint * kinEff=NULL;
  unsigned int effIdx = capabilities.findFrameOffset("GripperFrame");
  if(effIdx!=-1U) // does it have the "pure" reference frame?
    kinEff = kine->getKinematicJoint(effIdx); // yes, try that
  if(kinEff==NULL) // if nothing has worked yet, try the last joint
    kinEff = kine->getKinematicJoint(ArmOffset+NumArmJoints-1);
  if(kinEff==NULL) // if that didn't work, give up
    return false;
  KinematicJoint * effector = kinEff->cloneBranch(); // make a local copy so we can do thread-safe IK
  
  // do the IK
  bool conv = effector->getIK().solve(offset,*effector,IKSolver::Point(tgt));
  
  // copy results to member storage
  while(effector!=NULL) {
    if(effector->outputOffset<NumOutputs)
      setOutputCmd(effector->outputOffset, effector->getQ());
    effector = effector->getParent();
  }
  return conv;
#endif  
}

bool ArmMC::moveOffsetToPointWithOrientation(const fmat::Column<3>& offset, const fmat::Column<3>& tgt, const fmat::Quaternion& ori) {
#ifndef TGT_HAS_ARMS
  return false;
#else
  const KinematicJoint * kinEff=NULL;
  unsigned int effIdx = capabilities.findFrameOffset("GripperFrame");
  if(effIdx!=-1U) // does it have the "pure" reference frame?
    kinEff = kine->getKinematicJoint(effIdx); // yes, try that
  if(kinEff==NULL) // if nothing has worked yet, try the last joint
    kinEff = kine->getKinematicJoint(ArmOffset+NumArmJoints-1);
  if(kinEff==NULL) // if that didn't work, give up
    return false;
  KinematicJoint * effector = kinEff->cloneBranch(); // make a local copy so we can do thread-safe IK
  
  // do the IK
  bool conv = effector->getIK().solve(offset, IKSolver::Rotation(fmat::Quaternion()), *effector, IKSolver::Point(tgt), 1, IKSolver::Rotation(ori), 0);
  
  // copy results to member storage
  while(effector!=NULL) {
    if(effector->outputOffset<NumOutputs)
      setOutputCmd(effector->outputOffset, effector->getQ());
    effector = effector->getParent();
  }
  return conv;
#endif  
}

bool ArmMC::setFingerGap(float dist) {
#ifdef TGT_CALLIOPE5KP
    PostureEngine mypos;
    mypos.setOutputCmd(LeftFingerOffset, 0);
    mypos.setOutputCmd(RightFingerOffset, 0);
    fmat::Column<3> lf = mypos.getPosition(LeftFingerOffset);
    fmat::Column<3> rf = mypos.getPosition(RightFingerOffset);
    float c = (lf-rf).norm();
    fmat::Transform trans = mypos.linkToLink(LeftFingerFrameOffset, LeftFingerOffset);
    AngSignPi alpha = asin(trans(0,0));
    float r = sqrt ( trans(0,3)*trans(0,3) + trans(1,3)*trans(1,3) );
    if ( dist < 0 || dist > 2*r+c ) // assumes fingers can open full 180 degrees
      return false;
    AngSignPi q = asin((dist/2-c/2)/r) - alpha;
    setJointValue(LeftFingerOffset, -q);
    setJointValue(RightFingerOffset, q);
#endif
    return true;
}

bool ArmMC::openGripper(float percentage) {
#if defined(TGT_IS_CALLIOPE2)
  float gripperAngle = outputRanges[GripperOffset][MinRange] +
    percentage * (outputRanges[GripperOffset][MaxRange] - outputRanges[GripperOffset][MinRange]);
  setJointValue(GripperOffset-ArmOffset, gripperAngle);
#elif defined(TGT_IS_CALLIOPE5)
  float gripperAngleL = outputRanges[LeftFingerOffset][MaxRange] +
    percentage * (outputRanges[LeftFingerOffset][MinRange] - outputRanges[LeftFingerOffset][MaxRange]);
  float gripperAngleR = outputRanges[RightFingerOffset][MinRange] +
    percentage * (outputRanges[RightFingerOffset][MaxRange] - outputRanges[RightFingerOffset][MinRange]);
  setJointValue(LeftFingerOffset-ArmOffset, gripperAngleL);
  setJointValue(RightFingerOffset-ArmOffset, gripperAngleR);
#endif
  return true;
}

int ArmMC::updateOutputs() {
  int prevDirty=isDirty();
  if(prevDirty || hold) {  // we have joints to command
    dirty=false;
#ifdef TGT_HAS_ARMS
    for(unsigned int i=0; i < NumArmJoints; i++) {
      if(maxSpeed[i]<=0) {
        armCmds[i].value=armTargets[i];
        motman->setOutput(this,i+ArmOffset,armCmds[i]);
      } else { // we may be trying to exceeded maxSpeed
        unsigned int f=0;
        while(armTargets[i]>armCmds[i].value+maxSpeed[i] && f<NumFrames) {
          armCmds[i].value+=maxSpeed[i];
          motman->setOutput(this,i+ArmOffset,armCmds[i],f);
          f++;
        }
        while(armTargets[i]<armCmds[i].value-maxSpeed[i] && f<NumFrames) {
          armCmds[i].value-=maxSpeed[i];
          motman->setOutput(this,i+ArmOffset,armCmds[i],f);
          f++;
        }
        if(f<NumFrames) { //we reached target value, fill in rest of frames
          armCmds[i].value=armTargets[i];
          for(;f<NumFrames;f++)
            motman->setOutput(this,i+ArmOffset,armCmds[i],f);
        } else // we didn't reach target value, still dirty
          dirty=true;
      }
    }
#endif
    if(!dirty && !completionReported) {
      postEvent(EventBase(EventBase::motmanEGID,getID(),EventBase::statusETID));
      completionReported=true;
      targetTimestamp=get_time();
    }
  }
  return prevDirty;
}

int ArmMC::isAlive() {
#ifdef TGT_HAS_ARMS
  if(dirty || !completionReported)
    return true;

  if(completionReported && (!hold || get_time()-targetTimestamp>timeout)) { //prevents a conflicted ArmPointerMC's from fighting forever
    if(get_time()-targetTimestamp>timeout && getAutoPrune())
      serr->printf("WARNING: ArmPointerMC (mcid %d) timed out - possible joint conflict or out-of-range target\n",getID());
    return false;
  }
  float maxdiff=0;
  for(unsigned int i=0; i<NumArmJoints; i++) {
    float diff=fabsf(state->outputs[ArmOffset+i]-armTargets[i]);
    if(diff>maxdiff)
      maxdiff=diff;
  }
  return (maxdiff>tolerance);
#else
  return false;
#endif
}

void ArmMC::setDirty() {
  dirty=true;
  completionReported=false;
#ifdef TGT_HAS_ARMS
  for(unsigned int i=0; i<NumArmJoints; i++)
    armCmds[i].value=motman->getOutputCmd(ArmOffset+i).value; //not state->outputs[armOffset+i]; - see function documentation
#endif
}

bool ArmMC::ensureValidJoint(unsigned int& i) {
#ifdef TGT_HAS_ARMS
  if(i<NumArmJoints)
    return true;
  if(i>=ArmOffset && i<ArmOffset+NumArmJoints) {
    i-=ArmOffset;
    serr->printf("WARNING: ArmMC received a joint index of %d (ArmOffset+%d).\n",i+ArmOffset,i);
    serr->printf("         Since all parameters are assumed to be relative to ArmOffset,\n");
    serr->printf("         you should just pass %d directly.\n",i);
    serr->printf("WARNING: Assuming you meant %d...\n",i);
    return true;
  }
  serr->printf("ERROR: ArmMC received a joint index of %d (ArmOffset%+d).\n",i,i-ArmOffset);
  serr->printf("ERROR: This does not appear to be a arm joint.  ArmPointerMC only controls\n");
  serr->printf("       arm joints, and assumes its arguments are relative to ArmOffset\n");
#endif
  return false;
}