\LARGE Domain Specific Language for Geometric Relations between Rigid Bodies targeted to robotic applications
Tinne De Laet
Wouter Schaekers
Jonas de Greef
and Herman Bruyninckx% <-this % stops a space \thanks{Tinne De Laet and Herman Bruyninckx are with the Department of Mechanical Engineering
Katholieke Universiteit Leuven
Belgium. Wouter Schaekers and Jonas de Greef are students at the Computer Science Engineering Department
This paper presents a DSL for geometric relations between rigid bodies such as relative position, orientation, pose, linear velocity, angular velocity, and twist. The DSL is the formal model of the recently proposed semantics for the standardization of geometric relations between rigid bodies~\cite{DeLaet-ram2012a,DeLaet-rrfc2012}, referred to as `geometric semantics'. This semantics explicitly states the coordinate-invariant properties and operations, and, more importantly, all the choices that are made in coordinate representations of these geometric relations. This results in a set of concrete suggestions for standardizing terminology and notation, allowing programmers to write fully unambiguous software interfaces, including automatic checks for semantic correctness of all geometric operations on rigid-body coordinate representations. The DSL is implemented in two different ways: an external DSL in Xcore and an internal DSL in Prolog. Besides defining a grammar and operations, the DSL also implements constraints. In the Xcore model, the Object Constraint Language language is used, while in the Prolog model, the constraint are natively modelled in Prolog. This paper discusses the implemented DSL and the tools developed on top of this DSL. In particular an editor, checking the semantic constraints and providing semantic meaningful errors during editing is proposed. % Some domain-specific languages expand over time to include full-featured programming tools, which further complicates the question of whether a language is domain-specific or not. A good example is the functional language XSLT, specifically designed for transforming one XML graph into another, which has been extended since its inception to allow (particularly in its 2.0 version) for various forms of filesystem interaction, string and date manipulation, and data typing. % In model-driven engineering many examples of domain-specific languages may be found like OCL, a language for decorating models with assertions or QVT, a domain specific transformation language.
document.tex
                                      
\documentclass[letter, 10pt, conference]{ieeeconf}     
                                                        
\IEEEoverridecommandlockouts                              
                                                          
\overrideIEEEmargins

\usepackage{cite}      
\usepackage{graphicx}  
\usepackage{subfigure} 
\usepackage{url}       
\usepackage{amsmath}  
\usepackage{amssymb}
\usepackage{epsfig}     % include Encapsulated PostScript in LaTeX documents.
\usepackage{color}
\usepackage[mathcal]{euscript}
\usepackage{mathrsfs}
\usepackage[ruled,vlined,linesnumbered]{algorithm2e}
\usepackage[scale={0.8,0.8}]{geometry}
\usepackage{rotating}
\usepackage{url}
\usepackage{multirow}
\usepackage{multicol}
\usepackage{eurosym}
\usepackage{framed}
\usepackage{footnote}
\usepackage{float}
\usepackage{caption}
\usepackage{fancyhdr}
\usepackage{paralist}
\usepackage{etoolbox}
\usepackage{listings}
\usepackage{textcomp}
\definecolor{listinggray}{gray}{0.95}
\definecolor{lbcolor}{rgb}{0.95,0.95,0.95}

\newcommand{\footnoteremember}[2]{
\footnote{#2}
\newcounter{#1}
\setcounter{#1}{\value{footnote}}
}
\newcommand{\footnoterecall}[1]{
\footnotemark[\value{#1}]
}



% correct bad hyphenation here
\hyphenation{}

\input{macroGeometricPrimitives.tex}
\input{macroSemantics.tex}
\input{macroSemanticOperations.tex}
\input{listingsSettings}

\graphicspath{{images/}{images_geometricSemantics/}{listings/}}


\providecommand{\SetAlgoLined}{\SetLine}
\providecommand{\DontPrintSemicolon}{\dontprintsemicolon}



\title{\LARGE \bf Domain Specific Language for Geometric Relations between Rigid Bodies targeted to robotic applications} 

\author{Tinne De Laet, Wouter Schaekers, Jonas de Greef, and Herman Bruyninckx% <-this
% stops a space 
\thanks{Tinne De Laet and Herman Bruyninckx are with the Department of Mechanical Engineering,
Katholieke Universiteit Leuven, Belgium. 
Wouter Schaekers and Jonas de Greef are students at the Computer Science Engineering Department,
Katholieke Universiteit Leuven, Belgium. 
Corresponding author: Tinne De Laet
(\protect\url{Tinne.DeLaet@mech.kuleuven.be})}% 
}

\begin{document}
% % paper title
% \title{Two level sequential online multitarget tracking and localization algorithm: theory and experimental validation}
% \author{\name Tinne De Laet \email tinne.delaet@mech.kuleuven.be \\
%        \name Joris De Schutter \email joris.deschutter@mech.kuleuven.be \\
%        \name Herman Bruyninckx \email herman.bruyninckx@mech.kuleuven.be \\
%        \addr Department of Mechanical Engineering, Katholieke Universiteit Leuven,\\
%        Celestijnenlaan 300B, box 2420, 3001 Heverlee, Belgium}
% %\author{Tinne De Laet}
       
\maketitle
\thispagestyle{empty}
\pagestyle{empty}
       
       
% make the title area
\maketitle

% abstract
\begin{abstract}
This paper presents a DSL for geometric relations between rigid bodies such as relative position, orientation, pose, linear velocity, angular velocity, and twist.
The DSL is the formal model of the recently proposed semantics for the standardization of geometric relations between rigid bodies~\cite{DeLaet-ram2012a,DeLaet-rrfc2012}, referred to as `geometric semantics'. This semantics explicitly states the
coordinate-invariant properties and operations, and, more importantly, all
the choices that are made in coordinate representations of these geometric
relations. This results in a set of concrete suggestions for standardizing
terminology and notation, allowing programmers to write fully unambiguous
software interfaces, including automatic checks for semantic correctness of
all geometric operations on rigid-body coordinate representations.

The DSL is implemented in two different ways:
an external DSL in Xcore and an internal DSL in Prolog.
Besides defining a grammar and operations, the DSL also implements constraints.
In the Xcore model, the Object Constraint Language language is used, while in the Prolog model, the constraint are natively modelled in Prolog.

This paper discusses the implemented DSL and the tools developed on top of this DSL. In particular an editor, checking the semantic constraints and providing semantic meaningful errors during editing is proposed.

% Some domain-specific languages expand over time to include full-featured programming tools, which further complicates the question of whether a language is domain-specific or not. A good example is the functional language XSLT, specifically designed for transforming one XML graph into another, which has been extended since its inception to allow (particularly in its 2.0 version) for various forms of filesystem interaction, string and date manipulation, and data typing.
% In model-driven engineering many examples of domain-specific languages may be found like OCL, a language for decorating models with assertions or QVT, a domain specific transformation language.
\end{abstract}


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
\section{Introduction}
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
\label{sec:introduction}
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

When developing robotic applications, robot programmers and application developers have to deal with three-dimensional motion and relations between rigid bodies (manipulated objects, robot links, or mobile bases).
Rigid bodies are essential primitives in the modelling of robotic devices,
tasks and perception. Basic geometric relations between rigid bodies include
relative position, orientation, pose (combining position and orientation), linear velocity, angular
velocity, and twist (combining linear and angular velocity). 
To express geometric relations and perform mathematical operations on them (e.g. composition of relative motion, time differentiation, or integration), robot programmers have to choose coordinate representations with which to perform the corresponding numerical operations.

Until recently, and despite a history of over 50 years, the geometric properties of rigid-body operations, and their coordinate representations, were not standardized, which has led to a proliferation of mutually incompatible software libraries, in the robot control products of commercial manufacturers as well as in \emph{open source} libraries such as KDL (Kinematics and Dynamics Library)~\cite{kdl-url}, ROS (Robot Operating System)~\cite{ros-url}, RL (Robotics Library)~\cite{robotics-library-url}, \dots.
All geometric relations and their coordinate representations entail a surprisingly large number of choices or assumptions, which are often made implicitly, but which are necessary to consider in view of (i) understanding the physical meaning of the numerical values that constitute the coordinate representation of a geometric relation and (ii) performing physically meaningful mathematical operations on these numerical values.
Not explicitly stating the above assumptions may lead to errors in the calculations (composition of geometric relations expressed in different coordinate frames, composition of poses and orientation coordinate representations in wrong order,\ldots \cite{DeLaet-ram2012a}).
To alleviate this problem, we recently proposed semantics for the standardization of geometric relations between rigid bodies~\cite{DeLaet-ram2012a}, referred to as `geometric semantics'. This semantics explicitly states the
coordinate-invariant properties and operations, and, more importantly, all
the choices that are made in coordinate representations of these geometric
relations. This results in a set of concrete suggestions for standardizing
terminology and notation, allowing programmers to write fully unambiguous
software interfaces, including automatic checks for semantic correctness of
all geometric operations on rigid-body coordinate representations.
This resulted in a Robot Request for Comments~\cite{DeLaet-rrfc2012} for the Robot Engineering Task Force~\cite{retf-url}.
Furthermore, software providing a C++ implementation of the software is developed and available as open-source~\cite{DeLaet-ram2012b,GeometricSemanticsSoftware-url}. 


Domain Specific Languages are lightweight programming languages designed to concisely express the concepts of a particular domain. Commonly two types are distinguished: internal and external DSLs. The former are built on top of an existing language, while the latter are developed from scratch resulting in a custom syntax and making them independent from existing languages.
By reusing existing infrastructure, internal DSLs are easier to create, maintain, and combine with other DSLs than external ones~\cite{Klotzbuecher2011-IROS}.
External DSLs, while suffering from an increased cost for creating and maintenance, are not constrained by any other language.
Therefore, the choice between an internal or external design often depends on the particular application, use case, available tools, and preferences of the designer.
In this paper we develop both types for the geometric semantics: \begin{inparaenum}
                                                                  \item an \emph{external} DSL in Xcore and
                                                                  \item an \emph{internal} DSL in Prolog.
                                                                 \end{inparaenum}
% We discuss the advantages and disadvantages of both.


The goal of this paper is fourfold. Firstly, we want to build a DSL for geometric relations between rigid bodies such as relative position, orientation, pose, linear velocity, angular velocity, and twist founded on the geometric semantics~\cite{DeLaet-ram2012a,DeLaet-rrfc2012}.
This DSL advances with respect to the available available implementation in the general-purpose programming language C++, by formalizing the underlying model of the geometric semantics.
Furthermore, the DSL is the basis for the developments of tools that assist the robot programmers and application developers to write fully unambiguous
software interfaces and prevent common errors in geometric calculations. In particular this paper presents and editor built on top of the proposed DSL that automatically checks the semantic correctness of
all geometric operations on rigid-body coordinate representations, while writing and editing the code.
Secondly, we want to explore the impact of different design choices (internal, external), work flows, and tools.
Thirdly, we believe that due to the concise and mature nature of the underlying geometric semantics theory and its relevance for the robotics domain it will prove to be an excellent example for future DSL development in robotics. 
Fourthly, we will highlight the unfulfilled robotic needs still present in Model Driven Engineering.

% To explore the different language capabilities we implemented the DSL in Eclipse Xcore (Xtext + OCL constraint modelling) and in Prolog. 

% 
% The goal of the developed DSL is threefold:
% \begin{inparaenum}
%  \item to support robot programmers for geometric calculations, in order to prevent common errors,
%  \item to explore the impact of different design choices (internal, external) and workflows (tools),
%  \item to be an example for future DSL development in robotics.
% \end{inparaenum}

 




% We believe excellent example because of the maturity of the available theory on the geometric semantics + small enough 

% We will show how the development and the final pose and twist graph profit from the geometric semantics. 
% This paper advances the state of the art in robotics in different ways:
% \begin{inparaenum}
%  \item it presents the first scene graph explicitly supporting and relying on the \emph{semantics for standardization of geometric relations between rigid bodies};
%  \item to our knowledge it is the first one to propose a \emph{twist graph}, to do native calculations with twists;
%  \item it allows for \emph{semantic queries} on the scene graph,
%  \item hereby it naturally accommodates for \emph{constant poses},
%  \item it can use \emph{any particular coordinate representation}, and
%  \item automatically \emph{checks the constraints imposed by the particular coordinate representations}.
% \end{inparaenum}
% Finally, this paper comes with pointers to all software released under a dual BSD/LGPLv2.1 license.


%\paragraph{Paper Overview}
Section~\ref{sec:related} gives an overview of related work.
Section~\ref{sec:geometricSemantics} provides a short summary of the geometric semantics theory relevant for this paper.
Section~\ref{sec:levelsAbstraction} situates this paper's contributions using the four levels of abstraction in Model Driven Engineering.
% Section~\ref{sec:experiments} contains a robotics example illustrating the use of the pose and twist graph and the accompanying software. 
% Finally, Section~\ref{sec:discussion} discusses the contributions of this paper and points to future work.

  

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
\section{Related work}
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
\label{sec:related}
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Since we are not aware of any DSL on the semantics for geometric relationships between rigid bodies, our related work will rather point to some other DSLs developed in the robotics domain.

Frigerio et al.'s DSL is the DSL most related to the DSL proposed in this paper. They propose a DSL for kinematic models and fast implementation of robot dynamic algorithms.
The DSL allows to model algorithms that are parametrised on the kinematics/dynamics model of a robot, hereby facilitating the generation of executable code tailored for a specific robot. This approach only requires the users to provide a high level description of their robot and relieves them from hand-crafted development.
\newline
Furthermore, we want to mention the Mechatronics Description Language (MDL), which is a domain-specific language that can model the kinematic structure of individual robot modules and declaratively describe their possible interconnections. From this description, the MDL compiler generates the code that is needed to simulate the resulting robots within Webots, a widely used commercial robot simulator, and the software component needed for spatial structure computations by a virtual machine-based runtime system, which we have developed and use for programming physical modular robots~\cite{bordignon2010}. 

Klotzb\"ucher et al.~\cite{Klotzbuecher2010-DYROS} propose a DSL for specifying robotic tasks using the task frame formalism as an example of lua as a lightweight and composable metamodelling language for specification and validation of internal DSLs.
In later work Klotzb\"ucher et al.~\cite{Klotzbuecher2011-IROS}) propose a DSL allowing to separate task specification and coordination of these tasks using state charts.

% Moghadam et al.~\cite{Moghadam2011} propose a DSL python-basedDSL for self-reconfigurable modular robotics research.

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
\section{Levels of abstraction in Model Driven Engineering}
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
\label{sec:levelsAbstraction}
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Figure~\ref{fig:levelsAbstraction} illustrates a systematic approach to model a certain domain in \emph{four levels of abstraction} \cite{Bezivin2005,Klotzbuecher2011-IROS}. 
These four levels have the following meaning for the context of the geometric semantics:
\begin{itemize}
 \item[\textbf{M0:}] the level of the \emph{concrete implementations}, for instance a particular set of geometric semantics calculations using the C++ library of the geometric semantics~\cite{GeometricSemanticsSoftware-url},
 \item[\textbf{M1:}] the level of a \emph{particular set} of geometric semantics calculations using the geometric semantics DSL,
 \item[\textbf{M2:}] the level of the \emph{application independent geometric semantics DSL}, which provides a language for both coordinate representation independent and dependent (taking into account the constraints of a particular coordinate representation) geometric calculations.
 \item[\textbf{M3:}] the highest level of abstraction, that is, the model in terms of which we describe our meta-models (M2). For example, ecore that we can use to describe our geometric semantics DSL.
 %represents all the constraints that a geometric semantics DSL has to satisfy in order to be a valid geometric semantics DSL.
\end{itemize}

\begin{figure*}
\centering
	\resizebox{0.65\textwidth}{!}{\includegraphics{mlevels_image}}
	\caption{The four levels of abstraction.)}
	\label{fig:levelsAbstraction}
\end{figure*}

The geometric semantics theory~\cite{DeLaet-ram2012a}, summarized in Section~\ref{sec:geometricSemantics}, can be considered as the basis for the \textbf{M2} level DSL, as it describes (in language) the constraints on the geometric relations semantics, the possible operations on the geometric relations, the constraints on the relations and operations, and the constraints imposed by particular coordinate representations.
The available C++ implementation~\cite{GeometricSemanticsSoftware-url} and the applications implemented in it, are examples of the \textbf{M0} level.

This paper provides DSL implementations, both an external DSL and an internal DSL, on the \textbf{M2} level.
Furthermore, this paper presents tools based on the developed DSLs, that allow the DSL users to implement their particular set of geometric semantic calculations, i.e. to work on the \textbf{M1} level.
To illustrate the proposed approach, we provide an example on a M1 implementation for a particular geometric calculation and show how the developed DSL and the accompanying tools will help to prevent commonly made errors.


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
\section{Geometric semantics, background \cite{DeLaet-ram2012a}}
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
\label{sec:geometricSemantics}
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
\subsection{Geometric relations}
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
\label{subsec:geometricSemantics:geometricRelations}
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

Geometric relations between bodies are described using a set of \emph{geometric primitives}\footnote{This background contains a short summary of the semantics for the standardization of geometric relations between rigid bodies, for more details we refer to \cite{DeLaet-ram2012a}.}:
points ($\point{e}$), vectors, orientation frames ($\orientation{a}$ , they represent an orientation, by means of
three orthonormal vectors indicating the frame's X-axis $\axis{X}$, Y-axis
$\axis{Y}$, and Z-axis $\axis{Z}$), and frames ($\refframe{g}$). 
Figure~\ref{fig:primitves} presents the geometric primitives body, point, vector, orientation frame, and frame graphically. To help the reader we will consistently use the following naming for the geometric primitives to represent the geometric relation of a body $\body{C}$ with respect to body $\body{D}$ in this document:  $\fixedTo{\point{e}}{C}$, $\fixedTo{\orientation{a}}{C}$, $\fixedTo{\refframe{g}}{C}$, $\fixedTo{\point{f}}{D}$, $\fixedTo{\orientation{b}}{D}$, and $\fixedTo{\refframe{h}}{D}$.

\begin{figure}
\centering
	\resizebox{0.7\columnwidth}{!}{\includegraphics{primitives_image}}
	\caption{The geometric relation between rigid bodies are described using a set of \emph{geometric primitives}: points, vectors, orientation frames, and frames. The above figure shows the geometric primitives that are useful to define the position, orientation, pose, linear velocity, angular velocity, and twist of body $\body{C}$ with respect to body $\body{D}$:
	an orientation frame $\protect\orientation{a}$, a point $\protect\point{e}$, and frame $\protect\refframe{g}$ fixed to body $\body{C}$, an orientation frame $\protect\orientation{b}$, a point $\protect\point{f}$, and frame $\protect\refframe{h}$ fixed to body $\body{D}$, and a coordinate frame $\protect\orientation{r}$, considered instantaneously fixed to body $\body{D}$, in which the coordinates are expressed. (Extract from~\cite{DeLaet-ram2012a}.)}
	\label{fig:primitves}
\end{figure}


Table~\ref{tab:semantics} summarizes the minimal but complete set of geometric primitives and the (coordinate) semantics for the geometric relations position, orientation, pose,  twist between rigid bodies, which are the most relevant relations for this paper. 

 \begin{table*}
  \begin{center}
   \begin{tabular*}{1\textwidth}{@{\extracolsep{\fill}}llllc} \hline
\textbf{Geometric Relation} & \textbf{(Coordinate) semantics} & \textbf{Geometric primitives} & \textbf{Graphical representation} \\ \hline \\
\textbf{Position}  & $\Position{e}{C}{f}{D}{}$  & point $\point{e}$ & \multirow{6}{*}{
\resizebox{0.12\textwidth}{!}{\includegraphics{relposition_image}}
}\\ 
& $\Position{e}{C}{f}{D}{r}$		& body $\body{C}$ \\
&& reference point $\point{f}$ \\
&& reference body $\body{D}$ \\
&& coordinate frame $\orientation{r}$ \\ 
 \multicolumn{3}{p{15cm}}{\emph{Position of point $\point{e}$ fixed to body $\body{C}$ ($\fixedTo{\point{e}}{C}$) with respect to 
point $\point{f}$ fixed to body $\body{D}$ ($\fixedTo{\point{f}}{D}$), expressed in coordinate frame $\orientation{r}$}}
\\
 \\
\textbf{Orientation}  & $\Orientation{a}{C}{b}{D}{}$  & orientation frame $\orientation{a}$ 
& \multirow{3}{*}{
\resizebox{0.12\textwidth}{!}{\includegraphics{relorientation_image}}
}
\\
& $\Orientation{a}{C}{b}{D}{r}$ & body $\body{C}$ \\
&& reference orientation frame $\orientation{b}$ \\
&& reference body $\body{D}$ \\
&& coordinate frame $\orientation{r}$ \\
 \multicolumn{3}{p{15cm}}{\emph{Orientation of orientation frame $\orientation{a}$ fixed to body $\body{C}$ ($\fixedTo{\orientation{a}}{C}$) with respect to 
orientation frame $\orientation{b}$ fixed to body $\body{D}$ ($\fixedTo{\orientation{b}}{D}$), expressed in coordinate frame $\orientation{r}$}}
\\
\\
\textbf{Pose}  & $\Pose{e}{a}{C}{f}{b}{D}{}$  & point $\point{e}$ 
& \multirow{3}{*}{
\resizebox{0.12\textwidth}{!}{\includegraphics{relpose_image}}
}
\\
& $\Pose{e}{a}{C}{f}{b}{D}{r}$ & orientation frame $\orientation{a}$ \\
&& body $\body{C}$ \\
&& reference point $\point{f}$ \\
&& reference orientation frame $\orientation{b}$ \\
&& reference body $\body{D}$ \\
&& coordinate frame $\orientation{r}$ \\ 
\\
\multicolumn{3}{p{15cm}}{\emph{Pose of point $\point{e}$ and orientation frame $\orientation{a}$ fixed to body $\body{C}$ ($\fixedTo{(\point{e},\orientation{a})}{C}$) with respect to 
point $\point{f}$ and orientation frame $\orientation{b}$ fixed to body $\body{D}$ ($\fixedTo{(\point{f},\orientation{b})}{D}$), expressed in coordinate frame $\orientation{r}$}}
\\
\\

& $\Pose{g}{g}{C}{h}{h}{D}{}$    & frame $\refframe{g}$ 
& \multirow{6}{*}{
\resizebox{0.12\textwidth}{!}{\includegraphics{relpose2_image}}
}
\\
& $\Pose{g}{g}{C}{h}{h}{D}{r}$ & body $\body{C}$ \\
&& frame $\refframe{h}$ \\
&& reference body $\body{D}$ \\ 
&& coordinate frame $\orientation{r}$ \\
\multicolumn{3}{p{15cm}}{\emph{Pose of frame $\refframe{g}$ fixed to body $\body{C}$ ($\fixedTo{\refframe{g}}{C}$) with respect to 
frame $\refframe{g}$ fixed to body $\body{D}$ ($\fixedTo{\refframe{g}}{D}$), expressed in coordinate frame $\orientation{r}$}}
\\ \\
\\
\textbf{Linear velocity }& $\LinearVelocity{e}{C}{}{D}{}$ & point $\point{e}$ 
& \multirow{6}{*}{
\resizebox{0.12\textwidth}{!}{\includegraphics{reltransvel_image}}
} 
\\
&  $\LinearVelocity{e}{C}{}{D}{r}$ & body $\body{C}$ \\
&& reference body $\body{D}$ \\
&& coordinate frame $\orientation{r}$ \\
\multicolumn{3}{p{15cm}}{\emph{Linear velocity of point $\point{e}$ fixed to body $\body{C}$ ($\fixedTo{\point{e}}{C}$) with respect to body $\body{D}$, expressed in coordinate frame $\orientation{r}$}}
\\ \\ 
\\
\textbf{Angular velocity} & $\AngularVelocity{}{C}{}{D}{}$ & body $\body{C}$
& \multirow{6}{*}{
\resizebox{0.12\textwidth}{!}{\includegraphics{relrotvel_image}}
} 
\\
 &    $\AngularVelocity{}{C}{}{D}{r}$ & reference body $\body{D}$ \\
&& coordinate frame $\orientation{r}$ \\
\multicolumn{3}{p{15cm}}{\emph{Angular velocity of body $\body{C}$ with respect to body $\body{D}$, expressed in coordinate frame $\orientation{r}$}}
\\
\\
\textbf{Twist} & $\Velocity{e}{}{C}{}{}{D}{}$  & point $\point{e}$ 
& \multirow{6}{*}{
\resizebox{0.12\textwidth}{!}{\includegraphics{reltwist_image}}
} 
\\
&    $\Velocity{e}{}{C}{}{}{D}{r}$ & body $\body{C}$ \\
&& reference body $\body{D}$ \\
&& coordinate frame $\orientation{r}$ \\
\multicolumn{3}{p{15cm}}{\emph{Twist of body $\body{C}$ with velocity reference point $\point{e}$ ($\fixedTo{\point{e}}{C}$) with respect to body $\body{D}$, expressed in coordinate frame $\orientation{r}$}}
\\
\\ \hline
   \end{tabular*}
     \end{center}
\caption{Minimal semantics and coordinate semantics (expressed in coordinate frame $\protect\orientation{r}$) 
including the minimal but complete set of geometric primitives for the position, orientation, pose, linear velocity, angular velocity, and twist of body $\body{C}$ with point $\protect\point{e}$, orientation frame $\protect\orientation{a}$, and frame $\protect\refframe{g}$ with respect to $\body{D}$ with point $\protect\point{f}$, orientation frame $\protect\orientation{b}$, and frame $\protect\refframe{h}$, including a graphical representation. (extracted from~\cite{DeLaet-ram2012a})}
\label{tab:semantics}
  \end{table*} 
  
  
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
\subsection{Semantic operations}
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
\label{subsec:geometricSemantics:semanticOperations}
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  
% Besides modelling the geometric relations, De Laet et al.~\cite{DeLaet-ram2012a} also present semantic operations on the geometric relations.
On the geometric relations defined in Section~\ref{subsec:geometricSemantics:geometricRelations}, semantic operations that compose the geometric relations or that change the point, orientation frame, reference point, reference orientation frame, or coordinate frame of the geometric relation can be applied.
These semantic operations themselves impose constraints on the geometric relation they are applied to and on the operation arguments (which are themselves geometric relations) of the operator.
While \cite{DeLaet-ram2012a} provides an overview of semantic operations that can be applied to geometric relations and lists the constraints imposed by the operations, we will only give an example illustrating the concept of the semantic operation and the constraints imposed by it.


As an example, consider the semantic operation to change the point used to describe the position of body $\body{C}$ with respect to body $\body{D}$. 
Imagine $\Position{e_1}{C}{f}{D}{r}$ is the semantic description of the position of body $\body{C}$ with respect to body $\body{D}$. To change the point to describe the position from the current point $\point{e_1}$ to a new point $\point{e_2}$, the position of the new point $\point{e_2}$ fixed to body $\body{C}$ with respect to $\point{e_1}$ fixed to body $\body{C}$ and expressed in the same coordinate frame $\orientation{r}$ is needed, i.e. $\Position{e_2}{C}{e_1}{C}{r}$.
If the semantic operator $\changePointPosition{}{}$ is applied to the geometric relation of which the point has to be changed (in our example $\Position{e_1}{C}{f}{D}{r}$) and has as an argument the geometric relation needed to achieve this change of reference point, the $\changePointPosition{}{}$ imposes the following constraints: (1) the argument of $\changePointPosition{}{}$ should be a PositionCoord geometric relation; (2) the reference point of the argument should be equal to the point of the position the operator is applied on; (3) the body of the argument should be equal to the body of the position the operator is applied on;
(4) the reference body of the argument should be equal to body of the position the operator is applied on; and (5) the coordinate frame of the argument should 
be equal to the point of the position the operator is applied on.
This can be visually illustrated as follows:
\begin{multline}
\Position{e_2}{C}{f}{D}{r} =\\ {\Position{\underline{e_1}}{\underline{\underline{C}}}{f}{D}{\underline{\underline{\underline{r}}}}} .\textrm{changePointPosition}( \\
{\Position{e_2}{\underline{\underline{C}}}{\underline{e_1}}{\underline{\underline{C}}}{\underline{\underline{\underline{r}}}}}),
\end{multline}
the semantic
constraints imposed on the geometric relation the operation is applied to, and on the operation arguments, are shown by using the same names for the geometric primitives when equality of the primitives is imposed, furthermore the lines indicate ones again the geometric primitives that should be equal to obtain a semantically correct operation.


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
\section{Geometric semantics DSL (M2) and the tooling (M1)}
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
\label{sec:geometricSemanticsDSL}
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
\subsection{External DSL}
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
\label{subsec:geometricSemanticsDSL:external}
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
\subsubsection{DSL design}
The external DSL is developed using Xcore. As this DSL is not using the java specific syntax parts of Xcore, it can be considered as a plain text file and therefore as an external DSL.
The geometric semantics DSL uses the Object Contraint language (OCL) DSL to define the constraints in the geometric semantics. Defining these constraints only requires a small set of constraints of OCL, making it feasible (although not necessarily desired) to eliminate the dependency on OCL with limited effort.

% Xcore is a tool designed to make a model for a DSL. The difference between Xcore and
% Xtext is the fact that Xtext starts from a grammar and generates a model from it,
% while Xcore starts from a model. This has the advantage that you can focus yourself
% on the model and the structure of the DSL. The disadvantage is that if you want to
% generate a grammar from this model, the grammar will be autogenerated and probably not
% what you want as an end product.
% 
% Xcore has several keywords that you have to know in order to build a descent model.
%  * 
%  * 'package': The name of the package where all the generated classes will be included.
%  * 
%  * 'class': The ordinary definition of a new class.
%  * 
%  * 'String': A string.
%  * 
%  * 'extends': Extends an other class (eg class Point extends Primitive).
%  * Multiple inheritance is allowed.
%  * 
%  * 'abstract': Makes it impossible to make an object with that type directly.
%  * It is of course possible to make objects that have a subclass as type.
%  * This is mainly used to make a separation between classes and group the
%  * classes that belong together.
%  * 
%  * 'refers': This keyword implies that a reference to another object will follow.
%  * Objects are always referred by name (String). This keyword will always be followed by
%  * an indication of multiplicity between square brackets.
%  * Eg refers OrientationFrame [1] orientationFrame. This means that this class contains
%  * one referation to exact one OrientationFrame. Notice that the [1] notation is the same as
%  * the [1..1] notation. Another special notation is [], which means an undefined amount and
%  * is short for [0..*]. Other notations like [2..4] (min 2 and max 4 objects) are also allowed.
%  * The reference to an OrientationFrame is allowed, since it contains a 'String name'.
%  * 
%  * 'contains': This is similar to 'refers', but may not be confused with it. It doesn't contain
%  * a reference to an object, but contains the definition of the object itself. This keyword
%  * also means that a notion of multiplicity will follow.
%  * This keyword is only used once in this model:
%  * class DomainModel
%  * {
%  *	contains DomainRule [] domainRules
%  * }
%  * This means that the DomainModel (the root of this model) contains an unspecified amount of domainRules.
%  * 
%  * At the beginning of this file, there is an @Ecore reference. This is necessary to be able to use OCL constraints.
%  * There are two ways to refer to the Ecore file. They both use the '@Pivot' annotation:
%  * 
%  * '@Ecore(constraints="...")': This contains the name of the constraints that you want to impose. The actual
%  * definition of the constraints itself are located in the '@Pivot' annotation. This annotation can contain
%  * more than one constraint.
%  * 
%  * 'derived': Derived properties are properties that are reconstructed from other objects in that class.
%  * So, there is indirect access to a specific object. The actual definition of the derivation is again located
%  * in the '@Pivot' annotation.
%  * 
%  * 'oclIsTypeOf': This is an OCL keyword. It returns true if the object is of a specific type.
%  * Eg
%  * class A
%  * class B extends A
%  * B b
%  * b.oclIsTypeOf(B) -> true
%  * b.oclIsTypeOf(A) -> false
%  * 'oclIsKindOf' would make the last one true.
%  * 
%  * 'oclAsType': This is a typecast. This is usefull if you have an object of a superclass and you are sure that
%  * the object is of a type that is a subclass, you can cast to that subclass using this keyword and use the objects
%  * of that subclass.
%  * Eg
%  * class A
%  * class B extends A{
%  * 	String bla
%  * }
%  * class C{
%  * 	A a
%  * }
%  * if a.oclIsTypeOf(B) then a.oclAsType(B).bla ...
%  * 'a.bla' will give a compile error in the generated Ecore file.
%  * 
%  * 
%  * 
% THE MODEL
Next we discuss the design of the DSL in Xcore.
Our DSL consists of a `root' class called `DomainModel'.
This DomainModel class contains DomainRules. A DomainRule consists of `Primitive', `GeometricRelation', `GeometricCoordinateRelation', `SemanticOperation'
and `SemanticCoordinateOperation'.
% These five classes extend from the DomainRule class. Note that the DomainRule class
% is an abstract class and thus cannot be instantiated.
Listing~1 shows the definition of the primitive Point and the geometric (coordinate) relation PositionSemantics and PositionCoordinateSemantics.

\begin{figure*}
 \resizebox{\textwidth}{!}{
 \centering
 \includegraphics{listingXcore1.pdf}
 % listingXcore2.pdf: 483x187 pixel, 72dpi, 17.04x6.60 cm, bb=0 0 483 187
}
\end{figure*}


% \begin{xcoreEnv}[float,caption=Geometric semantics primitive and relation example in Xcore,label=listing:geometricSemanticsDSL:xcore1]
% * Root class
% abstract class DomainRule{}
% * Definition of primitives
% abstract class Primitive extends DomainRule{}
% class Point extends Primitive {String name }
% * Definition of geometric (coordinate) relation Position
% abstract class GeometricRelation extends DomainRule{}
% abstract class GeometricCoordinateRelation extends DomainRule{}
% 
% class PositionSemantics extends GeometricRelation, SuperPosition
% {
% 	String name
% 	refers Point [1] point
% 	refers Body [1] body
% 	refers Point [1] refPoint
% 	refers Body [1] refBody
% }
% 
% class PositionCoordinateSemantics extends GeometricCoordinateRelation, SuperCoordinatePosition
% {
% 	String name
% 	@Pivot(derivation="self.positionSemantics.point")
% 	refers derived Point point
% 	@Pivot(derivation="self.positionSemantics.body")
% 	refers derived Body body
% 	@Pivot(derivation="self.positionSemantics.refPoint")
% 	refers derived Point refPoint
% 	@Pivot(derivation="self.positionSemantics.refBody")
% 	refers derived Body refBody
% 	refers PositionSemantics [1] positionSemantics
% 	refers OrientationFrame [1] coordFrame
% }
% \end{xcoreEnv}

Listing~2 shows the definition of the PositionChangePoint geometric operation and the constraints (defined using OCL) to which this operation has to comply.
% A SemanticRelation is divided in a PositionOperation, OrientationOperation, PoseOperation, \ldots and they contain the definition of the operations with a particular geometric relation (for instance Position) as an outcome.
% A PostionOperation contains all the operations with a Position as outcome.
% Eg:
% class PositionChangePoint extends PositionOperation
% abstract class PositionOperation extends SemanticRelation
% abstract class SemanticRelation extends DomainRule
% abstract class DomainRule

\begin{figure*}
 \resizebox{\textwidth}{!}{
 \centering
 \includegraphics{listingXcore2.pdf}
 % listingXcore2.pdf: 483x187 pixel, 72dpi, 17.04x6.60 cm, bb=0 0 483 187
}
\end{figure*}


% This is the basic idea of the model. 
% \begin{xcoreEnv}[float,caption=Geometric semantics operation example in Xcore,label=listing:geometricSemanticsDSL:xcore2]
% * Definition of PositionChangePoint semantic operation and the constraints
% @Ecore(constraints="SamePoint SameBody") 
% @Pivot(SamePoint="if notNull then self.superPos0.superPoint = self.superPos1.superRefPoint else true endif",
% 	SameBody="if notNull then (self.superPos0.superBody = self.superPos1.superBody and self.superPos0.superBody = self.superPos1.superRefBody) else true endif")
% class PositionChangePoint extends PositionOperation, SuperPosition
% {
% 	@Pivot(derivation="self.superPos1.superPoint")
% 	refers derived Point point
% 	@Pivot(derivation="self.superPos0.superBody")
% 	refers derived Body body
% 	@Pivot(derivation="self.superPos0.superRefPoint")
% 	refers derived Point refPoint
% 	@Pivot(derivation="self.superPos0.superRefBody")
% 	refers derived Body refBody
% 	@Pivot(derivation="self.superPos0 <> null and self.superPos1 <> null")
% 	derived Boolean notNull
% 	String name
% 	refers SuperPosition [1] superPos0
% 	refers SuperPosition [1] superPos1
% }
% \end{xcoreEnv}


% There is only one additional set of abstract superclasses that
% have to be explained. They all start with Super*, like SuperPosition. SuperPosition is an abstract
% class that unites all the PositionClasses. In this case, all the classes that extend from
% PositionOperation and the PositionSemantics class itself. In this way all the operations that
% require a Position in their calculation will use a reference to a SuperPosition class.
% Eg:
% class PositionChangePoint extends PositionOperation, SuperPosition
% {
% 	String name
% 	refers SuperPosition [1] superPos0
% 	refers SuperPosition [1] superPos1
% }
% A SuperPosition object can have any kind of origin. It can be a PositionSemantics object or the result
% of an other operation. The only thing that we have to know is that a SuperPosition has a
% Position as outcome. This way we can use Positions recursively.
% 
% If you take a look at the SuperPosition class, you'll see that it only contains derivations.
% These derivations are necessary, because the class that will use a SuperPosition object doesn't know
% what kind of type this object has.
% What if we left out this kind of derivations? If we did this, then we need to check in the subclasses itself
% what the actual type of the SuperPosition is. This SuperPosition once again, can be all sorts of Positions,
% so his type has also to be checked. In theory, we can make an infinite regression of Positions, using other Positions.
% Which will lead to an infinite amount of combinations and thus is impossible to implement. Even with a limited
% amount of regression, eg 5, we will have n^5 amount of combinations we will have to consider (with n the amount of
% different PossitionTypes).
% 
% Note that there are also derivations in the subclasses. These derivations are not really necessary.
% They can be delegated to the Super* classes where these derivations are actually used.
% The reason why these derivations are located in the subclasses itself is because the responsibility
% of giving the solution to an operation lies in the Operation class itself.
% Ofcourse this is a matter of taste and approach. These derivations can easily be transfered to the Super* classes.
% 
% There is one last thing that has to be explained and that is the use of the 'notNull' derivation.
% If objects are not checked against the fact that they are null or not, the compiler will give an OCL error
% while the user is still typing. With this kind of check, the user will only see the grammatical error that says
% that the statement isn't finished yet.
% 
% The constraints itself are quite straightforward and don't need much explanation.

 
% \paragraph{Work flow and tooling}
\subsubsection{Work flow and tooling}
Thanks to the Xcore support in DSL we can make use of a Model Driven Engineering work flow in Eclipse.
The DSL can be loaded inside Eclispe and converted into an ecore model and subsequently in an Xtext model. The tooling (such as an editor) made available by Xtext can be created to support the M1 level for the geometric semantics.
The Xtext editor hereby allows for semantic checking of the geometric semantics during editing, hereby reducing application development time since errors are detected very early.


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
\subsection{Internal DSL}
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
\label{subsec:geometricSemanticsDSL:internal}
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
\subsubsection{DSL design}
The internal DSL is built on top of Prolog. 
This way Prolog can be used to define the grammar, and in particular the logic constraints of the geometric semantics.
Furthermore it provides a good mechanism to provide bookkeeping of the geometric primitives and relations in a particular application (which points, orientation frames, poses, \ldots are defined and check if they are uniquely defined).
% As this DSL is not using the java specific syntax parts of Xcore, it can be considered as a plain text file and therefore as an external DSL.
% Next we discuss the design of the DSL in Prolog.
Listing~3 shows the definition of the geometric (coordinate) relation Position (defining both PositionSemantics and PositionCoordinateSemantics).


\begin{figure*}
 \resizebox{\textwidth}{!}{
 \centering
 \includegraphics{listingProlog1.pdf}
 % listingXcore2.pdf: 483x187 pixel, 72dpi, 17.04x6.60 cm, bb=0 0 483 187
}
\end{figure*}

% \begin{prologEnv}[float,caption=Geometric semantics primitive and relation example in Prolog,label=listing:geometricSemanticsDSL:prolog1]
% % Definition of geometric (coordinate) relation Position 
%  position(Name,P1,P2) :-
%     inferBody(P1,Point1,Body1),inferBody(P2,Point2,Body2),
%     handleParamsInit([Name,Point1,Body1,Point2,Body2],position,
%                      [position,point,body,point,body]),!.
% position(Name,P1,P2,OrientationFrame) :- position(Name,positionCoordinates,P1,P2,OrientationFrame).
% \end{prologEnv}

Listing~4 shows the definition of the PositionChangePoint geometric operation and the constraints to which this operation has to comply.

\begin{figure*}
 \resizebox{\textwidth}{!}{
 \centering
 \includegraphics{listingProlog2.pdf}
 % listingXcore2.pdf: 483x187 pixel, 72dpi, 17.04x6.60 cm, bb=0 0 483 187
}
\end{figure*}

% \begin{prologEnv}[float,caption=Geometric semantics primitive and relation example in Prolog,label=listing:geometricSemanticsDSL:prolog2]
% changePoint(Result,Changee,Change) :-
%     incrementLC,
%     exists(Changee,Type1),
%     exists(Change),
%     constrainChangePoint(Type1,Result,Changee,Change).
% changePoint(Result,Changee,Change1,Change2) :-
%     incrementLC,
%     exists(Change1),
%     exists(Change2),
%     exists(Changee,Type),
%     constrainChangePoint(Type,Result,Changee,Change1,Change2). 
% 
% % Constraint check for point change
% checkPointChange(Point,Body,Change,NewPoint) :-
%     getPosition(Change,position,NewPoint,NewBody,NewRefPoint,NewRefBody),
%     ((NewBody = Body, NewRefPoint = Point, NewRefBody = Body) -> true ;
%       writeError(['Constraint error on changing point: Position(',NewPoint,'|',NewBody,',',NewRefPoint,'|',NewRefBody,')',
%                   ' when Position(',NewPoint,'|',Body,',',Point,'|',Body,') was expected.'])).
% \end{prologEnv}



\subsubsection{Work flow and tooling}
The work flow is a particular work flow based on the Prolog language. As a tool we developed our own editor that allows the DSL user to use the syntax as proposed in the geometric semantics theory~\cite{DeLaet-ram2012a}, but parses it to Prolog code which is subsequently executed in the background to do the semantic checking. This way, similar functionality as obtained with the Xtext editor is obtained, i.e. semantic checking is done and meaningful error statements are produced during editing.

% Model Driven Engineering work flow in Eclipse.
% 
% Xcore -> Ecore -> Xtext -> editor for M1 level



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
\section{Example}
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
\label{sec:example}
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
This section illustrates how the DSL can be used to prevent common errors in geometric calculations.
To this end we use the following semantic operations:
\begin{multline}
{\Position{\underline{e_1}}{\underline{\underline{C}}}{f}{D}{}} .\textrm{changePointPosition}( \\
{\Position{e_2}{\underline{\underline{C}}}{\underline{e_1}}{\underline{\underline{C}}}{}}).
\end{multline}
In the above statement the lines illustrate the constraints on the semantic operation i.e. (we refer to the PositionCoord to which the operator is applied to as the subject and to the PositionCoord that is used as an argument in the operation as the argument):
\begin{inparaenum}
 \item the point of the subject has to be equal to the reference point of the argument,
 \item the body of the subject has to be equal to the reference body of the argument,
  \item the body of the subject has to be equal to the body of the argument,
\end{inparaenum}
The figures below how the Xtext (Figure~\ref{fig:errorXtextEditor}) and Prolog-based (Figure~\ref{fig:errorPrologEditor}) editor react on a mistake on the first constraint in the above list. As shown in the figures they both provide information on the kind of error.

\begin{figure*}
\centering
	\resizebox{1\textwidth}{!}{\includegraphics{constraintCheckerXtext-cut.png}}
	\caption{Xtext editor example when violating the constraint that the point of the subject has to be equal to the reference point of the argument when applying the geometric operation changePoint.}
	\label{fig:errorXtextEditor}
\end{figure*}

\begin{figure*}
\centering
	\resizebox{1\textwidth}{!}{\includegraphics{constraintCheckerProlog.png}}
	\caption{Prolog-based editor example when violating the constraint that the point of the subject has to be equal to the reference point of the argument when applying the geometric operation changePoint.}
	\label{fig:errorPrologEditor}
\end{figure*}

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
\section{Discussion}
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
\label{sec:discussion}
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

\subsection{Xcore versus Prolog DSL}
In this section we want to highlight some advantages and disadvantages of the Xcore and Prolog DSL for two use cases: the DSL developer and the DSL user.

For the \textbf{DSL user} both the external Xcore DSL and internal Prolog DSL currently provide an editor that offers checking of the constraints defined in the DSL.
However, because the Xcore DSL is easy to integrate into Eclipse, it immediately opens the way to all the tooling available in Eclipse. An example is the nice Xtext editor for the M1 level that can be generated in Eclipse from the Xcore DSL. 
Since, the Xcore DSL is however basically a text model, it is still possible to create any other parser or editor. Therefore, the Xcore DSL does not create a hard dependency on Eclipse or Xtext, while the tools of Eclipse and Xtext can still be used when desired.
The tooling around Prolog is not as developed as around Eclipse. Therefore, we implemented a simple editor for the M1 level ourselves. While the editor only offers basic checking and simple error reporting, it provides all the basic functionality to check the constraints defined in the DSL. 
The Prolog DSL has the extra advantage that uses the Prolog language, which is already executable. Therefore, it is more easy to create executable (Prolog) code from the M1 models defined using the Prolog DSL.

%Moreover, since the editor is simple and based on the Prolog DSL, it is easy to maintain.

% The involved syntax of the OCL constraints used in de Xcore DSL make it however harder to `read'. Therefore, if the readability is an issue, it could be decided to natively implement the constraints in Xcore rather than using the OCL constraints. This is feasible in this case since we only use a small subset of the available OCL constraints.
% 
% The tooling around Prolog is not as developed as around Eclipse. Therefore, we implemented a simple editor for the M1 level ourselves. While the editor only offers basic checking, it provides all the basic functionality to check the constraints defined in the DSL. Moreover, since the editor is simple and based on the Prolog DSL, it is easy to maintain.
% Since the Prolog design relies on the quite intuitive syntax of Prolog it is easy to `read'. The syntax has also proven to be rich enough for our DSL.

A \textbf{DSL developer} has to adapt or extend the external Xcore DSL and/or the internal Prolog DSL. The involved syntax of the OCL constraints make the Xcore DSL harder to `read'. Therefore, if the readability is an issue, it could be decided to natively implement the constraints in Xcore rather than using the OCL constraints. This is feasible in this case since we only use a small subset of the available OCL constraints. 
Since the Prolog syntax is quite intuitive it makes the internal Prolog DSL easier to `read'.

% \subsection{Model to model transformations}
% Currently, we have no support to perform model to model transformations.
% However, since the Prolog-DSL editor and Xcore-DSL editor use the same syntax, the implementation of a model to model transformation at the M1 level should be straightforward.
% % normally at M1
% % Need to be able transform be

\subsection{Code generation: from M1 to M0}
An important limitation so far is that we have no code-generation support, i.e. the automatic transformation from the M1 to the M0 level is lacking.
In the robotics context this is an important limitation, since we need to obtain executable code. Moreover, preferably we want support for different programming languages (C++, python, \dots) and execution on different types of hardware (FPGA, normal PC, \ldots).
Therefore in future work we will also look at tools as Epsilon that allow to generate executable code.

% We believe that generic code-generation support is still lacking in the Model Driven Engineering domain.

\subsection{Future in robotics}
To ensure a future in robotics, not only code generation for the geometric semantics DSL is essential. Moreover, we need better support to write \textbf{entire robotic applications} at the M1 level. In robotics the code typically originates from different domains: geometry, kinematics, dynamics, state machines, estimators, etc. Therefore, it should be possible to write code that interleaves different DSLs.
To this end, different DSL (geometric semantics DSL, component models, kinematic and dynamic algorithms DSL~\cite{FrigerioBuchliCaldwell2011}, state charts~\cite{Klotzbuecher2010-DYROS}, motion specification DSL~\cite{Klotzbuecher2011-IROS}), \ldots) have to be supported \underline{at the same time}.
Tools will have to be developed that are \textbf{composable}, such that it possible to, depending on the application, load the relevant DSLs and to generate code from M1 specifications that are interleaving code of different DSLs.
Finally, the entire robotic application developed at M1 level has to be transformed to executable code (M0 level).

