Feature: Add statechart helpers for obstacle avoiding platform unit.

data/RobotAPI/VariantInfo-RobotAPI.xml

@@ -40,6 +40,14 @@
             getterName="getPlatformUnit"
             propertyName="PlatformUnitName"
             propertyIsOptional="false" />
+        <Proxy include="RobotAPI/interface/units/PlatformUnitInterface.h"
+            humanName="Platform Unit (obstacle avoiding)"
+            typeName="PlatformUnitInterfacePrx"
+            memberName="obstacleAvoidingPlatformUnit"
+            getterName="getObstacleAvoidingPlatformUnit"
+            propertyName="ObstacleAvoidingPlatformUnitName"
+            propertyDefaultValue="ObstacleAvoidingPlatformUnit"
+            propertyIsOptional="true" />
         <Proxy include="RobotAPI/interface/observers/PlatformUnitObserverInterface.h"
             humanName="Platform Unit Observer"
             typeName="PlatformUnitObserverInterfacePrx"

source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/ObstacleAvoidingPlatformUnit.cpp

@@ -619,7 +619,7 @@ const
                                               angular_similarities.end(),
                                               0.f,
                                               std::plus<float>());
-        const float max_vel_factor = std::pow(mean_angular_similarity, 2);
+        const float max_vel_factor = std::pow(mean_angular_similarity, 1.5);
 
         return max_vel_factor * m_control_data.max_vel;
     }

source/RobotAPI/libraries/RobotStatechartHelpers/CMakeLists.txt

@@ -14,6 +14,7 @@ PositionControllerHelper.cpp
 ForceTorqueHelper.cpp
 KinematicUnitHelper.cpp
 RobotNameHelper.cpp
+ObstacleAvoidingPlatformUnitHelper.cpp
 )
 set(LIB_HEADERS
 VelocityControllerHelper.h
@@ -21,6 +22,7 @@ PositionControllerHelper.h
 ForceTorqueHelper.h
 KinematicUnitHelper.h
 RobotNameHelper.h
+ObstacleAvoidingPlatformUnitHelper.h
 )
 
 armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${COMPONENT_LIBS}")

source/RobotAPI/libraries/RobotStatechartHelpers/ObstacleAvoidingPlatformUnitHelper.cpp

+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package    RobotAPI::ArmarXObjects::ObstacleAvoidingPlatformUnit
+ * @author     Christian R. G. Dreher <c.dreher@kit.edu>
+ * @date       2020
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+
+#include <RobotAPI/libraries/RobotStatechartHelpers/ObstacleAvoidingPlatformUnitHelper.h>
+
+
+// STD/STL
+#include <limits>
+
+// Simox
+#include <SimoxUtility/math.h>
+
+
+armarx::ObstacleAvoidingPlatformUnitHelper::ObstacleAvoidingPlatformUnitHelper(
+    armarx::PlatformUnitInterfacePrx platform_unit,
+    VirtualRobot::RobotPtr robot,
+    const float pos_reached_threshold,
+    const float ori_reached_threshold) :
+    m_platform_unit{platform_unit},
+    m_robot{robot},
+    m_pos_reached_threshold{pos_reached_threshold},
+    m_ori_reached_threshold{ori_reached_threshold}
+{
+    const float inf = std::numeric_limits<float>::infinity();
+    m_target.pos = Eigen::Vector2f{inf, inf};
+    m_target.ori = inf;
+}
+
+
+armarx::ObstacleAvoidingPlatformUnitHelper::~ObstacleAvoidingPlatformUnitHelper()
+{
+    m_platform_unit->stopPlatform();
+}
+
+
+void
+armarx::ObstacleAvoidingPlatformUnitHelper::setTarget(
+    const Eigen::Vector2f& target_pos,
+    const float target_ori)
+{
+    m_target.pos = target_pos;
+    m_target.ori = target_ori;
+}
+
+
+void
+armarx::ObstacleAvoidingPlatformUnitHelper::update()
+{
+    m_platform_unit->moveTo(m_target.pos.x(), m_target.pos.y(), m_target.ori,
+                            m_pos_reached_threshold, m_ori_reached_threshold);
+}
+
+
+bool
+armarx::ObstacleAvoidingPlatformUnitHelper::isFinalTargetReached()
+const
+{
+    using namespace simox::math;
+
+    // Determine agent position.
+    const Eigen::Vector2f agent_pos = m_robot->getGlobalPosition().head<2>();
+    const float agent_ori =
+        periodic_clamp<float>(mat4f_to_rpy(m_robot->getGlobalPose()).z(), -M_PI, M_PI);
+
+    // Determine distance and angular distance to goal position and orientation.
+    const float target_dist = (m_target.pos - agent_pos).norm();
+    const float target_angular_dist = periodic_clamp<float>(m_target.ori - agent_ori, -M_PI, M_PI);
+
+    return target_dist < m_pos_reached_threshold and
+           std::fabs(target_angular_dist) < m_ori_reached_threshold;
+}

source/RobotAPI/libraries/RobotStatechartHelpers/ObstacleAvoidingPlatformUnitHelper.h

+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package    RobotAPI::ArmarXObjects::ObstacleAvoidingPlatformUnit
+ * @author     Christian R. G. Dreher <c.dreher@kit.edu>
+ * @date       2020
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+
+#pragma once
+
+
+// Eigen
+#include <Eigen/Core>
+
+// Simox
+#include <VirtualRobot/Nodes/RobotNode.h>
+
+// RobotAPI
+#include <RobotAPI/components/units/ObstacleAvoidingPlatformUnit/ObstacleAvoidingPlatformUnit.h>
+
+
+namespace armarx
+{
+
+    class ObstacleAvoidingPlatformUnitHelper
+    {
+
+    public:
+
+        ObstacleAvoidingPlatformUnitHelper(
+            armarx::PlatformUnitInterfacePrx platform_unit,
+            VirtualRobot::RobotPtr robot,
+            float pos_reached_threshold,
+            float ori_reached_threshold);
+
+        virtual
+        ~ObstacleAvoidingPlatformUnitHelper();
+
+        void
+        setTarget(const Eigen::Vector2f& target_pos, float target_ori);
+
+        void
+        update();
+
+        bool
+        isFinalTargetReached()
+        const;
+
+    private:
+
+        struct target
+        {
+            Eigen::Vector2f pos;
+            float ori;
+        };
+
+        armarx::PlatformUnitInterfacePrx m_platform_unit;
+
+        VirtualRobot::RobotPtr m_robot;
+
+        target m_target;
+
+        float m_pos_reached_threshold;
+        float m_ori_reached_threshold;
+
+    };
+
+}