Skip to content

Commit

Permalink
Updates to use maliput's math last bindings.
Browse files Browse the repository at this point in the history
Signed-off-by: Franco Cipollone <[email protected]>
  • Loading branch information
francocipollone committed Mar 30, 2023
1 parent 238f7b6 commit 04ab9b0
Showing 1 changed file with 1 addition and 5 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -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):
Expand Down

0 comments on commit 04ab9b0

Please sign in to comment.