diff --git a/data/armarx_navigation/controller_config/PlatformTrajectory/default.json b/data/armarx_navigation/controller_config/PlatformTrajectory/default.json
index 4cd38813e9ad112a708650aeb00912e426037425..e69bb8299d15eab5f3de0eb5219308b88f9cd51c 100644
--- a/data/armarx_navigation/controller_config/PlatformTrajectory/default.json
+++ b/data/armarx_navigation/controller_config/PlatformTrajectory/default.json
@@ -1,18 +1,18 @@
{
"params": {
"pidPos": {
- "Kp": 1,
+ "Kp": 0.1,
"Ki": 0,
"Kd": 0
},
"pidOri": {
- "Kp": 1,
+ "Kp": 0.1,
"Ki": 0,
"Kd": 0
},
"limits": {
- "linear": 300,
- "angular": 0.2
+ "linear": 500,
+ "angular": 0.5
}
},
"targets": {
diff --git a/source/armarx/navigation/core/CMakeLists.txt b/source/armarx/navigation/core/CMakeLists.txt
index 4ee7e461279c24d83a67e7739b2df3f17ae1a822..d95cd0ed7c210e76675b2a9f286ccdd0854f6401 100644
--- a/source/armarx/navigation/core/CMakeLists.txt
+++ b/source/armarx/navigation/core/CMakeLists.txt
@@ -13,6 +13,7 @@ armarx_add_library(core
SOURCES
StaticScene.cpp
Trajectory.cpp
+ basic_types.cpp
types.cpp
Graph.cpp
aron_conversions.cpp
diff --git a/source/armarx/navigation/core/Trajectory.cpp b/source/armarx/navigation/core/Trajectory.cpp
index ce659b7c5995d447265d4c7cf081a8d563cdedca..fc5c9b0ba2d7d2efc9516e1979ee04c62115517a 100644
--- a/source/armarx/navigation/core/Trajectory.cpp
+++ b/source/armarx/navigation/core/Trajectory.cpp
@@ -6,18 +6,6 @@
#include <iterator>
#include <limits>
-#include <range/v3/action/insert.hpp>
-#include <range/v3/numeric/accumulate.hpp>
-#include <range/v3/range/conversion.hpp>
-#include <range/v3/view/all.hpp>
-#include <range/v3/view/concat.hpp>
-#include <range/v3/view/for_each.hpp>
-#include <range/v3/view/group_by.hpp>
-#include <range/v3/view/reverse.hpp>
-#include <range/v3/view/sliding.hpp>
-#include <range/v3/view/transform.hpp>
-#include <range/v3/view/zip.hpp>
-
#include <Eigen/Core>
#include <Eigen/Geometry>
@@ -34,6 +22,17 @@
#include <armarx/navigation/conversions/eigen.h>
#include <armarx/navigation/core/basic_types.h>
#include <armarx/navigation/core/types.h>
+#include <range/v3/action/insert.hpp>
+#include <range/v3/numeric/accumulate.hpp>
+#include <range/v3/range/conversion.hpp>
+#include <range/v3/view/all.hpp>
+#include <range/v3/view/concat.hpp>
+#include <range/v3/view/for_each.hpp>
+#include <range/v3/view/group_by.hpp>
+#include <range/v3/view/reverse.hpp>
+#include <range/v3/view/sliding.hpp>
+#include <range/v3/view/transform.hpp>
+#include <range/v3/view/zip.hpp>
// FIXME move to simox
@@ -158,13 +157,14 @@ namespace armarx::navigation::core
} // namespace conv
- Projection
- GlobalTrajectory::getProjection(const Position& point,
- const VelocityInterpolation& velocityInterpolation) const
+
+ GlobalTrajectory::InternalProjection
+ GlobalTrajectory::projectInternal(const Position& point,
+ const VelocityInterpolation& velocityInterpolation) const
{
float distance = std::numeric_limits<float>::max();
- Projection bestProj;
+ InternalProjection bestProj;
for (size_t i = 0; i < pts.size() - 1; i++)
{
@@ -198,26 +198,9 @@ namespace armarx::navigation::core
math::LinearInterpolatedPose ip(
wpBefore.waypoint.pose.matrix(), wpAfter.waypoint.pose.matrix(), 0, 1, true);
- bestProj.wayPointBefore = wpBefore;
- bestProj.wayPointAfter = wpAfter;
+ bestProj.indexBefore = i;
bestProj.projection.waypoint.pose = ip.Get(t);
- bestProj.segment = [&]
- {
- if (i == 0)
- {
- return Projection::Segment::FIRST;
- }
-
- if (i == (pts.size() - 2))
- {
- return Projection::Segment::FINAL;
- }
-
- return Projection::Segment::INTERMEDIATE;
- }();
-
-
switch (velocityInterpolation)
{
case VelocityInterpolation::LinearInterpolation:
@@ -236,6 +219,82 @@ namespace armarx::navigation::core
return bestProj;
}
+ Projection
+ GlobalTrajectory::getProjection(const Position& point,
+ const VelocityInterpolation& velocityInterpolation) const
+ {
+ InternalProjection intProj = projectInternal(point, velocityInterpolation);
+
+ const auto& wpBefore = pts.at(intProj.indexBefore);
+ const auto& wpAfter = pts.at(intProj.indexBefore + 1);
+
+ Projection bestProj;
+
+ bestProj.wayPointBefore = wpBefore;
+ bestProj.wayPointAfter = wpAfter;
+ bestProj.projection = intProj.projection;
+
+ bestProj.segment = [&]
+ {
+ if (intProj.indexBefore == 0)
+ {
+ return Projection::Segment::FIRST;
+ }
+
+ if (intProj.indexBefore == (pts.size() - 2))
+ {
+ return Projection::Segment::FINAL;
+ }
+
+ return Projection::Segment::INTERMEDIATE;
+ }();
+
+ return bestProj;
+ }
+
+ std::pair<GlobalTrajectory, bool>
+ GlobalTrajectory::getSubTrajectory(const Position& point, float distance) const
+ {
+ const InternalProjection intProj =
+ projectInternal(point, VelocityInterpolation::LinearInterpolation);
+
+ GlobalTrajectory subTraj{};
+ subTraj.mutablePoints().push_back(intProj.projection);
+
+ float remainingDistance = distance;
+ GlobalTrajectoryPoint lastWp = intProj.projection;
+ for (size_t i = intProj.indexBefore + 1; i < pts.size(); i++)
+ {
+ const auto& nextWp = pts.at(i);
+
+ const float segmentDistance =
+ (nextWp.waypoint.pose.translation() - lastWp.waypoint.pose.translation()).norm();
+
+ if (segmentDistance < remainingDistance)
+ {
+ subTraj.mutablePoints().push_back(nextWp);
+ lastWp = nextWp;
+ remainingDistance -= segmentDistance;
+ }
+ else
+ {
+ // fraction of distance between segment end points
+ const float t = remainingDistance / segmentDistance;
+
+ math::LinearInterpolatedPose ip(
+ lastWp.waypoint.pose.matrix(), nextWp.waypoint.pose.matrix(), 0, 1, true);
+ const float velocity = lastWp.velocity + (nextWp.velocity - lastWp.velocity) * t;
+
+ subTraj.mutablePoints().push_back({{static_cast<Pose>(ip.Get(t))}, velocity});
+
+ return {subTraj, false};
+ }
+ }
+
+ // remaining trajectory is shorter than the specified distance
+ return {subTraj, true};
+ }
+
std::vector<Eigen::Vector3f>
GlobalTrajectory::positions() const noexcept
{
@@ -273,9 +332,9 @@ namespace armarx::navigation::core
GlobalTrajectory
GlobalTrajectory::FromPath(const Pose& start,
- const Positions& waypoints,
- const Pose& goal,
- const float velocity)
+ const Positions& waypoints,
+ const Pose& goal,
+ const float velocity)
{
// currently, only 2D version
@@ -331,7 +390,8 @@ namespace armarx::navigation::core
{
core::Positions resampledPath;
- const auto toPoint = [](const GlobalTrajectoryPoint& wp) -> Pose { return wp.waypoint.pose; };
+ const auto toPoint = [](const GlobalTrajectoryPoint& wp) -> Pose
+ { return wp.waypoint.pose; };
const core::Path originalPath = pts | ranges::views::transform(toPoint) | ranges::to_vector;
@@ -601,7 +661,7 @@ namespace armarx::navigation::core
constexpr int nSamples = 100;
- for(int j = 0; j < nSamples; j++)
+ for (int j = 0; j < nSamples; j++)
{
const float progress = static_cast<float>(j) / nSamples;
@@ -675,7 +735,6 @@ namespace armarx::navigation::core
return indices;
}
-
const std::vector<LocalTrajectoryPoint>&
LocalTrajectory::points() const
{
@@ -689,7 +748,7 @@ namespace armarx::navigation::core
}
Evaluation
- LocalTrajectory::evaluate(armarx::core::time::DateTime timestamp) const
+ LocalTrajectory::evaluate(const armarx::core::time::DateTime& timestamp) const
{
const auto cmp = [](const core::LocalTrajectoryPoint& lhs,
const DateTime& timestamp) -> bool
@@ -700,20 +759,43 @@ namespace armarx::navigation::core
if (lower == pts.end() - 1)
{
// if we arrived at the last point, the velocity is 0
- return {lower->pose, 0};
+ return {lower->pose, core::Twist::Zero()};
}
const Duration d1 = timestamp - lower->timestamp;
const Duration dT = lower[1].timestamp - lower->timestamp;
+ // the waypoint before this timestamp
+ const auto& global_T_wp_before = lower->pose;
+
+ // the waypoint after this timestamp
+ const auto& global_T_wp_after = lower[1].pose;
+
// fraction of time between segment end points
const float t = d1 / dT;
- math::LinearInterpolatedPose ip(lower->pose.matrix(), lower[1].pose.matrix(), 0, 1, true);
+ math::LinearInterpolatedPose ip(
+ global_T_wp_before.matrix(), global_T_wp_after.matrix(), 0, 1, true);
+
+ const core::Pose wp_before_T_wp_after = global_T_wp_before.inverse() * global_T_wp_after;
+ const Eigen::Matrix3f& global_R_wp_before = global_T_wp_before.linear();
+
+ // the movement direction in the global frame
+ const Eigen::Vector3f global_V_linear_movement_direction =
+ global_R_wp_before * wp_before_T_wp_after.translation().normalized();
const Eigen::Vector3f distance = lower[1].pose.translation() - lower->pose.translation();
- const float speed = distance.norm() / dT.toSecondsDouble();
+ const float linearVelocity = static_cast<float>(distance.norm()) / dT.toSecondsDouble();
+
+
+ // angular velocity
+
+ Eigen::AngleAxisf angleAx(wp_before_T_wp_after.linear());
+
+ const core::Twist feedforwardTwist{
+ .linear = linearVelocity * global_V_linear_movement_direction,
+ .angular = angleAx.axis() * angleAx.angle() / dT.toSecondsDouble()};
- return {static_cast<core::Pose>(ip.Get(t)), speed};
+ return {static_cast<core::Pose>(ip.Get(t)), feedforwardTwist};
}
diff --git a/source/armarx/navigation/core/Trajectory.h b/source/armarx/navigation/core/Trajectory.h
index 19745b6d0eb83d76248436053e2884cbd29bc973..20da0ba1f577865ab3e7938a1dddb4f3e565275a 100644
--- a/source/armarx/navigation/core/Trajectory.h
+++ b/source/armarx/navigation/core/Trajectory.h
@@ -24,6 +24,7 @@
#include <cstddef>
#include <memory>
+
#include "ArmarXCore/core/time/DateTime.h"
#include <armarx/navigation/core/basic_types.h>
@@ -76,6 +77,15 @@ namespace armarx::navigation::core
Projection getProjection(const Position& point,
const VelocityInterpolation& velocityInterpolation) const;
+ /**
+ * @brief Project point onto the trajectory and return a new trajectory along the old one
+ * from that point for the specified distance.
+ * @return The subtrajectory and whether the subtrajectory ends at the same point,
+ * as this trajectory
+ */
+ std::pair<GlobalTrajectory, bool> getSubTrajectory(const Position& point,
+ const float distance) const;
+
[[nodiscard]] std::vector<Position> positions() const noexcept;
[[nodiscard]] std::vector<Pose> poses() const noexcept;
@@ -117,6 +127,16 @@ namespace armarx::navigation::core
*/
Indices allConnectedPointsInRange(std::size_t idx, float radius) const;
+ private:
+ struct InternalProjection
+ {
+ GlobalTrajectoryPoint projection;
+ size_t indexBefore;
+ };
+ InternalProjection
+ projectInternal(const Position& point,
+ const VelocityInterpolation& velocityInterpolation) const;
+
private:
std::vector<GlobalTrajectoryPoint> pts;
@@ -134,7 +154,7 @@ namespace armarx::navigation::core
struct Evaluation
{
core::Pose pose;
- float velocity; // [mm/s]
+ core::Twist feedforwardTwist; // [mm/s]
};
@@ -151,7 +171,7 @@ namespace armarx::navigation::core
std::vector<LocalTrajectoryPoint>& mutablePoints();
- Evaluation evaluate(DateTime timestamp) const;
+ Evaluation evaluate(const DateTime& timestamp) const;
private:
diff --git a/source/armarx/navigation/core/basic_types.cpp b/source/armarx/navigation/core/basic_types.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..460c560bc8905f63a3b3eb168193c6faf5d11bef
--- /dev/null
+++ b/source/armarx/navigation/core/basic_types.cpp
@@ -0,0 +1,18 @@
+#include "basic_types.h"
+
+namespace armarx::navigation::core
+{
+ core::Pose
+ Twist::poseDiff(const float dt) const
+ {
+ return core::Pose(Eigen::Translation3f(linear * dt)) *
+ core::Pose(Eigen::AngleAxisf(angular.norm() * dt, angular.normalized()));
+ }
+
+ Twist
+ Twist::Zero()
+ {
+ static const core::Twist zero{Eigen::Vector3f::Zero(), Eigen::Vector3f::Zero()};
+ return zero;
+ }
+} // namespace armarx::navigation::core
diff --git a/source/armarx/navigation/core/basic_types.h b/source/armarx/navigation/core/basic_types.h
index 0917270df14fcf9568f56f05ddb4a7492cc9e70e..6121a739f2359abe352a61ff82d765cf21c7c306 100644
--- a/source/armarx/navigation/core/basic_types.h
+++ b/source/armarx/navigation/core/basic_types.h
@@ -50,4 +50,14 @@ namespace armarx::navigation::core
Pose pose;
};
+ struct Twist
+ {
+ LinearVelocity linear;
+ AngularVelocity angular;
+
+ Pose poseDiff(float dt) const;
+
+ static Twist Zero();
+ };
+
} // namespace armarx::navigation::core
diff --git a/source/armarx/navigation/core/types.cpp b/source/armarx/navigation/core/types.cpp
index 346cd4de2e7bc71ef3b0ec7ac7acc3f22a6e995c..7a25d9285728277b8121613da84fe939a7d3d4f1 100644
--- a/source/armarx/navigation/core/types.cpp
+++ b/source/armarx/navigation/core/types.cpp
@@ -2,17 +2,17 @@
namespace armarx::navigation::core
{
- core::Pose
- Twist::poseDiff(const float dt) const
- {
- return core::Pose(Eigen::Translation3f(linear * dt)) *
- core::Pose(Eigen::AngleAxisf(angular.norm() * dt, angular.normalized()));
- }
+ // core::Pose
+ // Twist::poseDiff(const float dt) const
+ // {
+ // return core::Pose(Eigen::Translation3f(linear * dt)) *
+ // core::Pose(Eigen::AngleAxisf(angular.norm() * dt, angular.normalized()));
+ // }
- Twist
- Twist::Zero()
- {
- static const core::Twist zero{Eigen::Vector3f::Zero(), Eigen::Vector3f::Zero()};
- return zero;
- }
+ // Twist
+ // Twist::Zero()
+ // {
+ // static const core::Twist zero{Eigen::Vector3f::Zero(), Eigen::Vector3f::Zero()};
+ // return zero;
+ // }
} // namespace armarx::navigation::core
diff --git a/source/armarx/navigation/core/types.h b/source/armarx/navigation/core/types.h
index 9ce943acd75f34d153eda8c06974f6b5bfcc64d5..ade2630f76f818af1928cf77365246c79b923560 100644
--- a/source/armarx/navigation/core/types.h
+++ b/source/armarx/navigation/core/types.h
@@ -58,15 +58,7 @@ namespace armarx::navigation::core
};
- struct Twist
- {
- LinearVelocity linear;
- AngularVelocity angular;
-
- Pose poseDiff(float dt) const;
-
- static Twist Zero();
- };
+
class TimeServerInterface;
diff --git a/source/armarx/navigation/local_planning/TimedElasticBands.cpp b/source/armarx/navigation/local_planning/TimedElasticBands.cpp
index 0d61b9ed53fb3b164a1d8de17f8ad5b5fd384914..35881b6672026a908a9c3e6c6564b81da181f40a 100644
--- a/source/armarx/navigation/local_planning/TimedElasticBands.cpp
+++ b/source/armarx/navigation/local_planning/TimedElasticBands.cpp
@@ -101,16 +101,20 @@ namespace armarx::navigation::local_planning
std::optional<LocalPlannerResult>
TimedElasticBands::plan(const core::GlobalTrajectory& goal)
{
- const teb_local_planner::TimedElasticBand globalPath = conv::toRos(goal);
+ const core::Pose currentPose{scene.robot->getGlobalPose()};
+
+ // TODO (SALt): put distance parameter into config (note: distance is in mm)
+ // prune global trajectory
+ const auto& [prunedGoal, planToDest] =
+ goal.getSubTrajectory(currentPose.translation(), 2000);
+
+ const teb_local_planner::TimedElasticBand globalPath = conv::toRos(prunedGoal);
teb_globalPath = globalPath.poses();
hcp_->setGlobalPath(&teb_globalPath);
- const core::Pose currentPose{scene.robot->getGlobalPose()};
-
const teb_local_planner::PoseSE2 start = conv::toRos(currentPose);
-
- const FindTargetResult target = findTarget(currentPose, goal);
- const teb_local_planner::PoseSE2 end = conv::toRos(target.target);
+ const teb_local_planner::PoseSE2 end =
+ conv::toRos(prunedGoal.points().back().waypoint.pose);
geometry_msgs::Twist velocity_start = conv::toRos(scene.platformVelocity);
@@ -119,7 +123,8 @@ namespace armarx::navigation::local_planning
try
{
- hcp_->plan(start, end, &velocity_start, !target.planToDestination);
+ // TODO (SALt): free goal velocity is not respected inside homotopy planner
+ hcp_->plan(start, end, &velocity_start, !planToDest);
}
catch (std::exception& e)
{
@@ -135,20 +140,6 @@ namespace armarx::navigation::local_planning
}
- TimedElasticBands::FindTargetResult
- TimedElasticBands::findTarget(const core::Pose& currentPose,
- const core::GlobalTrajectory& globalPath)
- {
- // TODO (SALt): implement
-
- const core::Projection projection = globalPath.getProjection(
- currentPose.translation(), core::VelocityInterpolation::LinearInterpolation);
-
- return {conv::to2D(projection.projection.waypoint.pose),
- conv::to2D(globalPath.points().back().waypoint.pose),
- true};
- }
-
void
TimedElasticBands::fillObstacles()
{
diff --git a/source/armarx/navigation/local_planning/TimedElasticBands.h b/source/armarx/navigation/local_planning/TimedElasticBands.h
index caf2c832a94559ef91ae886d3b25a0c0e055fa36..6bded06728b9b26664c57065b20b641884df8693 100644
--- a/source/armarx/navigation/local_planning/TimedElasticBands.h
+++ b/source/armarx/navigation/local_planning/TimedElasticBands.h
@@ -59,16 +59,7 @@ namespace armarx::navigation::local_planning
std::optional<LocalPlannerResult> plan(const core::GlobalTrajectory& goal) override;
private:
- struct FindTargetResult
- {
- core::Pose2D projection;
- core::Pose2D target;
- bool planToDestination;
- };
-
void initDefaultConfig();
- FindTargetResult findTarget(const core::Pose& currentPose,
- const core::GlobalTrajectory& globalPath);
void fillObstacles();
protected:
diff --git a/source/armarx/navigation/trajectory_control/local/TrajectoryFollowingController.cpp b/source/armarx/navigation/trajectory_control/local/TrajectoryFollowingController.cpp
index e4e2cbc4192cb0972852c8a50f7ed9f86b856305..a16cb65692e8cc6e5934d84f82bc76ef24cf32f3 100644
--- a/source/armarx/navigation/trajectory_control/local/TrajectoryFollowingController.cpp
+++ b/source/armarx/navigation/trajectory_control/local/TrajectoryFollowingController.cpp
@@ -154,9 +154,18 @@ namespace armarx::navigation::traj_ctrl::local
pidPos.update(mat4f_to_pos(currentPose.matrix()), mat4f_to_pos(target.pose.matrix()));
pidOri.update(mat4f_to_rpy(currentPose.matrix()), mat4f_to_rpy(target.pose.matrix()));
- const core::Twist twist{.linear = pidPos.getControlValue(),
+
+ const core::Twist& twistFeedForward = target.feedforwardTwist;
+
+ const core::Twist twistError{.linear = pidPos.getControlValue(),
.angular = pidOri.getControlValue()};
+ const core::Twist twist
+ {
+ .linear = twistFeedForward.linear + twistError.linear,
+ .angular = twistFeedForward.angular + twistError.angular
+ };
+
const auto twistLimited = applyTwistLimits(twist);
ARMARX_VERBOSE << deactivateSpam(1) << "Twist limited " << twistLimited.linear.transpose();
ARMARX_VERBOSE << deactivateSpam(1) << "Twist angular " << twistLimited.angular.transpose();