00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030 #include "WalkMC.h"
00031
00032 #include "Shared/WorldState.h"
00033
00034 #include "Events/EventRouter.h"
00035 #include "Events/LocomotionEvent.h"
00036
00037 #include "Motion/MotionManager.h"
00038
00039 #include <stdlib.h>
00040 #include <stdio.h>
00041 #include <iostream>
00042 #include <fcntl.h>
00043 #include <unistd.h>
00044 #include <fstream>
00045
00046
00047
00048 #define BOUND_MOTION
00049
00050 const float WalkMC::MAX_DX = 180;
00051 const float WalkMC::MAX_DY = 140;
00052 const float WalkMC::MAX_DA = 1.8;
00053 const vector3d WalkMC::max_accel_xya(MAX_DX*2,MAX_DY*2,MAX_DA*2);
00054
00055 unsigned int checksum(const char *data,int num);
00056 template <class data> int read_file(data *arr,int num,const char *filename);
00057 template <class data> int save_file(data *arr,int num,const char *filename);
00058
00059 WalkMC::WalkMC(const char* pfile)
00060 : MotionCommand(), isPaused(false), wp(), body_loc(), body_angle(),
00061 pos_delta(0,0,0), angle_delta(0), travelTime(get_time()),
00062 time(get_time()), TimeStep(FrameTime), vel_xya(0,0,0),
00063 target_vel_xya(0,0,0)
00064 {
00065 init(pfile);
00066 }
00067
00068 void WalkMC::DoStart() {
00069 MotionCommand::DoStart();
00070 LocomotionEvent e(EventBase::locomotionEGID,getID(),EventBase::activateETID,0);
00071 e.setXYA(target_vel_xya.x,target_vel_xya.y,target_vel_xya.z);
00072 postEvent(e);
00073 travelTime=get_time();
00074 }
00075
00076 void WalkMC::DoStop() {
00077 unsigned int t=get_time();
00078 LocomotionEvent e(EventBase::locomotionEGID,getID(),EventBase::deactivateETID,t-travelTime);
00079 e.setXYA(target_vel_xya.x,target_vel_xya.y,target_vel_xya.z);
00080 postEvent(e);
00081 travelTime=t;
00082 MotionCommand::DoStop();
00083 }
00084
00085 void WalkMC::init(const char* pfile)
00086 {
00087
00088 body_loc.init(vector3d(0,0,wp.body_height),vector3d(0,0,0));
00089 body_angle.init(vector3d(0,wp.body_angle,0),vector3d(0,0,0));
00090
00091 for(unsigned int i=0; i<4; i++){
00092 legw[i].air = false;
00093 }
00094
00095 if(pfile!=NULL)
00096 load(pfile);
00097 else
00098 load("walk.prm");
00099
00100 double zeros[JointsPerLeg];
00101 for(unsigned int i=0; i<JointsPerLeg; i++)
00102 zeros[i]=0;
00103 for(unsigned int i=0; i<NumLegs; i++)
00104 GetLegPosition(legpos[i],zeros,i);
00105
00106
00107 }
00108
00109 void WalkMC::load(const char* pfile)
00110 {
00111 mzero(wp);
00112 int n = read_file(&wp,1,pfile);
00113
00114 printf("Loaded WalkMC Parameters %s n=%ud checksum=0x%X\n",pfile,n,checksum(reinterpret_cast<const char*>(&wp),sizeof(wp)));
00115 }
00116
00117 void WalkMC::save(const char* pfile) const {
00118 int n = save_file(&wp,1,pfile);
00119 printf("Saved WalkMC Parameters %s n=%ud checksum=0x%X\n",pfile,n,checksum(reinterpret_cast<const char*>(&wp),sizeof(wp)));
00120 }
00121
00122
00123 void WalkMC::setTargetVelocity(double dx,double dy,double da)
00124 {
00125 #ifdef BOUND_MOTION
00126 da = bound(da, -MAX_DA, MAX_DA);
00127 double fa = 1.0 - fabs(da / MAX_DA);
00128
00129 vector2d v(dx/MAX_DX,dy/MAX_DY);
00130 double l = v.length();
00131 if(l > 1) v /= l;
00132
00133 dx = MAX_DX * v.x * fa;
00134 dy = MAX_DY * v.y * fa;
00135 #endif
00136
00137 target_vel_xya.set(dx,dy,da);
00138
00139 if(isActive()) {
00140 unsigned int t=get_time();
00141 LocomotionEvent e(EventBase::locomotionEGID,getID(),EventBase::statusETID,t-travelTime);
00142 e.setXYA(dx,dy,da);
00143 postEvent(e);
00144 travelTime=t;
00145 }
00146 }
00147
00148 int WalkMC::updateOutputs() {
00149
00150 if(!isDirty())
00151 return 0;
00152
00153 double t = TimeStep / 1000.0;
00154
00155 vel_xya.x = bound(target_vel_xya.x, vel_xya.x-max_accel_xya.x*t, vel_xya.x+max_accel_xya.x*t);
00156 vel_xya.y = bound(target_vel_xya.y, vel_xya.y-max_accel_xya.y*t, vel_xya.y+max_accel_xya.y*t);
00157 vel_xya.z = bound(target_vel_xya.z, vel_xya.z-max_accel_xya.z*t, vel_xya.z+max_accel_xya.z*t);
00158
00159 BodyPosition delta;
00160 delta.loc.set(vel_xya.x*t,vel_xya.y*t,0);
00161 delta.angle.set(0,0,vel_xya.z*t);
00162
00163 time=get_time();
00164
00165 for(unsigned int frame=0; frame<NumFrames; frame++) {
00166 target_vel_xya.rotate_z(-delta.angle.z);
00167
00168
00169 angle_delta += delta.angle.z;
00170 pos_delta += delta.loc.rotate_z(angle_delta);
00171
00172
00173
00174 double cycle = (double)(time % wp.period) / wp.period;
00175 double sway = wp.sway*cos(2*M_PI*cycle);
00176 double hop = wp.hop*sin(4*M_PI*cycle);
00177 double height = wp.body_height;
00178 BodyPosition nextpos;
00179 nextpos.loc.set(0,-sway,height+hop);
00180 nextpos.angle.set(0,wp.body_angle,0);
00181
00182
00183 for(unsigned int i=0; i<NumLegs; i++){
00184
00185 bool air = (cycle >= wp.leg[i].lift_time) && (cycle < wp.leg[i].down_time);
00186 double air_f = wp.leg[i].down_time - wp.leg[i].lift_time;
00187 double nextlegangles[JointsPerLeg];
00188
00189 if(air != legw[i].air){
00190 if(air){
00191 t = wp.period/1000.0 * 0.75;
00192 vector3d vfp;
00193 vfp.x = bound(target_vel_xya.x, vel_xya.x-max_accel_xya.x*t, vel_xya.x+max_accel_xya.x*t);
00194 vfp.y = bound(target_vel_xya.y, vel_xya.y-max_accel_xya.y*t, vel_xya.y+max_accel_xya.y*t);
00195 vfp.z = 0.0;
00196 double vfa = bound(target_vel_xya.z, vel_xya.z-max_accel_xya.z*t, vel_xya.z+max_accel_xya.z*t);
00197 double b = (wp.period/1000.0) * (1.0 - air_f) / 2.0;
00198 vector3d target = (wp.leg[i].neutral + vfp*b).rotate_z(vfa*b);
00199 legw[i].airpath.create(legpos[i],target,wp.leg[i].lift_vel,wp.leg[i].down_vel);
00200 }else{
00201 legpos[i].z = 0.0;
00202 }
00203 legw[i].air = air;
00204 }
00205
00206 if(air){
00207 t = (cycle - wp.leg[i].lift_time) / air_f;
00208 legpos[i] = legw[i].airpath.eval(t);
00209 }else{
00210 legpos[i] = (legpos[i] - delta.loc).rotate_z(-delta.angle.z);
00211 }
00212
00213 GetLegAngles(nextlegangles,legpos[i],nextpos,i);
00214 for(unsigned int j=0; j<JointsPerLeg; j++)
00215 cmds[LegOffset+i*JointsPerLeg+j][frame].set(nextlegangles[j]);
00216 }
00217
00218 time += TimeStep;
00219 }
00220
00221 for(unsigned int joint=LegOffset; joint<LegOffset+NumLegJoints; joint++)
00222 motman->setOutput(this,joint,cmds[joint]);
00223
00224
00225 return NumLegs*JointsPerLeg;
00226 }
00227
00228 void WalkMC::resetLegPos() {
00229 for(unsigned int i=0; i<NumLegs; i++) {
00230 double tmp[JointsPerLeg];
00231 for(unsigned int j=0; j<JointsPerLeg; j++)
00232 tmp[j]=state->outputs[LegOffset+i*JointsPerLeg+j];
00233 GetLegPosition(legpos[i],tmp,i);
00234 }
00235 }
00236
00237 unsigned int checksum(const char *data,int num)
00238 {
00239 unsigned long c;
00240 int i;
00241
00242 c = 0;
00243 for(i=0; i<num; i++){
00244 c = c ^ (data[i]*13 + 37);
00245 c = (c << 13) | (c >> 19);
00246 }
00247
00248 return(c);
00249 }
00250
00251 template <class data>
00252 int read_file(data *arr,int num,const char *filename)
00253 {
00254 char name[256];
00255 int fd;
00256
00257 int n;
00258
00259 sprintf(name,"/ms/data/motion/%s",filename);
00260
00261 fd = open(name,O_RDONLY);
00262 if(fd < 0) return(0);
00263 n = read(fd,arr,sizeof(data)*num);
00264
00265 close(fd);
00266 if(n%sizeof(data)!=0)
00267 cout << "*** WARNING - short data read" << endl;
00268 n /= sizeof(data);
00269
00270 return(n);
00271 }
00272
00273 template <class data>
00274 int save_file(data *arr,int num,const char *filename) {
00275 char name[256];
00276 int fd;
00277
00278 int n;
00279
00280 sprintf(name,"/ms/data/motion/%s",filename);
00281
00282 cout << "writing " << name << " " << sizeof(data) << endl;
00283 fd = open(name,O_RDWR|O_CREAT|O_TRUNC);
00284 if(fd < 0) {
00285 cout << "open returned " << fd << endl;
00286 return(0);
00287 }
00288 n = write(fd,(const void*)arr,sizeof(data)*num);
00289
00290 close(fd);
00291 if(n%sizeof(data)!=0)
00292 cout << "*** WARNING - short data write" << endl;
00293 n /= sizeof(data);
00294
00295 return(n);
00296 }
00297
00298
00299
00300
00301
00302
00303
00304
00305
00306
00307
00308
00309
00310
00311