OldHeadPointerMC.hGo to the documentation of this file.00001
00002 #ifndef INCLUDED_OldHeadPointerMC_h
00003 #define INCLUDED_OldHeadPointerMC_h
00004
00005 #include "MotionCommand.h"
00006 #include "OutputCmd.h"
00007 #include "Shared/RobotInfo.h"
00008
00009
00010 class OldHeadPointerMC : public MotionCommand {
00011 public:
00012
00013 OldHeadPointerMC();
00014
00015
00016 virtual ~OldHeadPointerMC() {}
00017
00018
00019 enum CoordFrame_t {
00020 BodyRelative,
00021 GravityRelative
00022 };
00023
00024 void setWeight(double w);
00025
00026
00027 void setWeight(RobotInfo::TPROffset_t i, double weight) {
00028 dirty=true; targetReached=false; headCmds[i].weight=weight; }
00029
00030 void setActive(bool a) { active=a; }
00031 bool getActive() const { return active; }
00032
00033
00034 void noMaxSpeed() { for(unsigned int i=0; i<NumHeadJoints; i++) maxSpeed[i]=0; }
00035
00036 void defaultMaxSpeed();
00037
00038 void setMaxSpeed(TPROffset_t i, float x) { maxSpeed[i]=x*FrameTime/1000; }
00039 float getMaxSpeed(TPROffset_t i) { return maxSpeed[i]*1000/FrameTime; }
00040
00041
00042 double convert(RobotInfo::TPROffset_t i, double v, CoordFrame_t srcmode, CoordFrame_t tgtmode) const {
00043 return (srcmode==tgtmode)?v:convFromBodyRelative(i,convToBodyRelative(i, v, srcmode),tgtmode);
00044 }
00045
00046
00047
00048
00049
00050 void setJoints(double tilt, double pan, double roll);
00051
00052
00053 void setMode(CoordFrame_t m, bool convert=true);
00054
00055
00056 void setJointMode(RobotInfo::TPROffset_t i, CoordFrame_t m, bool convert=true);
00057
00058
00059 void setJointValue(RobotInfo::TPROffset_t i, double value)
00060 { dirty=true; targetReached=false; headTargets[i]=(headModes[i]==BodyRelative)?clipAngularRange(i+HeadOffset,value):value; }
00061
00062
00063 void setJointValueAndMode(RobotInfo::TPROffset_t i, double value, CoordFrame_t m)
00064 { dirty=true; targetReached=false; headTargets[i]=(m==BodyRelative)?clipAngularRange(i+HeadOffset,value):value; headModes[i]=m; }
00065
00066
00067 void setJointValueFromMode(RobotInfo::TPROffset_t i, double value, CoordFrame_t m)
00068 { dirty=true; targetReached=false; headTargets[i]=convert(i,value,m,headModes[i]); }
00069
00070
00071 CoordFrame_t getJointMode(RobotInfo::TPROffset_t i) const { return headModes[i]; }
00072
00073
00074 double getJointValue(RobotInfo::TPROffset_t i) const { return headTargets[i]; }
00075
00076
00077
00078
00079 virtual int updateOutputs();
00080 virtual const OutputCmd& getOutputCmd(unsigned int i);
00081 virtual int isDirty() { return ((dirty || !targetReached) && active)?1:0; }
00082 virtual int isAlive() { return true; }
00083 virtual void DoStart() { MotionCommand::DoStart(); dirty=true; targetReached=false; }
00084
00085
00086 protected:
00087 double convToBodyRelative(TPROffset_t i, double v, CoordFrame_t mode) const;
00088 double convFromBodyRelative(TPROffset_t i, double v, CoordFrame_t mode) const;
00089 static float normalizeAngle(float x) { return x-rint(x/(2*M_PI))*(2*M_PI); }
00090
00091 static float clipAngularRange(unsigned int i, float x) {
00092 float min=outputRanges[i][MinRange];
00093 float max=outputRanges[i][MaxRange];
00094 if(x<min || x>max) {
00095 float mn_dist=fabs(normalizeAngle(min-x));
00096 float mx_dist=fabs(normalizeAngle(max-x));
00097 if(mn_dist<mx_dist)
00098 return min;
00099 else
00100 return max;
00101 } else
00102 return x;
00103 }
00104
00105 bool dirty;
00106 bool active;
00107 bool targetReached;
00108 OutputCmd headCmds[NumHeadJoints] ;
00109 float headTargets[NumHeadJoints];
00110 CoordFrame_t headModes[NumHeadJoints];
00111 float maxSpeed[NumHeadJoints];
00112 };
00113
00114
00115
00116
00117
00118
00119
00120
00121
00122
00123
00124
00125 #endif
00126
|