Homepage

Demos

Overview

Downloads

Tutorials

Reference

Credits

WalkMC.cc

Go to the documentation of this file.

//This class is ported from Carnegie Mellon's 2001 Robosoccer entry, and falls under their license:
/*=========================================================================
    CMPack'02 Source Code Release for OPEN-R SDK v1.0
    Copyright (C) 2002 Multirobot Lab [Project Head: Manuela Veloso]
    School of Computer Science, Carnegie Mellon University
  -------------------------------------------------------------------------
    This software is distributed under the GNU General Public License,
    version 2.  If you do not have a copy of this licence, visit
    www.gnu.org, or write: Free Software Foundation, 59 Temple Place,
    Suite 330 Boston, MA 02111-1307 USA.  This program is distributed
    in the hope that it will be useful, but WITHOUT ANY WARRANTY,
    including MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  -------------------------------------------------------------------------
    Additionally licensed to Sony Corporation under the following terms:

    This software is provided by the copyright holders AS IS and any
    express or implied warranties, including, but not limited to, the
    implied warranties of merchantability and fitness for a particular
    purpose are disclaimed.  In no event shall authors be liable for
    any direct, indirect, incidental, special, exemplary, or consequential
    damages (including, but not limited to, procurement of substitute
    goods or services; loss of use, data, or profits; or business
    interruption) however caused and on any theory of liability, whether
    in contract, strict liability, or tort (including negligence or
    otherwise) arising in any way out of the use of this software, even if
    advised of the possibility of such damage.
  =========================================================================
*/

#include "WalkMC.h"

#include "Shared/WorldState.h"

#include "Events/EventRouter.h"
#include "Events/LocomotionEvent.h"

#include "Motion/MotionManager.h"

#include <stdlib.h>
#include <stdio.h>
#include <iostream>
#include <fcntl.h>
#include <unistd.h>
#include <fstream>
#include <cmath>

//REGIMP(WalkMC);

#define BOUND_MOTION

const float WalkMC::MAX_DX   = 180;//225; // mm/sec
const float WalkMC::MAX_DY   = 140;//170; // mm/sec
const float WalkMC::MAX_DA   = 1.8;//2.1; // rad/sec
// tss "SmoothWalk" modification follows
// const vector3d WalkMC::max_accel_xya(MAX_DX*2,MAX_DY*2,MAX_DA*2);
const vector3d WalkMC::max_accel_xya(MAX_DX*2,MAX_DY*2,MAX_DA*2);

unsigned int checksum(const char *data,int num); //!< computes a file checksum
template <class data> int read_file(data *arr,int num,const char *filename); //!< reads a set of walk parameters from a file
template <class data> int save_file(data *arr,int num,const char *filename); //!< saves the current walk parameters to a file

WalkMC::WalkMC(const char* pfile/*=NULL*/)
// tss "SmoothWalk" modification follows
//  : MotionCommand(), isPaused(false), wp(), body_loc(), body_angle(),
//    pos_delta(0,0,0), angle_delta(0), travelTime(get_time()),
//    time(get_time()), TimeStep(FrameTime), vel_xya(0,0,0),
//    target_vel_xya(0,0,0)
  : MotionCommand(), isPaused(false), wp(), body_loc(), body_angle(),
    pos_delta(0,0,0), angle_delta(0), travelTime(get_time()),
    time(get_time()), TimeStep(FrameTime), slowmo(1.0f),
    CycleOffset(0), TimeOffset(0), NewCycleOffset(false),
    vel_xya(0,0,0), target_vel_xya(0,0,0)
{
  init(pfile);
}

void WalkMC::DoStart() {
  MotionCommand::DoStart();
  LocomotionEvent e(EventBase::locomotionEGID,getID(),EventBase::activateETID,0);
  e.setXYA(target_vel_xya.x,target_vel_xya.y,target_vel_xya.z);
  postEvent(e);
  travelTime=get_time();
}

