Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Expose 'specified duration' variant of TOPP-RA in planner #92

Merged
merged 2 commits into from
Aug 24, 2024
Merged

Expose 'specified duration' variant of TOPP-RA in planner #92

Changes from 1 commit
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
0d8a904
added topp_sd
vonHartz Aug 1, 2024
74f6e9f
merged toppra and toppsd funcs
vonHartz Aug 13, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
35 changes: 35 additions & 0 deletions mplib/planner.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -378,6 +378,41 @@ def TOPP(self, path, step=0.1, verbose=False):
qdds_sample = jnt_traj(ts_sample, 2)
return ts_sample, qs_sample, qds_sample, qdds_sample, jnt_traj.duration # type: ignore

def TOPP_SD(self, path, duration, step=0.1, verbose=False):
"""
Path Parameterization with desired duration (in seconds)
Args:
path: numpy array of shape (n, dof)
duration: desired duration of the path in seconds
step: step size for the discretization
verbose: if True, will print the log of TOPPRA
"""

N_samples = path.shape[0]
dof = path.shape[1]
assert dof == len(self.joint_vel_limits)
assert dof == len(self.joint_acc_limits)
ss = np.linspace(0, 1, N_samples)
path = ta.SplineInterpolator(ss, path)
pc_vel = constraint.JointVelocityConstraint(self.joint_vel_limits)
pc_acc = constraint.JointAccelerationConstraint(self.joint_acc_limits)
instance = algo.TOPPRAsd(
[pc_vel, pc_acc], path, parametrizer="ParametrizeConstAccel"
)
instance.set_desired_duration(duration)
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

seems to me this is the only change. would you mind just modifying the TOPP function above and add duration=None as default argument?

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yes, that's correct.
Though you might want to know that TOPPRAsd is a bit less efficient than TOPRRA.
See: https://github.com/hungpham2511/toppra/blob/730aa8ad6854c7da01304ad1ed8c0819400862f4/toppra/algorithm/reachabilitybased/desired_duration_algorithm.py#L20
If that is not a concern for you, I'm happy to make the change.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Ah, nevermind. I'll do it with a conditional.

jnt_traj = instance.compute_trajectory()
if jnt_traj is None:
if verbose:
print(instance.problem_data)
print(instance.problem_data.K)
instance.inspect()
raise RuntimeError("Fail to parameterize path")
ts_sample = np.linspace(0, jnt_traj.duration, int(jnt_traj.duration / step)) # type: ignore
qs_sample = jnt_traj(ts_sample)
qds_sample = jnt_traj(ts_sample, 1)
qdds_sample = jnt_traj(ts_sample, 2)
return ts_sample, qs_sample, qds_sample, qdds_sample, jnt_traj.duration # type: ignore

# TODO: change method name to align with PlanningWorld API?
def update_point_cloud(self, points, resolution=1e-3, name="scene_pcd"):
"""
Expand Down