Scaling support for RobotStateComponent (also fixes issues with poses of...

Scaling support for RobotStateComponent (also fixes issues with poses of scaled models in the world)

source/RobotAPI/components/RobotState/RobotStateComponent.cpp

@@ -68,6 +68,14 @@ namespace armarx
         this->_synchronized = VirtualRobot::RobotIO::loadRobot(robotFile, VirtualRobot::RobotIO::eStructure);
         _synchronized->setName(getProperty<std::string>("AgentName").getValue());
 
+        robotModelScaling = getProperty<float>("RobotModelScaling").getValue();
+        ARMARX_INFO << "scale factor: " << robotModelScaling;
+        if (robotModelScaling != 1.0f)
+        {
+            ARMARX_INFO << "Scaling robot model with scale factor " << robotModelScaling;
+            _synchronized = _synchronized->clone(_synchronized->getName(), _synchronized->getCollisionChecker(), robotModelScaling);
+        }
+
         if (this->_synchronized)
         {
             ARMARX_VERBOSE << "Loaded robot from file " << robotFile << ". Robot name: " << this->_synchronized->getName();
@@ -101,6 +109,7 @@ namespace armarx
         }
         usingTopic(robotNodeSetName + "State");
 
+
         /*VirtualRobot::RobotNodeSetPtr pns = this->_synchronized->getRobotNodeSet("Platform");
         if (pns)
         {
@@ -198,6 +207,11 @@ namespace armarx
         return relativeRobotFile;
     }
 
+    float RobotStateComponent::getScaling(const Ice::Current&) const
+    {
+        return robotModelScaling;
+    }
+
     std::vector<string> RobotStateComponent::getArmarXPackages(const Current&) const
     {
         std::vector<string> result;

source/RobotAPI/components/RobotState/RobotStateComponent.h

@@ -51,6 +51,7 @@ namespace armarx
             defineRequiredProperty<std::string>("RobotNodeSetName", "Set of nodes that is controlled by the KinematicUnit");
             defineRequiredProperty<std::string>("RobotFileName", "Filename of VirtualRobot robot model (e.g. robot_model.xml)");
             defineRequiredProperty<std::string>("AgentName", "Name of the agent for which the sensor values are provided");
+            defineOptionalProperty<float>("RobotModelScaling", 1.0f, "Scaling of the robot model");
         }
     };
 
@@ -126,6 +127,8 @@ namespace armarx
             return "RobotStateComponent";
         }
         void setRobotStateObserver(RobotStateObserverPtr observer);
+
+        float getScaling(const Ice::Current&) const;
     protected:
         /**
          * Load and create a VirtualRobot::Robot instance from the RobotFileName
@@ -162,7 +165,7 @@ namespace armarx
 
         std::string robotNodeSetName;
 
-
+        float robotModelScaling;
     };
 
 }

source/RobotAPI/interface/core/RobotState.ice

@@ -157,6 +157,14 @@ module armarx
        ["cpp:const"]
        idempotent string getRobotName() throws NotInitializedException;
 
+
+       /**
+        * @return The scaling of the robot model represented by this component.
+        *
+        */
+       ["cpp:const"]
+       idempotent float getScaling();
+
        /**
         * @return The name of the robot node set that is represented by this component.
         *

source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp

@@ -322,10 +322,10 @@ namespace armarx
 
     VirtualRobot::RobotPtr RemoteRobot::createLocalClone(RobotStateComponentInterfacePrx robotStatePrx, const string& filename)
     {
-        return createLocalClone(robotStatePrx->getSynchronizedRobot(), filename);
+        return createLocalClone(robotStatePrx->getSynchronizedRobot(), filename, robotStatePrx->getScaling());
     }
 
-    RobotPtr RemoteRobot::createLocalClone(SharedRobotInterfacePrx sharedRobotPrx, const string& filename)
+    RobotPtr RemoteRobot::createLocalClone(SharedRobotInterfacePrx sharedRobotPrx, const string& filename, float scaling)
     {
         boost::recursive_mutex::scoped_lock cloneLock(m);
         ARMARX_VERBOSE_S << "Creating local clone of remote robot (filename:" << filename << ")" << endl;
@@ -357,6 +357,12 @@ namespace armarx
                 ARMARX_ERROR_S << "Could not load robot file " << filename << ". Aborting..." << endl;
                 return result;
             }
+
+            if (scaling != 1.0f)
+            {
+                ARMARX_INFO_S << "Scaling robot to " << scaling;
+                result = result->clone(result->getName(), result->getCollisionChecker(), scaling);
+            }
         }
 
         synchronizeLocalClone(result, sharedRobotPrx);

source/RobotAPI/libraries/core/remoterobot/RemoteRobot.h

@@ -187,7 +187,7 @@ namespace armarx
             */
         static VirtualRobot::RobotPtr createLocalClone(RobotStateComponentInterfacePrx robotStatePrx, const std::string& filename = std::string());
 
-        static VirtualRobot::RobotPtr createLocalClone(SharedRobotInterfacePrx sharedRobotPrx, const std::string& filename = std::string());
+        static VirtualRobot::RobotPtr createLocalClone(SharedRobotInterfacePrx sharedRobotPrx, const std::string& filename = std::string(), float scaling = 1.0f);
 
         /*!
                 Use this method to synchronize (i.e. copy the joint values) from the remote robot to the local clone.