bulletrccar1.h

Go to the documentation of this file.

#ifndef GCOP_BULLETRCCAR1_H
#define GCOP_BULLETRCCAR1_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 Bulletrccar1 : public Rccar 
  {
  public:
    Bulletrccar1(BulletWorld& m_world, vector<double> *zs_ = 0);

    ~Bulletrccar1()
    {
      delete m_vehicleRayCaster;

      delete m_vehicle;

      delete m_wheelShape;
    }
    
    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 Step1(Vector4d &xb, const Vector2d &u, 
                double h, const VectorXd *p = 0,
                Matrix4d *A = 0, Matrix42d *B = 0, Matrix4pd *C = 0);

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

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

    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;//Mass of the car
    btVector3 car_halfdims;//half dimensions of car width, height, length

    double  gEngineForce ;//Engine Torque
    double  gBreakingForce;//Breaking Force on the vehicle

    double maxEngineForce;
    double  maxBreakingForce;

    double  gVehicleSteering;//Vehicle Steering angle
    double  steeringClamp;// Clamp on Steering angle
    double torqueClamp;//Clamp on velocity of car
    double  wheelRadius;//Radius of wheel
    double  wheelWidth;//Width of wheel
    double  wheelFriction;//wheel friction usually large value
    double  suspensionStiffness;//Stiffness of suspension. If not enough, the car will sink into ground
    double  suspensionDamping;//Damping on suspension
    double  suspensionCompression;//Compression factor decides by how much the car will compress wrto external loads
    double  rollInfluence;//Decides whether the car will topple or not
    btScalar suspensionRestLength;//Rest length of suspension
    btScalar m_defaultContactProcessingThreshold;// if contact goes above this value, it will process

    btVector3 wheelDirectionCS0;//direction from the car towards the wheel contact point
    btVector3 wheelAxleCS;//Wheel Axle Direction

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

    btTransform offsettrans;//To Account for the difference to coordinate system Usage: worldpose_inregcoordsys = offsettrans.inv()*worldpose_bullet*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;
    btRaycastVehicle* m_vehicle;
    btCollisionShape* m_wheelShape;
    btRaycastVehicle::btVehicleTuning m_tuning;
    btVehicleRaycaster* m_vehicleRayCaster;
    BulletWorld &m_world;
    //btDynamicsWorld*    m_dynamicsWorld;
  };
}


#endif