Modules/MotionControl/DebugMotionControl.cpp

Go to the documentation of this file.

/**
* @file DebugMotionControl.cpp
*
* Implementation of class DebugMotionControl
* 
* @author Max Risler
*/

#include "DebugMotionControl.h"

void DebugMotionControl::execute()
{
  for (int i = 0; i < jointDataBufferNumOfFrames; i++)
  {
    JointData &currentFrame = jointDataBuffer.frame[i];
    
    if (sequencer.isRunning())
      //execute sequence
      sequencer.nextSequenceData(currentFrame);
    else
      //do nothing if sequence empty
      currentFrame = JointData();
    
    // stay as forced for joints not used
    executeStayAsForced(sensorDataBuffer.lastFrame(), currentFrame);
    
    // the obgligatory GT tail wag
    // other direction to distinguish obviously from DefaultMotionControl
    if (currentFrame.data[JointData::tailTilt] == jointDataInvalidValue)
    {
      static int tailcount = 0;
      int j=tailcount++;
      if (j>64) j = 128-j;
      currentFrame.data[JointData::tailTilt] = 10000*(j - 32);
      tailcount &= 127;
    }
    if (currentFrame.data[JointData::tailPan] == jointDataInvalidValue)
      currentFrame.data[JointData::tailPan] = 0;
    
    //stabilize(0, motionRequest, currentFrame, odometryData, sensorData);
    
  }
  clip();
}

bool DebugMotionControl::handleMessage(InMessage& message)
{
  switch(message.getMessageID())
  {
  case idJointDataSequence:
    message.bin >> sequencer;
    sequencer.start();
    return true;
  }
  return false;
}

#define CLIP(x,min,max) \
  if (x > max) x=max; \
  else if (x < min) x=min;


void DebugMotionControl::clip()
{
  for (int i = 0; i < jointDataBufferNumOfFrames; i++)
  {
    CLIP(jointDataBuffer.frame[i].data[JointData::legFR1], toMicroRad(jointLimitLeg1FN), toMicroRad(jointLimitLeg1FP));
    CLIP(jointDataBuffer.frame[i].data[JointData::legFR2], toMicroRad(jointLimitLeg2N), toMicroRad(jointLimitLeg2P));
    CLIP(jointDataBuffer.frame[i].data[JointData::legFR3], toMicroRad(jointLimitLeg3N), toMicroRad(jointLimitLeg3P));
    CLIP(jointDataBuffer.frame[i].data[JointData::legFL1], toMicroRad(jointLimitLeg1FN), toMicroRad(jointLimitLeg1FP));
    CLIP(jointDataBuffer.frame[i].data[JointData::legFL2], toMicroRad(jointLimitLeg2N), toMicroRad(jointLimitLeg2P));
    CLIP(jointDataBuffer.frame[i].data[JointData::legFL3], toMicroRad(jointLimitLeg3N), toMicroRad(jointLimitLeg3P));
    CLIP(jointDataBuffer.frame[i].data[JointData::legHR1], toMicroRad(jointLimitLeg1HN), toMicroRad(jointLimitLeg1HP));
    CLIP(jointDataBuffer.frame[i].data[JointData::legHR2], toMicroRad(jointLimitLeg2N), toMicroRad(jointLimitLeg2P));
    CLIP(jointDataBuffer.frame[i].data[JointData::legHR3], toMicroRad(jointLimitLeg3N), toMicroRad(jointLimitLeg3P));
    CLIP(jointDataBuffer.frame[i].data[JointData::legHL1], toMicroRad(jointLimitLeg1HN), toMicroRad(jointLimitLeg1HP));
    CLIP(jointDataBuffer.frame[i].data[JointData::legHL2], toMicroRad(jointLimitLeg2N), toMicroRad(jointLimitLeg2P));
    CLIP(jointDataBuffer.frame[i].data[JointData::legHL3], toMicroRad(jointLimitLeg3N), toMicroRad(jointLimitLeg3P));
    CLIP(jointDataBuffer.frame[i].data[JointData::neckTilt], toMicroRad(jointLimitNeckTiltN), toMicroRad(jointLimitNeckTiltP));
    CLIP(jointDataBuffer.frame[i].data[JointData::headPan], toMicroRad(jointLimitHeadPanN), toMicroRad(jointLimitHeadPanP));
    CLIP(jointDataBuffer.frame[i].data[JointData::headTilt], toMicroRad(jointLimitHeadTiltN), toMicroRad(jointLimitHeadTiltP));
  }
}


#define SET_STAYASFORCED(joint,tolerance) \
setStayAsForcedValue(sensorData,jointData, SensorData::joint,JointData::joint,tolerance)

