Platform Unit Gui sends now only velocities directly instead of calculating...

Platform Unit Gui sends now only velocities directly instead of calculating new positions from velocity

source/RobotAPI/gui_plugins/PlatformUnitPlugin/JoystickControlWidget.cpp

@@ -173,8 +173,10 @@ namespace armarx
             }
         }
 
+
+
         //set position
-        nibble->setPos( pos);
+        nibble->setPos( pos );
 
         //flip y (from downwards to upwards)
         pos.setY(-pos.y());
@@ -183,6 +185,8 @@ namespace armarx
         emit positionChanged(pos);
 
         emit rotationChanged(getRotation());
+
+        emit changed(pos, getRotation());
     }
 
     void JoystickControlWidget::resizeEvent(QResizeEvent*)

source/RobotAPI/gui_plugins/PlatformUnitPlugin/JoystickControlWidget.h

@@ -163,6 +163,11 @@ namespace armarx
              */
             void rotationChanged(double);
 
+            /**
+             * @brief Passes the position and angle of the control in polar coordinates (-pi,pi].
+             */
+            void changed(QPointF, double);
+
             /**
              * @brief Emitted when the nibble was released.
              */

source/RobotAPI/gui_plugins/PlatformUnitPlugin/PlatformUnitGuiPlugin.cpp

@@ -36,19 +36,16 @@ PlatformUnitWidget::PlatformUnitWidget() :
     //init joystick controls
     std::unique_ptr<JoystickControlWidget> speed{new JoystickControlWidget{}};
     speedCtrl = speed.get();
-    speedCtrl->setSteps(5);
+    speedCtrl->setSteps(0);
     //use upper semicircle for rotation
     std::unique_ptr<JoystickControlWidget> rotat{new JoystickControlWidget{false}};
     rotaCtrl = rotat.get();
-    rotaCtrl->setSteps(1);
+    rotaCtrl->setSteps(0);
     //add joystick controls
     ui.verticalLayoutRotation->addWidget(new QLabel{"Rotation"});
     ui.verticalLayoutRotation->addWidget(rotat.release());
     ui.verticalLayoutSpeed->addWidget(new QLabel{"Direction"});
     ui.verticalLayoutSpeed->addWidget(speed.release());
-    //set up the timer
-    ctrlEvaluationTimer.setSingleShot(false);
-    ctrlEvaluationTimer.start(CONTROL_TICK_RATE);//tickrate in ms
 }
 
 
@@ -63,6 +60,17 @@ void PlatformUnitWidget::onConnectComponent()
 {
     connectSlots();
     platformUnitProxy = getProxy<PlatformUnitInterfacePrx>(platformUnitProxyName);
+
+    //set up the timer
+    ctrlEvaluationTimer.setSingleShot(false);
+    ctrlEvaluationTimer.start(CONTROL_TICK_RATE);//tickrate in ms
+
+}
+
+void PlatformUnitWidget::onDisconnectComponent()
+{
+    ctrlEvaluationTimer.stop();
+
 }
 
 void PlatformUnitWidget::onExitComponent()
@@ -104,6 +112,8 @@ void PlatformUnitWidget::connectSlots()
 {
     connect(ui.buttonMoveToPosition, SIGNAL(clicked()), this, SLOT(moveTo()));
     connect(&ctrlEvaluationTimer, SIGNAL(timeout()), this, SLOT(controlTimerTick()));
+//    connect(speedCtrl, SIGNAL(positionChanged(QPointF)), SLOT(moveTranslational(QPointF)));
+//    connect(rotaCtrl, SIGNAL(rotationChanged(double)), SLOT(rotate(double)));
     connect(speedCtrl,SIGNAL(released()), this, SLOT(stopPlatform()));
     connect(rotaCtrl, SIGNAL(released()), this, SLOT(stopPlatform()));
 }
@@ -119,19 +129,35 @@ void PlatformUnitWidget::moveTo()
     platformUnitProxy->moveTo(positionX, positionY, rotation, posAcc, rotAcc);
 }
 
+void PlatformUnitWidget::setNewPlatformPoseLabels(float x, float y, float alpha)
+{
+    ui.labelCurrentPositionX->setText(QString::number(x));
+    ui.labelCurrentPositionY->setText(QString::number(y));
+    ui.labelCurrentRotation->setText(QString::number(alpha));
+
+}
+
+void PlatformUnitWidget::setNewTargetPoseLabels(float x, float y, float alpha)
+{
+    ui.editTargetPositionX->setText(QString::number(x));
+    ui.editTargetPositionY->setText(QString::number(y));
+    ui.editTargetRotation->setText(QString::number(alpha));
+}
+
 void PlatformUnitWidget::reportPlatformPose(::Ice::Float currentPlatformPositionX, ::Ice::Float currentPlatformPositionY, ::Ice::Float currentPlatformRotation, const Ice::Current& c)
 {
-    ui.labelCurrentPositionX->setText(QString::number(currentPlatformPositionX));
-    ui.labelCurrentPositionY->setText(QString::number(currentPlatformPositionY));
-    ui.labelCurrentRotation->setText(QString::number(currentPlatformRotation));
-    platformRotation =currentPlatformRotation;
+    // moved to qt thread for thread safety
+    QMetaObject::invokeMethod(this, "setNewPlatformPoseLabels", Q_ARG(float, currentPlatformPositionX), Q_ARG(float, currentPlatformPositionY), Q_ARG(float, currentPlatformRotation));
+    platformRotation = currentPlatformRotation;
 }
 
 void PlatformUnitWidget::reportNewTargetPose(::Ice::Float newPlatformPositionX, ::Ice::Float newPlatformPositionY, ::Ice::Float newPlatformRotation, const Ice::Current& c)
 {
-    ui.editTargetPositionX->setText(QString::number(newPlatformPositionX));
-    ui.editTargetPositionY->setText(QString::number(newPlatformPositionY));
-    ui.editTargetRotation->setText(QString::number(newPlatformRotation));
+    // moved to qt thread for thread safety
+    QMetaObject::invokeMethod(this, "setNewTargetPoseLabels",
+                              Q_ARG(float, newPlatformPositionX),
+                              Q_ARG(float, newPlatformPositionY),
+                              Q_ARG(float, newPlatformRotation));
 }
 
 void PlatformUnitWidget::reportPlatformVelocity(::Ice::Float currentPlatformVelocityX, ::Ice::Float currentPlatformVelocityY, ::Ice::Float currentPlatformVelocityRotation, const Ice::Current& c)
