diff --git a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp
index 376606c4bb3e7636ad1af468f71ac95f69ac5a55..e26ab9226cd947bbd8a154ebc2be55a6bf7b34ab 100644
--- a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp
+++ b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp
@@ -37,6 +37,7 @@
 #include <Inventor/nodes/SoComplexity.h>
 #include <Inventor/nodes/SoCoordinate3.h>
 #include <Inventor/nodes/SoPointSet.h>
+#include <Inventor/nodes/SoTranslation.h>
 #include <Core/core/system/ArmarXDataPath.h>
 #include <Core/core/system/cmake/CMakePackageFinder.h>
 
@@ -373,6 +374,52 @@ void DebugDrawerComponent::drawPointCloud(const PointCloudData &d)
     layer.mainNode->addChild(sep);
 }
 
+void DebugDrawerComponent::drawColoredPointCloud(const ColoredPointCloudData &d)
+{
+    ScopedRecursiveLockPtr l = getScopedVisuLock();
+
+    auto& layer=requestLayer(d.layerName);
+
+    removeColoredPointCloud(d.layerName,d.name);
+
+    if (!d.active)
+        return;
+
+    SoSeparator *sep = new SoSeparator;
+    int sz = d.pointCloud.size();
+
+    SoSeparator* points = new SoSeparator;
+    sep->addChild(points);
+    SoSphere* sphere = new SoSphere;
+    sphere->radius = 25;
+    for(int i = 0; i < sz; i++)
+    {
+        SoSeparator* point = new SoSeparator;
+        points->addChild(point);
+
+        const DebugDrawerColoredPointCloudElement &e = d.pointCloud[i];
+
+        SoTranslation* trans = new SoTranslation;
+        trans->translation.setValue(e.x, e.y, e.z);
+        point->addChild(trans);
+
+        SoMaterial *material = new SoMaterial;
+        material->ambientColor.setValue(e.color.r, e.color.g, e.color.b);
+        material->diffuseColor.setValue(e.color.r, e.color.g, e.color.b);
+        material->transparency.setValue(1 - e.color.a);
+        point->addChild(material);
+
+
+
+        point->addChild(sphere);
+    }
+
+    ARMARX_INFO << d.name << "PointCloud Update " << sz;
+
+    layer.addedColoredPointCloudVisualizations[d.name] = sep;
+    layer.mainNode->addChild(sep);
+}
+
 void DebugDrawerComponent::drawPolygon(const DebugDrawerComponent::PolygonData &d)
 {
     ScopedRecursiveLockPtr l = getScopedVisuLock();
@@ -639,6 +686,23 @@ void DebugDrawerComponent::removePointCloud(const std::string &layerName, const
     layer.addedPointCloudVisualizations.erase(name);
 }
 
+void DebugDrawerComponent::removeColoredPointCloud(const std::string &layerName, const std::string &name)
+{
+    ScopedRecursiveLockPtr l = getScopedVisuLock();
+    if(!hasLayer(layerName))
+    {
+        return;
+    }
+    auto& layer=layers.at(layerName);
+    if(layer.addedColoredPointCloudVisualizations.find(name) == layer.addedColoredPointCloudVisualizations.end())
+    {
+        return;
+    }
+
+    layer.mainNode->removeChild(layer.addedColoredPointCloudVisualizations[name]);
+    layer.addedColoredPointCloudVisualizations.erase(name);
+}
+
 void DebugDrawerComponent::removePolygon(const std::string &layerName, const std::string &name)
 {
     ScopedRecursiveLockPtr l = getScopedVisuLock();
@@ -987,6 +1051,24 @@ void DebugDrawerComponent::removeSphereDebugLayerVisu(const std::string &sphereN
     removeSphereVisu(DEBUG_LAYER_NAME, sphereName);
 }
 
+void DebugDrawerComponent::setColoredPointCloudVisu(const std::string &layerName, const std::string &pointCloudName, const DebugDrawerColoredPointCloud &pointCloud, const Ice::Current &)
+{
+    {
+        ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+        std::string entryName = "__" + layerName + "__" + pointCloudName + "__";
+        ColoredPointCloudData &d = accumulatedUpdateData.coloredpointcloud[entryName];
+        d.pointCloud = pointCloud;
+        d.layerName = layerName;
+        d.name = pointCloudName;
+        d.active = true;
+    }
+}
+
+void DebugDrawerComponent::setColoredPointCloudDebugLayerVisu(const std::string &pointCloudName, const DebugDrawerColoredPointCloud &pointCloud, const Ice::Current &)
+{
+    setColoredPointCloudVisu(DEBUG_LAYER_NAME, pointCloudName, pointCloud);
+}
+
 void DebugDrawerComponent::setPointCloudVisu(const std::string &layerName, const std::string &pointCloudName, const DebugDrawerPointCloud &pointCloud, const Ice::Current &)
 {
     {
@@ -1005,6 +1087,23 @@ void DebugDrawerComponent::setPointCloudDebugLayerVisu(const std::string &pointC
     setPointCloudVisu(DEBUG_LAYER_NAME, pointCloudName, pointCloud);
 }
 
+void DebugDrawerComponent::removeColoredPointCloudVisu(const std::string &layerName, const std::string &pointCloudName, const Ice::Current &)
+{
+    {
+        ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+        std::string entryName = "__" + layerName + "__" + pointCloudName + "__";
+        ColoredPointCloudData &d = accumulatedUpdateData.coloredpointcloud[entryName];
+        d.layerName = layerName;
+        d.name = pointCloudName;
+        d.active = false;
+    }
+}
+
+void DebugDrawerComponent::removeColoredPointCloudDebugLayerVisu(const std::string &pointCloudName, const Ice::Current &)
+{
+    removeColoredPointCloudVisu(DEBUG_LAYER_NAME, pointCloudName);
+}
+
 void DebugDrawerComponent::removePointCloudVisu(const std::string &layerName, const std::string &pointCloudName, const Ice::Current &)
 {
     {
@@ -1356,6 +1455,12 @@ void DebugDrawerComponent::updateVisualization()
     }
     accumulatedUpdateData.pointcloud.clear();
 
+    for (auto i = accumulatedUpdateData.coloredpointcloud.begin(); i != accumulatedUpdateData.coloredpointcloud.end(); i++)
+    {
+        drawColoredPointCloud(i->second);
+    }
+    accumulatedUpdateData.coloredpointcloud.clear();
+
     for (auto i = accumulatedUpdateData.polygons.begin(); i != accumulatedUpdateData.polygons.end(); i++)
     {
         drawPolygon(i->second);
diff --git a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.h b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.h
index 1993abcd7e8377e2843867a83e64ebedd834ade1..81da38139b674fe14e56e0ced83c88f348319527 100644
--- a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.h
+++ b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.h
@@ -171,6 +171,11 @@ public:
     virtual void removePointCloudVisu(const std::string &layerName, const std::string &pointCloudName, const ::Ice::Current& = ::Ice::Current());
     virtual void removePointCloudDebugLayerVisu(const std::string& pointCloudName, const ::Ice::Current& = ::Ice::Current());
 
+    virtual void setColoredPointCloudDebugLayerVisu(const std::string &pointCloudName, const DebugDrawerColoredPointCloud &pointCloud, const ::Ice::Current& = ::Ice::Current());
+    virtual void setColoredPointCloudVisu(const std::string &layerName, const std::string &pointCloudName, const DebugDrawerColoredPointCloud &pointCloud, const ::Ice::Current& = ::Ice::Current());
+    virtual void removeColoredPointCloudVisu(const std::string &layerName, const std::string &pointCloudName, const ::Ice::Current& = ::Ice::Current());
+    virtual void removeColoredPointCloudDebugLayerVisu(const std::string &pointCloudName, const ::Ice::Current& = ::Ice::Current());
+
     virtual void setPolygonVisu(const std::string &layerName, const std::string &polygonName, const std::vector< ::armarx::Vector3BasePtr > &polygonPoints, const DrawColor &colorInner, const DrawColor &colorBorder, float lineWidth, const ::Ice::Current& = ::Ice::Current());
     virtual void setPolygonDebugLayerVisu(const std::string &polygonName, const std::vector< ::armarx::Vector3BasePtr > &polygonPoints, const DrawColor &colorInner, const DrawColor &colorBorder, float lineWidth, const ::Ice::Current& = ::Ice::Current());
     virtual void removePolygonVisu(const std::string &layerName, const std::string &polygonName, const ::Ice::Current& = ::Ice::Current());
@@ -286,6 +291,10 @@ protected:
     {
         DebugDrawerPointCloud pointCloud;
     };
+    struct ColoredPointCloudData : public DrawData
+    {
+        DebugDrawerColoredPointCloud pointCloud;
+    };
     struct PolygonData : public DrawData
     {
         std::vector< Eigen::Vector3f > points;
@@ -325,6 +334,7 @@ protected:
         std::map<std::string, TextData> text;
         std::map<std::string, SphereData> sphere;
         std::map<std::string, PointCloudData> pointcloud;
+        std::map<std::string, ColoredPointCloudData> coloredpointcloud;
         std::map<std::string, PolygonData> polygons;
         std::map<std::string, ArrowData> arrows;
         std::map<std::string, RobotData> robots;
@@ -349,6 +359,7 @@ protected:
     void drawText(const TextData &d);
     void drawSphere(const SphereData &d);
     void drawPointCloud(const PointCloudData &d);
+    void drawColoredPointCloud(const ColoredPointCloudData &d);
     void drawPolygon(const PolygonData &d);
     void drawArrow(const ArrowData &d);
     void drawRobot(const RobotData &d);
@@ -359,6 +370,7 @@ protected:
     void removeText(const std::string& layerName, const std::string &name);
     void removeSphere(const std::string& layerName, const std::string &name);
     void removePointCloud(const std::string& layerName, const std::string &name);
+    void removeColoredPointCloud(const std::string& layerName, const std::string &name);
     void removePolygon(const std::string& layerName, const std::string &name);
     void removeArrow(const std::string& layerName, const std::string &name);
     void removeRobot(const std::string& layerName, const std::string &name);
@@ -377,6 +389,7 @@ protected:
         std::map<std::string, SoSeparator*> addedTextVisualizations;
         std::map<std::string, SoSeparator*> addedSphereVisualizations;
         std::map<std::string, SoSeparator*> addedPointCloudVisualizations;
+        std::map<std::string, SoSeparator*> addedColoredPointCloudVisualizations;
         std::map<std::string, SoSeparator*> addedPolygonVisualizations;
         std::map<std::string, SoSeparator*> addedArrowVisualizations;
         std::map<std::string, SoSeparator*> addedRobotVisualizations;
diff --git a/source/RobotAPI/components/units/TCPControlUnit.cpp b/source/RobotAPI/components/units/TCPControlUnit.cpp
index 4cfa4a860688efdbb93df9fffa2071ab9f246ae2..c974eed9eba28859b92f16bb32c5e3e705381f9d 100644
--- a/source/RobotAPI/components/units/TCPControlUnit.cpp
+++ b/source/RobotAPI/components/units/TCPControlUnit.cpp
@@ -347,8 +347,10 @@ namespace armarx
                                     * Eigen::AngleAxisf(data.orientationVelocity->x*cycleTime*0.001, Eigen::Vector3f::UnitX());
                 if (data.orientationVelocity->frame != refFrame)
                 {
-                    Eigen::Matrix4f trafoOriginalFrameToGlobal = localRobot->getRobotNode(data.orientationVelocity->frame)->getGlobalPose();
-                    Eigen::Matrix4f trafoRefFrameToGlobal = localRobot->getRobotNode(data.orientationVelocity->frame)->getGlobalPose();
+                    Eigen::Matrix4f trafoOriginalFrameToGlobal = Eigen::Matrix4f::Identity();
+                    if (data.orientationVelocity->frame != GlobalFrame) trafoOriginalFrameToGlobal = localRobot->getRobotNode(data.orientationVelocity->frame)->getGlobalPose();
+                    Eigen::Matrix4f trafoRefFrameToGlobal = Eigen::Matrix4f::Identity();
+                    if (refFrame != GlobalFrame) trafoRefFrameToGlobal = localRobot->getRobotNode(refFrame)->getGlobalPose();
                     Eigen::Matrix4f trafoOriginalToRef = trafoRefFrameToGlobal.inverse() * trafoOriginalFrameToGlobal;
                     rotInRefFrame = trafoOriginalToRef.block<3,3>(0,0) * rotInOriginalFrame * trafoOriginalToRef.block<3,3>(0,0).inverse();
                 }
diff --git a/source/RobotAPI/interface/visualization/DebugDrawerInterface.ice b/source/RobotAPI/interface/visualization/DebugDrawerInterface.ice
index e35b44c532771dfcc7d16cabb4bb12d90f7cec88..657b41aacd2dd7725e399bca9124e476228ebe66 100644
--- a/source/RobotAPI/interface/visualization/DebugDrawerInterface.ice
+++ b/source/RobotAPI/interface/visualization/DebugDrawerInterface.ice
@@ -65,6 +65,18 @@ module armarx
     };
     sequence<DebugDrawerPointCloudElement> DebugDrawerPointCloud;
 
+    const int three = 3;
+
+    struct DebugDrawerColoredPointCloudElement
+    {
+        float x;
+        float y;
+        float z;
+        DrawColor color;
+        float size = three;
+    };
+    sequence<DebugDrawerColoredPointCloudElement> DebugDrawerColoredPointCloud;
+
     sequence< Vector3Base > PolygonPointList;
 
     /*!
@@ -88,6 +100,7 @@ module armarx
         void setTextVisu(string layerName, string textName, string text, Vector3Base globalPosition, DrawColor color, int size);
         void setSphereVisu(string layerName, string sphereName, Vector3Base globalPosition, DrawColor color, float radius);
         void setPointCloudVisu(string layerName, string pointCloudName, DebugDrawerPointCloud pointCloud);
+        void setColoredPointCloudVisu(string layerName, string pointCloudName, DebugDrawerColoredPointCloud pointCloud);
         void setPolygonVisu(string layerName, string polygonName, PolygonPointList polygonPoints, DrawColor colorInner, DrawColor colorBorder, float lineWidth);
         void setArrowVisu(string layerName, string arrowName, Vector3Base position, Vector3Base direction, DrawColor color, float length, float width);
         
@@ -122,6 +135,7 @@ module armarx
         void setTextDebugLayerVisu(string textName, string text, Vector3Base globalPosition, DrawColor color, int size);
         void setSphereDebugLayerVisu(string sphereName, Vector3Base globalPosition, DrawColor color, float radius);
         void setPointCloudDebugLayerVisu(string pointCloudName, DebugDrawerPointCloud pointCloud);
+        void setColoredPointCloudDebugLayerVisu(string pointCloudName, DebugDrawerColoredPointCloud pointCloud);
         void setPolygonDebugLayerVisu(string polygonName, PolygonPointList polygonPoints, DrawColor colorInner, DrawColor colorBorder, float lineWidth);
         void setArrowDebugLayerVisu(string arrowName, Vector3Base position, Vector3Base direction, DrawColor color, float length, float width);
 
@@ -136,6 +150,7 @@ module armarx
         void removeTextVisu(string layerName, string textName);
         void removeSphereVisu(string layerName, string sphereName);
         void removePointCloudVisu(string layerName, string pointCloudName);
+        void removeColoredPointCloudVisu(string layerName, string pointCloudName);
         void removePolygonVisu(string layerName, string polygonName);
         void removeArrowVisu(string layerName, string arrowName);
 
@@ -148,6 +163,7 @@ module armarx
         void removeTextDebugLayerVisu(string textName);
         void removeSphereDebugLayerVisu(string sphereName);
         void removePointCloudDebugLayerVisu(string pointCloudName);
+        void removeColoredPointCloudDebugLayerVisu(string pointCloudName);
         void removePolygonDebugLayerVisu(string polygonName);
         void removeArrowDebugLayerVisu(string arrowName);