void WalkMC::DoStop() {
  unsigned int t=get_time();
  LocomotionEvent e(EventBase::locomotionEGID,getID(),EventBase::deactivateETID,t-travelTime);
  e.setXYA(target_vel_xya.x,target_vel_xya.y,target_vel_xya.z);
  postEvent(e);
  travelTime=t;
  MotionCommand::DoStop();
}

void WalkMC::init(const char* pfile)
{
  //  RegInit();
  body_loc.init(vector3d(0,0,wp.body_height),vector3d(0,0,0));
  body_angle.init(vector3d(0,wp.body_angle,0),vector3d(0,0,0));

  for(unsigned int i=0; i<4; i++){
    legw[i].air = false;
  }

  if(pfile!=NULL)
    load(pfile);
  else
    load("walk.prm");

  double zeros[JointsPerLeg];
  for(unsigned int i=0; i<JointsPerLeg; i++)
    zeros[i]=0;
  for(unsigned int i=0; i<NumLegs; i++)
    GetLegPosition(legpos[i],zeros,i);

  //  cmds[HeadOffset+TiltOffset].set(.3333,1);
}

// tss "SmoothWalk" addition follows
int WalkMC::isDirty()
{
  if(isPaused) return false;
  if((target_vel_xya.x == 0) && (target_vel_xya.y == 0) && (target_vel_xya.z == 0)) {
  if((vel_xya.x != 0) || (vel_xya.y != 0) || (vel_xya.z != 0)) return true
;
  else return false;
  }
  return true;
}
// tss "SmoothWalk" addition ends

void WalkMC::load(const char* pfile)
{
  mzero(wp);
  int n = read_file(&wp,1,pfile);
  //  pprintf(TextOutputStream,"Loaded WalkMC Paramaters %s n=%d checksum=0x%X\n\xFF",pfile,n,checksum((char*)&wp,sizeof(wp)));
  printf("Loaded WalkMC Parameters %s n=%ud checksum=0x%X\n",pfile,n,checksum(reinterpret_cast<const char*>(&wp),sizeof(wp)));
}

void WalkMC::save(const char* pfile) const {
  int n = save_file(&wp,1,pfile);
  printf("Saved WalkMC Parameters %s n=%ud checksum=0x%X\n",pfile,n,checksum(reinterpret_cast<const char*>(&wp),sizeof(wp)));
}


void WalkMC::setTargetVelocity(double dx,double dy,double da)
{
#ifdef BOUND_MOTION
  da = bound(da, -MAX_DA, MAX_DA);
  double fa = 1.0 - fabs(da / MAX_DA);

  vector2d v(dx/MAX_DX,dy/MAX_DY);
  double l = v.length();
  if(l > 1) v /= l;

  dx = MAX_DX * v.x * fa;
  dy = MAX_DY * v.y * fa;
#endif

  target_vel_xya.set(dx,dy,da);

  if(isActive()) {
    unsigned int t=get_time();
    LocomotionEvent e(EventBase::locomotionEGID,getID(),EventBase::statusETID,t-travelTime);
    e.setXYA(dx,dy,da);
    postEvent(e);
    travelTime=t;
  }
}

