Skip to content

Commit

Permalink
Fix Bezier construction to match Drake's changed convention (#43)
Browse files Browse the repository at this point in the history
  • Loading branch information
mpetersen94 authored Feb 25, 2023
1 parent d975258 commit 60b2074
Showing 1 changed file with 7 additions and 4 deletions.
11 changes: 7 additions & 4 deletions gcs/bezier.py
Original file line number Diff line number Diff line change
Expand Up @@ -66,18 +66,18 @@ def __init__(self, regions, order, continuity, edges=None, hdot_min=1e-6, full_d
self.u_vars = np.concatenate((u_control.flatten("F"), u_duration))
self.u_r_trajectory = BsplineTrajectory_[Expression](
BsplineBasis_[Expression](order + 1, order + 1, KnotVectorType.kClampedUniform, 0., 1.),
list(u_control.T))
u_control)
self.u_h_trajectory = BsplineTrajectory_[Expression](
BsplineBasis_[Expression](order + 1, order + 1, KnotVectorType.kClampedUniform, 0., 1.),
list(np.expand_dims(u_duration, 1)))
np.expand_dims(u_duration, 0))

edge_vars = np.concatenate((u_control.flatten("F"), u_duration, v_control.flatten("F"), v_duration))
v_r_trajectory = BsplineTrajectory_[Expression](
BsplineBasis_[Expression](order + 1, order + 1, KnotVectorType.kClampedUniform, 0., 1.),
list(v_control.T))
v_control)
v_h_trajectory = BsplineTrajectory_[Expression](
BsplineBasis_[Expression](order + 1, order + 1, KnotVectorType.kClampedUniform, 0., 1.),
list(np.expand_dims(v_duration, 1)))
np.expand_dims(v_duration, 0))

# Continuity constraints
self.contin_constraints = []
Expand Down Expand Up @@ -344,6 +344,9 @@ def SolvePath(self, rounding=False, verbose=False, preprocessing=False):
for ii in range(len(time_control_points)):
time_control_points[ii] -= offset

path_control_points = np.array(path_control_points).T
time_control_points = np.array(time_control_points).T

path = BsplineTrajectory(BsplineBasis(self.order + 1, knots), path_control_points)
time_traj = BsplineTrajectory(BsplineBasis(self.order + 1, knots), time_control_points)

Expand Down

0 comments on commit 60b2074

Please sign in to comment.