carchassis.cc

机器人人3D仿真工具,可以加入到Simbad仿真环境下应用。

/*
 *  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 generic car chassis
 * Author: Andrew Howard
 * Date: 23 Oct 2004
 * CVS: $Id: CarChassis.cc,v 1.17 2006/02/22 14:58:57 natepak Exp $
 */

/// @addtogroup models
/// @{
/** @defgroup CarChassis Car Chassis

@htmlinclude CarChassis_view.html

The CarChassis model simulates a generic vehicle chassis with 4
wheels, rear wheel drive and front wheel steering.


@par libgazebo interfaces

This model supports the @ref position interface.

@par Player drivers

Odometry information is available through the %gz_position driver.

@par Attributes

The following attributes are supported.

@htmlinclude default_attr_include.html

- skinFile (string, filename)
  - Specify an 3D geometry file to create an arbitrary appearance.
  - Default: empty

- skinXyz (float tuple, meters)
  - Skin pose relative to the model
  - Default: 0 0 0

- skinRpy (float tuple, degrees)
  - Skin rotation relative to the model in Euler angles: roll, pitch, yaw
  - Default: 0.0 0.0 0.0

- skinScale (float tuple, meters)
  - Scale factor for skin file.
  - Default: 1.0 1.0 1.0

@par Bodies

The following bodies are created by this model.

@htmlinclude default_body_include.html

@par Example

@verbatim
<model:CarChassis>
  <xyz>0 0 0</xyz>
</model:CarChassis>
@endverbatim

@par Views

@htmlinclude CarChassis_more_views.html

@par Authors

Andrew Howard

*/
/// @}

#if HAVE_CONFIG_H
#include <config.h>
#endif

#include <assert.h>
#include "gazebo.h"
#include "World.hh"
#include "WorldFile.hh"
#include "ModelFactory.hh"
#include "Error.hh"
#include "Body.hh"
#include "BoxGeom.hh"
#include "WheelGeom.hh"
#include "HingeJoint.hh"
#include "Hinge2Joint.hh"

#include "CarChassis.hh"


//////////////////////////////////////////////////////////////////////////////
// Register this model
GZ_REGISTER_STATIC("CarChassis", CarChassis);


//////////////////////////////////////////////////////////////////////////////
// Constructor
CarChassis::CarChassis( World *world )
    : Model( world )
{
  this->chassis = NULL;
  this->frontLeftWheel = NULL;
  this->frontLeftJoint = NULL;
  this->frontRightWheel = NULL;
  this->frontRightJoint = NULL;
  this->rearLeftWheel = NULL;
  this->rearLeftJoint = NULL;
  this->rearRightWheel = NULL;
  this->rearRightJoint = NULL;

  return;
}


//////////////////////////////////////////////////////////////////////////////
// Destructor
CarChassis::~CarChassis()
{
  if (this->chassis)
    delete this->chassis;

  if (this->frontLeftWheel)
    delete this->frontLeftWheel;

  if (this->frontLeftJoint)
    delete this->frontLeftJoint;

  if (this->frontRightWheel)
    delete this->frontRightWheel;

  if (this->frontRightJoint)
    delete this->frontRightJoint;

  if (this->rearLeftWheel)
    delete this->rearLeftWheel;

  if (this->rearLeftJoint)
    delete this->rearLeftJoint;

  if (this->rearRightWheel)
    delete this->rearRightWheel;

  if (this->rearRightJoint)
    delete this->rearRightJoint;

  this->chassis = NULL;
  this->frontLeftWheel = NULL;
  this->frontLeftJoint = NULL;
  this->frontRightWheel = NULL;
  this->frontRightJoint = NULL;
  this->rearLeftWheel = NULL;
  this->rearLeftJoint = NULL;
  this->rearRightWheel = NULL;
  this->rearRightJoint = NULL;

  return;
}


