[PATCH] add division operator and diff/addDelta global ops

Signed-off-by: Markus Klotzbuecher <markus [dot] klotzbuecher [..] ...>
---
kdl_typekit/src/kdlTypekit.hpp | 1 +
kdl_typekit/src/kdlTypekitOperators.cpp | 17 +++++++++++++++++
2 files changed, 18 insertions(+), 0 deletions(-)

diff --git a/kdl_typekit/src/kdlTypekit.hpp b/kdl_typekit/src/kdlTypekit.hpp
index 5fd172b..e55fb7a 100644
--- a/kdl_typekit/src/kdlTypekit.hpp
+++ b/kdl_typekit/src/kdlTypekit.hpp
@@ -55,6 +55,7 @@
#include <rtt/types/OperatorTypes.hpp>
#include <rtt/internal/mystd.hpp>
#include <rtt/os/StartStopManager.hpp>
+#include <rtt/internal/GlobalService.hpp>

#include <kdl/frames_io.hpp>

diff --git a/kdl_typekit/src/kdlTypekitOperators.cpp b/kdl_typekit/src/kdlTypekitOperators.cpp
index e8cc924..db26ba9 100644
--- a/kdl_typekit/src/kdlTypekitOperators.cpp
+++ b/kdl_typekit/src/kdlTypekitOperators.cpp
@@ -33,6 +33,23 @@ namespace KDL{
oreg->add( newBinaryOperator( "*", multiplies3<Twist, Frame, Twist>() ) );
oreg->add( newBinaryOperator( "*", multiplies3<Vector, Frame, Vector>() ) );
oreg->add( newBinaryOperator( "*", multiplies3<Vector, Rotation, Vector>() ) );
+ oreg->add( newBinaryOperator( "/", divides3<Vector, Vector, double>() ) );
+ oreg->add( newBinaryOperator( "/", divides3<Wrench, Wrench, double>() ) );
+ oreg->add( newBinaryOperator( "/", divides3<Twist, Twist, double>() ) );
+
+ RTT::Service::shared_ptr gs = RTT::internal::GlobalService::Instance();
+
+ gs->provides("KDL")->addOperation("Vector_diff",(Vector (*) (const Vector&, const Vector&, double)) &diff).doc("");
+ gs->provides("KDL")->addOperation("Rotation_diff",(Vector (*) (const Rotation&, const Rotation&, double)) &diff).doc("");
+ gs->provides("KDL")->addOperation("Twist_diff",(Twist (*) (const Twist&, const Twist&, double)) &diff).doc("");
+ gs->provides("KDL")->addOperation("Wrench_diff",(Wrench (*) (const Wrench&, const Wrench&, double)) &diff).doc("");
+ gs->provides("KDL")->addOperation("Frame_diff",(Twist (*) (const Frame&, const Frame&, double)) &diff)
+ .doc("Returns the twist that is needed to move from frame f1 to frame f2 in a time d. The resulting twist is represented in the same reference frame as f1 and f2, and has reference point at the origin of f1");
+
+ gs->provides("KDL")->addOperation("Vector_addDelta",(Vector (*) (const Vector&, const Vector&, double)) &addDelta).doc("");
+ gs->provides("KDL")->addOperation("Rotation_addDelta",(Rotation (*) (const Rotation&, const Vector&, double)) &addDelta).doc("");
+ gs->provides("KDL")->addOperation("Frame_addDelta", (Frame (*) (const Frame&, const Twist&, double)) &addDelta)
+ .doc("Constructs a frame that is obtained by: starting from frame f, apply twist t, during time d");

return true;
}