Homepage

Demos

Overview

Downloads

Tutorials

Reference

Credits

WalkMC.h

Go to the documentation of this file.

//-*-c++-*-

//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.
  =========================================================================
*/

#ifndef INCLUDED_WalkMC_h
#define INCLUDED_WalkMC_h

#include "MotionCommand.h"
#include "Geometry.h"
#include "Kinematics.h"
#include "Path.h"
#include "Shared/get_time.h"
#include "OutputCmd.h"

//! A nice walking class from Carnegie Mellon University's 2001 Robosoccer team, modified to fit this framework, see their <a href="../CMPack_license.txt">license</a>
/*! Moves the feet through a looping path in order to walk - default parameters use
 *  a walk low to the ground so you don't walk over the ball.
 *
 *  This portion of the code falls under CMPack's license:
 *  @verbinclude CMPack_license.txt
 *
 *  @bug the legs try (briefly) to straighten out when first starting to move
 */
class WalkMC : public MotionCommand {
public:
  typedef SplinePath<vector3d,double> splinepath; //!<for convenience
  typedef HermiteSplineSegment<vector3d,double> spline; //!<for convenience

  //! holds state about each leg's path
  struct LegWalkState {
    LegWalkState() : airpath(), air(0) {} //!< constructor
    spline airpath; //!< the path to follow
    bool air; //!< true if in the air
  };
  
  //! holds parameters about how to move each leg
  struct LegParam {
    LegParam() : neutral(), lift_vel(), down_vel(), lift_time(0), down_time(0) {} //!< constructor
    vector3d neutral; //!< defines the "neutral" point of each leg - where it is in midstep
    vector3d lift_vel; //!< give the velocities to use when raising the paw
    vector3d down_vel; //!< give the velocities to use when lowering the paw
    double lift_time; //!< the time (as percentage of WalkParam::period) in the cycle to lift (so you can set different offsets between the paws)
    double down_time; //!< the time (as percentage of WalkParam::period) in the cycle to put down (so you can set different offsets between the paws)
  };

  //! holds more general parameters about the walk
  struct WalkParam {
    WalkParam() : body_height(0), body_angle(0), hop(0), sway(0), period(0), reserved() {} //!< constructor
    LegParam leg[4]; //!< a set of LegParam's, one for each leg
    double body_height; //!< the height to hold the body (mm)
    double body_angle; //!< the angle to hold the body (rad - 0 is level)
    double hop;  //!< sinusoidal hop amplitude
    double sway; //!< sinusoidal sway in y direction
    long period; //!< the time between steps
    long reserved; //!< live with it
  };

  //! constructor
  WalkMC(const char* pfile=NULL);

  virtual void DoStart(); //!< sends an activate LocomotionEvent
  virtual void DoStop();  //!< sends a deactivate LocomotionEvent

  virtual int updateOutputs(); //!< calculates current positions of the paws
  
  //! Returns true if we are walking. This modified isDirty allows the AIBO to slow down to a stop rather than stopping immediately.
  virtual int isDirty();
  
  //! always true - never autoprunes
  virtual int isAlive() { return true; }

  //! loads parameters from a file (@todo use LoadSave)
  void load(const char* pfile);
  //! saves parameters to a file (@todo use LoadSave)
  void save(const char* pfile) const;
  //! set the direction to walk - can specify x (forward), y (left), and angular (counterclockwise) velocities
  void setTargetVelocity(double dx,double dy,double da);
  //! returns current velocity we're trying to go
  const vector3d& getTargetVelocity() { return target_vel_xya; }
  //! returns the velocity we're actually moving (subject to clipping at max_accel_xya), doesn't reflect value of getPaused()...
  const vector3d& getCurVelocity() const { return vel_xya;}
  //! returns the time we've been traveling along the current vector
  unsigned int getTravelTime() { return get_time()-travelTime; }
  
  void setPaused(bool p) { isPaused=p; } //!< if set to true, will stop moving
  bool getPaused() const { return isPaused; } //!< if is true, we aren't moving
  void setHeight(double h) { wp.body_height=h; } //!< sets WalkParam::body_height of #wp
  double getHeight() { return wp.body_height; } //!< gets WalkParam::body_height of #wp
  void setAngle(double a) { wp.body_angle=a; } //!< sets WalkParam::body_angle of #wp
  double getAngle() { return wp.body_angle; } //!< gets WalkParam::body_angle of #wp
  void setHop(double h) { wp.hop=h; } //!< sets WalkParam::hop of #wp
  double getHop() { return wp.hop; } //!< gets WalkParam::hop of #wp
  void setSway(double h) { wp.sway=h; } //!< sets WalkParam::sway of #wp
  double getSway() { return wp.sway; } //!< gets WalkParam::sway of #wp
  void setPeriod(long p) { wp.period=p; } //!< sets WalkParam::period of #wp
  long getPeriod() { return wp.period; } //!< gets WalkParam::period of #wp
  void setSlowMo(float p) { slowmo=p; } //!< sets slowmo
  float* getSlowMo() { return &slowmo; } //!< gets slowmo