//////////////////////////////////////////////////////////////////////////////
// Load the model
int CarChassis::Load( WorldFile *file, WorldFileNode *node )
{
  // Create the ODE objects
  if (this->OdeLoad(file, node) != 0)
    return -1;
  
  // Steering controller
  this->steerKp = node->GetTupleDouble("steerPD", 0, 10.0);
  this->steerKd = node->GetTupleDouble("steerPD", 1, 0.0);

  return 0;
}


/////////////////////////////////////////////////////////////////////////////
// Load ODE objects
int CarChassis::OdeLoad( WorldFile *file, WorldFileNode *node )
{
  Geom *geom;
  double chassisMass;
  double wheelBase, trackWidth;
  double wheelDiam, tireDiam, wheelMass;
  double frameClear;
  double tireKp, tireKd;
  double tireMu1, tireMu2;
  GzVector chassisSize, chassisPos;  
  GzPose rearLeftPose, rearRightPose;
  GzPose frontLeftPose, frontRightPose;
  GzColor wheelColor, chassisColor;
  const char *skinFile;

  // Dimensions for Land Rover Discovery
  wheelBase = 2.540;
  trackWidth = 1.560;
  tireDiam = 0.20;
  wheelDiam = 0.80;
  //diffClear = 0.208;
  frameClear = 0.254;
  
  wheelMass = 20;
  chassisMass = 2000;

  this->steerMaxAngle = DTOR(50);

  wheelColor = GzColor(0.2, 0.2, 0.2);
  chassisColor = GzColor(0.5, 0.5, 0.5);
  
  chassisSize = GzVectorSet(wheelBase, trackWidth - wheelDiam, 0.5);
  chassisPos = GzVectorSet(0, 0, frameClear + chassisSize.z / 2);

  rearLeftPose = GzPoseSet(GzVectorSet(-wheelBase / 2, +trackWidth / 2, wheelDiam / 2),
                           GzQuaternFromAxis(1, 0, 0, -M_PI / 2));
  rearRightPose = GzPoseSet(GzVectorSet(-wheelBase / 2, -trackWidth / 2, wheelDiam / 2),
                            GzQuaternFromAxis(1, 0, 0, +M_PI / 2));

  frontLeftPose = GzPoseSet(GzVectorSet(+wheelBase / 2, +trackWidth / 2, wheelDiam / 2),
                            GzQuaternFromAxis(1, 0, 0, -M_PI / 2));
  frontRightPose = GzPoseSet(GzVectorSet(+wheelBase / 2, -trackWidth / 2, wheelDiam / 2),
                             GzQuaternFromAxis(1, 0, 0, +M_PI / 2));

  // Spring-damper constants for tires 
  tireKp = node->GetTupleDouble("tireHardness", 0, 1e6);
  tireKd = node->GetTupleDouble("tireHardness", 1, 1e4);

  // Friction coefficients for tires
  tireMu1 = node->GetTupleDouble("tireFriction", 0, 1.0);
  tireMu2 = node->GetTupleDouble("tireFriction", 1, 1.0);  

  // Create the main chassis of the robot.  Note that we offset this
  // geom from the body that so the vehicle sits on the ground, and
  // use the chassis body as the canonical body.
  this->chassis = new Body(this->world);
  geom = new BoxGeom(this->chassis, this->modelSpaceId,
                     chassisSize.x, chassisSize.y, chassisSize.z);  
  geom->SetMass(chassisMass);
  geom->SetColor(chassisColor);
  geom->SetRelativePosition(chassisPos);
  this->AddBody(this->chassis, true);

  // Apply a skin to chassis
  skinFile = node->SearchFilename("skinFile", GAZEBO_SKINS_PATH, NULL);
  if (skinFile)
  {
    PRINT_MSG1(1, "loading skin file [%s]", skinFile);  
    if (geom->SetSkinFile(skinFile) != 0)
    {
      PRINT_ERR("unable to load skin file");
      return -1;
    }
  }

  // Skin geometry
  GzVector pos;
  GzQuatern rot;
  pos = node->GetPosition("skinXyz", GzVectorZero());
  rot = node->GetRotation("skinRpy", GzQuaternIdent());  
  geom->SetSkinPose(GzPoseSet(pos, rot));
  geom->SetSkinScale(node->GetPosition("skinScale", GzVectorSet(1, 1, 1)));  
  
  // Create the left rear wheel
  this->rearLeftWheel = new Body(this->world, "rearLeftWheel");
  geom = new WheelGeom(this->rearLeftWheel, this->modelSpaceId,
                       wheelDiam / 2, tireDiam / 2);
  geom->SetMass(wheelMass);
  geom->SetColor(wheelColor);
  geom->SetHardness(tireKp, tireKd);
  geom->SetFriction(tireMu1, tireMu2);
  this->AddBody(this->rearLeftWheel);
  this->rearLeftWheel->SetPose(rearLeftPose);

  // Create left rear joint
  this->rearLeftJoint = new HingeJoint(this->world);
  this->rearLeftJoint->Attach(this->rearLeftWheel, this->chassis);
  this->rearLeftJoint->SetAnchor(rearLeftPose.pos);
  this->rearLeftJoint->SetAxis(GzVectorSet(0, 1, 0));

  // Create the right rear wheel
  this->rearRightWheel = new Body(this->world, "rearRightWheel");
  geom = new WheelGeom(this->rearRightWheel, this->modelSpaceId,
                       wheelDiam / 2, tireDiam / 2);
  geom->SetMass(wheelMass);
  geom->SetColor(wheelColor);
  geom->SetHardness(tireKp, tireKd);
  geom->SetFriction(tireMu1, tireMu2);
  this->AddBody(this->rearRightWheel);
  this->rearRightWheel->SetPose(rearRightPose);

  // Create right rear joint
  this->rearRightJoint = new HingeJoint(this->world);
  this->rearRightJoint->Attach(this->rearRightWheel, this->chassis);
  this->rearRightJoint->SetAnchor(rearRightPose.pos);
  this->rearRightJoint->SetAxis(GzVectorSet(0, 1, 0));

  // Create the left front wheel
  this->frontLeftWheel = new Body(this->world, "frontLeftWheel");
  geom = new WheelGeom(this->frontLeftWheel, this->modelSpaceId,
                       wheelDiam / 2, tireDiam / 2);
  geom->SetMass(wheelMass);
  geom->SetColor(wheelColor);
  geom->SetHardness(tireKp, tireKd);
  geom->SetFriction(tireMu1, tireMu2);
  this->AddBody(this->frontLeftWheel);
  this->frontLeftWheel->SetPose(frontLeftPose);
  
  // Create left front joint
  this->frontLeftJoint = new Hinge2Joint(this->world);
  this->frontLeftJoint->Attach(this->chassis, this->frontLeftWheel);
  this->frontLeftJoint->SetAnchor(frontLeftPose.pos);
  this->frontLeftJoint->SetAxis1(GzVectorSet(0, 0, -1));
  this->frontLeftJoint->SetAxis2(GzVectorSet(0, +1, 0));

  // Stops seems to cause instability; so dont use them
  //this->frontLeftJoint->SetParam(dParamLoStop, -steerMaxAngle);
  //this->frontLeftJoint->SetParam(dParamHiStop, +steerMaxAngle);

  // Create the right front wheel
  this->frontRightWheel = new Body(this->world, "frontRightWheel");
  geom = new WheelGeom(this->frontRightWheel, this->modelSpaceId,
                       wheelDiam / 2, tireDiam / 2);
  geom->SetMass(wheelMass);
  geom->SetColor(wheelColor);
  geom->SetHardness(tireKp, tireKd);
  geom->SetFriction(tireMu1, tireMu2);
  this->AddBody(this->frontRightWheel);
  this->frontRightWheel->SetPose(frontRightPose);

  // Create right front joint
  this->frontRightJoint = new Hinge2Joint(this->world);
  this->frontRightJoint->Attach(this->chassis, this->frontRightWheel);
  this->frontRightJoint->SetAnchor(frontRightPose.pos);
  this->frontRightJoint->SetAxis1(GzVectorSet(0, 0, -1));
  this->frontRightJoint->SetAxis2(GzVectorSet(0, +1, 0));

  // Stops seems to cause instability; so dont use them
  //this->frontRightJoint->SetParam(dParamLoStop, -steerMaxAngle);
  //this->frontRightJoint->SetParam(dParamHiStop, +steerMaxAngle);
  
  return 0;
}