@@ -146,64 +172,23 @@ void PlatformUnitWidget::stopPlatform()
     platformUnitProxy->moveRelative(0, 0, 0, posAcc, rotAcc);
 }
 
-void PlatformUnitWidget::controlTimerTick()
+void PlatformUnitWidget::moveTranslational(const QPointF &pos)
 {
-    //speed control cant be tested in the simulator:
-    //platformUnitProxy->move(targetPlatformVelocityX, targetPlatformVelocityY, targetPlatformVelocityRotation);
-    //[20496][12:09:21.196][PlatformUnitDynamicSimulation][PlatformUnitDynamicSimulation]: NYI
-    //
-    // the manual transformation in the platform system using the rotation has problems:
-    // - more floating point operations => bigger errors
-    // - the platform moves => the rotation changes a small bit => the platforms starts to turn
-    //
-    // the problems maybe will be solved by setting velocities (but this is nyi in the simulator)
-
-    ::Ice::Float posAcc = 10.0f;
-    ::Ice::Float rotAcc = 0.1f;
-
-    //normalize to [-1,1]
-    auto rot = -rotaCtrl->getRotation()/static_cast<float>(M_PI);
-    ::Ice::Float rotInc = rot;
-
-    auto pos = speedCtrl->getPosition()*60;
-    //flip y
-    pos.setY(-pos.y());
-    auto len = std::hypot(pos.x(),pos.y());
+    platformUnitProxy->move(pos.x(), pos.y(),0);
+}
 
-    ::Ice::Float posXInc = 0;
-    ::Ice::Float posYInc = 0;
+void PlatformUnitWidget::rotate(const double &alpha)
+{
+    platformUnitProxy->move(0,0, alpha);
+}
 
+void PlatformUnitWidget::controlTimerTick()
+{
+    float factor = 1;
+    ARMARX_INFO << deactivateSpam(0.5) << speedCtrl->getPosition().x()*factor << ", " << speedCtrl->getPosition().y()*factor;
+    ARMARX_INFO << deactivateSpam(0.5)  << VAROUT(rotaCtrl->getRotation());
+    platformUnitProxy->move(speedCtrl->getPosition().x()*factor, -1 * speedCtrl->getPosition().y()*factor, -1 * rotaCtrl->getRotation());
 
-    ::Ice::Float rotationDelta=0;
-    if(std::abs(len)>0.01)
-    {
-        //we are moving and need to adjust the rotation
-        rotationDelta=platformRotation-platformRotationAtMoveStart;
-        //did the platform just start to move?
-        if(!platformMoves)
-        {
-            platformMoves=true;
-            platformRotationAtMoveStart=platformRotation;
-            rotationDelta=0;
-        }
-        // rotation=0 => lookDirection = (0,1)
-        //
-        //rotate the vector
-        //  cos alpha  -sin alpha  * x
-        //  sin alpha   cos alpha    y
-        posXInc =pos.x()*std::cos(platformRotationAtMoveStart) - pos.y()*std::sin(platformRotationAtMoveStart);
-        posYInc =pos.x()*std::sin(platformRotationAtMoveStart) + pos.y()*std::cos(platformRotationAtMoveStart);
-    }
-    else
-    {
-        //platform stopped moving
-        platformMoves=false;
-    }
-    //only update positions if required
-    if(rotInc!=0 || posXInc !=0 || posYInc!=0|| rotInc!=0)
-    {
-        platformUnitProxy->moveRelative(posXInc,posYInc,rotInc-rotationDelta, posAcc, rotAcc);
-    }
 }
 
 Q_EXPORT_PLUGIN2(armar3_gui_PlatformUnitGuiPlugin, PlatformUnitGuiPlugin)

source/RobotAPI/gui_plugins/PlatformUnitPlugin/PlatformUnitGuiPlugin.h

@@ -87,6 +87,7 @@ namespace armarx
         // inherited from Component
         virtual void onInitComponent();
         virtual void onConnectComponent();
+        virtual void onDisconnectComponent();
         virtual void onExitComponent();
 
         // slice interface implementation
@@ -109,6 +110,9 @@ namespace armarx
 
         void moveTo();
 
+        void setNewPlatformPoseLabels(float x, float y, float alpha);
+        void setNewTargetPoseLabels(float x, float y, float alpha);
+
     protected:
         void connectSlots();
 
@@ -125,6 +129,9 @@ namespace armarx
           \brief Stops the platform
           */
         void stopPlatform();
+
+        void moveTranslational(const QPointF& pos);
+        void rotate(const double& alpha);
     private:
         std::string platformUnitProxyName;
         std::string platformName;
@@ -169,7 +176,7 @@ namespace armarx
         /**
          * \brief The tick rate (in ms) for the ctrlEvaluationTimer.
          */
-        static const int CONTROL_TICK_RATE = 250;
+        static const int CONTROL_TICK_RATE = 50;
     };
     typedef boost::shared_ptr<PlatformUnitWidget> PlatformUnitGuiPluginPtr;
 }