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 IGNITION_PHYSICS_RELATIVEQUANTITY_HH_
#define IGNITION_PHYSICS_RELATIVEQUANTITY_HH_

#include <ignition/physics/FrameID.hh>
#include <ignition/physics/FrameData.hh>

namespace ignition
{
  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
      // ignition/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>>;
    IGN_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>>>;
    IGN_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>>;
    IGN_PHYSICS_MAKE_ALL_TYPE_COMBOS(RelativePosition)

    
    template <typename Scalar, std::size_t Dim>
    using RelativeForce = RelativeQuantity<
        LinearVector<Scalar, Dim>, Dim, detail::VectorSpace<Scalar, Dim>>;
    IGN_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>>;
    IGN_PHYSICS_MAKE_ALL_TYPE_COMBOS(RelativeTorque)

    
    template <typename Scalar, std::size_t Dim>
    using RelativeAlignedBox = RelativeQuantity<
        AlignedBox<Scalar, Dim>, Dim, detail::AABBSpace<Scalar, Dim>>;
    IGN_PHYSICS_MAKE_ALL_TYPE_COMBOS(RelativeAlignedBox)

    
    template <typename Scalar, std::size_t Dim>
    using RelativeFrameData = RelativeQuantity<
        FrameData<Scalar, Dim>, Dim, detail::FrameSpace<Scalar, Dim>>;
    IGN_PHYSICS_MAKE_ALL_TYPE_COMBOS(RelativeFrameData)

    
    template <typename Scalar, std::size_t Dim>
    using RelativeWrench = RelativeQuantity<
        Wrench<Scalar, Dim>, Dim, detail::WrenchSpace<Scalar, Dim>>;
    IGN_PHYSICS_MAKE_ALL_TYPE_COMBOS(RelativeWrench)
  }
}

#include <ignition/physics/detail/RelativeQuantity.hh>

#endif