  WalkParam & getWP() { return wp; }; //!< returns the current walk parameter structure
  
  //! takes current leg positions from WorldState and tries to match the point in the cycle most like it
  void resetLegPos();

  static const float MAX_DX; //!< ==180 mm/sec
  static const float MAX_DY; //!< ==140 mm/sec
  static const float MAX_DA; //!< ==1.8 rad/sec
// tss "SmoothWalk" modification follows (actually only comment is changed)
  // static const vector3d max_accel_xya; //!< vector version of MAX_DX,MAX_DY,MAX_DA
  static const vector3d max_accel_xya; //!< maximum acceleration of x, y, and a velocity

 protected:
  //! holds current joint commands
  OutputCmd cmds[NumOutputs][NumFrames];

 protected:
  //! does some setup stuff, calls load(pfile)
  void init(const char* pfile);

  bool isPaused; //!< true if we are paused

  WalkParam wp; //!< current walking parameters (note that it's not static - different WalkMC's can have different setting, handy...
  LegWalkState legw[NumLegs]; //!< current state of each leg
  vector3d legpos[NumLegs]; //!< current position of each leg
  splinepath body_loc; //!< the path the body goes through while walking (?)
  splinepath body_angle; //!< the path the body goes through while walking (?)

  vector3d pos_delta; //!< how much we've moved
  double angle_delta; //!< how much we've turned
  
  unsigned int travelTime; //!< the time since the last call to setTargetVelocity - handy to check the time we've been traveling current vector
  int time; //!< time of last call to updateJointCmds() (scaled by slowmo)
  int TimeStep; //!< time to pretend passes between each call to updateJointCmds() - usually RobotInfo::FrameTime
  float slowmo; //!< scales time values to make the walk move in slow motion for analysis (or fast forward)

  // tss "SmoothWalk" addition follows
  /*! The CycleOffset variable is used to ensure that each time the AIBO
   *  starts walking, it starts at the same point in the walk cycle as
   *  where it stopped. This measure is intended to decrease the amount
   *  of jerking (and hence deviation) that occurs when the AIBO starts
   *  walking forward and then suddenly stops. */
  int CycleOffset;
  /*! Each CycleOffset corresponds to a different TimeOffset once the
   *  robot starts walking again. Consider this example: the robot
   *  stops 2/3 of the way through the cycle, then starts again 1/3
   *  of the way through the cycle on the absolute clock. The time
   *  offset to advance the clock to the right part of the cycle is
   *  1/3 of a cycle, so we set TimeOffset to 1/3 cycle and add that to
   *  every clock value used in the walk code. */
  int TimeOffset;
  /*! Every time we stop, we know we'll have a new CycleOffset, and we'll
   *  need to compute a new TimeOffset. This boolean says as much. */
  bool NewCycleOffset;
// tss "SmoothWalk" addition ends

  vector3d vel_xya; //!< the current velocity we're moving
  vector3d target_vel_xya; //!< the velocity that was requested
};

/* struct LegState{
   long attr,reserved;
   point3d pos;
   double angles[3];
   };
   
   struct HeadState{
   long attr,reserved;
   vector3d target;
   double angles[3];
   };
   
   struct BodyState{
   BodyPosition pos;
   LegState leg[4];
   HeadState head;
   }; 
*/

/*! @file
 * @brief Describes WalkMC, a MotionCommand for walking around
 * @author CMU RoboSoccer 2001-2002 (Creator)
 * @author ejt (ported)
 * @author PA Gov. School for the Sciences 2003 Team Project - Haoqian Chen, Yantian Martin, Jon Stahlman (modifications)
 * 
 * @verbinclude CMPack_license.txt
 *
 * $Author: ejt $
 * $Name: tekkotsu-1_5 $
 * $Revision: 1.14 $
 * $State: Rel $
 * $Date: 2003/10/07 01:00:40 $
 */

#endif

Tekkotsu v1.5

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