Rbs se3 viz

test/viz/test_wrench_visualization.rb

@@ -0,0 +1,126 @@
+require_relative 'helpers'
+require 'orocos'
+Orocos.load_typekit 'base'
+
+module Vizkit
+    describe 'WrenchVisualization' do
+        include TestHelpers
+
+        before do
+            register_widget(@vizkit3d_widget =
+                Vizkit.default_loader.create_plugin("vizkit3d::Vizkit3DWidget"))
+            @viz = @vizkit3d_widget.createPlugin('base', 'WrenchVisualization')
+
+            @wrench = Types.base.samples.Wrench.new
+            @wrench.frame_id = "test_frame"
+            @wrench.force = Types.base.Vector3d.new(1,1,1)
+            @wrench.torque = Types.base.Vector3d.new(-1,-1,1)
+            @viz.updateData(@wrench)
+
+            @viz.setResolution(1.0)
+
+            @vizkit3d_widget.setCameraEye(5, 4, 5)
+            @vizkit3d_widget.setCameraLookAt(0, 0, 0)
+            @vizkit3d_widget.show
+        end
+
+        # describe "updateData(Wrench)" do
+        #     @wrench = Types.base.samples.Wrench.new
+        #     @wrench.frame_id = "test_frame"
+        #     @wrench.force = Types.base.Vector3d.new(1,1,1)
+        #     @wrench.torque = Types.base.Vector3d.new(-1,-1,1)
+
+        #     it "updates wrench" do
+        #         @viz.updateData(@wrench)
+        #         confirm 'Force should be displayed as a red arrow from (0,0,0) to (1,1,1) and torque as a green arrow from (0,0,0) to (-1,-1,1) with a circular arrow rotating counter-clockwise around axis'
+        #     end
+
+        #     it "updates wrench" do
+        #         @wrench.force = Types.base.Vector3d.new(1,0,0)
+        #         @wrench.torque = Types.base.Vector3d.new(0,1,0)
+        #         @viz.updateData(@wrench)
+        #         confirm 'Force should be displayed as a red arrow from (0,0,0) to (1,0,0) and torque as a green arrow from (0,0,0) to (0,1,0) with a circular arrow rotating counter-clockwise around axis'
+        #     end
+
+        #     it "updates wrench" do
+        #         @wrench.force = Types.base.Vector3d.new(0,1,0)
+        #         @wrench.torque = Types.base.Vector3d.new(0,0,1)
+        #         @viz.updateData(@wrench)
+        #         confirm 'Force should be displayed as a red arrow from (0,0,0) to (0,1,0) and torque as a green arrow from (0,0,0) to (0,0,1) with a circular arrow rotating counter-clockwise around axis'
+        #     end
+
+        #     it "updates wrench" do
+        #         @wrench.force = Types.base.Vector3d.new(0,0,1)
+        #         @wrench.torque = Types.base.Vector3d.new(1,0,0)
+        #         @viz.updateData(@wrench)
+        #         confirm 'Force should be displayed as a red arrow from (0,0,0) to (0,0,1) and torque as a green arrow from (0,0,0) to (1,0,0) with a circular arrow rotating counter-clockwise around axis'
+        #     end
+
+        #     it "updates wrench" do
+        #         @wrench.force = Types.base.Vector3d.new(0,0,0)
+        #         @wrench.torque = Types.base.Vector3d.new(0,0,0)
+        #         @viz.updateData(@wrench)
+        #         confirm 'Nothing should be displayed'
+        #     end
+
+        #     it "updates wrench" do
+        #         @wrench.force = Types.base.Vector3d.new(0.1,0,0)
+        #         @wrench.torque = Types.base.Vector3d.new(0.3,0,0)
+        #         @viz.updateData(@wrench)
+        #         confirm 'Force should be displayed as a red arrow from (0,0,0) to (0,0,0.1) and torque as a green arrow from (0,0,0) to (0.3,0,0) with a circular arrow rotating counter-clockwise around axis. Both arrows should be displayed without the arrow head'
+        #     end
+        # end
+
+        describe "setSeperateAxesTorque" do
+            it "displays torque axis component" do
+                torque = Types.base.Vector3d.new(1,1,1)
+                @viz.setTorque(torque)
+                @viz.setSeperateAxesTorque(true)
+                confirm 'Torque should be displayed as three arrows along axes each with length 1 and a circular arrow rotating counter-clockwise'
+
+            end
+
+            it "displays torque" do
+                torque = Types.base.Vector3d.new(1,1,1)
+                @viz.setTorque(torque)
+                @viz.setSeperateAxesTorque(false)
+                confirm 'Torque should be displayed as one arrows starting from (0,0,0) to (1,1,1) with circular arrow rotating counter-clockwise'
+            end
+        end
+
+        describe "setSeperateAxesForce" do
+            it "displays force axis component" do
+                force = Types.base.Vector3d.new(1,1,1)
+                @viz.setForce(force)
+                @viz.setSeperateAxesForce(true)
+                confirm 'Force should be displayed as three arrows along axes each with length 1'
+            end
+
+            it "displays force" do
+                force = Types.base.Vector3d.new(1,1,1)
+                @viz.setForce(force)
+                @viz.setSeperateAxesForce(false)
+                confirm 'Force should be displayed as one arrows starting from (0,0,0) to (1,1,1)'
+            end
+        end
+
+        # describe "setResolution" do
+        #     wrench = Types.base.samples.Wrench.new
+        #     wrench.frame_id = "test_frame"
+        #     wrench.force = Types.base.Vector3d.new(1,0,0)
+        #     wrench.torque = Types.base.Vector3d.new(0,1,0)
+
+        #     it "sets resolution to 1" do
+        #         @viz.updateData(wrench)
+        #         @viz.setResolution(1.0)
+        #         confirm "Force and torque will be displayed as arrows from (0,0,0) to (1,0,0) / (0,1,0)"
+        #     end
+        #     it "it sets resolution to 0.1" do
+        #         @viz.updateData(wrench)
+        #         @viz.setResolution(0.1)
+        #         confirm "Force and torque will be displayed as arrows from (0,0,0) to (0.1,0,0) / (0,0.1,0) without arrow head"
+        #     end
+        # end
+    end
+end
+

