www.pudn.com > Product_Submit2004.rar > LocomotionCommand.cc, change:2004-06-23,size:5202b


#include "LocomotionCommand.h" 
 
double LocomotionCommand::defaultFrontStrideHeight; 
double LocomotionCommand::defaultFrontForwardOffset; 
double LocomotionCommand::defaultFrontSideOffset; 
double LocomotionCommand::defaultFrontHeight; 
 
double LocomotionCommand::defaultBackStrideHeight; 
double LocomotionCommand::defaultBackForwardOffset; 
double LocomotionCommand::defaultBackSideOffset; 
double LocomotionCommand::defaultBackHeight; 
 
int LocomotionCommand::defaultLocusType; 
 
void LocomotionCommand::SetDefaults(double _frontHeight, double _frontStrideHeight, double _frontForwardOffset, double _frontSideOffset, double _backHeight, double _backStrideHeight, double _backForwardOffset, double _backSideOffset, int _locusType) { 
  defaultFrontStrideHeight = _frontStrideHeight; 
  defaultFrontForwardOffset = _frontForwardOffset; 
  defaultFrontSideOffset = _frontSideOffset; 
  defaultFrontHeight = _frontHeight; 
 
  defaultBackStrideHeight = _backStrideHeight; 
  defaultBackForwardOffset = _backForwardOffset; 
  defaultBackSideOffset = _backSideOffset; 
  defaultBackHeight = _backHeight; 
 
  defaultLocusType = _locusType; 
} 
 
 
void LocomotionCommand::Set(int _type, double  _frontStrideLength, double _backStrideLength, double _turn, double _strafe, double _stepFrequency, double _frontHeight, double _frontStrideHeight, double _frontForwardOffset, double _frontSideOffset, double _backHeight, double _backStrideHeight, double _backForwardOffset, double _backSideOffset) { 
  motionType = _type; 
  stepFrequency = _stepFrequency; 
  frontStrideLength = _frontStrideLength; 
  backStrideLength = _backStrideLength; 
    
  turn = _turn; 
  strafe = _strafe; 
 
  odomTurn = turn; 
 
  frontHeight = _frontHeight; 
  frontStrideHeight = _frontStrideHeight; 
  frontForwardOffset = _frontForwardOffset; 
  frontSideOffset = _frontSideOffset; 
 
  backHeight = _backHeight; 
  backStrideHeight = _backStrideHeight; 
  backForwardOffset = _backForwardOffset; 
  backSideOffset = _backSideOffset; 
 
  locusType = defaultLocusType; 
  offsetWalk = false; 
} 
 
void LocomotionCommand::Set(int _type, double _frontStrideLength, double _backStrideLength, double _turn, double _strafe, double _stepFrequency) { 
  motionType = _type; 
  stepFrequency = _stepFrequency; 
  frontStrideLength = _frontStrideLength; 
  backStrideLength = _backStrideLength; 
 
  turn = _turn; 
  strafe = _strafe; 
 
  odomTurn = turn; 
 
  frontHeight = defaultFrontHeight; 
  frontStrideHeight = defaultFrontStrideHeight; 
  frontForwardOffset = defaultFrontForwardOffset; 
  frontSideOffset = defaultFrontSideOffset; 
 
  backHeight = defaultBackHeight; 
  backStrideHeight = defaultBackStrideHeight; 
  backForwardOffset = defaultBackForwardOffset; 
  backSideOffset = defaultBackSideOffset; 
 
  locusType = defaultLocusType; 
  offsetWalk = false; 
} 
 
void LocomotionCommand::SetKick(int _type, bool _allowHeadUse) { 
  motionType = _type; 
  allowHeadUse = _allowHeadUse; 
 
  stepFrequency = 0.0; 
  frontStrideLength = 0.0; 
  backStrideLength = 0.0; 
 
  turn = 0.0; 
  strafe = 0.0; 
 
  odomTurn = turn; 
 
  frontHeight = defaultFrontHeight; 
  frontStrideHeight = defaultFrontStrideHeight; 
  frontForwardOffset = defaultFrontForwardOffset; 
  frontSideOffset = defaultFrontSideOffset; 
 
  backHeight = defaultBackHeight; 
  backStrideHeight = defaultBackStrideHeight; 
  backForwardOffset = defaultBackForwardOffset; 
  backSideOffset = defaultBackSideOffset; 
 
  locusType = defaultLocusType; 
  offsetWalk = false; 
} 
 
void LocomotionCommand::CombineLocomotionCommands(LocomotionCommand* first, LocomotionCommand* second, double x) { 
  //if (x < 0.5) { 
    motionType = first->motionType; 
    allowHeadUse = first->allowHeadUse; 
    locusType = first->locusType; 
  //} else { 
  //  motionType = second->motionType; 
  //  allowHeadUse = second->allowHeadUse; 
  //  locusTtype = second->locusType; 
 // } 
 
  stepFrequency = (second->stepFrequency - first->stepFrequency)*x + first->stepFrequency; 
  frontStrideLength = (second->frontStrideLength - first->frontStrideLength)*x + first->frontStrideLength; 
  backStrideLength = (second->backStrideLength - first->backStrideLength)*x + first->backStrideLength; 
 
  turn = (second->turn - first->turn)*x + first->turn; 
  strafe = (second->strafe - first->strafe)*x + first->strafe; 
 
  odomTurn = turn; 
 
  frontHeight = (second->frontHeight - first->frontHeight)*x + first->frontHeight; 
  frontStrideHeight = (second->frontStrideHeight - first->frontStrideHeight)*x + first->frontStrideHeight; 
  frontForwardOffset = (second->frontForwardOffset - first->frontForwardOffset)*x + first->frontForwardOffset; 
  frontSideOffset = (second->frontSideOffset - first->frontSideOffset)*x + first->frontSideOffset; 
  backHeight = (second->backHeight - first->backHeight)*x + first->backHeight;  
  backStrideHeight = (second->backStrideHeight - first->backStrideHeight)*x + first->backStrideHeight; 
  backForwardOffset = (second->backForwardOffset - first->backForwardOffset)*x + first->backForwardOffset; 
  backSideOffset = (second->backSideOffset - first->backSideOffset)*x + first->backSideOffset; 
 
  offsetWalk = false; 
}