% \begin{itemize}
%  \item program at M1 level, to this end need to combine different DSL  Therefore tools to combine DSL
%  \item need for automatic conversion to M0 level to obtain executable code
% \end{itemize}


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
\section{Conclusion}
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
\label{sec:conclusion}
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
In this paper we presented both an external DSL in Xcore and an internal DSL in Prolog for geometric relations between rigid bodies such as relative position, orientation, pose, linear velocity, angular velocity, and twist founded on the geometric semantics~\cite{DeLaet-ram2012a,DeLaet-rrfc2012}.
These DSLs advance with respect to the available implementation in the general-purpose programming language (C++) by formalizing the underlying model of the geometric semantics.
Furthermore, we showed that these DSLs are the basis tools that assist the robot programmers and application developers. In an example we showed how editors built on top of the DSLs automatically check the semantic correctness of geometric operations on rigid-body coordinate representations while writing and editing the code. Furthermore, it was shown that these editors produce meaningful error statements when semantic constraints are violated.
We listed our experiences from writing the DSL up to using the editors. Finally, we discussed some things that are still lacking to integrate the geometric semantics DSL into the work flow of a robot programmer or application developer.

We believe that this paper has shown that the geometric semantics, due to is mature but concise nature, is an excellent example for the development of DSLs in robotics and the use of these DSLs in the work flow of a robot programmer or application developer.

  
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 \section*{Acknowledgements}
 \small
