From 3286a3eaea0be5482ea9865de95e8637625a48e4 Mon Sep 17 00:00:00 2001
From: "Christian R. G. Dreher" <c.dreher@kit.edu>
Date: Wed, 6 May 2020 13:38:02 +0200
Subject: [PATCH] Feature: Add statechart helpers for obstacle avoiding
platform unit.
---
data/RobotAPI/VariantInfo-RobotAPI.xml | 8 ++
.../ObstacleAvoidingPlatformUnit.cpp | 2 +-
.../RobotStatechartHelpers/CMakeLists.txt | 2 +
.../ObstacleAvoidingPlatformUnitHelper.cpp | 91 +++++++++++++++++++
.../ObstacleAvoidingPlatformUnitHelper.h | 83 +++++++++++++++++
5 files changed, 185 insertions(+), 1 deletion(-)
create mode 100644 source/RobotAPI/libraries/RobotStatechartHelpers/ObstacleAvoidingPlatformUnitHelper.cpp
create mode 100644 source/RobotAPI/libraries/RobotStatechartHelpers/ObstacleAvoidingPlatformUnitHelper.h
diff --git a/data/RobotAPI/VariantInfo-RobotAPI.xml b/data/RobotAPI/VariantInfo-RobotAPI.xml
index ba5d53afa..0913ca801 100644
--- a/data/RobotAPI/VariantInfo-RobotAPI.xml
+++ b/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"
diff --git a/source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/ObstacleAvoidingPlatformUnit.cpp b/source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/ObstacleAvoidingPlatformUnit.cpp
index 3e4bca389..9a53de2ab 100644
--- a/source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/ObstacleAvoidingPlatformUnit.cpp
+++ b/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;
}
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/CMakeLists.txt b/source/RobotAPI/libraries/RobotStatechartHelpers/CMakeLists.txt
index f52b52216..9725023a8 100644
--- a/source/RobotAPI/libraries/RobotStatechartHelpers/CMakeLists.txt
+++ b/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}")
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/ObstacleAvoidingPlatformUnitHelper.cpp b/source/RobotAPI/libraries/RobotStatechartHelpers/ObstacleAvoidingPlatformUnitHelper.cpp
new file mode 100644
index 000000000..27525c42d
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/ObstacleAvoidingPlatformUnitHelper.cpp
@@ -0,0 +1,91 @@
+/*
+ * 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;
+}
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/ObstacleAvoidingPlatformUnitHelper.h b/source/RobotAPI/libraries/RobotStatechartHelpers/ObstacleAvoidingPlatformUnitHelper.h
new file mode 100644
index 000000000..96bd461e7
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/ObstacleAvoidingPlatformUnitHelper.h
@@ -0,0 +1,83 @@
+/*
+ * 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;
+
+ };
+
+}
--
GitLab