From b46a5f0cddfc4486b4fcfec75d9636ed1c7c2516 Mon Sep 17 00:00:00 2001
From: Pierre Fernbach <pierre.fernbach@laas.fr>
Date: Wed, 27 Nov 2019 18:05:13 +0100
Subject: [PATCH] [Python] piecewiseSE3.append from final transform correctly
raises error when piecewise is empty
---
python/curves_python.cpp | 4 ++++
python/test/test.py | 3 +++
2 files changed, 7 insertions(+)
diff --git a/python/curves_python.cpp b/python/curves_python.cpp
index 5a561d7..715488a 100644
--- a/python/curves_python.cpp
+++ b/python/curves_python.cpp
@@ -360,6 +360,8 @@ Motion_t piecewiseSE3ReturnDerivatePinocchio(const piecewise_SE3_curve_t& curve,
}
void addFinalSE3(piecewise_SE3_curve_t& self, const SE3_t& end, const real time) {
+ if(self.num_curves() == 0)
+ throw std::runtime_error("Piecewise append : you need to add at least one curve before using append(finalPoint) method.");
if (self.is_continuous(1) && self.num_curves()>1 )
std::cout << "Warning: by adding this final transform to the piecewise curve, you loose C1 continuity and only "
"guarantee C0 continuity."
@@ -378,6 +380,8 @@ matrix3_t piecewiseSE3returnRotation(const piecewise_SE3_curve_t& curve, const r
pointX_t piecewiseSE3returnTranslation(const piecewise_SE3_curve_t& curve, const real t) { return pointX_t(curve(t).translation()); }
void addFinalTransform(piecewise_SE3_curve_t& self, const matrix4_t& end, const real time) {
+ if(self.num_curves() == 0)
+ throw std::runtime_error("Piecewise append : you need to add at least one curve before using append(finalPoint) method.");
if (self.is_continuous(1) && self.num_curves()>1 )
std::cout << "Warning: by adding this final transform to the piecewise curve, you loose C1 continuity and only "
"guarantee C0 continuity."
diff --git a/python/test/test.py b/python/test/test.py
index de0996b..a44dbe5 100644
--- a/python/test/test.py
+++ b/python/test/test.py
@@ -638,6 +638,9 @@ class TestCurves(unittest.TestCase):
self.assertTrue(isclose(pmin[0:3, 3], translation(min)).all())
self.assertTrue(isclose(pmax[0:3, 3], end_translation).all())
self.assertTrue(pc.is_continuous(0))
+ pc = piecewise_SE3_curve()
+ with self.assertRaises(RuntimeError):
+ pc.append(end_pose,max)
if CURVES_WITH_PINOCCHIO_SUPPORT:
--
GitLab