Home FAQ Player Utilities Stage Gazebo Contrib Documentation Publications Contributors Users Project Download Bugs/Feedback Mailing lists Radish Old news Old stuff

SegwayRMP.hh Go to the documentation of this file. /* * Gazebo - Outdoor Multi-Robot Simulator * Copyright (C) 2003 * Nate Koenig & Andrew Howard * * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation; either version 2 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program; if not, write to the Free Software * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA * */ /* Desc: Model for a SickLMS200 * Author: Andrew Howard * Date: 8 May 2003 * CVS: $Id: SegwayRMP.hh,v 1.16 2004/11/16 04:47:08 inspectorg Exp $ */ #ifndef SEGWAYRMP_HH #define SEGWAYRMP_HH #include "Model.hh" // Forward declarations class Body; class HingeJoint; class SliderJoint; class Geom; class SphereGeom; class GeomData; typedef struct gz_position gz_position_t; class SegwayRMP : public Model { // Construct, destructor public: SegwayRMP(World *world); public: virtual ~SegwayRMP(); // Load the model public: virtual int Load( WorldFile *file, WorldFileNode *node ); // Initialize the model public: virtual int Init( WorldFile *file, WorldFileNode *node ); // Finalize the model public: virtual int Fini(); // Update the model state public: virtual void Update( double step ); // Update the odometry private: void UpdateOdometry( double step ); // Load ODE stuff private: int OdeLoad( WorldFile *file, WorldFileNode *node ); // Initialize ODE private: int OdeInit( WorldFile *file, WorldFileNode *node ); // Finalize ODE private: int OdeFini(); // Initialize the external interface private: int IfaceInit(); // Finalize the external interface private: int IfaceFini(); // Get commands from the external interface private: void IfaceGetCmd(); // Update the data in the external interface private: void IfacePutData(); // Robot parameters private: double wheelSep, wheelDiam; // ODE components private: Body *base, *top, *tires[2]; private: SliderJoint *topJoint; private: HingeJoint *tireJoints[2]; // External interface private: gz_position_t *iface; // Wheel speeds private: double wheelSpeed[2]; double desLinVel, curLinVel; double desAngVel; double maxLinVel; double maxLinAcc; // Control class private: class PID { public: PID(double Kp, double Ki, double Kd) : Kp(Kp), Ki(Ki), Kd(Kd) , e(0), de(0), ie(0) {} double Kp,Ki,Kd; double e, de, ie; double s(double e) { de = e - this->e; ie += e; this->e = e; return Kp*e + Ki*ie + Kd*de; } }; // States for the segway control class ControlState { public: ControlState() : dist(0), vel(0), pitch(0), dpitch(0), yaw(0), dyaw(0) {} double dist, vel; double pitch, dpitch; double yaw, dyaw; }; // Control private: PID* angVelPID; PID* linVelPID; ControlState* desState; ControlState* curState; // Odometric pose estimate private: double odomPose[3]; private: double odomOrient[2]; }; #endif

---

Last updated $Date: 2004/12/21 01:49:15 $

---