Extending your geometry library with semantic checking

Imagine you have your own geometry library with support for geometric relation coordinate representations and calculations with these coordinate representations. You however would like to have semantic support on top of this geometry library. Probably the best thing to do in this case is to mimic our support for the Orocos Kinematics and Dynamics Library. To have a look at it do:

roscd geometric_semantics_kdl/

Template specialization

The only thing you have to do is write template specializations. So for instance to get support for KDL::Rotation, which is a coordinate representation for a Orientation geometric relation, you have to write the template specialization for OrientationCoordinates<T>, i.e. OrientationCoordinates<KDL::Rotation>.

Semantic constraints invoked by your coordinate representations

The first thing to find out is which semantic constraints are invoked by the particular coordinate representation you use. For instance a KDL::Rotation represents a 3x3 rotation matrix and invokes the semantic constraint that the reference orientation frame is equal to the coordinate frame.

The possible semantic constraints are listed in the *Coordinates.h files in the geometric_semantics core library. So for instance for OrientationCoordinates we find there an enumeration of the different possible semantic constraints imposed by Orientation coordinate representations: 

    /**                                                                                                                                                                                                                                      
      *\brief Constraints imposed by the orientation coordinate representation to the semantics                                                                                                                                              
      */                                                                                                                                                                                                                                     
    enum Constraints{                                                                                                                                                                                                                        
        noConstraints = 0x00,                                                                                                                                                                                                                
        coordinateFrame_equals_referenceOrientationFrame = 0x01, // constraint that the orientation frame on the reference body has to be equal to the coordinate frame                                                                      
    };

You should specify the constraint when writing the template specialization of the OrientationCoordinates<KDL::Rotation>: 

// template specialization for KDL::Rotation                                                                                                                                                                                             
    template <>                                                                                                                                                                                                                              
    OrientationCoordinates<KDL::Rotation>::OrientationCoordinates(const KDL::Rotation& coordinates):                                                                                                                                         
        data(coordinates),                                                                                                                                                                                                                   
        constraints(coordinateFrame_equals_referenceOrientationFrame){    };

Specializing other functions to do actual coordinate calculations

The other function template specializations specify the actual coordinate calculations that have to be performed for semantic operations like inverse, changing the coordinate frame, changing the orientation frame, ... For instance, to specialize the inverse for KDL::Rotation coordinate representations: 

   template <>                                                                                                                                                                                                                              
    OrientationCoordinates<KDL::Rotation> OrientationCoordinates<KDL::Rotation>::inverse2Impl() const                                                                                                                                        
    {                                                                                                                                                                                                                                        
        return OrientationCoordinates<KDL::Rotation>(this->data.Inverse());                                                                                                                                                                  
    }

»