int WalkMC::updateOutputs() {
  //  cout << "WalkMC,,," << flush;
  if(!isDirty())
    return 0;
  
  double t = TimeStep * slowmo / 1000;

  vel_xya.x = bound(target_vel_xya.x, vel_xya.x-max_accel_xya.x*t, vel_xya.x+max_accel_xya.x*t);
  vel_xya.y = bound(target_vel_xya.y, vel_xya.y-max_accel_xya.y*t, vel_xya.y+max_accel_xya.y*t);
  vel_xya.z = bound(target_vel_xya.z, vel_xya.z-max_accel_xya.z*t, vel_xya.z+max_accel_xya.z*t);

  BodyPosition delta;
  delta.loc.set(vel_xya.x*t,vel_xya.y*t,0);
  delta.angle.set(0,0,vel_xya.z*t);

  time=(int)(get_time()*slowmo);

// tss "SmoothWalk" addition follows
  // If necessary, we compute a new TimeOffset here.
  if(NewCycleOffset) {
    TimeOffset = CycleOffset - time % wp.period;
    NewCycleOffset = false;
  }

  // Adjusted time--time adjusted for cycle matching
  int AdjustedTime = time + TimeOffset;

  // If walking speeds have dwindled down to zero, save our time offset from the beginning of the current walk cycle. Once we start walking again, we start up at the same offset to prevent jerky starts.
  if((vel_xya.x == 0) && (vel_xya.y == 0) && (vel_xya.z == 0)) {
    CycleOffset = AdjustedTime % wp.period;
    NewCycleOffset = true;
  }
// tss "SmoothWalk" addition ends

  for(unsigned int frame=0; frame<NumFrames; frame++) {
    target_vel_xya.rotate_z(-delta.angle.z);

    // incorporate movement delta
    angle_delta += delta.angle.z;
    pos_delta += delta.loc.rotate_z(angle_delta);

    //    cout << "setup,,," << flush;

// tss "SmoothWalk" modification follows
    // double cycle = (double)(time % wp.period) / wp.period;
    double cycle = (double)(AdjustedTime % wp.period) / wp.period;
    double sway   = wp.sway*cos(2*M_PI*cycle);
    double hop    = wp.hop*sin(4*M_PI*cycle);
    double height = wp.body_height;
    BodyPosition nextpos;
    nextpos.loc.set(0,-sway,height+hop);
    nextpos.angle.set(0,wp.body_angle,0);

    //    cout << "loop,,," << flush;
    for(unsigned int i=0; i<NumLegs; i++){

      bool air = (cycle >= wp.leg[i].lift_time) && (cycle < wp.leg[i].down_time);
      double air_f = wp.leg[i].down_time - wp.leg[i].lift_time;
      double nextlegangles[JointsPerLeg];

      if(air != legw[i].air){
        if(air){
          t = wp.period/1000.0 * 0.75;
          vector3d vfp;
          vfp.x = bound(target_vel_xya.x, vel_xya.x-max_accel_xya.x*t, vel_xya.x+max_accel_xya.x*t);
          vfp.y = bound(target_vel_xya.y, vel_xya.y-max_accel_xya.y*t, vel_xya.y+max_accel_xya.y*t);
          vfp.z = 0.0;
          double vfa   = bound(target_vel_xya.z, vel_xya.z-max_accel_xya.z*t, vel_xya.z+max_accel_xya.z*t);
          double b = (wp.period/1000.0) * (1.0 - air_f) / 2.0;
          vector3d target = (wp.leg[i].neutral + vfp*b).rotate_z(vfa*b);
          legw[i].airpath.create(legpos[i],target,wp.leg[i].lift_vel,wp.leg[i].down_vel);
        }else{
          legpos[i].z = 0.0;
        }
        legw[i].air = air;
      }

      if(air){
        t = (cycle - wp.leg[i].lift_time) / air_f;
        legpos[i] = legw[i].airpath.eval(t);

// Core tss "SmoothWalk" addition follows
        // KLUDGY MOD. Goal: reduce the height of the
        // AIBO's steps as its velocity nears zero.
        // Since I don't know how most of this code 
        // works, I'll directly alter legpos[i].z in
        // the following code to change the z height
        // with velocity.
        double velfraction_x = fabs(vel_xya.x / MAX_DX);
        double velfraction_y = fabs(vel_xya.y / MAX_DY);
        double velfraction_a = fabs(vel_xya.z / MAX_DA);

        // Choose the biggest velfraction
        double velfraction =
          (velfraction_x > velfraction_y) ?
            velfraction_x : velfraction_y;
        if(velfraction_a > velfraction)
          velfraction = velfraction_a;

        // Modify legpos[i].z with velfraction to
        // shrink it down
        //velfraction = atan(velfraction * M_PI);

        velfraction-=1;
        velfraction*=velfraction;
        velfraction*=velfraction;

        legpos[i].z *= 1-velfraction;
// Core tss "SmoothWalk" addition ends
      }else{
        legpos[i] = (legpos[i] - delta.loc).rotate_z(-delta.angle.z);
      }

      GetLegAngles(nextlegangles,legpos[i],nextpos,i);
      for(unsigned int j=0; j<JointsPerLeg; j++)
        cmds[LegOffset+i*JointsPerLeg+j][frame].set(nextlegangles[j]);
    }

    // tss "SmoothWalk" modification follows
    AdjustedTime = time+TimeOffset+(int)(frame*TimeStep*slowmo);
  }
  
  for(unsigned int joint=LegOffset; joint<LegOffset+NumLegJoints; joint++)
    motman->setOutput(this,joint,cmds[joint]);

  //    cout << "WalkMC-done" << endl;
  return NumLegs*JointsPerLeg;
}

