From 7c478bb7538e5417a22cef04f360118b5c6bdeb8 Mon Sep 17 00:00:00 2001 From: Florent Lamiraux Date: Wed, 22 Jul 2026 16:12:10 +0200 Subject: [PATCH 1/3] [pyhpp.pinocchio] Add a function to project a configuration inside the bounds. --- src/pyhpp/pinocchio/utils.py | 28 ++++++++++++++++++++++++++++ 1 file changed, 28 insertions(+) diff --git a/src/pyhpp/pinocchio/utils.py b/src/pyhpp/pinocchio/utils.py index 4d2d86b0..33efdfe0 100644 --- a/src/pyhpp/pinocchio/utils.py +++ b/src/pyhpp/pinocchio/utils.py @@ -55,3 +55,31 @@ def shrinkJointRange(robot, joints, ratio): M = mean + 0.5 * ratio * width model.lowerPositionLimit[iq] = m model.upperPositionLimit[iq] = M + +def projectInJointRange(robot, q, epsilon): + """ + Project a configuration into the joint bounds of a robot + + Input + - robot: an instance of Robot class, + - q: input configuration + - epsilon: distance inside the joint bounds each configuration variable is projected to + in order to avoid projecting on the joint limits + """ + model = robot.model() + result = q.copy() + for i in range(model.njoints): + if model.joints[i].nq != 1: + continue + iq = model.joints[i].idx_q + m, M = [model.lowerPositionLimit[iq], model.upperPositionLimit[iq]] + if m < q[iq] and q[iq] < M: + continue + if M - m < 2*epsilon: + result[iq] = .5*(m+M) + else: + if q[iq] > M: + result[iq] = M - epsilon + elif q[iq] < m: + result[iq] = m + epsilon + return result From 43f065912f5a8a1b2f8703977e9a73e4546e649f Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Wed, 22 Jul 2026 14:52:02 +0000 Subject: [PATCH 2/3] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- src/pyhpp/pinocchio/utils.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/pyhpp/pinocchio/utils.py b/src/pyhpp/pinocchio/utils.py index 33efdfe0..26acad46 100644 --- a/src/pyhpp/pinocchio/utils.py +++ b/src/pyhpp/pinocchio/utils.py @@ -56,6 +56,7 @@ def shrinkJointRange(robot, joints, ratio): model.lowerPositionLimit[iq] = m model.upperPositionLimit[iq] = M + def projectInJointRange(robot, q, epsilon): """ Project a configuration into the joint bounds of a robot @@ -75,8 +76,8 @@ def projectInJointRange(robot, q, epsilon): m, M = [model.lowerPositionLimit[iq], model.upperPositionLimit[iq]] if m < q[iq] and q[iq] < M: continue - if M - m < 2*epsilon: - result[iq] = .5*(m+M) + if M - m < 2 * epsilon: + result[iq] = 0.5 * (m + M) else: if q[iq] > M: result[iq] = M - epsilon From 8e15df2a4fa96c2d7784b8d76ea4ec34e7af58c5 Mon Sep 17 00:00:00 2001 From: Florent Lamiraux Date: Thu, 23 Jul 2026 08:25:19 +0200 Subject: [PATCH 3/3] Update src/pyhpp/pinocchio/utils.py Co-authored-by: Guilhem Saurel --- src/pyhpp/pinocchio/utils.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/pyhpp/pinocchio/utils.py b/src/pyhpp/pinocchio/utils.py index 26acad46..fade1353 100644 --- a/src/pyhpp/pinocchio/utils.py +++ b/src/pyhpp/pinocchio/utils.py @@ -74,7 +74,7 @@ def projectInJointRange(robot, q, epsilon): continue iq = model.joints[i].idx_q m, M = [model.lowerPositionLimit[iq], model.upperPositionLimit[iq]] - if m < q[iq] and q[iq] < M: + if m + epsilon <= q[iq] <= M - epsilon: continue if M - m < 2 * epsilon: result[iq] = 0.5 * (m + M)