void DebugMotionControl::executeStayAsForced(
                                             const SensorData& sensorData,
                                             JointData& jointData
                                             )
{
  SET_STAYASFORCED(neckTilt,100000);
  SET_STAYASFORCED(headPan,100000);
  SET_STAYASFORCED(headTilt,50000);
  SET_STAYASFORCED(legFL1,100000);
  SET_STAYASFORCED(legHL1,100000);
  SET_STAYASFORCED(legFR1,100000);
  SET_STAYASFORCED(legHR1,100000);
  SET_STAYASFORCED(legFL2,100000);
  SET_STAYASFORCED(legHL2,100000);
  SET_STAYASFORCED(legFR2,100000);
  SET_STAYASFORCED(legHR2,100000);
  SET_STAYASFORCED(legFL3,100000);
  SET_STAYASFORCED(legHL3,100000);
  SET_STAYASFORCED(legFR3,100000);
  SET_STAYASFORCED(legHR3,100000);
  SET_STAYASFORCED(mouth,100000);
}

void DebugMotionControl::setStayAsForcedValue(
                                              const SensorData& sensorData,
                                              JointData& jointData,
                                              SensorData::sensors sensor,
                                              JointData::JointID joint,
                                              long tolerance
                                              )
{
  if (jointData.data[joint] == jointDataInvalidValue)
  {
    if (
      ((stayAsForcedOldValue.data[joint] - sensorData.data[sensor]) > tolerance)  ||
      ((stayAsForcedOldValue.data[joint] - sensorData.data[sensor]) < -tolerance)
      )
      jointData.data[joint] = stayAsForcedOldValue.data[joint] = sensorData.data[sensor];
    else
      jointData.data[joint] = stayAsForcedOldValue.data[joint];
  }
}

/*
* Change log :
* 
* $Log: DebugMotionControl.cpp,v $
* Revision 1.2  2004/05/27 17:13:37  jhoffman
* - renaming: tilt1 -> neckTilt,  pan -> headPan,  tilt2 -> headTilt
* - clipping included for setJoints
* - removed some microrad/rad-bugs
* - bodyPosture constructor and "=" operator fixed
*
* Revision 1.1.1.1  2004/05/22 17:20:35  cvsadm
* created new repository GT2004_WM
*
* Revision 1.4  2004/03/11 17:02:27  risler
* different limits for front and hind leg joint 1 for ERS-7
*
* Revision 1.3  2004/03/10 10:44:33  risler
* stay as forced for mouth
*
* Revision 1.2  2004/01/07 16:59:19  loetzsch
* added a clipping function
*
* Revision 1.1  2003/10/06 14:10:13  cvsadm
* Created GT2004 (M.J.)
*
* Revision 1.1.1.1  2003/07/02 09:40:24  cvsadm
* created new repository for the competitions in Padova from the 
* tamara CVS (Tuesday 2:00 pm)
*
* removed unused solutions
*
* Revision 1.4  2003/05/02 18:26:18  risler
* SensorDataBuffer added
* replaced SensorData with SensorDataBuffer
* full SensorData resolution now accessible
*
* Revision 1.3  2002/11/19 17:14:14  risler
* coding conventions: renamed JointData::joint to JointID, GetName to getName
*
* Revision 1.2  2002/09/11 00:06:58  loetzsch
* continued change of module/solution mechanisms
*
* Revision 1.1  2002/09/10 15:36:15  cvsadm
* Created new project GT2003 (M.L.)
* - Cleaned up the /Src/DataTypes directory
* - Removed challenge related source code
* - Removed processing of incoming audio data
* - Renamed AcousticMessage to SoundRequest
*
* Revision 1.2  2002/07/23 13:33:40  loetzsch
* new streaming classes
*
* removed many #include statements
*
* Revision 1.1.1.1  2002/05/10 12:40:15  cvsadm
* Moved GT2002 Project from ute to tamara.
*
* Revision 1.8  2002/04/05 14:08:43  jhoffman
* stabilizer stuff
*
* Revision 1.7  2002/02/23 14:51:23  risler
* increased tolerance values
*
* Revision 1.6  2002/02/21 16:29:21  risler
* added JointDataSequencer
*
* Revision 1.5  2002/02/13 16:21:05  risler
* increased stay-as-forced tolerance values
*
* Revision 1.4  2002/02/11 16:37:52  risler
* adjusted stay-as-forced tolerance values
*
* Revision 1.3  2002/02/08 22:38:13  risler
* added JointDataSequence, finished DebugMotionControl
*
* Revision 1.2  2002/02/08 22:31:46  risler
* added JointDataSequence, finished DebugMotionControl
*
* Revision 1.1  2002/02/08 20:00:01  risler
* added DebugMotionControl
*
*/

---