void WalkMC::resetLegPos() {
  for(unsigned int i=0; i<NumLegs; i++) {
    double tmp[JointsPerLeg];
    for(unsigned int j=0; j<JointsPerLeg; j++)
      tmp[j]=state->outputs[LegOffset+i*JointsPerLeg+j];
    GetLegPosition(legpos[i],tmp,i);
  }
}

unsigned int checksum(const char *data,int num)
{
  unsigned long c;
  int i;

  c = 0;
  for(i=0; i<num; i++){
    c = c ^ (data[i]*13 + 37);
    c = (c << 13) | (c >> 19);
  }

  return(c);
}

template <class data>
int read_file(data *arr,int num,const char *filename)
{
  char name[256];
  int fd;
  // HFS::FILE *in;
  int n;

  sprintf(name,"/ms/data/motion/%s",filename);

  fd = open(name,O_RDONLY);
  if(fd < 0) return(0);
  n = read(fd,arr,sizeof(data)*num);
  // cprintf(OutputBuf,"read_file: %d of %d\n\xFF",n,sizeof(data)*num);
  close(fd);
  if(n%sizeof(data)!=0)
    cout << "*** WARNING - short data read" << endl;
  n /= sizeof(data);

  return(n);
}

template <class data>
int save_file(data *arr,int num,const char *filename) {
  char name[256];
  int fd;
  // HFS::FILE *in;
  int n;

  sprintf(name,"/ms/data/motion/%s",filename);

  cout << "writing " << name << " " << sizeof(data) << endl;
  fd = open(name,O_RDWR|O_CREAT|O_TRUNC);
  if(fd < 0) {
    cout << "open returned " << fd << endl;
    return(0);
  }
  n = write(fd,(const void*)arr,sizeof(data)*num);
  // cprintf(OutputBuf,"read_file: %d of %d\n\xFF",n,sizeof(data)*num);
  close(fd);
  if(n%sizeof(data)!=0)
    cout << "*** WARNING - short data write" << endl;
  n /= sizeof(data);

  return(n);
}

/*! @file
 * @brief Implements WalkMC, a MotionCommand for walking around
 * @author CMU RoboSoccer 2001-2002 (Creator)
 * @author ejt (ported)
 *
 * @verbinclude CMPack_license.txt
 *
 * $Author: ejt $
<<<<<<< WalkMC.cc
 * $Name: tekkotsu-1_5 $
 * $Revision: 1.16 $
 * $State: Rel $
 * $Date: 2003/09/12 02:01:44 $
=======
 * $Name: tekkotsu-1_5 $
 * $Revision: 1.16 $
 * $State: Rel $
 * $Date: 2003/09/12 02:01:44 $
>>>>>>> 1.15
 */

Tekkotsu v1.5

Generated Fri Oct 10 15:52:00 2003 by Doxygen 1.3.4