MMM Player set joint modes only once

source/RobotAPI/components/MMMPlayer/MMMPlayer.cpp

@@ -69,24 +69,41 @@ void MMMPlayer::onConnectComponent()
             float frameDuration = motion->getMotionFrame(1)->timestep - motion->getMotionFrame(0)->timestep;
             ARMARX_CHECK_EXPRESSION(frameDuration != 0);
             ARMARX_CHECK_EXPRESSION(frameDuration > 0)
-            motionFPS = 1.0f/frameDuration;
+                    motionFPS = 1.0f/frameDuration;
             ARMARX_VERBOSE << VAROUT(motionFPS);
         }
-    }
-    kin = getProxy<KinematicUnitInterfacePrx>(getProperty<std::string>("KinematicUnitName").getValue());
-    debugObserver = getProxy<DebugObserverInterfacePrx>("DebugObserver");
-    desiredFPS = getProperty<float>("FPS").getValue();
-
-    start = 0.3;
-    end = 1.26;
-    direction = 1;
 
-    jointOffets["Right Arm Shoulder2"] = 15.0/180.0*M_PI;
-    jointOffets["Left Arm Shoulder2"] = 15.0/180.0*M_PI;
+        kin = getProxy<KinematicUnitInterfacePrx>(getProperty<std::string>("KinematicUnitName").getValue());
+        debugObserver = getProxy<DebugObserverInterfacePrx>("DebugObserver");
+        desiredFPS = getProperty<float>("FPS").getValue();
+
+        start = 0.3;
+        end = 1.26;
+        direction = 1;
+
+        jointOffets["Right Arm Shoulder2"] = 15.0/180.0*M_PI;
+        jointOffets["Left Arm Shoulder2"] = 15.0/180.0*M_PI;
+
+        NameControlModeMap modes;
+        for (size_t i = 0; i < jointNames.size(); ++i) {
+            const auto& jointName = jointNames.at(i);
+            if(usePIDController)
+            {
+                modes[jointName] = eVelocityControl;
+            }
+            else
+            {
+                modes[jointName] = ePositionControl;
+            }
+        }
+        kin->switchControlMode(modes);
+    }
 
     task = new PeriodicTask<MMMPlayer>(this, &MMMPlayer::run, 1000.0f/desiredFPS, true, "MMMPlayerTask");
     task->start();
 
+
+
 }
 
 
@@ -94,7 +111,7 @@ void MMMPlayer::run()
 {
     ScopedLock lock(playerMutex);
     NameValueMap targetVelocities;
-    NameControlModeMap modes;
+
 //    targetPositionValues.clear();
 ////    if(currentFrame % 2 == 0)
 //        targetPositionValues["Right Arm Elbow1"] = start + (end - start) * 0.01 * currentFrame;
@@ -111,7 +128,7 @@ void MMMPlayer::run()
 //    return;
 
 
-    if(currentFrame >= motion->getNumFrames() || currentFrame < 0)
+    if(currentFrame >= (int)motion->getNumFrames() || currentFrame < 0)
     {
         if(getProperty<bool>("LoopPlayback").getValue())
         {
@@ -125,7 +142,7 @@ void MMMPlayer::run()
     }
     auto frame = motion->getMotionFrame(currentFrame);
     MMM::MotionFramePtr nextFrame;
-    if(currentFrame +1 < motion->getNumFrames())
+    if(currentFrame +1 < (int)motion->getNumFrames())
         nextFrame = motion->getMotionFrame(currentFrame+1);
     targetPositionValues.clear();
 //    NameValueMap targetVelocities;
@@ -168,15 +185,9 @@ void MMMPlayer::run()
 //            debugObserver->setDebugDatafield(jointName, "i", new Variant(pid->second->integral* pid->second->Ki));
 //            debugObserver->setDebugDatafield(jointName, "d", new Variant(pid->second->derivative * pid->second->Kd));
 //            debugObserver->setDebugDatafield(jointName, "velocity", new Variant(targetVelocities[jointName]));
-            modes[jointName] = eVelocityControl;
-        }
-        else
-        {
-            modes[jointName] = ePositionControl;
         }
     }
     debugObserver->setDebugChannel("targetJointAngles", debugTargetValues);
-    kin->switchControlMode(modes);
     kin->setJointVelocities(targetVelocities);
     if(!usePIDController)
         kin->setJointAngles(targetPositionValues);