//////////////////////////////////////////////////////////////////////////////
// Initialize the model
int CarChassis::Init( WorldFile *file, WorldFileNode *node )
{
  // Create position interface for odometry and steering commands
  this->position = gz_position_alloc();
  assert(this->position);
  if (gz_position_create(this->position, this->world->gz_server, this->GetId(),
                         "CarChassis", this->GetIntId(), this->GetParentIntId()) != 0)
    return -1;

  this->updateTime = 0.0;
  this->cmdSpeed = 0;
  this->cmdSteer = 0;
  
  return 0;
}



//////////////////////////////////////////////////////////////////////////////
// Finalize the model
int CarChassis::Fini()
{
  // Delete interfaces
  gz_position_destroy(this->position);
  gz_position_free(this->position);
  this->position = NULL;

  return 0;
}


//////////////////////////////////////////////////////////////////////////////
// Update model
void CarChassis::Update( double step )
{
  double steerTorque, driveTorque;

  // TODO
  driveTorque = 10000;
  steerTorque = 1000;

  if (this->world->GetSimTime() - this->updateTime)
  {
    this->updateTime = this->world->GetSimTime();
      
    // Get the current speed/steer commands
    this->PositionGetCmd();

    // Constrain the steering angle to lie within the stops
    if (this->cmdSteer > +this->steerMaxAngle)
      this->cmdSteer = +this->steerMaxAngle;
    else if (this->cmdSteer < -this->steerMaxAngle)
      this->cmdSteer = -this->steerMaxAngle;
  }

  this->rearLeftJoint->SetParam(dParamFMax, driveTorque / 2);
  this->rearRightJoint->SetParam(dParamFMax, driveTorque / 2);

  this->frontLeftJoint->SetParam(dParamFMax, steerTorque);
  this->frontRightJoint->SetParam(dParamFMax, steerTorque);

  // TODO: proper acceleration model
  // Set the speed of the drive wheels
  this->rearLeftJoint->SetParam(dParamVel, this->cmdSpeed);
  this->rearRightJoint->SetParam(dParamVel, this->cmdSpeed);

  double v;
  double kp, kd;

  // Normalize the gain factors using the step time (dont what the PD
  // to depend on sim cycle rate)
  kp = this->steerKp;
  kd = this->steerKd;

  // Set the turn angle; PD control
  v = kp * (this->cmdSteer - this->frontLeftJoint->GetAngle1())
    - kd * this->frontLeftJoint->GetAngle1Rate();  
  this->frontLeftJoint->SetParam(dParamVel, v);

  // Set the turn angle; PD control
  v = kp * (this->cmdSteer - this->frontRightJoint->GetAngle1())
    - kd * this->frontRightJoint->GetAngle1Rate();
  this->frontRightJoint->SetParam(dParamVel, v);

  //this->frontLeftJoint->SetParam(dParamVel, sin(this->world->GetSimTime()));
  //this->frontRightJoint->SetParam(dParamVel, sin(this->world->GetSimTime()));

  return;
}



//////////////////////////////////////////////////////////////////////////////
// Get commands from the position interface
void CarChassis::PositionGetCmd()
{
  gz_position_data_t *data;

  data = this->position->data;
    
  gz_position_lock(this->position, 1);

  this->cmdSpeed = data->cmd_vel_pos[0];
  this->cmdSteer = data->cmd_vel_rot[2];

  gz_position_unlock(this->position);
  
  return;
}


//////////////////////////////////////////////////////////////////////////////
// Update the position interface
void CarChassis::PositionPutData()
{
  gz_position_data_t *data;

  data = this->position->data;

  gz_position_lock(this->position, 1);

  // TODO
  
  gz_position_unlock(this->position);

  return;
}