diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.cpp
index 156c482c4db8f9825ed9c66e3413f1c3633a5ad9..94c7b9d8481c0a58a0e9de33cadabdc828e53a75 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.cpp
@@ -87,6 +87,9 @@ namespace armarx
             throwIfInControlThread(BOOST_CURRENT_FUNCTION);
             std::lock_guard<std::mutex> guard {robotMutex};
             ARMARX_CHECK_IS_NULL(robot);
+
+            std::string robotName  = getProperty<std::string>("RobotName").getValue();
+
             robotNodeSetName    = getProperty<std::string>("RobotNodeSetName").getValue();
             robotProjectName    = getProperty<std::string>("RobotFileNameProject").getValue();
             robotFileName       = getProperty<std::string>("RobotFileName").getValue();
@@ -114,6 +117,14 @@ namespace armarx
                 try
                 {
                     robot = VirtualRobot::RobotIO::loadRobot(robotFileName, VirtualRobot::RobotIO::eFull);
+                    if (robotName.empty())
+                    {
+                        robotName = robot->getName();
+                    }
+                    else
+                    {
+                        robot->setName(robotName);
+                    }
                 }
                 catch (VirtualRobot::VirtualRobotException& e)
                 {
@@ -121,6 +132,7 @@ namespace armarx
                 }
                 ARMARX_INFO << "Loaded robot:"
                             << "\n\tProject      : " << robotProjectName
+                            << "\n\tName         : " << robotName
                             << "\n\tRobot file   : " << robotFileName
                             << "\n\tRobotNodeSet : " << robotNodeSetName
                             << "\n\tNodeNames    : " << robot->getRobotNodeSet(robotNodeSetName)->getNodeNames();
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.h
index a9313f9ccb587cf7c2c6294cea86960059f9eae7..722f0cbe7f12863081e27c2f6db8989ce46a6e85 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.h
@@ -47,6 +47,9 @@ namespace armarx
                     "RobotFileNameProject", "",
                     "Project in which the robot filename is located (if robot is loaded from an external project)");
 
+                defineOptionalProperty<std::string>(
+                    "RobotName", "",
+                    "Override robot name if you want to load multiple robots of the same type");
                 defineOptionalProperty<std::string>(
                     "RobotNodeSetName", "Robot",
                     "Robot node set name as defined in robot xml file, e.g. 'LeftArm'");
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp
index 600fa858c36e021b82b8ad49275997e5070707c3..11804dd583bafc6a6a27ed27b3dfc3a78bb479b5 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp
@@ -233,7 +233,7 @@ namespace armarx
             config->deviceName=controlDeviceName;                                                                       \
             const NJointControllerPtr& nJointCtrl = _module<ControllerManagement>().createNJointController(                                                       \
                                                     "NJointKinematicUnitPassThroughController",                                   \
-                                                    "NJointKU_PTCtrl_"+controlDeviceName+"_"+controlMode,                         \
+                                                    getName() + "_" + "NJointKU_PTCtrl_"+controlDeviceName+"_"+controlMode,                         \
                                                     config,                                                                     \
                                                     false, true);                                                               \
             pt = NJointKinematicUnitPassThroughControllerPtr::dynamicCast(nJointCtrl);                                          \
@@ -328,7 +328,8 @@ namespace armarx
             config->platformName = _module<RobotData>().getRobotPlatformName();
             auto ctrl = NJointHolonomicPlatformUnitVelocityPassThroughControllerPtr::dynamicCast(
                             _module<ControllerManagement>().createNJointController(
-                                "NJointHolonomicPlatformUnitVelocityPassThroughController", configName + "_VelPTContoller",
+                                "NJointHolonomicPlatformUnitVelocityPassThroughController",
+                                getName() + "_" + configName + "_VelPTContoller",
                                 config, false, true
                             )
                         );
@@ -339,7 +340,8 @@ namespace armarx
             configRelativePositionCtrlCfg->platformName = _module<RobotData>().getRobotPlatformName();
             auto ctrlRelativePosition = NJointHolonomicPlatformRelativePositionControllerPtr::dynamicCast(
                                             _module<ControllerManagement>().createNJointController(
-                                                "NJointHolonomicPlatformRelativePositionController", configName + "_RelativePositionContoller",
+                                                "NJointHolonomicPlatformRelativePositionController",
+                                                getName() + "_" + configName + "_RelativePositionContoller",
                                                 configRelativePositionCtrlCfg, false, true
                                             )
                                         );
diff --git a/source/RobotAPI/libraries/core/CartesianPositionController.cpp b/source/RobotAPI/libraries/core/CartesianPositionController.cpp
index 3ffd160461bef2710fc8041f66cf3d8588819fd4..4559e139c7d93e803f1ff7d070a77698396863a0 100644
--- a/source/RobotAPI/libraries/core/CartesianPositionController.cpp
+++ b/source/RobotAPI/libraries/core/CartesianPositionController.cpp
@@ -41,7 +41,8 @@ Eigen::VectorXf CartesianPositionController::calculate(const Eigen::Matrix4f& ta
     if (posLen)
     {
         Eigen::Vector3f targetPos = targetPose.block<3, 1>(0, 3);
-        Eigen::Vector3f errPos = targetPos - tcp->getPositionInRootFrame();
+        Eigen::Vector3f currentPos = tcp->getPositionInRootFrame();
+        Eigen::Vector3f errPos = targetPos - currentPos;
         Eigen::Vector3f posVel =  errPos * KpPos;
         if (maxPosVel > 0)
         {
diff --git a/source/RobotAPI/libraries/core/CartesianVelocityController.cpp b/source/RobotAPI/libraries/core/CartesianVelocityController.cpp
index 40dad31076f78f8753bfee2599e0649cb7dba6f1..3ec8849c620e0abbc05a548f625a771b76839199 100644
--- a/source/RobotAPI/libraries/core/CartesianVelocityController.cpp
+++ b/source/RobotAPI/libraries/core/CartesianVelocityController.cpp
@@ -69,7 +69,12 @@ Eigen::VectorXf CartesianVelocityController::calculate(const Eigen::VectorXf& ca
     Eigen::VectorXf nsv = Eigen::VectorXf::Zero(nullspace.rows());
     for (int i = 0; i < nullspace.cols(); i++)
     {
-        nsv += nullspace.col(i) * nullspace.col(i).dot(nullspaceVel) / nullspace.col(i).squaredNorm();
+        float squaredNorm = nullspace.col(i).squaredNorm();
+        // Prevent division by zero
+        if (squaredNorm > 1.0e-32f)
+        {
+            nsv += nullspace.col(i) * nullspace.col(i).dot(nullspaceVel) / nullspace.col(i).squaredNorm();
+        }
     }
 
     //    Eigen::VectorXf nsv = nullspace * nullspaceVel;
@@ -178,7 +183,7 @@ bool CartesianVelocityController::adjustJacobiForJointsAtJointLimits(VirtualRobo
 
         if (node->isLimitless() || // limitless joint cannot be out of limits
             std::abs(jointVel(i)) < 0.001f // If it the jacobi doesnt want this joint to move anyway, there is no point in recalculating the inverse jacobi
-            )
+           )
         {
             continue;
         }