CMakeLists.txt

@@ -21,6 +21,9 @@ depends_on_armarx_package(MemoryX "OPTIONAL")
 
 find_package(Eigen3 QUIET)
 find_package(Simox QUIET)
+if (Simox_FOUND)
+	setupSimoxExternalLibraries()
+endif()
 
 add_subdirectory(etc)
 add_subdirectory(source)

data/RobotComponents/VariantInfo-RobotComponents.xml

@@ -9,6 +9,22 @@
             propertyName="ViewSelectionName"
             propertyIsOptional="true"
             propertyDefaultValue="ViewSelection" />
+        <Proxy include="RobotComponents/interface/components/RobotIK.h"
+            humanName="RobotIK"
+            typeName="RobotIKInterfacePrx"
+            memberName="robotIK"
+            getterName="getRobotIK"
+            propertyName="RobotIKName"
+            propertyIsOptional="true"
+            propertyDefaultValue="RobotIK" />
+         <Proxy include="RobotComponents/interface/components/DMPComponentBase.h"
+            humanName="DMP Control Component"
+            typeName="DMPComponentBasePrx"
+            memberName="dmpComponent"
+            getterName="getDMPComponent"
+            propertyName="DMPComponentName"
+            propertyIsOptional="true"
+            propertyDefaultValue="DMPComponent" />
 <!--        <Proxy include="RobotComponents/interface/components/PathPlanner.h"
             humanName="PathPlanner"
             typeName="ViewSelectionInterfacePrx"

etc/cmake/ArmarXPackageVersion.cmake

 # armarx version file
 set(ARMARX_PACKAGE_LIBRARY_VERSION_MAJOR "0")
-set(ARMARX_PACKAGE_LIBRARY_VERSION_MINOR "7")
-set(ARMARX_PACKAGE_LIBRARY_VERSION_PATCH "7")
+set(ARMARX_PACKAGE_LIBRARY_VERSION_MINOR "8")
+set(ARMARX_PACKAGE_LIBRARY_VERSION_PATCH "0")
 
 set(ARMARX_PACKAGE_LIBRARY_VERSION "${ARMARX_PACKAGE_LIBRARY_VERSION_MAJOR}.${ARMARX_PACKAGE_LIBRARY_VERSION_MINOR}.${ARMARX_PACKAGE_LIBRARY_VERSION_PATCH}")
 

etc/doxygen/pages/__HowTos.dox__

-/**
+
 \page RobotComponents-HowTos  RobotComponents HowTos
 
 
-\section RobotComponents-HowTo-change_this HowTo Title
+\section RobotComponents-HowTo-ViewSelection How to use the Automatic View Selection
+
+\subsection The automatic mode
+
+The purpose of the automatic view selection is to make the robot autonomously look in the most useful direction at any time. If, for example, the current task is to grasp an object, and during the grasping both the object and the hand should be localized visually, the robot should look at them both at once if possible, or alternatingly otherwise.
+
+This is achieved by considering the localization uncertainties of the requested objects in the working memory. The priority of an object to be looked at is the logarithm of the volume of its uncertainty ellipsoid. The Automatic View Selection will find the view direction that maximizes the sum of the priorities of all objects that are visible in that direction. Details on this can be found in the following paper:
+
+K. Welke, D. Schiebener, T. Asfour and R. Dillmann: Gaze selection during manipulation tasks, IEEE International Conference on Robotics and Automation (ICRA), 2013
+
+Additionally, there is a little random component that slightly modifies the view direction, which is helpful when anobject is at the edge of the field of view or the localization only succeeds from time to time. Empiriclly, this leads to somewhat more stable localization results. The effect of this random component is most visible when no object has been requested and the robot just looks around without purpose.
+
+A new view direction is by default chosen every 2.5 seconds (this can be changed in the config of the ViewSelection component). This gives the robot enough time to move the head to the target configuration, get to rest there so the camera images are not blurry, and let localizers run a few times.
+
+The default maximal distance in which an object is considered to be localizable is 1.5 meters. If the object is further away, the view selection will ignore it. Other important parameters that you might need to adapt for your robot are the camera opening angle and the range of motion for the head. The default parameters are set for ARMAR-III.
+
+
+\subsection Setting manual view targets
 
-Description (the change_this part of the section identifier must be edited to be descriptive and unique)
+Most of the time, the fully automatic mode should work fine for your purposes. As long as the robot knows where the objects of interest are, it will look into the direction with the highest  accumulated uncertainty of relevant objects. However, if the robot doesn't know where an object is, e.g. because it has not been localized yet, you may need to set a view direction manually. You can do this with the method addManualViewTarget(const FramedPositionBasePtr& target). 
 
+When you set a manual view target, the next time a new view direction is set, it will be towards the position you requested. You can add several manual targets, they will be looked at in the order in which you added them in the usual 2.5 second interval. Afterwards, if the automatic view selection is activated, it will continue normally. If it is deactivated, your manual targets will be looked at ,and then the robot will not change the view direction further after the last one.
 
-*/
+In most cases, the best strategy is to set the expected possible object locations as manual view targets in the beginning, and let the view selection run automatically otherwise. You should check at key times in your statechart whether the required objects are successfully being localized (the standard skills like visual servoing and grasping do that), and if not, add manual view targets to find them.

source/RobotComponents/components/DMPComponent/DMPComponent.cpp

@@ -23,7 +23,7 @@
 #include "DMPComponent.h"
 #include <MMM/Motion/MotionReaderXML.h>
 #include <dmp/io/MMMConverter.h>
-
+#include <dmp/testing/testdataset.h>
 
 using namespace armarx;
 
@@ -106,15 +106,17 @@ void DMPComponent::instantiateDMP(int DMPType, const Ice::Current&)
         case ARMARX_DMPTYPE_SIMPLEENDVELODMP:
             basicdmp.reset(new DMP::SimpleEndVeloDMP);
             break;
-
         //    case ARMARX_DMPTYPE_ENDVELFORCEFILELDDMP: basicdmp.reset(new DMP::EndVeloForceFieldDMP);break;
         //    case ARMARX_DMPTYPE_QUATERNIONDMP: basicdmp.reset(new DMP::QuaternionDMP); break; //error: quanternionDMP is an abstract type.
         //    case ARMARX_DMPTYPE_ADAPTIVEGOAL3RDORDERDMP: basicdmp.reset(new DMP::AdaptiveGoal3rdOrderDMP); break; //error:
-        //    case ARMARX_DMPTYPE_PERIODICTRANSIENTDMP: basicdmp.reset(new DMP::PeriodicTransientDMP); break;
+        case ARMARX_DMPTYPE_PERIODICTRANSIENTDMP:
+            ARMARX_INFO << "instantiate PeriodicDMP";
+            basicdmp.reset(new DMP::PeriodicTransientDMP);
+            break;
         default:
             ARMARX_ERROR << "ERROR: It is not a valid dmp type. " ;
     }
-
+    tCurrent = 0;
 }
 
 
@@ -198,14 +200,24 @@ void DMPComponent::getDMPFromDatabase(const std::string& dmpName, const Ice::Cur
 void DMPComponent::trainDMP(const ::Ice::Current&)
 {
     // learn dmp
-    if (getBasicDMPType() == ARMARX_DMPTYPE_BASICDMP)
+    ARMARX_INFO << "In Train DMP";
+    if (dmpType == ARMARX_DMPTYPE_BASICDMP)
     {
+
         basicdmp->learnFromTrajectories(trajs);
     }
-    else if (getBasicDMPType() == ARMARX_DMPTYPE_DMP3RDORDER)
+    else if (dmpType == ARMARX_DMPTYPE_DMP3RDORDER)
     {
+
         dmp3rdorder->learnFromTrajectories(trajs);
     }
+    else if (dmpType == ARMARX_DMPTYPE_PERIODICTRANSIENTDMP)
+    {
+
+
+        basicdmp->learnFromTrajectories(trajs);
+    }
+    ARMARX_INFO << "Exit Train DMP";
 }
 
 void DMPComponent::calculateWholeTrajectory()
@@ -279,6 +291,33 @@ void DMPComponent::readTrajectoryFromFile(const std::string& file, const Ice::Cu
         DMP::SampledTrajectoryV2 traj;
         traj.readFromCSVFile(file);
         trajs.push_back(traj);
+
+    }
+    else if (ext == "vsg")
+    {
+        trajs.clear();
+        TestDataSet set;
+        set.readFromFile(file);
+        set.cutConstantBeginning();
+        DMP::SampledTrajectoryV2 testTraj;//= set.trajectoryForPositionsV2();
+
+        trajs.push_back(testTraj);
+        set.trajectoryForPositionsV2(trajs[0], usedDimensions);
+        //DVec anchorPoint;//delete set;
+        //anchorPoint.push_back(0.0);
+        //anchorPoint.push_back(1.0);
+        //anchorPoint.push_back(2.0);
+        //trajs[0].m_anchorPoint = anchorPoint;
+
+        ARMARX_INFO << "VSG File LOADED";
+        //trajs.resize(1);
+        //trajs[1] = traji;
+
+        ARMARX_INFO << "VSG File LOADED";
+        printf("Size of traj %f %f\n", trajs[0].m_anchorPoint[0], trajs[0].m_anchorPoint[1]);
+        ARMARX_INFO << "VSG File LOADED";
+        //ARMARX_INFO << "Size of loaded Trajectories " << trajs.size() << " and " << traj.getAnchorPoint();
+        ARMARX_INFO << "VSG File LOADED DONE";
     }
     else
     {
@@ -288,7 +327,6 @@ void DMPComponent::readTrajectoryFromFile(const std::string& file, const Ice::Cu
 
 }
 
-
 void DMPComponent::setDMPConfiguration()
 {
 
@@ -438,6 +476,7 @@ void DMPComponent::setTimeStamps(const DVector& value, const Ice::Current&)
 
 void DMPComponent::setCanonicalValues(const DVector& value, const Ice::Current&)
 {
+
     canonicalValues = DMP::DVec(value);
 }
 
@@ -473,21 +512,21 @@ void DMPComponent::setCanonicalValues(const DVector& value, const Ice::Current&)
 
 ::armarx::cStateVec DMPComponent::getNextState(const ::Ice::Current&)
 {
-
+    ARMARX_INFO << "In getNext State ";
     setDMPConfiguration();
 
     if (timestamps.size() == 0)
     {
         ARMARX_ERROR << "Timestampes must be specified.";
     }
-
+    ARMARX_INFO << "In getNext State 1";
     if (canonicalValues.size() == 0)
     {
         ARMARX_WARNING << "Canonical value is not specified. It will be set 1.0.";
         canonicalValues.push_back(1.0);
     }
 
-
+    ARMARX_INFO << "In getNext State 2";
 
     double tInit = timestamps[tCurrent++];
 
@@ -506,7 +545,7 @@ void DMPComponent::setCanonicalValues(const DVector& value, const Ice::Current&)
 
         tCurrent = 0;
     }
-
+    ARMARX_INFO << "In getNext State 3";
     double t = timestamps[tCurrent];
 
 
@@ -552,6 +591,15 @@ void DMPComponent::removeDMPFromDatabase(const std::string& dmpName, const ::Ice
     }
 }
 
+void DMPComponent::setDimensionsToLearn(const DVector& value, const Ice::Current&)
+{
+    usedDimensions.clear();
+    for (int i = 0; i < value.size(); i++)
+    {
+        usedDimensions.push_back(int(value[i]));
+    }
+}
+
 
 //PropertyDefinitionsPtr DMPComponent::createPropertyDefinitions()
 //{

source/RobotComponents/components/DMPComponent/DMPComponent.h

@@ -54,7 +54,7 @@
 #include "dmp/representation/dmp/simpleendvelodmp.h"
 //#include "dmp/representation/dmp/endveloforcefielddmp.h"
 //#include "dmp/representation/dmp/endveloforcefieldwithobjrepulsiondmp.h"
-//#include "dmp/representation/dmp/periodictransientdmp.h"
+#include "dmp/representation/dmp/periodictransientdmp.h"
 
 namespace armarx
 {
@@ -113,6 +113,7 @@ namespace armarx
         typedef boost::shared_ptr<DMP::DMPInterfaceTemplate<DMP::DMPState> > BasicDMPInterfacePtr;
         typedef boost::shared_ptr<DMP::DMPInterfaceTemplate<DMP::_3rdOrderDMP> > DMP3rdOrderInterfacePtr;
 
+
         DMPComponent():
             basicdmp(new DMP::BasicDMP()),
             dmp3rdorder(new DMP::DMP3rdOrder()),
@@ -168,7 +169,7 @@ namespace armarx
         virtual void trainDMP(const ::Ice::Current& = ::Ice::Current());
         virtual void storeDMPInDatabase(const std::string& name, const ::Ice::Current& = ::Ice::Current());
         virtual void removeDMPFromDatabase(const std::string& name, const ::Ice::Current& = ::Ice::Current());
-
+        virtual void setDimensionsToLearn(const DVector& value, const Ice::Current& = ::Ice::Current());
 
 
         //transmit data from server to client (using ice)
@@ -203,9 +204,11 @@ namespace armarx
         DMP::DVec timestamps;
         DMP::DVec canonicalValues;
         int tCurrent;
+        std::vector<int> usedDimensions;
     protected:
         BasicDMPInterfacePtr basicdmp;
         DMP3rdOrderInterfacePtr dmp3rdorder;
+
         DMP::Vec<DMP::SampledTrajectoryV2> trajs;
 
         int dmpType;

source/RobotComponents/components/MMMPlayer/MMMPlayer.cpp

@@ -40,8 +40,8 @@ void MMMPlayer::onInitComponent()
     usingProxy(getProperty<std::string>("KinematicUnitName").getValue());
     usingProxy("DebugObserver");
 
-    motionFPS = 1;
-    desiredFPS = 1;
+    motionFPS = 1.0f;
+    desiredFPS = 1.0f;
     frameOffset = 0;
     paused = true;
 
@@ -50,16 +50,15 @@ void MMMPlayer::onInitComponent()
     desiredFPS = getProperty<float>("FPS").getValue();
     maxVel = getProperty<float>("absMaximumVelocity").getValue();
 
-    if (usePIDController)
-    {
-        usingTopic(getProperty<std::string>("KinematicTopicName").getValue());
-    }
+
+    usingTopic(getProperty<std::string>("KinematicTopicName").getValue());
 }
 
 
 void MMMPlayer::onConnectComponent()
 {
-    kin = getProxy<KinematicUnitInterfacePrx>(getProperty<std::string>("KinematicUnitName").getValue());
+    kinematicUnit = getProxy<KinematicUnitInterfacePrx>(getProperty<std::string>("KinematicUnitName").getValue());
+
     debugObserver = getProxy<DebugObserverInterfacePrx>("DebugObserver");
     std::string armarxProjects = getProperty<std::string>("ArmarXProjects").getValue();
 
@@ -68,7 +67,7 @@ void MMMPlayer::onConnectComponent()
         std::vector<std::string> projects;
         boost::split(projects, armarxProjects, boost::is_any_of(",;"), boost::token_compress_on);
 
-        for (auto & p : projects)
+        for (std::string & p : projects)
         {
             //            ARMARX_INFO << "Adding to datapaths of " << p;
             armarx::CMakePackageFinder finder(p);
@@ -85,9 +84,9 @@ void MMMPlayer::onConnectComponent()
         }
     }
 
-    auto motionDefault = getProperty<std::string>("MMMFile").getValue();
+    std::string motionDefault = getProperty<std::string>("MMMFile").getValue();
 
-    if (motionDefault != "")
+    if (!motionDefault.empty())
     {
         load(motionDefault, "Armar4");
     }
@@ -148,8 +147,7 @@ bool MMMPlayer::pauseMMMPlayer(const Ice::Current&)
     if (!paused)
     {
         paused = true;
-        kin->setJointVelocities(nullVelocities);
-        sleep(1);
+        kinematicUnit->setJointVelocities(nullVelocities);
 
         if (direction < 0)
         {
@@ -194,7 +192,7 @@ bool MMMPlayer::stopMMMPlayer(const Ice::Current&)
             ARMARX_INFO << "stopped MMMPlayer task from GUI";
         }
 
-        kin->setJointVelocities(nullVelocities);
+        kinematicUnit->setJointVelocities(nullVelocities);
 
         return !(task->isRunning());
     }
@@ -224,7 +222,7 @@ void MMMPlayer::load(const std::string& MMMFile, const std::string& projects)
         std::vector<std::string> proj;
         boost::split(proj, projects, boost::is_any_of(",;"), boost::token_compress_on);
 
-        for (auto & p : proj)
+        for (std::string & p : proj)
         {
             ARMARX_INFO << "Adding to datapaths of " << p;
             armarx::CMakePackageFinder finder(p);
@@ -399,8 +397,8 @@ void MMMPlayer::load(const std::string& MMMFile, const std::string& projects)
         jointNamesUsed[jointName] = true;
     }
 
-    kin->switchControlMode(modes);
-    kin->setJointVelocities(nullVelocities);
+    kinematicUnit->switchControlMode(modes);
+    kinematicUnit->setJointVelocities(nullVelocities);
 }
 
 bool MMMPlayer::loadTrajectory(const std::string& MMMFile, const std::string& projects, const Ice::Current&)
@@ -420,6 +418,7 @@ bool MMMPlayer::loadTrajectory(const std::string& MMMFile, const std::string& pr
 
 int MMMPlayer::getNumberOfFrames(const Ice::Current&)
 {
+
     if (!motion)
     {
         return 0;
@@ -435,6 +434,7 @@ int MMMPlayer::getCurrentFrame(const Ice::Current&)
 
 bool MMMPlayer::setLoopPlayback(bool loop, const Ice::Current&)
 {
+
     loopPlayback = loop;
     return loopPlayback;
 }
@@ -449,8 +449,11 @@ StringList MMMPlayer::getJointNames(const Ice::Current&)
     return jointNames;
 }
 
-int MMMPlayer::setUsePID(int usePID, const Ice::Current&)
+int MMMPlayer::setUsePID(bool usePID, const Ice::Current&)
 {
+
+    ScopedRecursiveLock lock(mmmMutex);
+
     usePIDController = usePID;
     NameControlModeMap modes;
 
@@ -470,14 +473,14 @@ int MMMPlayer::setUsePID(int usePID, const Ice::Current&)
 
     try
     {
-        kin->switchControlMode(modes);
+        kinematicUnit->switchControlMode(modes);
     }
     catch (...) { }
 
     return usePIDController;
 }
 
-void MMMPlayer::setFPS(int FPS, const Ice::Current&)
+void MMMPlayer::setFPS(float FPS, const Ice::Current&)
 {
     desiredFPS = FPS;
     ARMARX_VERBOSE << "set FPS to " << FPS;
@@ -492,6 +495,9 @@ bool MMMPlayer::setJointsInUse(const std::string& jointName, bool inUse, const I
 
 void MMMPlayer::setCurrentFrame(int frameNumber, const Ice::Current&)
 {
+
+    ScopedRecursiveLock lock(mmmMutex);
+
     if (!motion)
     {
         return;
@@ -534,15 +540,14 @@ void MMMPlayer::setCurrentFrame(int frameNumber, const Ice::Current&)
     }
 
     debugObserver->setDebugChannel("targetJointAngles", debugTargetValues);
-    kin->setJointVelocities(nullVelocities);
-    kin->setJointAngles(targetPositionValues);
+    kinematicUnit->setJointVelocities(nullVelocities);
+    kinematicUnit->setJointAngles(targetPositionValues);
 
     ARMARX_INFO << "MMMPlayer: Current frame set to " << frameNumber;
 }
 
 void MMMPlayer::run()
 {
-
     NameValueMap targetVelocities;
 
     if (!motion)
@@ -574,7 +579,7 @@ void MMMPlayer::run()
         {
             ARMARX_VERBOSE << "current frame " << currentFrame;
 
-            kin->setJointVelocities(nullVelocities);
+            kinematicUnit->setJointVelocities(nullVelocities);
 
             if (loopPlayback)
             {
@@ -609,6 +614,7 @@ void MMMPlayer::run()
             ARMARX_INFO << deactivateSpam(1) << "frame: " << currentFrame << " " << VAROUT(interpolationValue);
             ARMARX_CHECK_EXPRESSION((int)jointNames.size() == frame->joint.rows());
 
+
             for (size_t i = 0; i < jointNames.size(); ++i)
             {
                 const auto& jointName = jointNames.at(i);
@@ -629,7 +635,7 @@ void MMMPlayer::run()
                         targetPosValue += it->second;
                     }
 
-                    assert(targetPosValue == targetPositionValues[jointName]);
+                    //                    assert(targetPosValue == targetPositionValues[jointName]);
                     debugTargetValues[jointName] = new Variant(targetPosValue);
                     float& targetVel = targetVelocities[jointName] =  0.0;
                     auto vel = frame->joint_vel[i];
@@ -647,7 +653,7 @@ void MMMPlayer::run()
 
                         targetVel = targetVelocities[jointName] = (nextFrame->joint[i] - frame->joint[i]) / (nextFrame->timestep - frame->timestep);
                     }
-
+                    targetVel *= 0.4;
                     MMM::MotionFramePtr nextNextFrame;
 
                     if (currentFrame + 2 < (int)motion->getNumFrames())
@@ -655,12 +661,6 @@ void MMMPlayer::run()
                         nextNextFrame = motion->getMotionFrame(currentFrame + 2);
                     }
 
-                    /*if (nextNextFrame)
-                    {
-                        float nextTargetVel = (nextNextFrame->joint[i] - nextFrame->joint[i]) / (nextNextFrame->timestep - nextFrame->timestep);
-                        targetVel += interpolationValue * (nextTargetVel - targetVel);
-                    } */
-
                     targetVel *= desiredFPS / motionFPS * direction;
 
                     //            debugObserver->setDebugDatafield(jointName, "p", new Variant(pid->second->previousError * pid->second->Kp));
@@ -670,25 +670,29 @@ void MMMPlayer::run()
                     //}
 
                     //        targetVelocities[jointName] =  0.0;
+
                     auto pid = PIDs.find(jointName);
 
-                    if (usePIDController && pid != PIDs.end())
+                    if (usePIDController)
                     {
-                        auto cv = pid->second->getControlValue();
-                        targetVel += cv;
+                        if (pid != PIDs.end())
+                        {
+                            auto cv = pid->second->getControlValue();
+                            targetVel += cv;
 
-                        ARMARX_INFO << deactivateSpam(0.5, jointName) << jointName << " ControlValue   " << cv;
+                            ARMARX_INFO << deactivateSpam(0.5, jointName) << jointName << " ControlValue   " << cv;
 
 
-                        if (cv > 20)
-                        {
-                            ARMARX_INFO << "" << (jointName) << ": v:" << (targetVel) << " pos: " << targetPosValue;
-                        }
+                            if (cv > 20)
+                            {
+                                ARMARX_INFO << "" << (jointName) << ": v:" << (targetVel) << " pos: " << targetPosValue;
+                            }
 
-                        //                    debugObserver->setDebugDatafield(jointName, "p", new Variant(pid->second->previousError * pid->second->Kp));
-                        //                    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]));
+                            //                    debugObserver->setDebugDatafield(jointName, "p", new Variant(pid->second->previousError * pid->second->Kp));
+                            //                    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]));
+                        }
                     }
 
                     targetVel  = std::min<double>(maxVel / 180.0 * M_PI, targetVel);
@@ -702,21 +706,17 @@ void MMMPlayer::run()
             tmpTargetValues = targetPositionValues;
         }
 
-        //    ARMARX_INFO << "target: " << targetPositionValues ;
-        std::stringstream out;
-
-        for (auto v : debugTargetValues)
         {
-            out << v.first << ": " << v.second->getFloat() << " " << tmpTargetValues[v.first] << "\n" ;
+            ScopedLock lock(jointAnglesMutex);
+            updatePIDController(latestJointAngles);
         }
 
-        //    ARMARX_INFO << "debugtarget: " << out.str() ;
         debugObserver->setDebugChannel("targetJointAngles", debugTargetValues);
-        kin->setJointVelocities(targetVelocities);
+        kinematicUnit->setJointVelocities(targetVelocities);
 
         if (!usePIDController)
         {
-            kin->setJointAngles(tmpTargetValues);
+            kinematicUnit->setJointAngles(tmpTargetValues);
         }
 
         ARMARX_VERBOSE << deactivateSpam(1) << "Playing frame " << currentFrame;
@@ -726,15 +726,12 @@ void MMMPlayer::run()
 
 void MMMPlayer::reportJointAngles(const NameValueMap& angles, bool, const Ice::Current&)
 {
-    ScopedRecursiveLock lock(mmmMutex);
-    std::stringstream out;
-
-    for (auto v : angles)
-    {
-        out << v.first << ": " << v.second << " " << targetPositionValues[v.first] << "\n" ;
-    }
+    ScopedLock lock(jointAnglesMutex);
+    latestJointAngles = angles;
+}
 
-    //    ARMARX_INFO  << deactivateSpam(0.5) << "jointangles reported: " << out.str();
+void MMMPlayer::updatePIDController(const NameValueMap& angles)
+{
     if (!usePIDController)
     {
         ARMARX_INFO << deactivateSpam() << "jointangles reporting DISABLED";
@@ -743,8 +740,13 @@ void MMMPlayer::reportJointAngles(const NameValueMap& angles, bool, const Ice::C
 
     for (const auto & joint : angles)
     {
-
         const std::string& name = joint.first;
+
+        if (targetPositionValues.find(name) == targetPositionValues.end())
+        {
+            continue;
+        }
+
         auto it = PIDs.find(name);
 
         if (it == PIDs.end())
@@ -755,21 +757,22 @@ void MMMPlayer::reportJointAngles(const NameValueMap& angles, bool, const Ice::C
             it = PIDs.find(name);
         }
 
-        ARMARX_INFO  << deactivateSpam() << name;
         PIDControllerPtr pid = it->second;
         pid->update(joint.second, targetPositionValues[name]);
-
     }
 }
 
+
 void MMMPlayer::onDisconnectComponent()
 {
+    ScopedRecursiveLock lock(mmmMutex);
+
     if (task)
     {
         task->stop();
     }
 
-    kin->stop();
+    kinematicUnit->stop();
 }
 
 

source/RobotComponents/components/MMMPlayer/MMMPlayer.h

@@ -136,13 +136,13 @@ namespace armarx
         void run();
         void setJointAngles(int frameNumber);
         MMM::MotionPtr motion;
-        KinematicUnitInterfacePrx kin;
+        KinematicUnitInterfacePrx kinematicUnit;
         DebugObserverInterfacePrx debugObserver;
         PeriodicTask<MMMPlayer>::pointer_type task;
         StringList jointNames;
         NameValueMap jointNamesUsed;
         std::string motionPath;
-        int usePIDController;
+        bool usePIDController;
         int currentFrame;
         double currentMotionTime, frameOffset;
         float motionFPS;
@@ -161,8 +161,11 @@ namespace armarx
         bool pause();
         bool stop();
         void load(const std::string& filename, const std::string& projects);
+        void updatePIDController(const NameValueMap& angles);
 
         RecursiveMutex mmmMutex;
+        NameValueMap latestJointAngles;
+        Mutex jointAnglesMutex;
         bool paused;
 
     public:
@@ -177,8 +180,8 @@ namespace armarx
         virtual bool setLoopPlayback(bool loop, const Ice::Current&);
         virtual std::string getMotionPath(const Ice::Current&);
         virtual StringList getJointNames(const Ice::Current&);
-        virtual int setUsePID(int usePID, const Ice::Current&);
-        virtual void setFPS(int FPS, const Ice::Current&);
+        virtual int setUsePID(bool usePID, const Ice::Current&);
+        virtual void setFPS(float FPS, const Ice::Current&);
         virtual bool setJointsInUse(const std::string& jointName, bool inUse, const Ice::Current&);
 
         // KinematicUnitListener interface

source/RobotComponents/components/ViewSelection/ViewSelection.cpp

@@ -49,6 +49,7 @@ void ViewSelection::onInitComponent()
     drawViewDirection = getProperty<bool>("VisualizeViewDirection").getValue();
 
     randomNoiseLevel = getProperty<float>("RandomNoiseLevel").getValue();
+    centralHeadTiltAngle = getProperty<float>("CentralHeadTiltAngle").getValue();
 
     std::string graphFileName;
 
@@ -174,17 +175,16 @@ void ViewSelection::process()
 {
     try
     {
-        float maxObjectDistance = getProperty<float>("MaxObjectDistance").getValue();
-        auto syncRobot = robotStateComponent->getSynchronizedRobot();
 
         if (manualViewTargets.size() > 0)
         {
             // look to the manually set target
+            SharedRobotInterfacePrx robotPrx = robotStateComponent->getSynchronizedRobot();
             FramedPositionPtr target;
             {
                 ScopedLock lock(manualViewTargetsMutex);
 
-                target = new FramedPosition(FramedPositionPtr::dynamicCast(manualViewTargets.at(0))->toEigen(), manualViewTargets.at(0)->frame, syncRobot->getName());
+                target = new FramedPosition(FramedPositionPtr::dynamicCast(manualViewTargets.at(0))->toEigen(), manualViewTargets.at(0)->frame, robotPrx->getName());
 
                 for (size_t i = 0; i < manualViewTargets.size() - 1; i++)
                 {
@@ -208,24 +208,18 @@ void ViewSelection::process()
                 nodeVisitedForObject.at(i) = -1;
             }
 
-            // generate new random noise
-            if (randomNoiseLevel > 0)
-            {
-                setRandomNoise();
-            }
 
-            auto robotPrx = robotStateComponent->getSynchronizedRobot();
             // collect localizable objects
             std::vector<memoryx::ObjectInstancePtr> localizableObjects;
             std::vector<FramedPositionPtr> localizableObjectPositions;
+            float maxObjectDistance = getProperty<float>("MaxObjectDistance").getValue();
+            SharedRobotInterfacePrx robotPrx = robotStateComponent->getSynchronizedRobot();
             int numberOfLostObjects = 0;
-            memoryx::EntityIdList objectIds = objectInstancesProxy->getAllEntityIds();
+            memoryx::EntityBaseList objectInstances = objectInstancesProxy->getAllEntities();
 
-            for (size_t i = 0; i < objectIds.size(); i++)
+            for (size_t i = 0; i < objectInstances.size(); i++)
             {
-                const std::string objectId = objectIds.at(i);
-                const memoryx::EntityBasePtr entityBase = objectInstancesProxy->getEntityById(objectId);
-                const memoryx::ObjectInstancePtr object = memoryx::ObjectInstancePtr::dynamicCast(entityBase);
+                const memoryx::ObjectInstancePtr object = memoryx::ObjectInstancePtr::dynamicCast(objectInstances.at(i));
 
                 if (object)
                 {
@@ -256,6 +250,53 @@ void ViewSelection::process()
                 }
             }
 
+
+            // generate new random noise
+            if (randomNoiseLevel > 0)
+            {
+                // if there are requested objects that were not localized yet, the random noise component will be bigger
+                memoryx::EntityBaseList requestedObjectClasses = objectClassesProxy->getAllEntities();
+
+                bool unlocalizedObjectExists = false;
+                for (size_t i = 0; i < requestedObjectClasses.size(); i++)
+                {
+                    bool isInInstancesList = false;
+                    for (size_t j = 0; j < objectInstances.size(); j++)
+                    {
+                        if (requestedObjectClasses.at(i)->getName() == objectInstances.at(j)->getName())
+                        {
+                            isInInstancesList = true;
+                            break;
+                        }
+                    }
+                    if (!isInInstancesList)
+                    {
+                        unlocalizedObjectExists = true;
+                        break;
+                    }
+                }
+
+                if (unlocalizedObjectExists)
+                {
+                    if (localizableObjects.size() == 0)
+                    {
+                        ARMARX_DEBUG << "There are objects requested, and none of them has been localized yet";
+                        setRandomNoise(centralHeadTiltAngle + 30, 3.0f);
+                    }
+                    else
+                    {
+                        ARMARX_DEBUG << "There are objects requested, and some but not all of them have been localized already";
+                        setRandomNoise(centralHeadTiltAngle + 20, 2.0f);
+                    }
+                }
+                else
+                {
+                    ARMARX_DEBUG << "There are no requested objects that were not localized yet. requestedObjectClasses.size() = " << requestedObjectClasses.size()
+                                 << ", objectInstances.size() = " << objectInstances.size();// << ", difference.size() = " << difference.size();
+                    setRandomNoise(centralHeadTiltAngle, 1.0f);
+                }
+            }
+
             const float probabilityToAddALostObject = (numberOfLostObjects > 0) ? getProperty<float>("ProbabilityToLookForALostObject").getValue() / numberOfLostObjects : 0;
 
 
@@ -279,10 +320,20 @@ void ViewSelection::process()
 
                     if (saliency > 0)
                     {
+                        // close objects get a smaller visibility angle to avoid problems due to the head being positioned unfortunately
+                        float modifiedHalfCameraOpeningAngle = halfCameraOpeningAngle;
+                        float objDist = position->toEigen().norm();
+                        float shortDistance = 0.5f * maxObjectDistance;
+                        if (objDist < shortDistance)
+                        {
+                            modifiedHalfCameraOpeningAngle = (objDist - 0.1f * shortDistance) / (0.9f * shortDistance) * halfCameraOpeningAngle;
+                            modifiedHalfCameraOpeningAngle = std::max(0.0f, modifiedHalfCameraOpeningAngle);
+                        }
+
                         TSphereCoord positionInSphereCoordinates;
                         MathTools::convert(position->toEigen() - offsetToHeadCenter, positionInSphereCoordinates);
                         int closestNodeIndex = graphLookupTable->getClosestNode(positionInSphereCoordinates);
-                        addSaliencyRecursive(closestNodeIndex, saliency, positionInSphereCoordinates, i);
+                        addSaliencyRecursive(closestNodeIndex, saliency, positionInSphereCoordinates, i, modifiedHalfCameraOpeningAngle);
                     }
                 }
             }
@@ -311,7 +362,7 @@ void ViewSelection::process()
 
             ARMARX_DEBUG << "Highest saliency: " << maxSaliency;
 
-            // select a new view if saliency is bigger than threshold, which converges towards 0 over time
+            // select a new view if saliency is bigger than threshold (which converges towards 0 over time)
             int timeSinceLastViewChange = (IceUtil::Time::now() - timeOfLastViewChange).toMilliSeconds();
             float saliencyThreshold = 0;
 
@@ -331,8 +382,11 @@ void ViewSelection::process()
                 if (drawer && robotPrx->hasRobotNode("Cameras") && drawViewDirection)
                 {
                     Vector3Ptr startPos = new Vector3(FramedPosePtr::dynamicCast(robotPrx->getRobotNode("Cameras")->getGlobalPose())->toEigen());
-                    FramedDirectionPtr direction = new FramedDirection(Eigen::Vector3f(1, 0, 0), "Cameras", robotPrx->getName());
-                    drawer->setArrowDebugLayerVisu("HeadRealViewDirection", startPos, direction->toGlobal(robotPrx), DrawColor {0, 1, 0, 0.2}, maxObjectDistance, 5);
+                    FramedDirectionPtr currentDirection = new FramedDirection(Eigen::Vector3f(1, 0, 0), "Cameras", robotPrx->getName());
+                    drawer->setArrowDebugLayerVisu("CurrentHeadViewDirection", startPos, currentDirection->toGlobal(robotPrx), DrawColor {0, 0.5, 0.5, 0.1}, maxObjectDistance, 5);
+                    Eigen::Vector3f plannedDirectionEigen = viewTargetPositionPtr->toGlobalEigen(robotPrx) - startPos->toEigen();
+                    Vector3Ptr plannedDirection = new Vector3(plannedDirectionEigen);
+                    drawer->setArrowDebugLayerVisu("PlannedHeadViewDirection", startPos, plannedDirection, DrawColor {0.5, 0.5, 0, 0.1}, maxObjectDistance, 5);
                 }
 
                 headIKUnitProxy->setHeadTarget(headIKKinematicChainName, viewTargetPositionPtr);
@@ -341,6 +395,10 @@ void ViewSelection::process()
                 boost::this_thread::sleep(boost::posix_time::milliseconds(sleepingTimeBetweenViewDirectionChanges));
             }
         }
+        else
+        {
+            boost::this_thread::sleep(boost::posix_time::milliseconds(5));
+        }
     }
     catch (...)
     {
@@ -351,11 +409,11 @@ void ViewSelection::process()
 
 
 
-void ViewSelection::addSaliencyRecursive(int currentNodeIndex, float saliency, TSphereCoord objectSphereCoord, int objectIndex)
+void ViewSelection::addSaliencyRecursive(const int currentNodeIndex, const float saliency, const TSphereCoord objectSphereCoord, const int objectIndex, const float maxDistanceOnArc)
 {
     // distance on arc between object projection center and node,
     // normalized by the maximal viewing angle of the camera (=> in [0,1])
-    float normalizedDistance = MathTools::getDistanceOnArc(objectSphereCoord, saliencyEgosphereGraph->getNodes()->at(currentNodeIndex)->getPosition()) / halfCameraOpeningAngle;
+    float normalizedDistance = MathTools::getDistanceOnArc(objectSphereCoord, saliencyEgosphereGraph->getNodes()->at(currentNodeIndex)->getPosition()) / maxDistanceOnArc;
 
     // increase value of node
     float newValue = ((CIntensityNode*)saliencyEgosphereGraph->getNodes()->at(currentNodeIndex))->getIntensity()
@@ -374,9 +432,9 @@ void ViewSelection::addSaliencyRecursive(int currentNodeIndex, float saliency, T
 
         if (nodeVisitedForObject.at(neighbourIndex) != objectIndex)
         {
-            if (MathTools::getDistanceOnArc(objectSphereCoord, saliencyEgosphereGraph->getNodes()->at(neighbourIndex)->getPosition()) <= halfCameraOpeningAngle)
+            if (MathTools::getDistanceOnArc(objectSphereCoord, saliencyEgosphereGraph->getNodes()->at(neighbourIndex)->getPosition()) <= maxDistanceOnArc)
             {
-                addSaliencyRecursive(neighbourIndex, saliency, objectSphereCoord, objectIndex);
+                addSaliencyRecursive(neighbourIndex, saliency, objectSphereCoord, objectIndex, maxDistanceOnArc);
             }
             else
             {
@@ -388,7 +446,7 @@ void ViewSelection::addSaliencyRecursive(int currentNodeIndex, float saliency, T
 
 
 
-void ViewSelection::setRandomNoise()
+void ViewSelection::setRandomNoise(const float centralAngleForVerticalDirection, const float directionVariabilityFactor)
 {
     if (!robotStateComponent)
     {
@@ -402,20 +460,21 @@ void ViewSelection::setRandomNoise()
     TSphereCoord currentViewDirection;
     MathTools::convert(currentViewTargetEigen, currentViewDirection);
 
-    const float centralAngle = getProperty<float>("CentralHeadTiltAngle").getValue();
-
     TNodeList* nodes = randomNoiseGraph->getNodes();
     const int randomFactor = 100000;
     const float noiseFactor = randomNoiseLevel / randomFactor;
+    const float distanceWeight = std::min(0.6f / directionVariabilityFactor, 1.0f);
+    const float centralityWeight = std::min(0.1f / directionVariabilityFactor, 0.2f);
 
     for (size_t i = 0; i < nodes->size(); i++)
     {
         CIntensityNode* node = (CIntensityNode*) nodes->at(i);
         TSphereCoord nodeCoord = node->getPosition();
         float distanceOnSphere = MathTools::getDistanceOnArc(currentViewDirection, nodeCoord);
-        float distanceFactor = 1.0f - 0.5f * distanceOnSphere / M_PI; // prefer directions close to current direction
-        float heightFactor = 1.0f - 0.2f * fabs(nodeCoord.fPhi - centralAngle) / 90.0f; // prefer directions that look down 20°
-        node->setIntensity(heightFactor * distanceFactor * noiseFactor * (rand() % randomFactor));
+        float distanceFactor = 1.0f - distanceWeight * distanceOnSphere / M_PI; // prefer directions close to current direction
+        float verticalCenterFactor = 1.0f - centralityWeight * fabs(nodeCoord.fPhi - centralAngleForVerticalDirection) / 90.0f; // prefer directions that look to the center
+        float horizontalCenterFactor = 1.0f - centralityWeight * fabs(nodeCoord.fTheta) / 120.0f;
+        node->setIntensity(verticalCenterFactor * horizontalCenterFactor * distanceFactor * noiseFactor * (rand() % randomFactor));
     }
 }
 

source/RobotComponents/components/ViewSelection/ViewSelection.h

@@ -71,11 +71,11 @@ namespace armarx
             defineOptionalProperty<std::string>("CameraFrameName", "VirtualCentralGaze", "Name of the frame of the head base in the robot model");
             defineOptionalProperty<float>("MaxObjectDistance", 1500.f, "Maximum distance of objects the head to be considered for observation");
             defineOptionalProperty<float>("HalfCameraOpeningAngle", 12.0 * M_PI / 180.0, "0.5 * minimal opening angles of the used cameras");
-            defineOptionalProperty<int>("SleepingTimeBetweenViewDirectionChanges", 2000, "Time between two view changes, to keep the head looking into one direction for a while (in ms)");
+            defineOptionalProperty<int>("SleepingTimeBetweenViewDirectionChanges", 2500, "Time between two view changes, to keep the head looking into one direction for a while (in ms)");
             defineOptionalProperty<bool>("ActiveAtStartup", true, "Decide whether the automatic view selection will be activated (can be changed via the proxy during runtime)");
             defineOptionalProperty<bool>("VisualizeViewDirection", false, "Draw view ray on DebugLayer.");
-            defineOptionalProperty<float>("MaxOverallHeadTiltAngle", 50.0f, "Maximal angle the head and eyes can look down (in degrees)");
-            defineOptionalProperty<float>("CentralHeadTiltAngle", 110.0f, "Defines the height direction that will be considered 'central' in the reachable area of the head (in degrees)");
+            defineOptionalProperty<float>("MaxOverallHeadTiltAngle", 55.0f, "Maximal angle the head and eyes can look down (in degrees)");
+            defineOptionalProperty<float>("CentralHeadTiltAngle", 110.0f, "Defines the height direction that will be considered 'central' in the reachable area of the head (in degrees). Default is looking 20 degrees downwards");
             defineOptionalProperty<float>("ProbabilityToLookForALostObject", 0.03f, "Probability that one of the objects that have been seen but could later not been localized again will be included in the view selection");
             defineOptionalProperty<float>("RandomNoiseLevel", 1.0f, "Maximum for the random noise that will be added to the localization necessities");
 
@@ -91,7 +91,7 @@ namespace armarx
      * @defgroup Component-ViewSelection ViewSelection
      * @ingroup RobotComponents-Components
      * @brief The ViewSelection component controls the head of the robot with inverse kinematics based on the uncertainty
-     * of the current requested robot locations.
+     * of the current requested object locations.
      * The uncertainty of objects grow based on their motion model and the timed passed since the last localization.
      * It can be activated or deactivated with the Ice interface and given manual target positions to look at.
      */
@@ -143,19 +143,21 @@ namespace armarx
 
         virtual void activateAutomaticViewSelection(const Ice::Current& c = Ice::Current())
         {
+            ARMARX_INFO << "activating automatic view selection";
             doAutomaticViewSelection = true;
         }
 
         virtual void deactivateAutomaticViewSelection(const Ice::Current& c = Ice::Current())
         {
+            ARMARX_INFO << "DEactivating automatic view selection";
             doAutomaticViewSelection = false;
         }
 
     private:
 
         void process();
-        void addSaliencyRecursive(int currentNodeIndex, float saliency, TSphereCoord objectSphereCoord, int objectIndex);
-        void setRandomNoise();
+        void addSaliencyRecursive(const int currentNodeIndex, const float saliency, const TSphereCoord objectSphereCoord, const int objectIndex, const float maxDistanceOnArc);
+        void setRandomNoise(const float centralAngleForVerticalDirection, const float directionVariabilityFactor = 1.0f);
 
         RobotStateComponentInterfacePrx robotStateComponent;
         memoryx::WorkingMemoryInterfacePrx memoryProxy;
@@ -169,6 +171,7 @@ namespace armarx
         std::string headFrameName, cameraFrameName;
 
         float randomNoiseLevel;
+        float centralHeadTiltAngle;
 
         armarx::PeriodicTask<ViewSelection>::pointer_type processorTask;
 

source/RobotComponents/gui-plugins/MMMPlayerPlugin/MMMPlayerGuiPlugin.cpp

@@ -71,6 +71,8 @@ void MMMPlayerWidget::onInitComponent()
 {
     usingProxy(MMMPLAYER_DEFAULT_NAME);
     //    ui.frameSlider->setEnabled(false);
+
+    connectSlots();
 }
 
 
@@ -87,8 +89,6 @@ void MMMPlayerWidget::onConnectComponent()
     ui.frameSlider->blockSignals(true);
     ui.frameSlider->setMaximum(MMMPlayer->getNumberOfFrames());
     on_loopPlayback_toggled(false);
-
-    connectSlots();
 }
 
 void MMMPlayerWidget::onExitComponent()

source/RobotComponents/gui-plugins/RobotIKPlugin/RobotIKGuiPlugin.cpp

@@ -305,7 +305,7 @@ armarx::StringList armarx::RobotIKWidgetController::getIncludePaths()
         StringList packages = robotStateComponentPrx->getArmarXPackages();
         packages.push_back(Application::GetProjectName());
 
-        for (const std::string & projectName : packages)
+        for (const std::string& projectName : packages)
         {
             if (projectName.empty())
             {
@@ -416,16 +416,18 @@ void armarx::RobotIKWidgetController::textFieldUpdateTimerCB(void* data, SoSenso
 
     if (controller->visualization->getIsVisualizing())
     {
-        Eigen::Matrix4f tcpMatrix = controller->robot->getRobotNodeSet(controller->ui.comboBox->currentText().toStdString())->getTCP()->getGlobalPose();
-
+        Eigen::Matrix4f tcpMatrix = controller->robot->getRobotNodeSet(controller->ui.comboBox->currentText().toStdString())->getTCP()->getPoseInRootFrame();
+        FramedPose actualPose(tcpMatrix,
+                              controller->robot->getRootNode()->getName(),
+                              controller->robot->getName());
         //Set text label to tcp matrix
-        std::stringstream buffer;
-        buffer << tcpMatrix;
-        std::string matrixText = buffer.str();
-        controller->ui.currentPoseMatrix->setText(QString::fromStdString(matrixText));
+        controller->ui.currentPoseMatrix->setText(QString::fromStdString(actualPose.output()));
 
         //Set text label for desired tcp pose
-        controller->ui.desiredPoseMatrix->setText(QString::fromStdString(controller->visualization->getUserDesiredPoseString()));
+        FramedPose desired(controller->robot->getRootNode()->toLocalCoordinateSystem(controller->visualization->getUserDesiredPose()),
+                           controller->robot->getRootNode()->getName(),
+                           controller->robot->getName());
+        controller->ui.desiredPoseMatrix->setText(QString::fromStdString(desired.output()));
     }
 }
 

source/RobotComponents/interface/components/DMPComponentBase.ice

@@ -61,6 +61,7 @@ module armarx
         void trainDMP() throws InvalidArgumentException;
         void storeDMPInDatabase(string cname) throws InvalidArgumentException;
         void readTrajectoryFromFile(string file);
+        void setDimensionsToLearn(DVector dimensions);
         void removeDMPFromDatabase(string dmpname);
 
     };

source/RobotComponents/interface/components/MMMPlayerInterface.ice

@@ -40,8 +40,8 @@ module armarx
         StringList getJointNames();
         void setCurrentFrame(int frame);
         bool setLoopPlayback(bool loop);
-        int setUsePID(int usePID);
-        void setFPS(int FPS);
+        int setUsePID(bool usePID);
+        void setFPS(float FPS);
         bool setJointsInUse(string jointName, bool inUse);
     };
 };