Gazebo 机器人仿真流程之 World 类（二）

前文介绍了 World 类和 WorldPrivate 类的成员变量及成员函数。这里集中梳理一下，在 Gazebo 仿真场景（World）中，都有哪些东西。

在 Gazebo 中，仿真相关的对象主要有以下几个：

（1）物理引擎 PhysicsEngine

（2）模型 Model

（3）实体 Entity

（4）场景状态 WorldState

（5）插件 PluginT

（1）物理引擎PhysicsEngine

在 Gazebo 中，物理引擎分别基于 ODE 和 Bullet。PhysicsEngine 类派生了基于 Bullet 的 BulletPhysics 类和基于 ODE 的 ODEPhysics 类：

class GZ_PHYSICS_VISIBLE BulletPhysics : public PhysicsEngine;

class GZ_PHYSICS_VISIBLE ODEPhysics : public PhysicsEngine;

在 PhysicsEngine 中，其主要成员变量有：（感觉重要的成员变量就只有 word 指针和 contactManager）

/// \brief Pointer to the world.
protected: WorldPtr world;

/// \brief Class that handles all contacts generated by the physics
/// engine.
protected: ContactManager *contactManager;

在 BulletPhysics 中，主要成员变量有：

private: btBroadphaseInterface *broadPhase;
private: btDefaultCollisionConfiguration *collisionConfig;
private: btCollisionDispatcher *dispatcher;
private: btSequentialImpulseConstraintSolver *solver;
private: btDiscreteDynamicsWorld *dynamicsWorld;

这些也都是 Bullet 原生的一些变量/类型

在 PhysicsEngine 中，主要成员函数有：
（a）初始化相关的成员函数

/// \brief Default constructor.
/// \param[in] _world Pointer to the world.
public: explicit PhysicsEngine(WorldPtr _world);

/// \brief Destructor.
public: virtual ~PhysicsEngine();

/// \brief Load the physics engine.
/// \param[in] _sdf Pointer to the SDF parameters.
public: virtual void Load(sdf::ElementPtr _sdf);

/// \brief Initialize the physics engine.
public: virtual void Init() = 0;

/// \brief Finilize the physics engine.
public: virtual void Fini();

/// \brief Rest the physics engine.
public: virtual void Reset() {}

（b）物理引擎、碰撞检测相关

/// \brief Update the physics engine collision.
/// This function works in tandem with PhysicsEngine::UpdatePhysics()
/// to update the world. This function will be called even
/// if the physics is disabled (when World::PhysicsEnabled())
/// returns false).
/// Which updates are done in which of the two functions
/// PhysicsEngine::UpdateCollision() and PhysicsEngine::UpdatePhysics()
/// is to some extent left to the implementing physics engine.
/// The intention is that PhysicsEngine::UpdateCollision() will update
/// the collision states of the world, including contact information,
/// and PhysicsEngine::UpdatePhysics() will update the dynamics of
/// the world, i.e. advance the world and react to the collision state.
/// However for some physics engines, both is done in one step, or
/// providing the contact information separately in UpdateCollision()
/// would mean double work, as it can‘t be avoided to be done again
/// in PhysicsEngine::UpdatePhysics() - in this case it is better that
/// PhysicsEngine::UpdateCollision does not actually update collision
/// and contact information, and instead leaves it to UpdatePhysics().
/// There should be one exception however when it still does make this
/// update: If World::PhysicsEnabled() returns false, and therefore
/// PhysicsEngine::UpdatePhysics() will not be called in the update
/// step, *then* PhysicsEngine::UpdateCollision will need to ensure that
/// collision and contact information will still be updated.
public: virtual void UpdateCollision() = 0;

/// \brief Return the physics engine type (ode|bullet|dart|simbody).
/// \return Type of the physics engine.
public: virtual std::string GetType() const = 0;

/// \brief Set the random number seed for the physics engine.
/// \param[in] _seed The random number seed.
public: virtual void SetSeed(uint32_t _seed) = 0;

/// \brief Get the simulation update period.
/// \return Simulation update period.
public: double GetUpdatePeriod();

/// \brief Get target real time factor
/// \return Target real time factor
public: double GetTargetRealTimeFactor() const;

/// \brief Get real time update rate
/// \return Update rate
public: double GetRealTimeUpdateRate() const;

/// \brief Get max step size.
/// \return Max step size.
public: double GetMaxStepSize() const;

/// \brief Set target real time factor
/// \param[in] _factor Target real time factor
public: void SetTargetRealTimeFactor(double _factor);

/// \brief Set real time update rate
/// \param[in] _rate Update rate
public: void SetRealTimeUpdateRate(double _rate);

