KinematicUnitGui: if switching to velocity control mode, setting velocity to zero

source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp

@@ -678,10 +678,15 @@ void KinematicUnitWidgetController::setControlModeVelocity()
         return;
     }
     NameControlModeMap jointModes;
+    NameValueMap jointVelocities;
 
     if (currentNode)
     {
         jointModes[currentNode->getName()] = eVelocityControl;
+
+        // set the velocity to zero to stop any previous controller (e.g. torque controller)
+        jointVelocities[currentNode->getName()] = 0;
+
         const bool isDeg = ui.checkBoxUseDegree->isChecked();
         const bool isRot = currentNode->isRotationalJoint();
         QString unit = isRot ?
@@ -697,6 +702,7 @@ void KinematicUnitWidgetController::setControlModeVelocity()
             if (kinematicUnitInterfacePrx)
             {
                 kinematicUnitInterfacePrx->switchControlMode(jointModes);
+                kinematicUnitInterfacePrx->setJointVelocities(jointVelocities);
             }
         }
         catch (...)