viz/CMakeLists.txt

@@ -22,6 +22,9 @@ rock_vizkit_plugin(base-viz
         SonarVisualization.cpp
         PointcloudVisualization.cpp
         DepthMapVisualization.cpp
+        RigidBodyStateSE3Visualization.cpp
+        WrenchModel.cpp
+        WrenchVisualization.cpp
         ${OPTIONAL_CPP}
     HEADERS 
         Uncertainty.hpp 
@@ -37,8 +40,11 @@ rock_vizkit_plugin(base-viz
         SonarVisualization.hpp
         PointcloudVisualization.hpp
         DepthMapVisualization.hpp
+        RigidBodyStateSE3Visualization.hpp
+        WrenchModel.hpp
+        WrenchVisualization.hpp
         ${OPTIONAL_HPP}
     DEPS base-types
     LIBS ${Boost_SYSTEM_LIBRARY}
-    DEPS_PKGCONFIG base-logging
+    DEPS_PKGCONFIG base-logging PrimitivesFactory osgViz
 )

viz/PluginLoader.cpp

@@ -11,94 +11,106 @@
 #include "SonarVisualization.hpp"
 #include "DepthMapVisualization.hpp"
 #include "DistanceImageVisualization.hpp"
+#include "RigidBodyStateSE3Visualization.hpp"
+#include "WrenchVisualization.hpp"
 
 namespace vizkit3d {
     class QtPluginVizkitBase : public vizkit3d::VizkitPluginFactory {
     private:
     public:
-
-	QtPluginVizkitBase() {
-	}
-
-	/**
-	* Returns a list of all available visualization plugins.
-	* @return list of plugin names
-	*/
-        virtual QStringList* getAvailablePlugins() const
-	{
-	    QStringList *pluginNames = new QStringList();
-	    pluginNames->push_back("WaypointVisualization");
-	    pluginNames->push_back("TrajectoryVisualization");
-	    pluginNames->push_back("MotionCommandVisualization");
-	    pluginNames->push_back("RigidBodyStateVisualization");
-	    pluginNames->push_back("BodyStateVisualization");
-	    pluginNames->push_back("LaserScanVisualization");
-	    pluginNames->push_back("SonarGroundDistanceVisualization");
-	    pluginNames->push_back("PointcloudVisualization");
-	    pluginNames->push_back("SonarBeamVisualization");
-	    pluginNames->push_back("SonarVisualization");
-	    pluginNames->push_back("DepthMapVisualization");
-	    pluginNames->push_back("DistanceImageVisualization");
-	    return pluginNames;
-	}
-
-        virtual QObject* createPlugin(QString const& pluginName)
+
+    QtPluginVizkitBase() {
+    }
+
+    /**
+    * Returns a list of all available visualization plugins.
+    * @return list of plugin names
+    */
+    virtual QStringList* getAvailablePlugins() const
+    {
+        QStringList *pluginNames = new QStringList();
+        pluginNames->push_back("WaypointVisualization");
+        pluginNames->push_back("TrajectoryVisualization");
+        pluginNames->push_back("MotionCommandVisualization");
+        pluginNames->push_back("RigidBodyStateVisualization");
+        pluginNames->push_back("BodyStateVisualization");
+        pluginNames->push_back("LaserScanVisualization");
+        pluginNames->push_back("SonarGroundDistanceVisualization");
+        pluginNames->push_back("PointcloudVisualization");
+        pluginNames->push_back("SonarBeamVisualization");
+        pluginNames->push_back("SonarVisualization");
+        pluginNames->push_back("DepthMapVisualization");
+        pluginNames->push_back("DistanceImageVisualization");
+        pluginNames->push_back("RigidBodyStateSE3Visualization");
+        pluginNames->push_back("WrenchVisualization");
+        return pluginNames;
+    }
+
+    virtual QObject* createPlugin(QString const& pluginName)
+    {
+        vizkit3d::VizPluginBase* plugin = 0;
+        if (pluginName == "WaypointVisualization")
+        {
+            plugin = new vizkit3d::WaypointVisualization();
+        }
+        else if (pluginName == "MotionCommandVisualization")
+        {
+            plugin = new vizkit3d::MotionCommandVisualization();
+        }
+        else if (pluginName == "TrajectoryVisualization")
+        {
+            plugin = new vizkit3d::TrajectoryVisualization();
+        }
+        else if (pluginName == "RigidBodyStateVisualization")
+        {
+            plugin = new vizkit3d::RigidBodyStateVisualization();
+        }
+        else if (pluginName == "BodyStateVisualization")
+        {
+            plugin = new vizkit3d::BodyStateVisualization();
+        }
+        else if (pluginName == "LaserScanVisualization")
+        {
+            plugin = new vizkit3d::LaserScanVisualization();
+        }
+        else if (pluginName == "SonarGroundDistanceVisualization")
         {
-	    vizkit3d::VizPluginBase* plugin = 0;
-	    if (pluginName == "WaypointVisualization")
-	    {
-		    plugin = new vizkit3d::WaypointVisualization();
-	    }
-	    else if (pluginName == "MotionCommandVisualization")
-	    {
-    		plugin = new vizkit3d::MotionCommandVisualization();
-	    }
-	    else if (pluginName == "TrajectoryVisualization")
-	    {
-	    	plugin = new vizkit3d::TrajectoryVisualization();
-	    }
-	    else if (pluginName == "RigidBodyStateVisualization")
-	    {
-		    plugin = new vizkit3d::RigidBodyStateVisualization();
-	    }
-	    else if (pluginName == "BodyStateVisualization")
-	    {
-    		plugin = new vizkit3d::BodyStateVisualization();
-	    }
-	    else if (pluginName == "LaserScanVisualization")
-	    {
-	        plugin = new vizkit3d::LaserScanVisualization();
-	    }
-	    else if (pluginName == "SonarGroundDistanceVisualization")
-	    {
-	        plugin = new vizkit3d::SonarGroundDistanceVisualization();
-	    }
-	    else if (pluginName == "PointcloudVisualization")
-	    {
-	        plugin = new vizkit3d::PointcloudVisualization();
-	    }
-	    else if (pluginName == "SonarBeamVisualization")
-	    {
-	        plugin = new vizkit3d::SonarBeamVisualization();
-	    }
-	    else if (pluginName == "SonarVisualization")
-	    {
-		    plugin = new vizkit3d::SonarVisualization();
-	    }
-	    else if (pluginName == "DepthMapVisualization")
-	    {
-	    	plugin = new vizkit3d::DepthMapVisualization();
-	    }
-	    else if (pluginName == "DistanceImageVisualization")
-	    {
-		plugin = new vizkit3d::DistanceImageVisualization();
-	    }
+            plugin = new vizkit3d::SonarGroundDistanceVisualization();
+        }
+        else if (pluginName == "PointcloudVisualization")
+        {
+            plugin = new vizkit3d::PointcloudVisualization();
+        }
+        else if (pluginName == "SonarBeamVisualization")
+        {
+            plugin = new vizkit3d::SonarBeamVisualization();
+        }
+        else if (pluginName == "SonarVisualization")
+        {
+            plugin = new vizkit3d::SonarVisualization();
+        }
+        else if (pluginName == "DepthMapVisualization")
+        {
+            plugin = new vizkit3d::DepthMapVisualization();
+        }
+        else if (pluginName == "DistanceImageVisualization")
+        {
+        plugin = new vizkit3d::DistanceImageVisualization();
+        }
+        else if (pluginName == "RigidBodyStateSE3Visualization")
+        {
+        plugin = new vizkit3d::RigidBodyStateSE3Visualization();
+        }
+        else if (pluginName == "WrenchVisualization")
+        {
+        plugin = new vizkit3d::WrenchVisualization();
+        }
 
-	    if (plugin) 
-	    {
-		    return plugin;
-	    }
-	    return NULL;
+        if (plugin) 
+        {
+            return plugin;
+        }
+        return NULL;
         };
     };
     Q_EXPORT_PLUGIN2(QtPluginVizkitBase, QtPluginVizkitBase)

viz/RigidBodyStateSE3Visualization.hpp

@@ -0,0 +1,199 @@
+#ifndef __RIGID_BODY_STATE_SE3_VISUALIZATION_HPP__
+#define __RIGID_BODY_STATE_SE3_VISUALIZATION_HPP__
+
+#include <vizkit3d/Vizkit3DPlugin.hpp>
+#include <Eigen/Geometry>
+#include <base/samples/RigidBodyStateSE3.hpp>
+#include <osgViz/OsgViz.hpp>
+#include <osgViz/modules/viz/Primitives/PrimitivesFactory.h>
+
+#include <osg/Image>
+#include <osg/Texture2D>
+#include <osg/MatrixTransform>
+#include "WrenchModel.hpp"
+
+namespace osgFX
+{
+    class BumpMapping;
+}
+
+namespace vizkit3d 
+{
+
+class RigidBodyStateSE3Visualization : public Vizkit3DPlugin<base::samples::RigidBodyStateSE3>
+{
+        Q_OBJECT
+        Q_PROPERTY(double size READ getSize WRITE setSize)
+        Q_PROPERTY(double sphereSize READ getMainSphereSize WRITE setMainSphereSize)
+        Q_PROPERTY(double textSize READ getTextSize WRITE setTextSize)
+        Q_PROPERTY(bool forcePositionDisplay READ isPositionDisplayForced WRITE setPositionDisplayForceFlag)
+        Q_PROPERTY(bool forceOrientationDisplay READ isOrientationDisplayForced WRITE setOrientationDisplayForceFlag)
+        Q_PROPERTY(QString modelPath READ getModelPath WRITE loadModel)
+        Q_PROPERTY(bool showPose READ isPoseDisplayed WRITE setPoseDisplayed)
+        Q_PROPERTY(bool showVelocity READ isVelocityDisplayed WRITE setVelocityDisplayed)
+        Q_PROPERTY(bool showAcceleration READ isAccelerationDisplayed WRITE setAccelerationDisplayed)
+        Q_PROPERTY(bool showWrench READ isWrenchDisplayed WRITE setWrenchDisplayed)
+        Q_PROPERTY(bool showSeperateAxes READ isSeperateAxes WRITE setSeperateAxes)
+        Q_PROPERTY(double resolution READ getResolution WRITE setResolution)
+
+    public:
+	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+	RigidBodyStateSE3Visualization(QObject* parent = NULL);
+	virtual ~RigidBodyStateSE3Visualization();
+
+        Q_INVOKABLE void updateData( const base::samples::RigidBodyStateSE3& state )
+        { return Vizkit3DPlugin<base::samples::RigidBodyStateSE3>::updateData(state); }
+        Q_INVOKABLE void updateRigidBodyStateSE3( const base::samples::RigidBodyStateSE3& state )
+        { return updateData(state); }
+
+    protected:
+        virtual osg::ref_ptr<osg::Node> createMainNode();
+	    virtual void updateMainNode(osg::Node* node);
+	    void updateDataIntern( const base::samples::RigidBodyStateSE3& state );
+        base::samples::RigidBodyStateSE3 state;
+
+    public slots: 
+        bool isPositionDisplayForced() const;
+        void setPositionDisplayForceFlag(bool flag);
+        bool isOrientationDisplayForced() const;
+        void setOrientationDisplayForceFlag(bool flag);
+
+        bool isPoseDisplayed() const;
+        void setPoseDisplayed(bool flag);
+
+        bool isVelocityDisplayed() const;
+        void setVelocityDisplayed(bool flag);
+
+        bool isAccelerationDisplayed() const;
+        void setAccelerationDisplayed(bool flag);
+
+        bool isWrenchDisplayed() const;
+        void setWrenchDisplayed(bool flag);
+
+        double getSize() const;
+        void setSize(double size);
+
+        void resetModel(double size);
+	    void resetModelSphere(double size);
+        void updateModel(double size);
+
+        QString getModelPath() const;
+        void loadModel(std::string const& path);
+        void loadModel(QString const& path);
+
+        /** When using the default body, sets the size of the main sphere,
+         * relative to the size of the complete object
+         *
+         * The default is 0.1
+         */
+        void setMainSphereSize(double size);
+
+        /** When using one of the default bodies, returns the size of the main
+         * sphere, relative to the size of the complete object
+         *
+         * The default is 0.1
+         */
+        double getMainSphereSize() const;
+
+        /** Sets the text size relative to the size of the complete object.
+         * If text size is positive, the name of the source frame is rendered in the visualization.
+         * The default is 0.0
+         */
+        void setTextSize(double size);
+        double getTextSize() const;
+
+        /** Sets the color of the default body model in R, G, B
+         *
+         * Values must be between 0 and 1
+         *
+         * If you call it after the plugin got attached, call resetModel to
+         * apply the new color
+         */
+        void setColor(base::Vector3d const& color);
+
+	    void setColor(const osg::Vec4d& color, osg::Geode* geode);
+
+        void setTexture(QString const& path);
+        void setTexture(std::string const& path);
+        void clearTexture();
+        void addBumpMapping(
+                QString const& diffuse_color_map_path,
+                QString const& normal_map_path);
+        void addBumpMapping(
+                std::string const& diffuse_color_map_path,
+                std::string const& normal_map_path);
+        void removeBumpMapping();
+
+        QVector3D getTranslation() const;
+        void setTranslation(QVector3D const& v);
+        void setRotation(QQuaternion const& q);
+
+        void setSeperateAxes(bool val = true) {
+            show_seperate_axes = val;
+            emit propertyChanged("showSeperateAxes");
+            updateModel(total_size);
+        }
+        bool isSeperateAxes() const {
+            return show_seperate_axes;
+        }
+        void setResolution(double res) {
+            resolution = res;
+            emit propertyChanged("resolution");
+            updateModel(total_size);
+        }
+        double getResolution() const {
+            return resolution;
+        }
+
+    private:
+        base::Vector3d color;
+        double total_size;
+        double main_size;
+        double text_size;
+
+        osg::Vec3 translation;
+        osg::Quat rotation;
+
+        enum BODY_TYPES
+        { BODY_NONE, BODY_SIMPLE, BODY_SPHERE, BODY_CUSTOM_MODEL };
+
+        BODY_TYPES body_type;
+	    osg::ref_ptr<osg::Node>  body_model;
+        osg::ref_ptr<osg::Group> createSimpleBody(double size);
+	    osg::ref_ptr<osg::Group> createSimpleSphere(double size);
+
+        osg::ref_ptr<osg::Image> image;
+        osg::ref_ptr<osg::Texture2D> texture;
+        bool texture_dirty;
+        void updateTexture();
+
+        osg::ref_ptr<osg::Image> diffuse_image;
+        osg::ref_ptr<osg::Image> normal_image;
+        osg::ref_ptr<osg::Texture2D> diffuse_texture;
+        osg::ref_ptr<osg::Texture2D> normal_texture;
+        osg::ref_ptr<osgFX::BumpMapping> bump_mapping;
+        bool bump_mapping_dirty;
+        void updateBumpMapping();
+
+        osg::ref_ptr<osg::PositionAttitudeTransform> linear_vel_transform, angular_vel_transform;
+        osg::ref_ptr<osg::PositionAttitudeTransform> linear_acc_transform, angular_acc_transform;
+        osg::ref_ptr<WrenchModel> wrench_model;
+
+        bool forcePositionDisplay;
+        bool forceOrientationDisplay;
+
+        bool showPose;
+        bool showVelocity;
+        bool showAcceleration;
+        bool showWrench;
+        bool show_seperate_axes;
+        double resolution;
+
+        QString model_path;
+
+        std::shared_ptr<osgviz::PrimitivesFactory> primitivesfactory;
+
+};
+
+}
+#endif // ROBOT_H

viz/WrenchModel.cpp

@@ -0,0 +1,136 @@
+#include "WrenchModel.hpp"
+
+WrenchModel::WrenchModel(double resolution)
+    : show_seperate_axes_force(false)
+    , show_seperate_axes_torque(false)
+    , resolution(resolution)
+{
+    primitivesfactory = osgviz::OsgViz::getModuleInstance<osgviz::PrimitivesFactory>("PrimitivesFactory");
+    if (!primitivesfactory) {
+        std::cerr << "Could not load PrimitivesFactory" << std::endl;
+        return;
+    }
+    buildGeometry();
+}
+
+osg::ref_ptr<osg::PositionAttitudeTransform> WrenchModel::createCircularArrow(const osg::Vec4f& color) {
+    osg::ref_ptr<osg::PositionAttitudeTransform> arr = primitivesfactory->createCircularArrow(0.1, 0.02, 12, 32, 1.5*M_PI, 2.0, color);
+    return arr;
+}
+
+
+void WrenchModel::buildGeometry() {
+    // force
+    force_node = primitivesfactory->createArrow(osg::Vec4f(1, 0, 0, 1.0), true);
+    addChild(force_node, true); // child 0
+
+    // torque
+    torque_group = new osg::Group;    
+
+    torque_group->addChild(primitivesfactory->createArrow(osg::Vec4f(0, 1, 0, 1.0), true));
+    torque_group->addChild(createCircularArrow(osg::Vec4f(0, 1, 0, 1.0)));
+    addChild(torque_group, true); // child 1
+
+    // seperated along axes
+    force_axes_group = new osg::Switch;
+    torque_axes_group = new osg::Switch;
+
+    for (unsigned int i=0; i<3; ++i) {
+        //add force axis
+        force_axes_group->addChild(primitivesfactory->createArrow(osg::Vec4f(1, 0, 0, 1.0), true), true);
+
+        //add torque axis
+        osg::ref_ptr<osg::Group> torque_axis_group = new osg::Group;
+        torque_axis_group->addChild(primitivesfactory->createArrow(osg::Vec4f(0, 1, 0, 1.0), true));
+        torque_axis_group->addChild(createCircularArrow(osg::Vec4f(0, 1, 0, 1.0)));
+        torque_axes_group->addChild(torque_axis_group, true);
+    }
+
+    addChild(force_axes_group, false); // child 2
+    addChild(torque_axes_group, false); // child 3
+}
+
+void WrenchModel::update(const base::Wrench& wrench) {
+    osg::Matrixd R, S;
+    osg::Quat Q;
+
+    if (show_seperate_axes_force) {
+        // for all axes
+        for (unsigned int i=0; i<3; ++i) {
+            if (wrench.force[i] == 0) {
+                // if component is zero, disable
+                force_axes_group->setValue(i, false);
+            } 
+            else {
+                //update transform
+                osg::PositionAttitudeTransform* force_transform = dynamic_cast<osg::PositionAttitudeTransform*>(force_axes_group->getChild(i));
+                osg::Vec3d force(0,0,0);
+                force[i] = wrench.force[i];
+                Q.makeRotate(osg::Vec3d(0,0,1), force);
+                force_transform->setScale(osg::Vec3f(resolution, resolution, resolution*force.length()));
+                force_transform->setAttitude(Q);
+                force_axes_group->setChildValue(force_transform, true);
+            }
+        }
+    } 
+    else {
+        if (wrench.force.isZero()) {
+            // if force is zero vector, disable
+            setChildValue(force_node, false);
+        } 
+        else {
+            // update transform
+            osg::Vec3d force(wrench.force.x(), wrench.force.y(), wrench.force.z());
+            Q.makeRotate(osg::Vec3d(0,0,1), force);
+            //force_node->setScale(osg::Vec3f(resolution, resolution, resolution*force.length()));
+
+            osgviz::ArrowNode* f_node = dynamic_cast<osgviz::ArrowNode*>(force_node.get());
+            f_node->setLength(resolution*force.length());
+            force_node->setAttitude(Q);
+
+            setChildValue(force_node, true);
+        }
+    }
+
+    if (show_seperate_axes_torque) {
+        for (unsigned int i=0; i<3; ++i) {
+            if (wrench.torque[i] == 0) {
+                torque_axes_group->setValue(i, false);
+            } 
+            else {
+                osg::Group* group = dynamic_cast<osg::Group*>(torque_axes_group->getChild(i));
+                osg::PositionAttitudeTransform* torque_vec_transform = dynamic_cast<osg::PositionAttitudeTransform*>(group->getChild(0));
+                osg::Vec3d torque(0,0,0);
+                torque[i] = wrench.torque[i];
+                Q.makeRotate(osg::Vec3d(0,0,1), torque);
+                torque_vec_transform->setScale(osg::Vec3f(resolution, resolution, resolution * torque.length()));
+                torque_vec_transform->setAttitude(Q);
+
+                osg::PositionAttitudeTransform* torque_transform = dynamic_cast<osg::PositionAttitudeTransform*>(group->getChild(1));
+                torque_transform->setAttitude(Q);
+                torque_transform->setPosition(torque * 0.4 * resolution);
+                //torque_transform->setScale(osg::Vec3f(resolution,resolution,resolution));
+                torque_axes_group->setValue(i, true);
+            }
+        }
+    } 
+    else {
+        if (wrench.torque.isZero()) {
+            // if torque is zero vector, disable
+            setChildValue(torque_group, false);
+        }
+        else {
+            // update transform
+            osgviz::ArrowNode* torque_vec_transform = dynamic_cast<osgviz::ArrowNode*>(torque_group->getChild(0));
+            osg::Vec3d torque(wrench.torque.x(), wrench.torque.y(), wrench.torque.z());
+            Q.makeRotate(osg::Vec3d(0,0,1), torque);
+            torque_vec_transform->setLength(resolution*torque.length());
+            torque_vec_transform->setAttitude(Q);
+
+            osg::PositionAttitudeTransform* torque_transform = dynamic_cast<osg::PositionAttitudeTransform*>(torque_group->getChild(1));
+            torque_transform->setAttitude(Q);
+            torque_transform->setPosition(torque * 0.4 * resolution);
+            setChildValue(torque_group, true);
+        }        
+    }
+}

viz/WrenchModel.hpp

@@ -0,0 +1,58 @@
+#ifndef WRENCHMODEL_HPP
+#define WRENCHMODEL_HPP
+
+#include <osg/Switch>
+#include <base/samples/Wrench.hpp>
+
+#include <osgViz/OsgViz.hpp>
+#include <osgViz/modules/viz/Primitives/PrimitivesFactory.h>
+#include <osgViz/modules/viz/Primitives/Primitives/ArrowNode.h>
+
+class WrenchModel : public osg::Switch {
+
+public:
+    WrenchModel(double resolution = 1.0);
+    ~WrenchModel() {}
+
+    void update(const base::Wrench& wrench);
+
+    void seperateAxes(bool val = true) {
+        seperateAxesForce(val);
+        seperateAxesTorque(val);
+    }
+    void seperateAxesForce(bool val=true) {
+        show_seperate_axes_force = val;
+        setChildValue(force_node, !val);
+        setChildValue(force_axes_group, val);
+    }
+    void seperateAxesTorque(bool val=true) {
+        show_seperate_axes_torque = val;
+        setChildValue(torque_group, !val);
+        setChildValue(torque_axes_group, val);
+    }
+    void setResolution(double res) {
+        resolution = res;
+    }
+
+protected:
+    //osg::ref_ptr<osg::Switch> model;
+
+    osg::ref_ptr<osg::PositionAttitudeTransform> createCircularArrow(const osg::Vec4f& color);
+    void buildGeometry();
+
+    bool show_seperate_axes_force;
+    bool show_seperate_axes_torque;
+    double resolution;
+
+    osg::ref_ptr<osgviz::Object> force_node;
+    osg::ref_ptr<osg::Group> torque_group;
+    osg::ref_ptr<osg::Switch> force_axes_group, torque_axes_group;
+
+    std::shared_ptr<osgviz::PrimitivesFactory> primitivesfactory;
+
+private:
+
+
+};
+
+#endif

viz/WrenchVisualization.cpp

@@ -0,0 +1,62 @@
+#include <iostream>
+#include "WrenchVisualization.hpp"
+
+using namespace vizkit3d;
+
+WrenchVisualization::WrenchVisualization()
+    : text_size(0.1)
+    , show_seperate_axes_force(false)
+    , show_seperate_axes_torque(false)
+    , resolution(1.0)
+{
+    wrench_model = new WrenchModel(resolution);
+}
+
+WrenchVisualization::~WrenchVisualization()
+{
+}
+
+osg::ref_ptr<osg::Node> WrenchVisualization::createMainNode()
+{
+    return wrench_model;
+}
+
+double WrenchVisualization::getTextSize() const
+{
+    return text_size;
+}
+
+void WrenchVisualization::setTextSize(double size)
+{
+    text_size = size;
+    text->setCharacterSize(text_size);
+    emit propertyChanged("textSize");
+    setDirty();
+}
+
+void WrenchVisualization::updateMainNode ( osg::Node* node )
+{
+    // WrenchModel* wrench_model = dynamic_cast<WrenchModel*>(node);
+    // if (wrench_model) {
+    //     wrench_model->seperateAxes(show_seperate_axes);
+    //     wrench_model->setResolution(resolution);
+    //     wrench_model->update(state);
+    // }
+    if (state.isValid())
+        wrench_model->update(state);
+}
+
+void WrenchVisualization::updateDataIntern(const base::Wrench& value)
+{
+    state.force = value.force;
+    state.torque = value.torque;
+}
+
+void WrenchVisualization::updateDataIntern(const base::samples::Wrench& value)
+{
+    state = value;
+}
+
+//Macro that makes this plugin loadable in ruby, this is optional.
+//VizkitQtPlugin(WrenchVisualization)
+

viz/WrenchVisualization.hpp

@@ -0,0 +1,104 @@
+#ifndef teleop_mapping_WrenchVisualization_H
+#define teleop_mapping_WrenchVisualization_H
+
+#include <boost/noncopyable.hpp>
+#include <vizkit3d/Vizkit3DPlugin.hpp>
+#include <osg/Geode>
+#include <base/samples/Wrench.hpp>
+#include <osgText/Text>
+#include <osg/Switch>
+#include <osg/AnimationPath>
+#include "WrenchModel.hpp"
+
+namespace vizkit3d
+{
+    class WrenchVisualization
+        : public vizkit3d::Vizkit3DPlugin<base::samples::Wrench>
+        , public vizkit3d::VizPluginAddType<base::Wrench>
+    {
+    Q_OBJECT
+    Q_PROPERTY(double textSize READ getTextSize WRITE setTextSize)
+    Q_PROPERTY(bool showForceSeperateAxes READ isSeperateAxesForce WRITE setSeperateAxesForce)
+    Q_PROPERTY(bool showTorqueSeperateAxes READ isSeperateAxesTorque WRITE setSeperateAxesTorque)
+    Q_PROPERTY(double resolution READ getResolution WRITE setResolution)
+    public:
+        WrenchVisualization();
+        ~WrenchVisualization();
+
+        Q_INVOKABLE void updateData(base::samples::Wrench const &sample)
+        {vizkit3d::Vizkit3DPlugin<base::samples::Wrench>::updateData(sample);}
+
+        Q_INVOKABLE void updateData(base::Wrench const &sample)
+        {vizkit3d::Vizkit3DPlugin<base::samples::Wrench>::updateData(sample);}
+
+        Q_INVOKABLE void setForce(const base::Vector3d& f) {
+            state.force = f;
+        }
+
+        Q_INVOKABLE void setTorque(const base::Vector3d& t) {
+            state.torque = t;
+        }
+
+    public slots: 
+        /** Sets the text size relative to the size of the complete object.
+         * If text size is positive, the name of the source frame is rendered in the visualization.
+         * The default is 0.0
+         */
+        void setTextSize(double size);
+        double getTextSize() const;
+
+        void setSeperateAxesForce(bool val = true) {
+            show_seperate_axes_force = val;
+            emit propertyChanged("showForceSeperateAxes");
+            wrench_model->seperateAxesForce(val);
+            setDirty();
+        }
+        bool isSeperateAxesForce() const {
+            return show_seperate_axes_force;
+        }
+        void setSeperateAxesTorque(bool val = true) {
+            show_seperate_axes_torque = val;
+            emit propertyChanged("showTorqueSeperateAxes");
+            wrench_model->seperateAxesTorque(val);            
+            setDirty();
+        }
+        bool isSeperateAxesTorque() const {
+            return show_seperate_axes_torque;
+        }
+        void setResolution(double res) {
+            resolution = res;
+            emit propertyChanged("resolution");
+            wrench_model->setResolution(res);
+            setDirty();
+        }
+        double getResolution() const {
+            return resolution;
+        }
+
+    protected:
+        virtual osg::ref_ptr<osg::Node> createMainNode();
+        virtual void updateMainNode(osg::Node* node);
+        virtual void updateDataIntern( const base::Wrench& wrench );
+        virtual void updateDataIntern( const base::samples::Wrench& wrench );
+
+    private:
+        base::samples::Wrench state;
+        osg::ref_ptr<WrenchModel> wrench_model;
+        osg::ref_ptr<osgText::Text> text;
+        double text_size;
+        bool show_seperate_axes_force;
+        bool show_seperate_axes_torque;
+        double resolution;
+
+    // public slots: 
+    //     void setForce(const base::Vector3d& f) {
+    //         state.force = f;
+    //     }
+
+    //     void setTorque(const base::Vector3d& t) {
+    //         state.torque = t;
+    //     }
+
+    };
+}
+#endif

viz/vizkit_plugin.rb

@@ -29,3 +29,9 @@
 Vizkit::UiLoader.register_3d_plugin_for('DepthMapVisualization', "/base/samples/DepthMap", :updateDepthMap)
 Vizkit::UiLoader.register_3d_plugin('DistanceImageVisualization', "base", 'DistanceImageVisualization')
 Vizkit::UiLoader.register_3d_plugin_for('DistanceImageVisualization', "/base/samples/DistanceImage", :updateDistanceImage)
+Vizkit::UiLoader.register_3d_plugin('WrenchVisualization', 'base', 'WrenchVisualization')
+Vizkit::UiLoader.register_3d_plugin_for('WrenchVisualization', "/base/Wrench", :updateData )
+Vizkit::UiLoader.register_3d_plugin_for('WrenchVisualization', "/base/samples/Wrench", :updateData )
+Vizkit::UiLoader.register_3d_plugin('RigidBodyStateSE3Visualization',"base", 'RigidBodyStateSE3Visualization')
+Vizkit::UiLoader.register_3d_plugin_for('RigidBodyStateSE3Visualization', "/base/samples/RigidBodyStateSE3", :updateRigidBodyStateSE3)
+Vizkit::UiLoader.register_3d_plugin_for('RigidBodyStateSE3Visualization', "/base/samples/RigidBodyStateSE3", :updateData)