From 6a3eba5c671e72e846af28439b87c8b7f3230e5c Mon Sep 17 00:00:00 2001
From: Florent Lamiraux <florent@laas.fr>
Date: Sun, 14 Jan 2018 23:01:34 +0100
Subject: [PATCH] Check input path of GraphPathValidation
check that the end configuration satisfies the constraint of the path.
---
src/graph-path-validation.cc | 6 ++++++
1 file changed, 6 insertions(+)
diff --git a/src/graph-path-validation.cc b/src/graph-path-validation.cc
index 2951f6a..710b16c 100644
--- a/src/graph-path-validation.cc
+++ b/src/graph-path-validation.cc
@@ -96,6 +96,12 @@ namespace hpp {
bool GraphPathValidation::impl_validate (const PathPtr_t& path,
bool reverse, PathPtr_t& validPart, PathValidationReportPtr_t& report)
{
+#ifndef NDEBUG
+ bool success;
+ Configuration_t q0 = (*path) (path->timeRange ().second, success);
+ assert (success);
+ assert (!path->constraints () || path->constraints ()->isSatisfied (q0));
+#endif
using pinocchio::displayConfig;
PathVectorPtr_t pathVector = HPP_DYNAMIC_PTR_CAST(PathVector, path);
if (pathVector)
--
GitLab