bulletrccar.h

Go to the documentation of this file.

#ifndef GCOP_BULLETRCCAR_H
#define GCOP_BULLETRCCAR_H

#include "system.h"
#include "rccar.h"
#include <BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h>
#include <BulletDynamics/Dynamics/btRigidBody.h>
#include <btBulletDynamicsCommon.h>
#include "bulletworld.h"

namespace gcop {
  
  using namespace std;
  using namespace Eigen;

  typedef Matrix<double, 4, 2> Matrix42d;
  typedef Matrix<double, 4, Dynamic> Matrix4pd;
  //typedef Matrix<double, 6, 1> Vector6d;

  

  class Bulletrccar : public Rccar 
  {
    public:

    struct CarState{
      btTransform cartransform;
      btVector3 carlinearvel;
      btVector3 carangularvel;
    };

    Bulletrccar(BulletWorld& m_world, vector<double> *zs_ = 0);

    ~Bulletrccar()
    {
      if(m_carChassis) 
      {
        //m_world.m_dynamicsWorld->removeRigidBody(m_carChassis);

        delete m_carChassis;
      }

      delete m_vehicleRayCaster;

      m_world.m_dynamicsWorld->removeVehicle(m_vehicle);
      delete m_vehicle;

      delete m_wheelShape;
      
      delete initialstate;

      
      //m_world.Reset();
    }
    
    double Step(Vector4d &xb, double t, const Vector4d &xa,
                const Vector2d &u, double h, const VectorXd *p,
                Matrix4d *A = 0, Matrix42d *B = 0, Matrix4pd *C = 0);

    double Step(Vector4d &xb, const Vector2d &u, 
                double h, const VectorXd *p = 0,
                Matrix4d *A = 0, Matrix42d *B = 0, Matrix4pd *C = 0);

    double Step(Vector4d &xb, const Vector2d &u,
                       double h, const VectorXd *p,
                      const Vector4d &w, Matrix4d *A = 0, Matrix42d *B = 0, 
                      Matrix4pd *C = 0, Matrix4d *D = 0);

    bool Reset(const Vector4d &x, double t = 0);

    void setinitialstate(const CarState &inputstate, Vector4d &x);
    void setinitialstate(Vector4d &x);

    bool NoiseMatrix(Matrix4d &Q, double t, const Vector4d &x, const Vector2d &u, double dt, const VectorXd *p);

    int rightIndex;
    int upIndex;
    int forwardIndex;

    double carmass;
    btVector3 car_halfdims;

    double  gEngineForce ;
    double  gBreakingForce;

    double maxEngineForce;
    double  maxBreakingForce;

    double  gVehicleSteering;
    double  steeringClamp;
    double velocityClamp;
    double  wheelRadius;
    double  wheelWidth;
    double  wheelFriction;
    double  suspensionStiffness;
    double  suspensionDamping;
    double  suspensionCompression;
    double  rollInfluence;//< Decides whether the car will topple or not
    btScalar suspensionRestLength;
    btScalar m_defaultContactProcessingThreshold;

    btVector3 wheelDirectionCS0;
    btVector3 wheelAxleCS;

    //Parameters for the car:
    double gain_cmdvelocity;
    double kp_torque;
    double kp_steer;
    double initialz;

    btTransform offsettrans;
    btTransform offsettransinv;

    //Additional Hidden state for publishing/rendering trajectories:
    vector<double> *zs;
    int count_zs;

    //Hacky way of ensuring the vehicle_velocity is reset after every reset
    bool reset_drivevel;

    //Bullet classes for holding car
    btRigidBody* m_carChassis;
    btCollisionShape* chassisShape;
    btRaycastVehicle* m_vehicle;
    btCollisionShape* m_wheelShape;
    btRaycastVehicle::btVehicleTuning m_tuning;
    btVehicleRaycaster* m_vehicleRayCaster;
    BulletWorld &m_world;//< Parent world to which the car belongs
    CarState *initialstate;
  };
}


#endif