Hathaway Zhu

FCL(Flexible Collision Library)

FCL是Flexible Collision Library的缩写，是A General Purpose Library for Collision and Proximity Queries，它具体的github repo在这里，相关的学术文章为FCL: A general purpose library for collision and proximity queries。

FCL is also used as the collision checking library for grasping and manipulation pipeline implemented on a PR2. In the course of the task, the overall algorithm makes multiple calls to inverse kinematics and motion planning modules that use FCL as the underlying collision checking library.

• The problems of collision detection and distance computations can be classified based on algorithms for
  1. Convex Polytope based Collision: The convex polytope collision in FCL is based on GJK(Gilbert–Johnson–Keerthi algorithm) and EPA(Expanding Polytope Algorithm)
  2. Bounding Volume Hierarchy based Collision: Typical examples of bounding volumes include axis-aligned bounding box (AABB), spheres, oriented bounding box (OBB), discrete oriented polytope (k-DOP) and swept sphere volume (SSV). They can be extended to deformable models by updating the hierarchies during each step of the simulation.
  3. Continuous Collision Detection: FCL uses these CCD(continuous collision detection) and CA(conservative advancement) algorithms.
  4. Broad-Phase Collision Detection: Check collisions between a large number of objects. FCL currently uses SaP(Sweep and Prune), which works well when moving objects have high spatial coherence.
  5. Point Cloud Collision Detection: With the recent advances in RGB-D cameras and LIDAR sensors, there is increased interest in performing various queries on noisy point-cloud datasets.
  6. Parallel Collision and Proximity Computation: Many GPU-based parallel collision and proximity computation algorithms have been proposed, especially for a single collision query, multiple collision queries or N-body collision.
• FCL models all collision and proximity queries between two objects as a traversal process along a hierarchical structure. The traversal process is performed in three steps in FCL:
  1. Object representation
     • Basic geometric shapes: FCL supports seven types of basic geometric shapes, including sphere, box, cone, cylinder, capsule, convex mesh and plane. They are represented using a single-level hierarchy with the corresponding bounding volume as the unique node.
     • Arbitrary geometric objects：The unstructured mesh/soup is represented as a bounding volume hierarchy(BVH) and the specific type of bounding volume is specified as a template parameter. In FCL, four BV types are currently supported: AABB, OBB, RSS and kDOP. The BVH data structure in FCL stores both the vertices and triangles of the underlying object. FCL uses a state machine to guarantee that the output of the construction process is a valid BVH structure.
     • The hierarchical structure along with the object’s configuration information is stored in a structure called CollisionObject.
  2. Traversal node initialization
     • The traversal node is the structure that stores the complete information required to perform the traversal for a specific query.
     • The traversal node also decides the traversal strategy for a given query.
     • Given two BVHs, the collision or proximity computation between them is usually performed by traversing the bounding volume test tree (BVTT) generated from the two BVHs.
     • In FCL, we separate the actual traversal framework from the traversal data and traversal strategies.
  3. Hierarchy traversal: After the traversal node initialization step, we traverse the hierarchical structure to perform a specific collision or proximity query.
     • In FCL, we use a traversal node to provide all the necessary information to access the BVH hierarchy structure and determine the traversal order.
     • All traversal node types are derived from one base class TraversalNode. Two subclasses CollisionTraversalNode, which is used for collision queries, and DistanceTraversalNode, which is distance queries.
     • Algorithm 1 is the recursive algorithm for collision queries whose input could be any traversal node derived from CollisionTraversalNode.
     • Algorithm 2 is the recursive algorithm for collision queries whose input could be any traversal node derived from CollisionTraversalNode.
     • FCL provides a self-collision recursive traversal scheme, which is mainly used for deformable models.

HPP(Humanoid Path Planner)

HPP是Humanoid Path Planner的缩写，是a software platform aimed at solving motion planning problems for humanoid robots，它具体的github repo在这里，相关的学术文章叫做HPP: A new software for constrained motion planning。

HPP的主要特征和概念：

• HPP is delivered with an exhaustive Python integration through CORBA(The client/server CORBA architecture adopted by HPP is an essential asset. It allows considering integration with other software developed in different languages, and distributed execution on robots and computers.)
• HPP is delivered with a viewer based on Open-SceneGraph, the Gepetto viewer. The viewer can receive Python command line prompts, and be used via a GUI, for testing problems.
• HPP includes Python methods to automatically export a computed motion to the open-source Blender software.
• HPP is delivered with tutorials addressing classic or constrained motion planning, available on the documentation.

HPP的主要概念:

• hpp-model
• hpp-constraints: a generic constraint framework implemented on top of hpp-model
• hpp-core: an object-oriented architecture that allows customizing parts of a motion planning algorithm
• Contrary to OMPL, and similarly to OpenRAVE and Choreonoid, HPP commits to a kinematic model.
• A kinematic chain is implemented as a tree of joints moving inertial and geometrical objects. The configuration space of a robot is the Cartesian product of the configuration spaces of its joints. The joints are handled joints in a way that configuration space is a Lie group. It uses quaternions rather than Euler angles.
• The configuration of the robot is described by a vector. Geometrical objects are stored using a modified version of FCL(Flexible Collision Library).
• OMPL does not handle constraints natively since it does not commit to a kinematic model. HPP handles indifferently any constraint that can be written with a differentiable function(Non-Linear equality and inequality constraints) in a unified way.
• HPP supports constraint graphs. A constraint graph can be seen as a finite state machine, with special nodes and transitions. Each node is characterized by a constraint.
• URDF files are used for the external representation of the kinematic model, making it compatible with ROS.

实例：

Pinocchio

Pinocchio是Rigid Body Algorithms for poly-articulated systems based on revisited Roy Featherstone’s algorithms，提供 the analytical derivatives of the main Rigid-Body Algorithms like the Recursive Newton-Euler Algorithm or the Articulated-Body Algorithm，它具体的github repo在这里，相关的学术文章为The Pinocchio C++ library: A fast and flexible implementation of rigid body dynamics algorithms and their analytical derivatives

Pinocchio provides support for many open-source and free visualizers:

• Gepetto Viewer: a C++ viewer based on OpenSceneGraph with Python bindings and Blender export. See here for a C++ example on mixing Pinocchio and Gepetto Viewer.
• Meshcat: supporting visualization in Python and which can be embeded inside any browser.
• Panda3d: supporting visualization in Python and which can be embeded inside any browser.

Many external viewers can also be integrated. See example here for more information.

Pinocchio releases are managed by robotpkg, a package manager tailored for robotics software, available on most Unix and BSD platforms.

• A fundamental paradigm of Pinocchio is the strict separation between model and data。
• By data, we mean all values which are the result of a computation.
• All the algorithms in Pinocchio follow the signature:
  • algorithm(model, data, arg1, arg2, …)
• By model, we mean the physical description of the robot, including kinematic and possibly inertial parameters defining its structure.
  • A model object never changes within an algorithm of Pinocchio.
  • A robot kinematic model is represented as a kinematic tree, containing a collection of all the joints, information about their connectivity, and, optionally, the inertial quantities associated with each link.
  • A geometric model is also introduced in Pinocchio for the robot, i.e. the volumes attached to the aforementioned kinematic tree, the kinematic model.
    • This model can be used for displaying the robot and computing quantities associated with collisions. The geometric model is stored in a GeometricModel object, while buffers and quantities used by associated algorithms are defined in a GeometricData object.
    • Bodies of the robot are attached to each joint, while obstacles of the environment are defined in the world frame.
    • Collision and distance algorithms for the kinematic trees are implemented, based on FCL methods.
  • A model can be created using the C++ API or loaded from an external file, which can be either URDF, Lua (following the RBDL standard), or Python.
  • In Pinocchio, a joint can have one or several degrees of freedom. Each type of joints is characterized by its own specific configuration and tangent spaces.
    1. Revolute joints
    2. Prismatic joints
    3. Spherical joints
    4. Translation joints
    5. Planar joints
    6. Free-floating joints
    7. Composite joint: More complex joints can be created as a collection of ordinary ones through the concept of Composite joint.
• Main Algorithms: The implementation of basic algorithms is recursive.
  • Forward kinematics: Given robot configuration, calculate the spatial placements of each joint and to store them as coordinate transformations. If the velocity is given, it also computes the spatial velocities of each joint (expressed in local frame), and similarly for accelerations.
  • Kinematic Jacobian: The spatial Jacobian of each joint can be easily computed with a single forward pass, either expressed locally or in the world frame.
  • Inverse dynamics: the Recursive Newton-Euler Algorithm (RNEA) computes the inverse dynamics. Given a desired robot configuration, velocity and acceleration, the torques required to execute this motion are computed and stored.
  • Joint space inertia matrix：The Composite Rigid Body Algorithm (CRBA) is employed to compute the joint space inertia matrix of the robot.
  • Forward dynamics: the Articulated Body Algorithm (ABA) computes the unconstrained forward dynamics. Given a robot configuration, velocity, torque and external forces, the resulting joint accelerations are computed.
  • Additional algorithms: Other methods are provided, most notably for constrained forward dynamics, impulse dynamics, inverse of the joint space inertia and centroidal dynamics.
  • Beside proposing standard forward and inverse dynamics algorithms, Pinocchio also provides efficient implementations of their analytical derivatives.
  • In addition to analytical derivatives, Pinocchio supports automatic differentiation.