All authors gratefully acknowledge the financial support by KU Leuven's
Concerted Research Action GOA/2010/011, %\emph{Global real-time optimal control of autonomous robots and mechatronic systems},
% KU Leuven-BOF PFV/10/002 Center-of-Excellence Optimization in Engineering (OPTEC), 
European FP7 project Rosetta (2008-ICT-230902), %, \emph{Robot control for skilled execution of tasks in natural interaction with humans; based on autonomy, cumulative knowledge and learning}),
European FP7 project BRICS (2008-ICT-231940), % \emph{Best practices in robotics}), and
European FP7 project RoboHow (FP7-ICT-288533). % \emph{RoboHow.Cog}).
Tinne De Laet is a Postdoctoral
Fellow of the Fund for Scientific Research--Flanders (F.W.O.) in Belgium.
%TODO: check optec acknowledgement + add quadrotor?
\normalsize
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% line out references
%\IEEEtriggeratref{26}
% bibliography
\input{document.bbl}
%\bibliographystyle{IEEEtran}
%\bibliographystyle{abbrv}
%\bibliography{shortrefs,actsens,shortcross}
%\bibliographystyle{plain}
%\bibliographystyle{theapa}
%\bibliography{shortrefs,actsens,shortcross}


\end{document}

listingsSettings.tex
\lstset{
	inputencoding=utf8x,
	backgroundcolor=\color{lbcolor},
	tabsize=2,
	rulecolor=,
	language=c++,
        basicstyle=\footnotesize \ttfamily,
        upquote=true,
        aboveskip={1.0\baselineskip},
        columns=fixed,
        showstringspaces=false,
        extendedchars=true,
        breaklines=true,
        prebreak = \raisebox{0ex}[0ex][0ex]{\ensuremath{\hookleftarrow}},
        %frame=single,
        showtabs=false,
        showspaces=false,
        showstringspaces=false,
        identifierstyle=\ttfamily,
        keywordstyle=\color[rgb]{0,0,1},
        commentstyle=\color[rgb]{0.133,0.545,0.133},
        stringstyle=\color[rgb]{0.627,0.126,0.941},
}

