From 7e9515992c3c9f73be9977fbda8322049c2195b6 Mon Sep 17 00:00:00 2001
From: Patrick Dormanns <patrick.dormanns@student.kit.edu>
Date: Fri, 23 Feb 2024 21:25:23 +0100
Subject: [PATCH] fix error that skill crashes when already pointing at target
---
source/armarx/control/pointing/core/Pointing.cpp | 15 +++++++++++----
source/armarx/control/pointing/core/PointingIK.h | 3 ++-
2 files changed, 13 insertions(+), 5 deletions(-)
diff --git a/source/armarx/control/pointing/core/Pointing.cpp b/source/armarx/control/pointing/core/Pointing.cpp
index f10ca43c..91f13a70 100644
--- a/source/armarx/control/pointing/core/Pointing.cpp
+++ b/source/armarx/control/pointing/core/Pointing.cpp
@@ -38,10 +38,17 @@ namespace armarx::control::pointing::core
addHandTrajectory(robot, traj, shapeJointValues);
}
- traj = traj->calculateTimeOptimalTrajectory(
- params.maxVelocity, params.maxAcceleration, 0.0, IceUtil::Time::milliSeconds(10));
+ if (traj->dim() > 0)
+ {
+ traj = traj->calculateTimeOptimalTrajectory(
+ params.maxVelocity, params.maxAcceleration, 0.0, IceUtil::Time::milliSeconds(10));
- playTrajectory(traj);
+ playTrajectory(traj);
+ }
+ else
+ {
+ ARMARX_INFO << "Already pointing at target";
+ }
}
void
@@ -89,7 +96,7 @@ namespace armarx::control::pointing::core
}
ARMARX_VERBOSE << "Done computing trajectory";
- if (traj->getLength() == 1) // already pointing at target
+ if (traj->size() == 1) // already pointing at target
{
return armarx::TrajectoryPtr(new Trajectory);
}
diff --git a/source/armarx/control/pointing/core/PointingIK.h b/source/armarx/control/pointing/core/PointingIK.h
index 028e5b4b..8660d52a 100644
--- a/source/armarx/control/pointing/core/PointingIK.h
+++ b/source/armarx/control/pointing/core/PointingIK.h
@@ -47,7 +47,8 @@ namespace armarx::control::pointing::core
*
* @param target Target position to point at in the global coordinate system (mm).
* @return The computed trajectory. Null if no solution was found. If the robot has already
- * pointed at the target, the trajectory only contains the initial timestep.
+ * pointed at the target, the trajectory only contains the initial timestep (i.e.
+ * size()==1).
*/
TrajectoryPtr computeTrajectory(Eigen::Vector3f& target);
--
GitLab