GT2005: Modules/MotionControl/MotionStabilizer.cpp Source File

00001 /**
00002 * @file MotionStabilizer.cpp
00003 * 
00004 * Implementation of class MotionStabilizer
00005 *
00006 * @author Jan Hoffmann
00007 */
00008 
00009 #include "MotionStabilizer.h"
00010 
00011 MotionStabilizer::MotionStabilizer() : 
00012 accelX (0, 0.05, 0, 0, -20, 20, 500000),
00013 accelY (0, 0.05, 0, 0, -20, 20, 500000),
00014 accelZ (-9.81, 0.05, 0, 0, -20, 20, 500000),
00015 xFore (1, .9, 0, 0, .2, 10, 500000),
00016 xHind (1, .9, 0, 0, .2, 10, 500000),
00017 yLeft (1, .9, 0, 0, .2, 10, 500000),
00018 yRight (1, .9, 0, 0, .2, 10, 500000),
00019 stabilizerScale(5.0)
00020 {
00021 }
00022 
00023 bool MotionStabilizer::stabilize(
00024                                  MotionRequest::MotionID lastMotionType,
00025                                  const MotionRequest& motionRequest, 
00026                                  JointData& jointData, 
00027                                  OdometryData& odometryData,
00028                                  const SensorDataBuffer& sensorDataBuffer)
00029 {
00030   if(lastMotionType != MotionRequest::walk) return false;   
00031   if (!motionRequest.stabilize) return 0;
00032 
00033   for (int frame = 0; frame < sensorDataBuffer.numOfFrames; frame++) 
00034   {    
00035     accelX.approximateVal(sensorDataBuffer.frame[frame].data[SensorData::accelerationX]/1000000.0); 
00036     accelY.approximateVal(sensorDataBuffer.frame[frame].data[SensorData::accelerationY]/1000000.0); 
00037     accelZ.approximateVal(sensorDataBuffer.frame[frame].data[SensorData::accelerationZ]/1000000.0); 
00038   }
00039  
00040     
00041   xFore.approximateVal(1-(accelX.getVal() - 2)/stabilizerScale);
00042   xHind.approximateVal(1+(accelX.getVal() - 2)/stabilizerScale);
00043 
00044   yLeft.approximateVal(1-(accelY.getVal())/stabilizerScale);
00045   yRight.approximateVal(1+(accelY.getVal())/stabilizerScale);
00046 
00047 
00048   /* fore legs */
00049   jointData.data[JointData::legFR1]  = (long )((double )jointData.data[JointData::legFR1] * yRight.getVal() * xFore.getVal());
00050   jointData.data[JointData::legFR2] *= 1;
00051   jointData.data[JointData::legFR3]  = (long )((double )jointData.data[JointData::legFR3] * yRight.getVal() * xFore.getVal());
00052 
00053   jointData.data[JointData::legFL1]  = (long )((double )jointData.data[JointData::legFL1] * yLeft.getVal() * xFore.getVal());
00054   jointData.data[JointData::legFL2] *= 1;
00055   jointData.data[JointData::legFL3]  = (long )((double )jointData.data[JointData::legFL3] * yLeft.getVal() * xFore.getVal());
00056 
00057   /* hind legs */
00058   jointData.data[JointData::legHR1]  = (long )((double )jointData.data[JointData::legHR1] * yRight.getVal() * xHind.getVal());
00059   jointData.data[JointData::legHR2] *= 1;
00060   jointData.data[JointData::legHR3]  = (long )((double )jointData.data[JointData::legHR3] * yRight.getVal() * xHind.getVal());
00061 
00062   jointData.data[JointData::legHL1]  = (long )((double )jointData.data[JointData::legHL1] * yLeft.getVal() * xHind.getVal());
00063   jointData.data[JointData::legHL2] *= 1;
00064   jointData.data[JointData::legHL3]  = (long )((double )jointData.data[JointData::legHL3] * yLeft.getVal() * xHind.getVal());
00065 
00066   return true;
00067 }