From 04ab9b0b1dd99915cb81e4b66cba6dd8de54aaa9 Mon Sep 17 00:00:00 2001 From: Franco Cipollone Date: Thu, 30 Mar 2023 18:22:35 -0300 Subject: [PATCH] Updates to use maliput's math last bindings. Signed-off-by: Franco Cipollone --- .../maliput_pure_pursuit_circuit.py | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/controllers/maliput_pure_pursuit_circuit/maliput_pure_pursuit_circuit.py b/controllers/maliput_pure_pursuit_circuit/maliput_pure_pursuit_circuit.py index 12a7d31..72ec554 100644 --- a/controllers/maliput_pure_pursuit_circuit/maliput_pure_pursuit_circuit.py +++ b/controllers/maliput_pure_pursuit_circuit/maliput_pure_pursuit_circuit.py @@ -16,17 +16,13 @@ import math import os -def norm(vector3): - return math.sqrt(vector3.x() * vector3.x() + vector3.y() * vector3.y() + vector3.z() * vector3.z()) - def ComputeCurvature(s_lookahead, rg, inertial_position, heading): goal_position = ComputeGoalPoint(s_lookahead, rg, inertial_position) inertial_position = inertial_position.xyz() x = inertial_position.x() y = inertial_position.y() delta_r = -(goal_position.x() - x) * math.sin(heading) + (goal_position.y() - y) * math.cos(heading) - difference_vector = maliput.math.Vector3(goal_position.x() - inertial_position.x(), goal_position.y() - inertial_position.y(), goal_position.z() - inertial_position.z()) - curvature = 2. * delta_r / math.pow(norm(difference_vector), 2.) + curvature = 2. * delta_r / math.pow((goal_position-inertial_position).norm(), 2.) return curvature def ComputeGoalPoint(s_lookahead, rg, inertial_position):