\lstdefinelanguage{pseudoCode}
{
keywordstyle=\color[rgb]{0.627,0.126,0.941},
morekeywords={Pose,PoseSemantics,PoseCoordinates,PoseCoordinatesSemantics,
Orientation,OrientationSemantics,OrientationCoordinates,OrientationCoordinatesSemantics,
Position,PositionSemantics,PositionCoordinates,PositionCoordinatesSemantics,
Twist,TwistSemantics,TwistCoordinates,TwistCoordinatesSemantics,
AngularVelocity,AngularVelocitySemantics,AngularVelocityCoordinates,AngularVelocityCoordinatesSemantics,
LinearVelocity,LinearVelocitySemantics,LinearVelocityCoordinates,LinearVelocityCoordinatesSemantics,
Force,ForceSemantics,ForceCoordinates,ForceCoordinatesSemantics,
Torque,TorqueSemantics,TorqueCoordinates,TorqueCoordinatesSemantics,
Wrench,WrenchSemantics,WrenchCoordinates,WrenchCoordinatesSemantics},
sensitive=false,
morecomment=[l]{//},
emph={Result},
% emphstyle=\color[rgb]{0.29,0.04,0.23},
emphstyle=\color[rgb]{0.627,0.126,0.941},
emph={[2]Semantic,output},
emphstyle={[2]\color[rgb]{0.74,0.08,0.32}},
emph={[3]NOK},
emphstyle={[3]\color{red}},
emph={[4]OK},
emphstyle={[4]\color[rgb]{0.2,0.6,0.2}},
morestring=[b]``,
keywordstyle=\color[rgb]{0,0.63,0.69},
% emphstyle=\color[rgb]{0.52,0.29,0.24},
% emph={[2]Semantic,output},
% emphstyle={[2]\color[rgb]{0.8,0.2,0.25}},
% emph={[3]NOK},
% emphstyle={[3]\color{red}},
% emph={[4]OK},
% emphstyle={[4]\color{green}},
% morestring=[b]``,
% keywordstyle=\color[rgb]{0,0.63,0.69},
}


\lstdefinelanguage{xcore}
{
keywordstyle=\color[rgb]{0.627,0.126,0.941},
morekeywords={class,extends,String,abstract,refers,@Pivot,derivation,self,derived},
sensitive=false,
morecomment=[l]{*},
emph={Result},
% emphstyle=\color[rgb]{0.29,0.04,0.23},
emphstyle=\color[rgb]{0.627,0.126,0.941},
emph={[2]Semantic,output},
emphstyle={[2]\color[rgb]{0.74,0.08,0.32}},
emph={[3]NOK},
emphstyle={[3]\color{red}},
emph={[4]OK},
emphstyle={[4]\color[rgb]{0.2,0.6,0.2}},
morestring=[b]``,
keywordstyle=\color[rgb]{0,0.63,0.69},
% emphstyle=\color[rgb]{0.52,0.29,0.24},
% emph={[2]Semantic,output},
% emphstyle={[2]\color[rgb]{0.8,0.2,0.25}},
% emph={[3]NOK},
% emphstyle={[3]\color{red}},
% emph={[4]OK},
% emphstyle={[4]\color{green}},
% morestring=[b]``,
% keywordstyle=\color[rgb]{0,0.63,0.69},
}

\lstnewenvironment{xcoreEnv}[1][]
  {\lstset{
  	language=xcore,
  	label=#1,
%   	caption=#1,
	inputencoding=utf8x,
	backgroundcolor=\color{lbcolor},
	tabsize=2,
	rulecolor=,
        basicstyle=\footnotesize \ttfamily,
        upquote=true,
        aboveskip={1.0\baselineskip},
        columns=fixed,
        showstringspaces=false,
        extendedchars=true,
        breaklines=true,
        prebreak = \raisebox{0ex}[0ex][0ex]{\ensuremath{\hookleftarrow}},
        %frame=single,
        showtabs=false,
        showspaces=false,
        showstringspaces=false,
        identifierstyle=\ttfamily,
        keywordstyle=\color[rgb]{0,0,1},
        commentstyle=\color[rgb]{0.133,0.545,0.133},
        stringstyle=\color[rgb]{0,0,0},
         keywordstyle=\color[rgb]{0.627,0.126,0.941}
,
% morekeywords={Pose,PoseSemantics,PoseCoordinates,PoseCoordinatesSemantics,
% Orientation,OrientationSemantics,OrientationCoordinates,OrientationCoordinatesSemantics,
% Position,PositionSemantics,PositionCoordinates,PositionCoordinatesSemantics,
% Twist,TwistSemantics,TwistCoordinates,TwistCoordinatesSemantics,
% AngularVelocity,AngularVelocitySemantics,AngularVelocityCoordinates,AngularVelocityCoordinatesSemantics,
% LinearVelocity,LinearVelocitySemantics,LinearVelocityCoordinates,LinearVelocityCoordinatesSemantics,
% Force,ForceSemantics,ForceCoordinates,ForceCoordinatesSemantics,
% Torque,TorqueSemantics,TorqueCoordinates,TorqueCoordinatesSemantics,
% Wrench,WrenchSemantics,WrenchCoordinates,WrenchCoordinatesSemantics,
% PoseGraph,TwistGraph,
% GetPose,GetTwist,
% AddPose,AddTwist
% % OrientationCoordinates<KDL::Rotation>,
% % Orientation<KDL::Rotation>
% },
}
}
{}


\lstnewenvironment{prologEnv}[1][]
  {\lstset{
  	language=Prolog,
  	label=#1,
%   	caption=#1,
	inputencoding=utf8x,
	backgroundcolor=\color{lbcolor},
	tabsize=2,
	rulecolor=,
        basicstyle=\footnotesize \ttfamily,
        upquote=true,
        aboveskip={1.0\baselineskip},
        columns=fixed,
        showstringspaces=false,
        extendedchars=true,
        breaklines=true,
        prebreak = \raisebox{0ex}[0ex][0ex]{\ensuremath{\hookleftarrow}},
        %frame=single,
        showtabs=false,
        showspaces=false,
        showstringspaces=false,
        identifierstyle=\ttfamily,
        keywordstyle=\color[rgb]{0,0,1},
        commentstyle=\color[rgb]{0.133,0.545,0.133},
        stringstyle=\color[rgb]{0,0,0},
         keywordstyle=\color[rgb]{0.627,0.126,0.941}
,
% morekeywords={Pose,PoseSemantics,PoseCoordinates,PoseCoordinatesSemantics,
% Orientation,OrientationSemantics,OrientationCoordinates,OrientationCoordinatesSemantics,
% Position,PositionSemantics,PositionCoordinates,PositionCoordinatesSemantics,
% Twist,TwistSemantics,TwistCoordinates,TwistCoordinatesSemantics,
% AngularVelocity,AngularVelocitySemantics,AngularVelocityCoordinates,AngularVelocityCoordinatesSemantics,
% LinearVelocity,LinearVelocitySemantics,LinearVelocityCoordinates,LinearVelocityCoordinatesSemantics,
% Force,ForceSemantics,ForceCoordinates,ForceCoordinatesSemantics,
% Torque,TorqueSemantics,TorqueCoordinates,TorqueCoordinatesSemantics,
% Wrench,WrenchSemantics,WrenchCoordinates,WrenchCoordinatesSemantics,
% PoseGraph,TwistGraph,
% GetPose,GetTwist,
% AddPose,AddTwist
% % OrientationCoordinates<KDL::Rotation>,
% % Orientation<KDL::Rotation>
% },
}
}
{}


\lstnewenvironment{cPlusPlusEnv}[1][]
  {\lstset{
  	language=c++,
  	label=#1,
%   	caption=#1,
	inputencoding=utf8x,
	backgroundcolor=\color{lbcolor},
	tabsize=2,
	rulecolor=,
        basicstyle=\footnotesize \ttfamily,
        upquote=true,
        aboveskip={1.0\baselineskip},
        columns=fixed,
        showstringspaces=false,
        extendedchars=true,
        breaklines=true,
        prebreak = \raisebox{0ex}[0ex][0ex]{\ensuremath{\hookleftarrow}},
        %frame=single,
        showtabs=false,
        showspaces=false,
        showstringspaces=false,
        identifierstyle=\ttfamily,
        keywordstyle=\color[rgb]{0,0,1},
        commentstyle=\color[rgb]{0.133,0.545,0.133},
        stringstyle=\color[rgb]{0,0,0},
        keywordstyle=\color[rgb]{0.627,0.126,0.941},
morekeywords={Pose,PoseSemantics,PoseCoordinates,PoseCoordinatesSemantics,
Orientation,OrientationSemantics,OrientationCoordinates,OrientationCoordinatesSemantics,
Position,PositionSemantics,PositionCoordinates,PositionCoordinatesSemantics,
Twist,TwistSemantics,TwistCoordinates,TwistCoordinatesSemantics,
AngularVelocity,AngularVelocitySemantics,AngularVelocityCoordinates,AngularVelocityCoordinatesSemantics,
LinearVelocity,LinearVelocitySemantics,LinearVelocityCoordinates,LinearVelocityCoordinatesSemantics,
Force,ForceSemantics,ForceCoordinates,ForceCoordinatesSemantics,
Torque,TorqueSemantics,TorqueCoordinates,TorqueCoordinatesSemantics,
Wrench,WrenchSemantics,WrenchCoordinates,WrenchCoordinatesSemantics,
PoseGraph,TwistGraph,
GetPose,GetTwist,
AddPose,AddTwist
% OrientationCoordinates<KDL::Rotation>,
% Orientation<KDL::Rotation>
},
}
}
{}

\lstnewenvironment{cPlusPlusEnvOneColumn}[1][]
  {\lstset{
  	language=c++,
  	label=#1,
%   	caption=#1,
	inputencoding=utf8x,
	backgroundcolor=\color{lbcolor},
	tabsize=2,
	rulecolor=,
        basicstyle=\footnotesize \ttfamily,
        upquote=true,
        aboveskip={1.0\baselineskip},
        columns=fixed,
        showstringspaces=false,
        extendedchars=true,
        breaklines=true,
        prebreak = \raisebox{0ex}[0ex][0ex]{\ensuremath{\hookleftarrow}},
        %frame=single,
        showtabs=false,
        showspaces=false,
        showstringspaces=false,
        identifierstyle=\ttfamily,
        keywordstyle=\color[rgb]{0,0,1},
        commentstyle=\color[rgb]{0.133,0.545,0.133},
        stringstyle=\color[rgb]{0,0,0},
        keywordstyle=\color[rgb]{0.627,0.126,0.941},
morekeywords={Pose,PoseSemantics,PoseCoordinates,PoseCoordinatesSemantics,
Orientation,OrientationSemantics,OrientationCoordinates,OrientationCoordinatesSemantics,
Position,PositionSemantics,PositionCoordinates,PositionCoordinatesSemantics,
Twist,TwistSemantics,TwistCoordinates,TwistCoordinatesSemantics,
AngularVelocity,AngularVelocitySemantics,AngularVelocityCoordinates,AngularVelocityCoordinatesSemantics,
LinearVelocity,LinearVelocitySemantics,LinearVelocityCoordinates,LinearVelocityCoordinatesSemantics,
Force,ForceSemantics,ForceCoordinates,ForceCoordinatesSemantics,
Torque,TorqueSemantics,TorqueCoordinates,TorqueCoordinatesSemantics,
Wrench,WrenchSemantics,WrenchCoordinates,WrenchCoordinatesSemantics,
PoseGraph,TwistGraph,
GetPose,GetTwist,
AddPose,AddTwist
},
}
}
{}

\lstnewenvironment{consoleOutputEnv}[1][]
  {\lstset{
  	language=consoleOutput,
  	label=#1,
%   	caption=#1,
	inputencoding=utf8x,
	backgroundcolor=\color{lbcolor},
	tabsize=2,
	rulecolor=,
        basicstyle=\footnotesize \ttfamily,
        upquote=true,
        aboveskip={1.0\baselineskip},
        columns=fixed,
        showstringspaces=false,
        extendedchars=true,
        breaklines=true,
        prebreak = \raisebox{0ex}[0ex][0ex]{\ensuremath{\hookleftarrow}},
        %frame=single,
        showtabs=false,
        showspaces=false,
        showstringspaces=false,
        identifierstyle=\ttfamily,
        commentstyle=\color[rgb]{0.133,0.545,0.133},
        stringstyle=\color[rgb]{0.627,0.126,0.941},
}
}
{}


\lstnewenvironment{consoleOutputEnvOneColumn}[1][]
  {\lstset{
  	language=consoleOutput,
  	label=#1,
%   	caption=#1,
	inputencoding=utf8x,
	backgroundcolor=\color{lbcolor},
	tabsize=2,
	rulecolor=,
        basicstyle=\footnotesize \ttfamily,
        upquote=true,
        aboveskip={1.0\baselineskip},
        columns=fixed,
        showstringspaces=false,
        extendedchars=true,
        breaklines=true,
        prebreak = \raisebox{0ex}[0ex][0ex]{\ensuremath{\hookleftarrow}},
        %frame=single,
        showtabs=false,
        showspaces=false,
        showstringspaces=false,
        identifierstyle=\ttfamily,
        commentstyle=\color[rgb]{0.133,0.545,0.133},
        stringstyle=\color[rgb]{0.627,0.126,0.941},
}
}
{}
\lstloadlanguages{C++}

\usepackage{caption}
\usepackage{courier}
\usepackage{etoolbox}

\DeclareCaptionFont{white}{\color{white}}
\DeclareCaptionFormat{listing}{\colorbox[cmyk]{0.43, 0.35, 0.35,0.01}{\parbox{\columnwidth}{\hspace{0pt}#1#2#3}}}

\captionsetup[lstlisting]{format=listing,labelfont=white,textfont=white, singlelinecheck=false, margin=0pt, font={bf,footnotesize}}
\captionsetup[consoleOutputEnv]{format=listing,labelfont=white,textfont=white, singlelinecheck=false, margin=0pt, font={bf,footnotesize}}
\captionsetup[consoleOutputEnvOneColumn]{format=listing,labelfont=white,textfont=white, singlelinecheck=false, margin=0pt, font={bf,footnotesize}}
\captionsetup[cPlusPlusEnv]{format=listing,labelfont=white,textfont=white, singlelinecheck=false, margin=0pt, font={bf,footnotesize}}
\captionsetup[cPlusPlusEnvOneColumn]{format=listing,labelfont=white,textfont=white, singlelinecheck=false, margin=0pt, font={bf,footnotesize}}


macroCoordinateRepresentations.tex
% \input{macroSemantics}

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% POSITION
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% position vector: from point #1 belonging to body #2 to point #3 belonging to body #4, expressed in orientation frame #5
\newcommand{\posvec}[5]
 {\vphantom{p}_{\orientation{#5}}
 {\hspace*{-0.1ex}\boldsymbol{p}^{\point{#1} \ifthenelse{\equal{#2}{}}{}{|\body{#2}}, \point{#3} \ifthenelse{\equal{#4}{}}{}{|\body{#4}}}}\vphantom{p}
 }
 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% ORIENTATION
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
%  \newcommand{\rot}[4]%
%   {\vphantom{R}_{\orientation{#3}\ifthenelse{\equal{#4}{}}{}{
%   \ifthenelse{\equal{#3}{}}{}{|}
%   \body{#4}}}^{\hspace*{0.1ex}\orientation{#1}\ifthenelse{\equal{#2}{}}{}{
%   \ifthenelse{\equal{#1}{}}{}{|}\body{#2}}%
%  }{ \hspace*{-0.2ex} \boldsymbol{R} }\vphantom{R}
%  } 

 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Euler axis and angle: from orientation frame #1 belonging to body #2 to orientation frame #3 belonging to body #4, expressed in orientation frame #5
\newcommand{\eulerAxisAngle}[5]
 {\vphantom{e}_{\orientation{#5}}
 {\hspace*{-0.1ex}\boldsymbol{e}^{\orientation{#1}\ifthenelse{\equal{#2}{}}{}{|\body{#2}} ,\orientation{#3}\ifthenelse{\equal{#4}{}}{}{|\body{#4}}}}\vphantom{e}
 }
 
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Rotation vector: from orientation frame #1 belonging to body #2 to orientation frame #3 belonging to body #4, expressed in orientation frame #5
\newcommand{\rotationVector}[5]
 {\vphantom{r}_{\orientation{#5}}
 {\hspace*{-0.1ex}\boldsymbol{r}^{\orientation{#1}\ifthenelse{\equal{#2}{}}{}{|\body{#2}} ,\orientation{#3}\ifthenelse{\equal{#4}{}}{}{|\body{#4}}}}\vphantom{r}
 }
 
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  % symbol for euler angle rotation of orientation frame #1 on body #2 with respect to orientation frame #3 on body #4, with rotation around the axes #5 with angles #6
 \newcommand{\eulerAngleRot}[6]
 {
 \rot{#1}{#2}{#3}{#4}\left(\axis{#5} \ifthenelse{\equal{#6}{}}{}{}{,{#6}} \right)
 }
 
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  % symbol for RPY-angle rotation of orientation frame #1 on body #2 with respect to orientation frame #3 on body #4, with angles #5
 \newcommand{\RPYAngleRot}[5]
 {
 \rot{#1}{#2}{#3}{#4}\left(\axis{R}\axis{P}\axis{Y} \ifthenelse{\equal{#5}{}}{}{}{,{#5}} \right)
 }
 
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%s
  %symbol for quaternion from orientation frame #1 belonging to body #2 to orientation frame #3 belonging to body #4, expressed in orientation frame #5
 \newcommand{\quaternion}[5]
  {\vphantom{q}_{\orientation{#5}}
 {\hspace*{-0.1ex}\boldsymbol{q}^{\orientation{#1}\ifthenelse{\equal{#2}{}}{}{|\body{#2}} ,\orientation{#3}\ifthenelse{\equal{#4}{}}{}{|\body{#4}}}}\vphantom{q}
 }
 
  
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% POSE
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
% homogeneous transformation matrix, from frame {#1} fixed to body #2 to 
% frame {#2} fixed to body #4:
\newcommand{\htrans}[4]%
 {\vphantom{T}_{\refframe{#3}\ifthenelse{\equal{#4}{}}{}{|\body{#4}}}^{\hspace*{0.1ex}\refframe{#1}\ifthenelse{\equal{#2}{}}{}{|\body{#2}}}%
 {\hspace*{-0.1ex}\boldsymbol{T}}\vphantom{T}}
 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%s
% finite displacement twist: pose of frame #1 fixed to body #2 wrt frame #3 fixed to body #4 
\newcommand{\finiteDisplacementTwist}[4]
{\vphantom{t}_{\refframe{#3}\ifthenelse{\equal{#4}{}}{}{|\body{#4}}}
^{\hspace*{0.1ex}\refframe{#1}\ifthenelse{\equal{#2}{}}{}{|\body{#2}}}
\textbf{\textsf{t}}_d 
\vphantom{\textbf{\textsf{t}}}
}

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%s
% screw axis: pose of frame #1 fixed to body #2 wrt frame #3 fixed to body #4 expressed in coordinate frame #5
\newcommand{\screwAxis}[5]
{\vphantom{SA}_{\orientation{#5}}
^{\hspace*{0.1ex}\refframe{#1}\ifthenelse{\equal{#2}{}}{}{|\body{#2}}}
\textbf{\textsf{SA}}_{\refframe{#3}\ifthenelse{\equal{#4}{}}{}{|\body{#4}}}
\vphantom{\textbf{\textsf{SA}}}
}

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% TRANSLATIONAL VELOCITY
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
 % translational velocity vector = derivative of position vector: from point #1 belonging to body #2 to point #3 belonging to body #4, expressed in orientation frame #5
\newcommand{\dposvec}[5]
 {
 \vphantom{p}_{\orientation{#5}}
 \hspace*{-0.1ex}\boldsymbol{\dot{p}}^{
 \ifthenelse{\equal{#1}{}}{
 {\ifthenelse{\equal{#2}{}}{}{\body{#2}} ,\point{#3}\ifthenelse{\equal{#4}{}}{}{|\body{#4}}}
 }
 {\forall\point{#1}\ifthenelse{\equal{#2}{}}{}{|\body{#2}} ,\point{#3}\ifthenelse{\equal{#4}{}}{}{|\body{#4}}}
 }
 \vphantom{p}
 }
 
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% ROTATIONAL VELOCITY
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
% time derivative of rotation matrix:
\newcommand{\dotR}[4]%
  {\vphantom{R}_{\orientation{#3}\ifthenelse{\equal{#4}{}}{}{
  \ifthenelse{\equal{#3}{}}{}{|}
  \body{#4}}}^{\hspace*{0.1ex}\orientation{#1}\ifthenelse{\equal{#2}{}}{}{
  \ifthenelse{\equal{#1}{}}{}{|}\body{#2}}%
 }\dot{ \hspace*{-0.2ex} \boldsymbol{R} }\vphantom{R}
 } 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%s
% the rotational velocity vector of orientation frame #1 with respect to orientation frame #2, expressed in orientation frame #3
 \newcommand{\rotvec}[5]
 {\vphantom{ \boldsymbol{\omega} }^{
 \ifthenelse{\equal{#1}{}}{}{
\ifthenelse{\equal{#2}{}}{}{ \forall}}
 \orientation{#1}
 \ifthenelse{\equal{#1}{}}{}{
\ifthenelse{\equal{#2}{}}{}{ |}}
 \ifthenelse{\equal{#2}{}}{}{\body{#2}}}_{\orientation{#5}} \boldsymbol{\omega}_{
 \ifthenelse{\equal{#3}{}}{}{
 \ifthenelse{\equal{#4}{}}{}{\forall}}
  \orientation{#3}
  \ifthenelse{\equal{#3}{}}{}{
\ifthenelse{\equal{#4}{}}{}{ |}}
  \ifthenelse{\equal{#4}{}}{}{\body{#4}}}  \vphantom{ \boldsymbol{\omega} } }
  
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Euler angle rates of orientation frame #1 on body #2 with respect to orientation frame #3 on body #4, with rotation around the axes #5 with angles #6
\newcommand{\eulerAngleRates}[6]
 {
 \dotR{#1}{#2}{#3}{#4}\left(\axis{#5} \ifthenelse{\equal{#6}{}}{}{}{,{#6}} \right)
 }

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% RPY angle rates of orientation frame #1 on body #2 with respect to orientation frame #3 on body #4, with rotation around the axes #5 with angles #6
\newcommand{\RPYAngleRates}[5]
 {
 \dotR{#1}{#2}{#3}{#4}\left(\axis{RPY} \ifthenelse{\equal{#5}{}}{}{}{,{#5}} \right)
 }

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%s
% quaternion rates from orientation frame #1 belonging to body #2 to orientation frame #3 belonging to body #4, expressed in orientation frame #5
\newcommand{\quaternionRates}[5]
  {\vphantom{q}_{\orientation{#5}}
 {\hspace*{-0.1ex}\boldsymbol{\dot{q}}^{\orientation{#1}\ifthenelse{\equal{#2}{}}{}{\ifthenelse{\equal{#1}{}}{}{|}\body{#2}} ,\orientation{#3}\ifthenelse{\equal{#4}{}}{}{\ifthenelse{\equal{#3}{}}{}{|}\body{#4}}}}\vphantom{q}
 }
 
  

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% TWIST
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

%twist: velocity of point #1 belonging to body #2 with respect to body #3 expressed in orientation frame #4 (leave empty if not needed)
\newcommand{\twist}[4]
{
\vphantom{\textbf{\textsf{t}}}
^{ \point{#1}\hspace{0.1ex}|\hspace{0.1ex} \body{#2} }
_{\ifthenelse{\equal{#4}{}}{}{\orientation{#4}}}
\textbf{\textsf{t}}
_{\body{#3}}
\vphantom{\textbf{\textsf{t}}}
}

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%pose twist: pose twist of orientation frame #1 and point #1 belonging to body #2 wrt orientation frame #3 and point #3 belonging to body #3 
\newcommand{\posetwist}[4]
{
\vphantom{\textbf{\textsf{pt}}}
^{ \point{#1}\hspace{0.1ex}|\hspace{0.1ex} \body{#2} }
_{\ifthenelse{\equal{#4}{}}{}{\orientation{#4}}}
\textbf{\textsf{pt}}
_{\body{#3}}
\vphantom{\textbf{\textsf{pt}}}
}

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%screw twist: screw twist of orientation frame and point #2 belonging to body #1 wrt to body #3
\newcommand{\screwtwist}[3]
{
\vphantom{\textbf{\textsf{st}}}
^{ \point{#1}\hspace{0.1ex}|\hspace{0.1ex} \body{#2} }
_{\ifthenelse{\equal{#1}{}}{}{\orientation{#1}}}
\textbf{\textsf{st}}
_{\body{#3}}
\vphantom{\textbf{\textsf{st}}}
}

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%body-fixed twist: body fixed  twist of a orientation frame #1 and origin of #1 belonging to body #2 wrt body #3
\newcommand{\bodyfixedtwist}[3]
{
\vphantom{\textbf{\textsf{bt}}}
^{ \point{#1}\hspace{0.1ex}|\hspace{0.1ex} \body{#2} }
_{\ifthenelse{\equal{#1}{}}{}{\orientation{#1}}}
\textbf{\textsf{bt}}
_{\body{#3}}
\vphantom{\textbf{\textsf{bt}}}
}

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% homogeneous transformation matrix derivative, from point {#1} fixed to body #2 to 
% wrt body #4 expressed in orientation frame #3:
\newcommand{\dothtrans}[4]%
 {\vphantom{T}_{\orientation{#3}\ifthenelse{\equal{#4}{}}{}{|\body{#4}}}^{\hspace*{0.1ex}\point{#1}\ifthenelse{\equal{#2}{}}{}{|\body{#2}}}%
 {\hspace*{-0.1ex}\dot{\boldsymbol{T}}}\vphantom{T}}

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%s
% instantaneous screw axis: velocity of body #1 wrt body #2 expressed in coordinate frame #3
\newcommand{\instantaneousScrewAxis}[3]
{\vphantom{ISA}_{\orientation{#3}}
^{\hspace*{0.1ex}\ifthenelse{\equal{#1}{}}{}{\body{#1}}}
\textbf{\textsf{ISA}}_{\body{#2}}
\vphantom{\textbf{\textsf{ISA}}}
}
macroGeometricPrimitives.tex

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%GEOMETRIC OBJECTS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% frame
\newcommand{\refframe}[1]
{ \ifthenelse{\equal{#1}{}}{}{ \left\{\MakeLowercase{#1}\right\} \hspace{-0.2ex} } }

% orientation
\newcommand{\orientation}[1]
{ \ifthenelse{\equal{#1}{}}{}{ \left[\MakeLowercase{#1}\right] \hspace{-0.1ex}} }

% point
\newcommand{\point}[1]
{\MakeLowercase{#1}}

% body
\newcommand{\body}[1]
% {{\MakeUppercase{#1}}\mathnormal{}}
%TODO: find alternative for mathcal that can put multiple letters
{\mathcal{\MakeUppercase{#1}}\mathnormal{}}

% axis 
\newcommand{\axis}[1]
{\MakeUppercase{#1}\mathnormal{}}

% geometric primitive #1 is fixed to body #2
\newcommand{\fixedTo}[2]
{
#1
\ifthenelse{\equal{#2}{}}{}{
\hspace{-0.2ex}|\hspace{-0.2ex}
}
\body{#2}
}

macroOperations.tex


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% change ref point from point #1 on body#3 to point #2 on body#3, expressed in frame #4  (for the body of which position is expressed)
\newcommand{\changeRefPointBody}[4]
 {\vphantom{\boldsymbol{M}}_{
 \ifthenelse{\equal{#4}{}}{}{ \orientation{#4}} }
 \hspace*{-0.3ex} \boldsymbol{M}_{\textrm{body}}^{\point{#1}
\ifthenelse{\equal{#3}{}}{}{\hspace{0.1ex}|\hspace{0.3ex} \body{#3} } 
, \point{#2} 
\ifthenelse{\equal{#3}{}}{}{\hspace{0.1ex}|\hspace{0.3ex} \body{#3} } }
\vphantom{\boldsymbol{M}}}

% change ref point from point #1 on body#3 to point #2 on body#3, expressed in frame #4 (for reference body)
\newcommand{\changeRefPointRefBody}[4]
 {\vphantom{\boldsymbol{M}}_{
 \ifthenelse{\equal{#4}{}}{}{ \orientation{#4}} }
 \hspace*{-0.3ex} \boldsymbol{M}_{\textrm{ref}}^{\point{#1}
\ifthenelse{\equal{#3}{}}{}{\hspace{0.1ex}|\hspace{0.3ex} \body{#3} } 
, \point{#2} 
\ifthenelse{\equal{#3}{}}{}{\hspace{0.1ex}|\hspace{0.3ex} \body{#3} } }
\vphantom{\boldsymbol{M}}}


% change ref frame from orientation frame #1 to orientation frame #2 
\newcommand{\changeRefFrame}[2]
 {\vphantom{\boldsymbol{P}}_{\orientation{#2}}^{\orientation{#1}}
  \hspace*{-0.3ex} \boldsymbol{P} \vphantom{\boldsymbol{N}} \hspace{0.3ex}}

% change orientation frame from orientation frame #1 on body#3 to orientation frame #2 on body#3, expressed in frame #4 (for the body of which orientation is expressed)
\newcommand{\changeOrientationFrameBody}[4]
 {\vphantom{\boldsymbol{N}}_{
 \ifthenelse{\equal{#4}{}}{}{ \orientation{#4}} }
 \hspace*{-0.3ex} \boldsymbol{N}_{\textrm{body}}^{\orientation{#1}
\ifthenelse{\equal{#3}{}}{}{\hspace{0.1ex}|\hspace{0.3ex} \body{#3} } 
, \orientation{#2} 
\ifthenelse{\equal{#3}{}}{}{\hspace{0.1ex}|\hspace{0.3ex} \body{#3} } }
\vphantom{\boldsymbol{N}}}

% change frame from frame #1 on body#3 to  frame #2 on body#3, expressed in frame #4 (for the body of which orientation is expressed)
\newcommand{\changeFrameBody}[4]
 {\vphantom{\boldsymbol{S}}_{
 \ifthenelse{\equal{#4}{}}{}{ \orientation{#4}} }
 \hspace*{-0.3ex} \boldsymbol{S}_{\textrm{body}}^{\refframe{#1}
\ifthenelse{\equal{#3}{}}{}{\hspace{0.1ex}|\hspace{0.3ex} \body{#3} } 
, \refframe{#2} 
\ifthenelse{\equal{#3}{}}{}{\hspace{0.1ex}|\hspace{0.3ex} \body{#3} } }
\vphantom{\boldsymbol{S}}}

% change frame from frame #1 on body#3 to  frame #2 on body#3, expressed in frame #4 (for the reference body)
\newcommand{\changeFrameRefBody}[4]
 {\vphantom{\boldsymbol{S}}_{
 \ifthenelse{\equal{#4}{}}{}{ \orientation{#4}} }
 \hspace*{-0.3ex} \boldsymbol{S}_{\textrm{ref}}^{\refframe{#1}
\ifthenelse{\equal{#3}{}}{}{\hspace{0.1ex}|\hspace{0.3ex} \body{#3} } 
, \refframe{#2} 
\ifthenelse{\equal{#3}{}}{}{\hspace{0.1ex}|\hspace{0.3ex} \body{#3} } }
\vphantom{\boldsymbol{S}}}


% change orientation frame from orientation frame #1 on body#3 to orientation frame #2 on body#3, expressed in frame #4 (for reference body)
\newcommand{\changeOrientationFrameRefBody}[4]
 {\vphantom{\boldsymbol{N}}_{
 \ifthenelse{\equal{#4}{}}{}{ \orientation{#4}} }
 \hspace*{-0.3ex} \boldsymbol{N}_{\textrm{ref}}^{\orientation{#1}
\ifthenelse{\equal{#3}{}}{}{\hspace{0.1ex}|\hspace{0.3ex} \body{#3} } 
, \orientation{#2} 
\ifthenelse{\equal{#3}{}}{}{\hspace{0.1ex}|\hspace{0.3ex} \body{#3} } }
\vphantom{\boldsymbol{N}}}

% change ref point and reference frame together from #1 to #2 of reference body
\newcommand{\changeRefPointOrientationFrameRefBody}[3]
 {\vphantom{\boldsymbol{S}}_{\refframe{#2}
 \ifthenelse{\equal{#3}{}}{}{\hspace{0.1ex}|\hspace{0.3ex} \body{#3} }
 }^{\refframe{#1}
 \ifthenelse{\equal{#3}{}}{}{\hspace{0.1ex}|\hspace{0.3ex} \body{#3} }
 }
  \hspace*{-0.3ex} \boldsymbol{S}_{\textrm{ref}} \vphantom{\boldsymbol{S}}
  \hspace{0.7ex}
  }
  
  % change ref point and reference frame together from #1 to #2 of body
\newcommand{\changeRefPointOrientationFrameBody}[3]
 {\vphantom{\boldsymbol{S}}_{\refframe{#2}
 \ifthenelse{\equal{#3}{}}{}{\hspace{0.1ex}|\hspace{0.3ex} \body{#3} }
 }^{\refframe{#1}
 \ifthenelse{\equal{#3}{}}{}{\hspace{0.1ex}|\hspace{0.3ex} \body{#3} }
 }
  \hspace*{-0.3ex} \boldsymbol{S}_{\textrm{body}} \vphantom{\boldsymbol{S}}
  \hspace{0.7ex}
  }
  
% change ref point and reference frame + frame in which expressed together from #1 to #2 fixed to body #3
\newcommand{\changeRefFrameRefBody}[3]
 {\vphantom{\boldsymbol{S}}_{\refframe{#2}
 \ifthenelse{\equal{#3}{}}{}{\hspace{0.1ex}|\hspace{0.3ex} \body{#3} } 
 }^{
 \refframe{#1}
 \ifthenelse{\equal{#3}{}}{}{\hspace{0.1ex}|\hspace{0.3ex} \body{#3} } 
 }
  \hspace*{-0.3ex} \boldsymbol{S} \vphantom{\boldsymbol{S}}
  \hspace{0.7ex}
  }  

% change ref point and orientation frame in which projected
\newcommand{\changeRefPointBodyRefFrame}[3]
 {\vphantom{\boldsymbol{S}}_{\refframe{#2}
 \ifthenelse{\equal{#3}{}}{}{\hspace{0.1ex}|\hspace{0.3ex} \body{#3} }
 }^{
 \refframe{#1}
 \ifthenelse{\equal{#3}{}}{}{\hspace{0.1ex}|\hspace{0.3ex} \body{#3} } 
 }
  \hspace*{-0.3ex} \boldsymbol{S} \vphantom{\boldsymbol{S}}
  \hspace{0.7ex}
  }  

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
\newcommand{\composePosition}[2]
{#1 + #2 }

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
\newcommand{\composeOrientation}[2]
{#1 + #2 }

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
\newcommand{\composePose}[2]
{#1 + #2 }


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
\newcommand{\composeTranslationVelocity}[2]
{#1 + #2 }

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
\newcommand{\composeRotationVelocity}[2]
{#1 + #2 }

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
\newcommand{\composeVelocity}[2]
{#1 + #2 }
macroSemanticOperations.tex

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% OPERATIONS ON RIGID BODY GEOMETRIC RELATION SEMANTICS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  changing the coordinate frame of #1 using #2
\newcommand{\changeCoordinateFrame}[2]
{#1.\textrm{changeCoordFrame}\left(#2\right)
}

%  changing the point of #1 using #2
\newcommand{\changePoint}[2]
{#1.\textrm{changePoint}\left(#2\right)
}

%  changing the reference point of #1 using #2
\newcommand{\changeRefPoint}[2]
{#1.\textrm{changeRefPoint}\left(#2\right)
}

%  changing the orientation frame of #1 using #2
\newcommand{\changeOrientationFrame}[2]
{#1.\textrm{changeOrientFrame}\left(#2\right)
}

%  changing the reference orientation frame of #1 using #2
\newcommand{\changeRefOrientationFrame}[2]
{#1.\textrm{changeRefOrientFrame}\left(#2\right)
}

% inverse
\newcommand{\inverse}[1]
{\textrm{inverse}(#1)
}

% inverse2
\newcommand{\inverseB}[1]
{\textrm{inverse2}(#1)
}

% integrate
\newcommand{\integrate}[2]
{\textrm{integrate}\left(#1,#2\right)
}

%  compose #1 and#2
\newcommand{\compose}[2]
{\textrm{compose}\left(#1,#2\right)
}

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% ORIENTATION
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
%  changing the orientation frame of orientation #1 using orientation#2
\newcommand{\changeOrientationFrameOrientation}[2]
{\changeOrientationFrame{#1}{#2}
}

%  changing the reference orientation frame of orientation #1 using orientation#2
\newcommand{\changeRefOrientationFrameOrientation}[2]
{\changeRefOrientationFrame{#1}{#2}
}

%  changing the coordinate frame of orientation #1 using orientation #2
\newcommand{\changeCoordinateFrameOrientation}[2]
{\changeCoordinateFrame{#1}{#2}
}

%  compose orientation #1 and orientation #2
\newcommand{\composeOrientation}[2]
{\compose{#1}{#2}
}

%  inverse orientation #1 
\newcommand{\inverseOrientation}[1]
{\inverse{#1}
}

%  inverse orientation #1  (also change coordinate frame)
\newcommand{\inverseOrientationB}[1]
{\inverseB{#1}
}


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% POSITION
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

%  changing the point of position#1 using position #2
\newcommand{\changePointPosition}[2]
{\changePoint{#1}{#2}
}

%  changing the reference point of position#1 using position #2
\newcommand{\changeRefPointPosition}[2]
{\changeRefPoint{#1}{#2}
}

%  changing the coordinate frame of position#1 using orientation #2
\newcommand{\changeCoordinateFramePosition}[2]
{\changeCoordinateFrame{#1}{#2}
}

%  compose position#1 and position #2
\newcommand{\composePosition}[2]
{\compose{#1}{#2}
}

%  inverse position #1 
\newcommand{\inversePosition}[1]
{\inverse{#1}
}

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% POSE
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

%  changing the point of pose#1 using position #2
\newcommand{\changePointPose}[2]
{\changePoint{#1}{#2}
}

%  changing the reference point of pose#1 using position #2
\newcommand{\changeRefPointPose}[2]
{\changeRefPoint{#1}{#2}
}

%  changing the orientation frame of pose #1 using orientation#2
\newcommand{\changeOrientationFramePose}[2]
{\changeOrientationFrame{#1}{#2}
}

%  changing the reference orientation frame of pose #1 using orientation#2
\newcommand{\changeRefOrientationFramePose}[2]
{\changeRefOrientationFrame{#1}{#2}
}

%  changing the coordinate frame of pose#1 using orientation #2
\newcommand{\changeCoordinateFramePose}[2]
{\changeCoordinateFrame{#1}{#2}
}

%  compose pose#1 and pose #2
\newcommand{\composePose}[2]
{\compose{#1}{#2}
}

%  inverse pose #1 
\newcommand{\inversePose}[1]
{\inverse{#1}
}

%  inverse pose #1  (also change coordinate frame)
\newcommand{\inversePoseB}[1]
{\inverseB{#1}
}

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% TRANSLATIONAL VELOCITY
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

%  changing the point of translational velocity #1 using position #2 and rotational velocity #3
\newcommand{\changePointLinearVelocity}[3]
{\changePoint{#1}{#2,#3}
}

%  changing the coordinate frame of translational velocity #1 using orientation #2
\newcommand{\changeCoordinateFrameLinearVelocity}[2]
{\changeCoordinateFrame{#1}{#2}
}

%  compose translational velocity #1 and translational velocity #2
\newcommand{\composeLinearVelocity}[2]
{\compose{#1}{#2}
}

%  inverse translational velocity #1 
\newcommand{\inverseLinearVelocity}[1]
{\inverse{#1}
}


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% ROTATIONAL VELOCITY
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

%  changing the coordinate frame of rotational velocity #1 using orientation #2
\newcommand{\changeCoordinateFrameAngularVelocity}[2]
{\changeCoordinateFrame{#1}{#2}
}

%  compose rotational velocity #1 and rotational veloicty #2
\newcommand{\composeAngularVelocity}[2]
{\compose{#1}{#2}
}

%  inverse rotational velocity #1 
\newcommand{\inverseAngularVelocity}[1]
{\inverse{#1}
}


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% VELOCITY
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

%  changing the point of velocity #1 using position #2 
\newcommand{\changePointVelocity}[2]
{\changePoint{#1}{#2}
}


%  changing the coordinate frame of velocity #1 using orientation #2
\newcommand{\changeCoordinateFrameVelocity}[2]
{\changeCoordinateFrame{#1}{#2}
}

%  compose velocity #1 and velocity #2
\newcommand{\composeVelocity}[2]
{\compose{#1}{#2}
}

%  inverse velocity #1 
\newcommand{\inverseVelocity}[1]
{\inverse{#1}
}


% integrate velocity #1 over time #2
\newcommand{\integrateVelocity}[2]
{\integrate{#1}{#2}
} 

macroSemantics.tex

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% RIGID BODY GEOMETRIC RELATION SEMANTICS and RIGID BODY GEOMETRIC RELATION COORDINATE SEMANTICS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


% \newcommand{\fixedTo}[2]
% {
% #1
% \ifthenelse{\equal{#2}{}}{}{
% \hspace{0.1ex}|\hspace{0.1ex}
% \body{#2}}
% }

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% ORIENTATION
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

%  orientation: orientation of orientation frame #1 with respect to orientation frame #2 expressed in  orientation frame #3 (leave empty if not needed)
\newcommand{\Orientation}[5]
{\textrm{Orientation\ifthenelse{\equal{#5}{}}{}{Coord}}\left(\fixedTo{\orientation{#1}}{#2}, \fixedTo{\orientation{#3}}{#4} \ifthenelse{\equal{#5}{}}{}{, \orientation{#5}} \right)}

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% POSITION
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

%  position: position of point #1 belonging to body #2 with respect to point #3 belonging to body #4 expressed in orientation frame #5 (leave empty if not needed)
\newcommand{\Position}[5]
{\textrm{Position\ifthenelse{\equal{#5}{}}{}{Coord}}\left(\fixedTo{\point{#1}}{#2} , \fixedTo{\point{#3}}{#4} \ifthenelse{\equal{#5}{}}{}{,\orientation{#5}} \right)}

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% POSE
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

%  pose: pose of orientation frame #2 and point #1 belonging to body #3 with respect to orientation frame #5 and point #4 belonging to body #6 expressed in orientation frame #7 (leave empty if not needed)
% if #2 is left blank: pose of frame #1 belonging to body #3
% if #5 is left blank: pose of frame #5 belonging to body #6
% if #1 == #2: pose of frame #1 belonging to body #3
% if #4 == #5: pose of frame #4 belonging to body #6
\newcommand{\Pose}[7]
{\textrm{Pose\ifthenelse{\equal{#7}{}}{}{Coord}}\left(
\fixedTo{
\ifthenelse{\equal{#1}{#2}}{\refframe{#1}}{\left(
 \point{#1}, \orientation{#2} \right)} }{#3}
 , 
 \fixedTo{
 \ifthenelse{\equal{#5}{}}{ \ifthenelse{\equal{#4}{}}{}{\point{#4}}} { \ifthenelse{\equal{#4}{#5}}{\refframe{#4}}{\left(\point{#4}, \orientation{#5}\right)}}
 }
 {#6}
 \ifthenelse{\equal{#7}{}}{}{,\orientation{#7}} \right)}
 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% ROTATIONAL VELOCITY
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

%  rotation velocity: rotation velocity of orientation frame #1 belonging to body #1 with respect to orientation frame #3 belonging to body #4 expressed in  orientation frame #5 (leave empty if not needed). If #6=1 the forall (it holds for all orientation frames on the bodies) are explicitly stated otherwise they are left out
\newcommand{\AngularVelocityBasic}[6]
{\textrm{AngularVelocity\ifthenelse{\equal{#5}{}}{}{Coord}}\left(
\ifthenelse{\equal{#1}{}}{\body{#2}}{
\ifthenelse{\equal{#6}{1}}{\forall}{}
\fixedTo{\orientation{#1}}{#2}
}
,
\ifthenelse{\equal{#3}{}}{\body{#4}}{
\ifthenelse{\equal{#6}{1}}{\forall}{}
\fixedTo{\orientation{#3}}{#4}
}  \ifthenelse{\equal{#5}{}}{}{,\orientation{#5}} \right)}

%  rotation velocity: rotation velocity of orientation frame #1 belonging to body #1 with respect to orientation frame #3 belonging to body #4 expressed in  orientation frame #5 (leave empty if not needed) with explicity stating that this holds for all orientation frames
\newcommand{\AngularVelocity}[5]
{\AngularVelocityBasic{#1}{#2}{#3}{#4}{#5}{1}}

%  rotation velocity: rotation velocity of orientation frame #1 belonging to body #1 with respect to orientation frame #3 belonging to body #4 expressed in  orientation frame #5 (leave empty if not needed) without explicity stating that this holds for all orientation frames
\newcommand{\AngularVelocityNoForAll}[5]
{\AngularVelocityBasic{#1}{#2}{#3}{#4}{#5}{0}}

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% TRANSLATIONAL VELOCITY
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

%  translational velocity: translational velocity of point #1 belonging to body #2 with respect to point #3 belonging to body #4 expressed in orientation frame #5 (leave empty if not needed)  If #6=1 the forall (it holds for all reference points on body#2) is explicitly stated otherwise it are left out
\newcommand{\LinearVelocityBasic}[6]
{\textrm{LinearVelocity\ifthenelse{\equal{#5}{}}{}{Coord}}\left(   \fixedTo{\point{#1}}{#2} , 
\ifthenelse{\equal{#3}{}}{\body{#4}}{\ifthenelse{\equal{#6}{1}}{\forall}{} 
\fixedTo{\point{#3}}{#4}} \ifthenelse{\equal{#5}{}}{}{,\orientation{#5}} \right)}

%  translational velocity: translational velocity of point #1 belonging to body #2 with respect to point #3 belonging to body #4 expressed in orientation frame #5 (leave empty if not needed), the  forall (it holds for all reference points on body#2) is explicitly stated
\newcommand{\LinearVelocity}[5]
{\LinearVelocityBasic{#1}{#2}{#3}{#4}{#5}{1}}

%  translational velocity: translational velocity of point #1 belonging to body #2 with respect to point #3 belonging to body #4 expressed in orientation frame #5 (leave empty if not needed) , the  forall (it holds for all reference points on body#2) is not explicitly stated
\newcommand{\LinearVelocityNoForAll}[5]
{\LinearVelocityBasic{#1}{#2}{#3}{#4}{#5}{0}}


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% VELOCITY
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
%  velocity: velocity of orientation frame #2 and point #1 belonging to body #3 with respect to orientation frame #5 and point #4 belonging to body #6 expressed in orientation frame #7 (leave empty if not needed)
% if #2 is left blank: velocity of frame #1 belonging to body #3
% if #5 is left blank: velocity of frame #5 belonging to body #6
% If #6==#1 the forall (it holds for all reference points#5 on body#6 and all orientation frames on body#1 and body#6) are explicitly stated otherwise it are left out
% if #1 == #2: velocity of frame #1 belonging to body #3
% if #4 == #5: velocity of frame #4 belonging to body #6
\newcommand{\VelocityBasic}[8]
{\textrm{Twist\ifthenelse{\equal{#7}{}}{}{Coord}}\left(
\fixedTo{
\ifthenelse{\equal{#2}{}}{\point{#1}}{
\ifthenelse{\equal{#1}{#2}}{\refframe{#1}}{\left(
 \point{#1}, \ifthenelse{\equal{#8}{1}}{\forall}{} \orientation{#2}\right)} }
 }{#3}
 , 
 \ifthenelse{\equal{#4}{}}{\body{#6}}{
 \fixedTo{
 \ifthenelse{\equal{#8}{1}}{\forall}{}
\ifthenelse{\equal{#5}{}}{\point{#4}}{
\ifthenelse{\equal{#4}{#5}}{\refframe{#4}}{\left(
 \point{#4},  \orientation{#5}\right)} }
 }{#6}
 }
%  \fixedTo{
%  \ifthenelse{\equal{#5}{}}
%  {
%  \ifthenelse{\equal{#4}{}}{}{
%  \ifthenelse{\equal{#8}{1}}{\forall}{} \point{#4}
%  \hspace{0.1ex}|\hspace{0.3ex}
%  }
%  }
%  {
%  \ifthenelse{\equal{#4}{}}{\ifthenelse{\equal{#8}{1}}{\forall}{} \orientation{#5}}{
%  \ifthenelse{\equal{#4}{#5}}{\refframe{#4}}{\ifthenelse{\equal{#8}{1}}{\forall}{}\left( \point{#4},\orientation{#5}\right)}
%  }\hspace{0.1ex}|\hspace{0.3ex}
%  }
%  }{#6}
 \ifthenelse{\equal{#7}{}}{}{,\orientation{#7}} \right)}


%  velocity: velocity of orientation frame #1 and point #2 belonging to body #3 with respect to orientation frame #4 and point #5 belonging to body #6 expressed in orientation frame #7 (leave empty if not needed)
% if #2 is left blank: velocity of frame #1 belonging to body #3
% if #5 is left blank: velocity of frame #5 belonging to body #6
% The forall (it holds for all reference points#5 on body#6 and all orientation frames on body#1 and body#6) are explicitly stated
\newcommand{\Velocity}[7]
{\VelocityBasic{#1}{#2}{#3}{#4}{#5}{#6}{#7}{1}}

%  velocity: velocity of orientation frame #1 and point #2 belonging to body #3 with respect to orientation frame #4 and point #5 belonging to body #6 expressed in orientation frame #7 (leave empty if not needed)
% if #2 is left blank: velocity of frame #1 belonging to body #3
% if #5 is left blank: velocity of frame #5 belonging to body #6
% The forall (it holds for all reference points#5 on body#6 and all orientation frames on body#1 and body#6) are not explicitly stated
\newcommand{\VelocityNoForAll}[7]
{\VelocityBasic{#1}{#2}{#3}{#4}{#5}{#6}{#7}{0}}





%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% FORCE
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

%  force: force of body #1 with respect to body #2 expressed in  orientation frame #3 (leave empty if not needed).
\newcommand{\ForceBasic}[3]
{\textrm{Force\ifthenelse{\equal{#3}{}}{}{Coord}}\left(
\body{#1},\body{#2}
\ifthenelse{\equal{#3}{}}{}{,\orientation{#3}} \right)}

%  force: force of body #1 with respect to body #2 expressed in  orientation frame #3 (leave empty if not needed).
\newcommand{\Force}[3]
{\ForceBasic{#1}{#2}{#3}}


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% TORQUE
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

%  torque: torque around point #1 belonging to body #2 with respect body #3 expressed in orientation frame #4 (leave empty if not needed)
\newcommand{\TorqueBasic}[4]
{\textrm{Torque\ifthenelse{\equal{#4}{}}{}{Coord}}\left(   \fixedTo{\point{#1}}{#2} , 
\body{#3}
\ifthenelse{\equal{#4}{}}{}{,\orientation{#4}} \right)
}

%  torque: torque around point #1 belonging to body #2 with respect to body #3 expressed in orientation frame #4 (leave empty if not needed)
\newcommand{\Torque}[4]
{\TorqueBasic{#1}{#2}{#3}{#4}}


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% WRENCH
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
%  wrench: wrench around point #1 belonging to body #2 with respect to body #3 expressed in orientation frame #4 (leave empty if not needed)
\newcommand{\WrenchBasic}[4]
{\textrm{Wrench\ifthenelse{\equal{#4}{}}{}{Coord}}\left(
\fixedTo{\point{#1}}{\body{#2}}, \body{#3}
 \ifthenelse{\equal{#4}{}}{}{,\orientation{#4}} \right)}


%  wrench: wrench around point #2 belonging to body #2 with respect to body #3 expressed in orientation frame #4 (leave empty if not needed)
\newcommand{\Wrench}[4]
{\WrenchBasic{#1}{#2}{#3}{#4}}


document.bbl
% Generated by IEEEtran.bst, version: 1.13 (2008/09/30)
\begin{thebibliography}{10}
\providecommand{\url}[1]{#1}
\csname url@samestyle\endcsname
\providecommand{\newblock}{\relax}
\providecommand{\bibinfo}[2]{#2}
\providecommand{\BIBentrySTDinterwordspacing}{\spaceskip=0pt\relax}
\providecommand{\BIBentryALTinterwordstretchfactor}{4}
\providecommand{\BIBentryALTinterwordspacing}{\spaceskip=\fontdimen2\font plus
\BIBentryALTinterwordstretchfactor\fontdimen3\font minus
  \fontdimen4\font\relax}
\providecommand{\BIBforeignlanguage}[2]{{%
\expandafter\ifx\csname l@#1\endcsname\relax
\typeout{** WARNING: IEEEtran.bst: No hyphenation pattern has been}%
\typeout{** loaded for the language `#1'. Using the pattern for}%
\typeout{** the default language instead.}%
\else
\language=\csname l@#1\endcsname
\fi
#2}}
\providecommand{\BIBdecl}{\relax}
\BIBdecl

\bibitem{DeLaet-ram2012a}
T.~De~Laet, S.~Bellens, R.~Smits, E.~Aertbeli\"en, H.~Bruyninckx, and
  J.~De~Schutter, ``Geometric relations between rigid bodies: Semantics for
  standardization,'' \emph{IEEE Rob.\ Autom.\ Mag.}, 2012.

\bibitem{DeLaet-rrfc2012}
T.~De~Laet, S.~Bellens, and H.~Bruyninckx, ``Semantics underlying geometric
  relations between rigid bodies in robotics,''
  \url{https://retf.info/rrfcs/0005}, 2012, last visited September 2012.

\bibitem{kdl-url}
R.~Smits, H.~Bruyninckx, and E.~Aertbeli\"en, ``{KDL}: {K}inematics and
  {D}ynamics {L}ibrary,'' \url{http://www.orocos.org/kdl}, 2001, last visited
  August 2012.

\bibitem{ros-url}
{Willow Garage}, ``{R}obot {O}perating {S}ystem ({ROS}),''
  \url{http://www.ros.org}, 2008, last visited 2012.

\bibitem{robotics-library-url}
``Robotics library,'' \url{http://sourceforge.net/apps/mediawiki/roblib}.

\bibitem{retf-url}
G.~Biggs, K.~Conley, B.~Gerkey, and I.~L\"{u}tkebohle, ``{R}obot {E}ngineering
  {T}ask {F}orce,'' \url{http://www.retf.info/}, 2011.

\bibitem{DeLaet-ram2012b}
T.~De~Laet, S.~Bellens, H.~Bruyninckx, and J.~De~Schutter, ``Geometric
  relations between rigid bodies: from semantics to software,'' \emph{IEEE
  Rob.\ Autom.\ Mag.}, 2012.

\bibitem{GeometricSemanticsSoftware-url}
T.~De~Laet and S.~Bellens, ``Geometric semantics software,''
  \url{http://www.orocos.org/wiki/geometric-relations-semantics-wiki}, 2012,
  last visited September 2012.

\bibitem{Klotzbuecher2011-IROS}
M.~Klotzbuecher, R.~Smits, H.~Bruyninckx, and J.~De~Schutter, ``Reusable hybrid
  force-velocity controlled motion specifications with executable domain
  specific languages,'' in \emph{Proc.\ IEEE/RSJ Int.\ Conf.\ Int.\ Robots and
  Systems}, San Francisco, California, 2011, pp. 4684--4689.

\bibitem{bordignon2010}
\BIBentryALTinterwordspacing
M.~Bordignon, U.~P. Schultz, and K.~Stoy, ``Model-based kinematics generation
  for modular mechatronic toolkits,'' in \emph{Proceedings of the ninth
  international conference on Generative programming and component
  engineering}, ser. GPCE '10.\hskip 1em plus 0.5em minus 0.4em\relax New York,
  NY, USA: ACM, 2010, pp. 157--166. [Online]. Available:
  \url{http://doi.acm.org/10.1145/1868294.1868318}
\BIBentrySTDinterwordspacing

\bibitem{Klotzbuecher2010-DYROS}
\BIBentryALTinterwordspacing
M.~Klotzbuecher, P.~Soetens, and H.~Bruyninckx, ``{OROCOS RTT}-{L}ua: an
  {E}xecution {E}nvironment for building {R}eal-time {R}obotic {D}omain
  {S}pecific {L}anguages,'' in \emph{Int. Workshop on DYn. languages for
  RObotic and Sensors}, 2010, pp. 284--289. [Online]. Available:
  \url{https://www.sim.informatik.tu-darmstadt.de/simpar/ws/sites/DYROS2010/03%
-DYROS.pdf}
\BIBentrySTDinterwordspacing

\bibitem{Bezivin2005}
J.~B\'{e}zivin, ``On the unification power of models,'' \emph{Software and
  Systems Modeling}, vol.~4, no.~2, pp. 171--188, 2005.

\bibitem{FrigerioBuchliCaldwell2011}
M.~Frigerio, J.~Buchli, and D.~G. Caldwell, ``A {D}omain {S}pecific {L}anguage
  for kinematic models and fast implementations of robot dynamics algorithms,''
  in \emph{Workshop on Domain Specific Languages for Robotics}, 2011.

\end{thebibliography}
