RelativeQuantity.hh

Go to the documentation of this file.

/*
 * Copyright (C) 2017 Open Source Robotics Foundation
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
*/
 
#ifndef GZ_PHYSICS_RELATIVEQUANTITY_HH_
#define GZ_PHYSICS_RELATIVEQUANTITY_HH_
 
#include <gz/physics/FrameID.hh>
#include <gz/physics/FrameData.hh>
 
namespace gz
{
  namespace physics
  {
    template <typename Q, std::size_t Dim, typename CoordinateSpace>
    class RelativeQuantity
    {
      public: template <typename... Args>
      explicit RelativeQuantity(const FrameID &_parentID, Args&&... _args);
 
      public: RelativeQuantity(const Q &_rawValue);
 
      public: Q &RelativeToParent();
 
      public: const Q &RelativeToParent() const;
 
      public: const FrameID &ParentFrame() const;
 
      public: void MoveToNewParentFrame(const FrameID &_newParentFrame);
 
      public: using Quantity = Q;
 
      public: using Space = CoordinateSpace;
 
      public: enum { Dimension = Dim };
 
      private: FrameID parentFrame;
 
      private: Q value;
    };
 
    template <typename Q, std::size_t Dim, typename CoordinateSpace>
    std::ostream& operator <<(
        std::ostream& stream,
        const RelativeQuantity<Q, Dim, CoordinateSpace> &_fq)
    {
      stream << "Parent Frame ID: " << _fq.ParentFrame().ID()
             << "\nRelative To Parent:\n" << _fq.RelativeToParent();
 
      return stream;
    }
 
    namespace detail
    {
      // Forward delcarations of CoordinateSpaces
      template <typename, std::size_t> struct SESpace;
      template <typename, std::size_t, typename> struct SOSpace;
      template <typename, std::size_t> struct EuclideanSpace;
      template <typename, std::size_t> struct LinearVelocitySpace;
      template <typename, std::size_t> struct AngularVelocitySpace;
      template <typename, std::size_t> struct LinearAccelerationSpace;
      template <typename, std::size_t> struct AngularAccelerationSpace;
      template <typename, std::size_t> struct VectorSpace;
      template <typename, std::size_t> struct FrameSpace;
      template <typename, std::size_t> struct AABBSpace;
      template <typename, std::size_t> struct WrenchSpace;
      // TODO(MXG): We can add more spaces to support other types like Moments
      // of Inertia, Jacobians, Spatial Velocities/Accelerations, Wrench+Point
      // pairs, and so on.
      //
      // Users can also define Spaces for their own types (see the header
      // gz/physics/detail/FrameSemantics.hpp for example implementations)
      // and use them seamlessly in the Frame Semantics infrastructure.
    }
 
    template <typename Scalar, std::size_t Dim>
    using RelativePose = RelativeQuantity<
        Pose<Scalar, Dim>, Dim, detail::SESpace<Scalar, Dim>>;
    GZ_PHYSICS_MAKE_ALL_TYPE_COMBOS(RelativePose)
 
    
    template <typename Scalar, std::size_t Dim>
    using RelativeRotationMatrix = RelativeQuantity<
        Eigen::Matrix<Scalar, Dim, Dim>, Dim,
        detail::SOSpace<Scalar, Dim, Eigen::Matrix<Scalar, Dim, Dim>>>;
    GZ_PHYSICS_MAKE_ALL_TYPE_COMBOS(RelativeRotationMatrix)
 
    
    template <typename Scalar>
    using RelativeQuaternion = RelativeQuantity<
        Eigen::Quaternion<Scalar>, 3, detail::SOSpace<Scalar, 3,
        Eigen::Quaternion<Scalar>>>;
    // Note: Eigen only supports quaternions for 3 dimensional space, so we do
    // not have a dimensionality template argument.
    using RelativeQuaterniond = RelativeQuaternion<double>;
    using RelativeQuaternionf = RelativeQuaternion<float>;
 
    template <typename Scalar, std::size_t Dim>
    using RelativePosition = RelativeQuantity<
        LinearVector<Scalar, Dim>, Dim, detail::EuclideanSpace<Scalar, Dim>>;
    GZ_PHYSICS_MAKE_ALL_TYPE_COMBOS(RelativePosition)
 
    
    template <typename Scalar, std::size_t Dim>
    using RelativeForce = RelativeQuantity<
        LinearVector<Scalar, Dim>, Dim, detail::VectorSpace<Scalar, Dim>>;
    GZ_PHYSICS_MAKE_ALL_TYPE_COMBOS(RelativeForce)
 
    
    template <typename Scalar, std::size_t Dim>
    using RelativeTorque = RelativeQuantity<
        AngularVector<Scalar, Dim>, Dim,
        detail::VectorSpace<Scalar, (Dim*(Dim-1))/2>>;
    GZ_PHYSICS_MAKE_ALL_TYPE_COMBOS(RelativeTorque)
 
    
    template <typename Scalar, std::size_t Dim>
    using RelativeAlignedBox = RelativeQuantity<
        AlignedBox<Scalar, Dim>, Dim, detail::AABBSpace<Scalar, Dim>>;
    GZ_PHYSICS_MAKE_ALL_TYPE_COMBOS(RelativeAlignedBox)
 
    
    template <typename Scalar, std::size_t Dim>
    using RelativeFrameData = RelativeQuantity<
        FrameData<Scalar, Dim>, Dim, detail::FrameSpace<Scalar, Dim>>;
    GZ_PHYSICS_MAKE_ALL_TYPE_COMBOS(RelativeFrameData)
 
    
    template <typename Scalar, std::size_t Dim>
    using RelativeWrench = RelativeQuantity<
        Wrench<Scalar, Dim>, Dim, detail::WrenchSpace<Scalar, Dim>>;
    GZ_PHYSICS_MAKE_ALL_TYPE_COMBOS(RelativeWrench)
  }
}
 
#include <gz/physics/detail/RelativeQuantity.hh>
 
#endif