From 0d8a9045fba7d91252cd1a06556bff173c6b6f0f Mon Sep 17 00:00:00 2001 From: Jan Ole von Hartz Date: Thu, 1 Aug 2024 11:33:39 +0200 Subject: [PATCH 1/2] added topp_sd --- mplib/planner.py | 35 +++++++++++++++++++++++++++++++++++ 1 file changed, 35 insertions(+) diff --git a/mplib/planner.py b/mplib/planner.py index ae21053..1294f7c 100644 --- a/mplib/planner.py +++ b/mplib/planner.py @@ -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) + 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"): """ From 74f6e9f7cecf276caf6eceb64c400907a139a139 Mon Sep 17 00:00:00 2001 From: Jan Ole von Hartz Date: Tue, 13 Aug 2024 12:52:24 +0200 Subject: [PATCH 2/2] merged toppra and toppsd funcs --- mplib/planner.py | 52 ++++++++++++------------------------------------ 1 file changed, 13 insertions(+), 39 deletions(-) diff --git a/mplib/planner.py b/mplib/planner.py index 1294f7c..19cb387 100644 --- a/mplib/planner.py +++ b/mplib/planner.py @@ -348,7 +348,7 @@ def IK( q_goals = q_goals[np.linalg.norm(q_goals - start_qpos, axis=1).argmin()] return status, q_goals - def TOPP(self, path, step=0.1, verbose=False): + def TOPP(self, path, step=0.1, verbose=False, duration=None): """ Time-Optimal Path Parameterization @@ -356,6 +356,9 @@ def TOPP(self, path, step=0.1, verbose=False): path: numpy array of shape (n, dof) step: step size for the discretization verbose: if True, will print the log of TOPPRA + duration: desired duration of the path in seconds. If None, retunrs + time-optimal path. Otherwise, returns path parameterized with + the desired duration. """ N_samples = path.shape[0] @@ -366,46 +369,17 @@ def TOPP(self, path, step=0.1, verbose=False): path = ta.SplineInterpolator(ss, path) pc_vel = constraint.JointVelocityConstraint(self.joint_vel_limits) pc_acc = constraint.JointAccelerationConstraint(self.joint_acc_limits) - instance = algo.TOPPRA( - [pc_vel, pc_acc], path, parametrizer="ParametrizeConstAccel" - ) - jnt_traj = instance.compute_trajectory() - if jnt_traj is None: - 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 - - 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) + if duration is None: + instance = algo.TOPPRA( + [pc_vel, pc_acc], path, parametrizer="ParametrizeConstAccel" + ) + else: + instance = algo.TOPPRAsd( + [pc_vel, pc_acc], path, parametrizer="ParametrizeConstAccel" + ) + instance.set_desired_duration(duration) 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)