/// \brief Set max step size.
/// \param[in] _stepSize Max step size.
public: void SetMaxStepSize(double _stepSize);

/// \brief Update the physics engine.
/// Will only be called if the physics are enabled, which
/// is the case when World::PhysicsEnabled() returns true.
/// \sa PhysicsEngine::UpdateCollision()
public: virtual void UpdatePhysics() {}

/// \brief Create a new model.
/// \param[in] _base Boost shared pointer to a new model.
public: virtual ModelPtr CreateModel(BasePtr _base);

/// \brief Create a new body.
/// \param[in] _parent Parent model for the link.
public: virtual LinkPtr CreateLink(ModelPtr _parent) = 0;

/// \brief Create a collision.
/// \param[in] _shapeType Type of collision to create.
/// \param[in] _link Parent link.
public: virtual CollisionPtr CreateCollision(const std::string &_shapeType, LinkPtr _link) = 0;

/// \brief Create a collision.
/// \param[in] _shapeType Type of collision to create.
/// \param[in] _linkName Name of the parent link.
public: CollisionPtr CreateCollision(const std::string &_shapeType, const std::string &_linkName);

/// \brief Create a physics::Shape object.
/// \param[in] _shapeType Type of shape to create.
/// \param[in] _collision Collision parent.
public: virtual ShapePtr CreateShape(const std::string &_shapeType, CollisionPtr _collision) = 0;

/// \brief Create a new joint.
/// \param[in] _type Type of joint to create.
/// \param[in] _parent Model parent.
public: virtual JointPtr CreateJoint(const std::string &_type, ModelPtr _parent = ModelPtr()) = 0;

上面这一大堆，应该就是物理引擎、碰撞检测等需要实现的部分了。

在 BulletPhysics 中，成员函数同样包括：
（a）初始化相关

/// \brief Constructor
public: explicit BulletPhysics(WorldPtr _world);

/// \brief Destructor
public: virtual ~BulletPhysics();

// Documentation inherited
public: virtual void Load(sdf::ElementPtr _sdf);

// Documentation inherited
public: virtual void Init();

// Documentation inherited
public: virtual void Reset();

// Documentation inherited
public: virtual void InitForThread();

// Documentation inherited
public: virtual void Fini();

（b）创建 Bullet 中的对象

// Documentation inherited
public: virtual LinkPtr CreateLink(ModelPtr _parent);

// Documentation inherited
public: virtual CollisionPtr CreateCollision(const std::string &_type, LinkPtr _body);

// Documentation inherited
public: virtual JointPtr CreateJoint(const std::string &_type, ModelPtr _parent);

// Documentation inherited
public: virtual ShapePtr CreateShape(const std::string &_shapeType, CollisionPtr _collision);

（c）Bullet 物理引擎的内容

// Documentation inherited
public: virtual void UpdateCollision();

// Documentation inherited
public: virtual void UpdatePhysics();

看了一下 BulletPhysics 中的内容，大致就是 UpdatePhysics （刚体的动力学计算）和 UpdateCollision （计算碰撞信息）两部分。

（二）模型Model / 实体 ‘Entity‘

感觉在 Gazebo 中，仿真场景中的所有元素是基于树状结构存储的，其根节点位于 rootElement

/// \brief The root of all entities in the world.
public: BasePtr rootElement;

在 Gazebo 中，Entity 是所有对象的基类，定义如下：

/// \class Entity Entity.hh physics/physics.hh
/// \brief Base class for all physics objects in Gazebo.
class GZ_PHYSICS_VISIBLE Entity : public Base

模型类 Model 也正是由 Entity 类派生而来的，定义如下：

/// \class Model Model.hh physics/physics.hh
/// \brief A model is a collection of links, joints, and plugins.
class GZ_PHYSICS_VISIBLE Model : public Entity

应该是说，一个 Model　就是仿真中的一个整体对象，比如一个机械臂，由许多个 link joint plugin 组成。而这些 link joint plugin 就是一个个的 Entity 。
比如，Link Joint Plugin 的定义分别为：

/// \class Link Link.hh physics/physics.hh
/// \brief Link class defines a rigid body entity, containing
/// information on inertia, visual and collision properties of
/// a rigid body.
class GZ_PHYSICS_VISIBLE Link : public Entity

/// \brief Bullet Link class
class GZ_PHYSICS_VISIBLE BulletLink : public Link

/// \class Joint Joint.hh physics/physics.hh
/// \brief Base class for all joints
class GZ_PHYSICS_VISIBLE Joint : public Base

/// \brief Base class for all joints
class GZ_PHYSICS_VISIBLE BulletJoint : public Joint