Gazebo 机器人仿真流程之 World 类

Posted 2021-02-28 wghou09

前文介绍了 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）

/// rief Pointer to the world.
protected: WorldPtr world;

/// rief 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）初始化相关的成员函数

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

/// rief Destructor.
public: virtual ~PhysicsEngine();

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

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

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

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

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

/// rief 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;

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

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

/// rief Get the simulation update period.
/// 
eturn Simulation update period.
public: double GetUpdatePeriod();

/// rief Get target real time factor
/// 
eturn Target real time factor
public: double GetTargetRealTimeFactor() const;

/// rief Get real time update rate
/// 
eturn Update rate
public: double GetRealTimeUpdateRate() const;

/// rief Get max step size.
/// 
eturn Max step size.
public: double GetMaxStepSize() const;

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

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

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

/// rief 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() {}

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

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

/// rief 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;

/// rief 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);

/// rief 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;

/// rief 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）初始化相关

/// rief Constructor
public: explicit BulletPhysics(WorldPtr _world);

/// rief 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

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

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

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

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

/// class Model Model.hh physics/physics.hh
/// rief 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
/// rief 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

/// rief Bullet Link class
class GZ_PHYSICS_VISIBLE BulletLink : public Link

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

/// rief Base class for all joints
class GZ_PHYSICS_VISIBLE BulletJoint : public Joint

---