From 804817cf3b4f560874312dad1baef8063b02a637 Mon Sep 17 00:00:00 2001 From: victorlouisdg Date: Wed, 20 Mar 2024 12:58:19 +0100 Subject: [PATCH] concatenating joint trajectories --- airo_planner/__init__.py | 8 + airo_planner/ompl/single_arm_ompl_planner.py | 58 +--- notebooks/02_planning_to_tcp_poses.ipynb | 318 +++++++++---------- 3 files changed, 172 insertions(+), 212 deletions(-) diff --git a/airo_planner/__init__.py b/airo_planner/__init__.py index 3de1af4..d267385 100644 --- a/airo_planner/__init__.py +++ b/airo_planner/__init__.py @@ -15,6 +15,11 @@ from airo_planner.interfaces import DualArmPlanner, SingleArmPlanner # isort:skip from airo_planner.ompl.single_arm_ompl_planner import InverseKinematicsFunctionType, SingleArmOmplPlanner # isort:skip from airo_planner.ompl.dual_arm_ompl_planner import DualArmOmplPlanner # isort:skip +from airo_planner.selection.goal_selection import ( + filter_with_distance_to_configurations, + rank_by_distance_to_desirable_configurations, +) +from airo_planner.selection.path_selection import choose_shortest_path __all__ = [ "SingleArmPlanner", @@ -28,4 +33,7 @@ "function_to_ompl", "bounds_to_ompl", "JointBoundsType", + "choose_shortest_path", + "rank_by_distance_to_desirable_configurations", + "filter_with_distance_to_configurations", ] diff --git a/airo_planner/ompl/single_arm_ompl_planner.py b/airo_planner/ompl/single_arm_ompl_planner.py index 89923f5..a81b759 100644 --- a/airo_planner/ompl/single_arm_ompl_planner.py +++ b/airo_planner/ompl/single_arm_ompl_planner.py @@ -115,12 +115,6 @@ def plan_to_tcp_pose( # noqa: C901 start_configuration: JointConfigurationType, tcp_pose_in_base: HomogeneousMatrixType, ) -> JointPathType | None: - # TODO: add options for specifying a preferred IK solutions, e.g. min distance to a joint configuration - # desirable_goal_joint_configurations = Optional[List[JointConfigurationType]] - # Without this we plan to all joint configs and pick the shortest path - # With it, we try the closest IK solution first and if it fails we try the next closest etc. - pass - if self.inverse_kinematics_fn is None: logger.warning("Planning to TCP pose attempted but inverse_kinematics_fn was provided, returing None.") return None @@ -166,8 +160,13 @@ def plan_to_tcp_pose( # noqa: C901 else: ik_solutions_filtered = ik_solutions_valid - # TODO rank the IK solutions - terminate_on_first_success = self.rank_goal_configurations_fn is not None + if self.rank_goal_configurations_fn is not None: + logger.info("Ranking IK solutions.") + terminate_on_first_success = True + ik_solutions_filtered = self.rank_goal_configurations_fn(ik_solutions_filtered) + else: + logger.info("No ranking function provided, will try planning to all valid IK solutions.") + terminate_on_first_success = False logger.info(f"Running OMPL towards {len(ik_solutions_filtered)} IK solutions.") start_time = time.time() @@ -199,46 +198,3 @@ def plan_to_tcp_pose( # noqa: C901 solution_path = self.choose_path_fn(paths) return solution_path - - # TODO: exhaustive mode vs iterative mode (~= greedy mode) - # depending on the helper functions provide to the planner, it will operate in different modes - # by default, it operates in an exhaustive mode, meaning that it will treat all IK solutions the same, and plan a path to them all - # then it will return the shortest path - - # path_distances = [np.linalg.norm(path[-1] - start_configuration) for path in paths] - - # path_desirablities = None - # if desirable_goal_configurations is not None: - # path_desirablities = [] - # for path in paths: - # min_distance = np.inf - # for desirable_goal in desirable_goal_configurations: - # distance = np.linalg.norm(path[-1] - desirable_goal) - # min_distance = min(min_distance, distance) # type: ignore # I don't understand the mypy error here - # path_desirablities.append(min_distance) - - # path_lengths = [] # TODO calculate - - # lengths_str = f"{[np.round(l, 3) for l in path_lengths]}" - # distances_str = f"{[np.round(d, 3) for d in path_distances]}" - # logger.info(f"Found {len(paths)} paths towards IK solutions:") - # logger.info(f"Path lengths: {lengths_str}") - # logger.info(f"Path distances: {distances_str}") - - # if path_desirablities is not None: - # desirabilities_str = f"{[np.round(d, 3) for d in path_desirablities]}" - # logger.info(f"Path desirabilities: {desirabilities_str}") - - # use_desirability = path_desirablities is not None - - # if use_desirability: - # idx = np.argmin(path_desirablities) - # logger.info( - # f"Length of chosen solution (= most desirable end): {path_lengths[idx]:.3f}, desirability: {path_desirablities[idx]:.3f}" - # ) - # else: - # idx = np.argmin(path_lengths) - # logger.info(f"Length of chosen solution (= shortest path): {path_lengths[idx]:.3f}") - # - # solution_path = paths[idx] - # return solution_path diff --git a/notebooks/02_planning_to_tcp_poses.ipynb b/notebooks/02_planning_to_tcp_poses.ipynb index 59c3ed5..6b50f3b 100644 --- a/notebooks/02_planning_to_tcp_poses.ipynb +++ b/notebooks/02_planning_to_tcp_poses.ipynb @@ -64,9 +64,8 @@ "outputs": [], "source": [ "import numpy as np\n", - "from pydrake.planning import RobotDiagramBuilder\n", + "from pydrake.planning import RobotDiagramBuilder, SceneGraphCollisionChecker\n", "from airo_drake import SingleArmScene, add_floor, add_manipulator, add_meshcat, finish_build\n", - "from pydrake.planning import SceneGraphCollisionChecker\n", "\n", "robot_diagram_builder = RobotDiagramBuilder()\n", "\n", @@ -133,6 +132,7 @@ "\n", "def inverse_kinematics_fn(tcp_pose: HomogeneousMatrixType) -> list[JointConfigurationType]:\n", " solutions = ur3e.inverse_kinematics_with_tcp(tcp_pose, tcp_transform)\n", + " solutions = [solution.squeeze() for solution in solutions]\n", " return solutions\n", "\n", "\n", @@ -204,22 +204,6 @@ "path = planner.plan_to_tcp_pose(start_joints, goal_pose)" ] }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "animate_joint_configurations(scene.meshcat, scene.robot_diagram, scene.arm_index, path)" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "**Double checking the path lengths:**" - ] - }, { "cell_type": "code", "execution_count": null, @@ -230,11 +214,14 @@ "\n", "path_lengths = [calculate_joint_path_length(p) for p in planner._all_paths]\n", "\n", - "print(\"Path lengths\")\n", + "print(\"Length of all paths the planner found:\")\n", "for i, path_length in enumerate(path_lengths):\n", " print(f\"{i}: {path_length:.2f}\")\n", "\n", - "print(f\"\\nLength of path that planner returned: {calculate_joint_path_length(path):.2f}\")" + "print(f\"\\nLength of the chosen path: {calculate_joint_path_length(path):.2f}\")\n", + "\n", + "print(f\"Amount of states in the path: {len(path)}\")\n", + "animate_joint_configurations(scene.meshcat, scene.robot_diagram, scene.arm_index, path)" ] }, { @@ -259,13 +246,13 @@ "metadata": {}, "outputs": [], "source": [ - "from airo_planner.selection.goal_selection import rank_by_distance_to_desirable_configurations\n", + "from airo_planner import rank_by_distance_to_desirable_configurations\n", "\n", "ranked_solutions = rank_by_distance_to_desirable_configurations(solutions, [start_joints])\n", "\n", "np.printoptions(precision=3, suppress=True)\n", "\n", - "print(\"Distances to ranked solutions\")\n", + "print(\"Distance from start joints to the ranked IK solutions\")\n", "for i, solution in enumerate(ranked_solutions):\n", " distance = np.linalg.norm(solution - start_joints)\n", " print(f\"{distance:.2f}\")" @@ -277,7 +264,11 @@ "metadata": {}, "outputs": [], "source": [ - "planner.rank_goal_configurations_fn = rank_by_distance_to_desirable_configurations\n", + "from functools import partial\n", + "\n", + "rank_fn = partial(rank_by_distance_to_desirable_configurations, desirable_configurations=[start_joints])\n", + "\n", + "planner.rank_goal_configurations_fn = rank_fn\n", "\n", "path = planner.plan_to_tcp_pose(start_joints, goal_pose)" ] @@ -288,7 +279,11 @@ "metadata": {}, "outputs": [], "source": [ - "print(f\"\\nLength of path that planner returned: {calculate_joint_path_length(path):.2f}\")" + "print(f\"\\nLength of the returned path: {calculate_joint_path_length(path):.2f}\")\n", + "\n", + "print(f\"Amount of states in the path: {len(path)}\")\n", + "\n", + "animate_joint_configurations(scene.meshcat, scene.robot_diagram, scene.arm_index, path)" ] }, { @@ -297,7 +292,7 @@ "metadata": {}, "outputs": [], "source": [ - "animate_joint_configurations(scene.meshcat, scene.robot_diagram, scene.arm_index, path)" + "scene.meshcat.Delete(\"goal_pose\")" ] }, { @@ -317,10 +312,6 @@ "metadata": {}, "outputs": [], "source": [ - "from airo_planner.interfaces import SingleArmPlanner\n", - "from pydrake.trajectories import Trajectory\n", - "\n", - "\n", "from airo_drake import visualize_frame\n", "\n", "\n", @@ -338,10 +329,6 @@ "grasp_pose[0:3, 0:3] = grasp_orientation\n", "grasp_pose[0:3, 3] = grasp_location\n", "\n", - "pregrasp_pose = grasp_pose.copy()\n", - "pregrasp_pose[0:3, 3] -= 0.25 * gripper_forward_direction\n", - "\n", - "visualize_frame(scene.meshcat, \"pregrasp_pose\", pregrasp_pose)\n", "visualize_frame(scene.meshcat, \"grasp_pose\", grasp_pose)" ] }, @@ -351,66 +338,75 @@ "metadata": {}, "outputs": [], "source": [ - "from ur_analytic_ik import ur3e\n", + "from airo_typing import InverseKinematicsFunctionType, JointConfigurationCheckerType, JointPathType\n", + "from loguru import logger\n", + "from pydrake.trajectories import PiecewisePose\n", + "from pydrake.math import RigidTransform\n", + "from airo_drake import discretize_drake_pose_trajectory\n", + "from airo_drake import calculate_valid_joint_paths\n", + "from functools import partial\n", + "from airo_planner import filter_with_distance_to_configurations\n", "\n", - "tcp_transform = np.identity(4)\n", - "tcp_transform[2, 3] = 0.175 # 175 mm in z\n", + "def plan_pregrasp(\n", + " grasp_pose: HomogeneousMatrixType,\n", + " start_configuration: JointConfigurationType,\n", + " inverse_kinematics_fn: InverseKinematicsFunctionType,\n", + " is_state_valid_fn_pregrasp: JointConfigurationCheckerType,\n", + " is_state_valid_fn_grasp: JointConfigurationCheckerType,\n", + ") -> tuple[JointPathType, JointPathType]:\n", "\n", - "start_configurations = ur3e.inverse_kinematics_with_tcp(pregrasp_pose, tcp_transform)\n", - "goal_configurations = ur3e.inverse_kinematics_with_tcp(grasp_pose, tcp_transform)\n", + " pregrasp_distances_to_try = [0.05, 0.1, 0.15, 0.2, 0.25]\n", "\n", - "start_configurations = np.array(start_configurations).squeeze()\n", - "goal_configurations = np.array(goal_configurations).squeeze()\n", + " planner = SingleArmOmplPlanner(is_state_valid_fn_pregrasp, inverse_kinematics_fn=inverse_kinematics_fn)\n", "\n", - "len(start_configurations), len(goal_configurations)" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "def path_collisions_as_emojis(is_collision_free: list[bool]):\n", - " \"\"\"Displays an emoji-based visualization of a path's collisions.\n", + " for distance in pregrasp_distances_to_try:\n", + " # 1. Compute pregrasp pose\n", + " pregrasp_pose = grasp_pose.copy()\n", + " pregrasp_pose[0:3, 3] -= distance * pregrasp_pose[0:3, 2]\n", "\n", - " Example output: \"✅✅💥✅✅✅💥✅✅✅✅\"\n", + " # 2. Compute grasp TCP path\n", + " rigid_transforms = [RigidTransform(pose) for pose in [pregrasp_pose, grasp_pose]]\n", + " times = np.linspace(0, 1, len(rigid_transforms))\n", + " pose_trajectory = PiecewisePose.MakeLinear(times=times, poses=rigid_transforms)\n", + " tcp_path = discretize_drake_pose_trajectory(pose_trajectory).poses\n", "\n", - " Args:\n", - " is_collision_free: A list of booleans, where True indicates no collision.\n", + " # 3 Compute valid grasp joint paths\n", + " joint_paths = calculate_valid_joint_paths(tcp_path, inverse_kinematics_fn, is_state_valid_fn_grasp)\n", + " grasp_path_starts = [path[0] for path in joint_paths]\n", "\n", - " Returns:\n", - " A string of emojis representing the collision status of the path.\n", - " \"\"\"\n", - " emojis = [\"✅\" if is_free else \"💥\" for is_free in is_collision_free]\n", - " emoji_str = \"\".join(emojis)\n", - " return emoji_str\n", + " # 4 plan to pregrasp tcp poses, filtering on the valid grasp joint paths\n", + " filter_fn = partial(filter_with_distance_to_configurations, joint_configurations_close=grasp_path_starts)\n", "\n", + " planner.filter_goal_configurations_fn = filter_fn\n", + " pregrasp_path = planner.plan_to_tcp_pose(start_configuration, pregrasp_pose)\n", "\n", - "print(path_collisions_as_emojis(collision_checker.CheckConfigsCollisionFree(start_configurations)))\n", - "print(path_collisions_as_emojis(collision_checker.CheckConfigsCollisionFree(goal_configurations)))" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "from pydrake.trajectories import PiecewisePose\n", - "from pydrake.math import RigidTransform\n", - "from airo_drake import discretize_drake_pose_trajectory\n", + " if pregrasp_path is None:\n", + " logger.info(f\"Failed to plan to pregrasp pose at distance {distance}, continuing to next distance.\")\n", + " continue\n", "\n", + " # TODO find the grasp path of which the start is closest to the pregrasp path end (=pregrasp end joints)\n", + " pregrasp_end_joints = pregrasp_path[-1]\n", + " distances = [np.linalg.norm(start - pregrasp_end_joints) for start in grasp_path_starts]\n", + " index_of_closest_start = np.argmin(distances)\n", "\n", - "rigid_transforms = [RigidTransform(pose) for pose in [pregrasp_pose, grasp_pose]]\n", - "times = np.linspace(0, 1, len(rigid_transforms))\n", - "pose_trajectory = PiecewisePose.MakeLinear(times=times, poses=rigid_transforms)\n", + " print(len(pregrasp_path))\n", "\n", + " assert np.isclose(distances[index_of_closest_start], 0, atol=0.01) # sanity check\n", "\n", - "tcp_path = discretize_drake_pose_trajectory(pose_trajectory).poses\n", + " grasp_path = joint_paths[index_of_closest_start]\n", "\n", - "for i, tcp_pose in enumerate(tcp_path):\n", - " visualize_frame(scene.meshcat, f\"tcp_path/pose_{i}\", tcp_pose, length=0.05, opacity=0.1)" + " return pregrasp_path, grasp_path\n", + " \n", + " logger.warning(\"Exhausted all pregrasp poses to try\")\n", + "\n", + "\n", + "pregrasp_path, grasp_path = plan_pregrasp(\n", + " grasp_pose,\n", + " start_joints,\n", + " inverse_kinematics_fn,\n", + " collision_checker.CheckConfigCollisionFree,\n", + " collision_checker.CheckConfigCollisionFree,\n", + ")" ] }, { @@ -419,19 +415,15 @@ "metadata": {}, "outputs": [], "source": [ - "from airo_drake import calculate_valid_joint_paths, calculate_joint_paths\n", - "\n", + "from airo_drake import time_parametrize_toppra\n", + "from pydrake.trajectories import CompositeTrajectory, Trajectory\n", "\n", - "def ur3e_inverse_kinematics(tcp_pose: HomogeneousMatrixType) -> list[JointConfigurationType]:\n", - " solutions_1x6 = ur3e.inverse_kinematics_with_tcp(tcp_pose, tcp_transform)\n", - " solutions = [solution.squeeze() for solution in solutions_1x6]\n", - " return solutions\n", + "pregrasp_trajectory = time_parametrize_toppra(\n", + " scene.robot_diagram.plant(), pregrasp_path)\n", "\n", "\n", - "joint_paths_ = calculate_joint_paths(tcp_path, ur3e_inverse_kinematics)\n", - "\n", - "for i, path in enumerate(joint_paths_):\n", - " print(f\"{path_collisions_as_emojis(collision_checker.CheckConfigsCollisionFree(path))}\")" + "grasp_trajectory = time_parametrize_toppra(\n", + " scene.robot_diagram.plant(), grasp_path)" ] }, { @@ -440,12 +432,31 @@ "metadata": {}, "outputs": [], "source": [ - "joint_paths = calculate_valid_joint_paths(\n", - " tcp_path, ur3e_inverse_kinematics, collision_checker.CheckConfigCollisionFree\n", - ")\n", + "from pydrake.trajectories import PiecewisePolynomial, PathParameterizedTrajectory, Trajectory\n", + "\n", + "\n", + "def shift_drake_trajectory_in_time(trajectory: Trajectory, time_shift: float) -> PathParameterizedTrajectory:\n", + " \"\"\"Shifts a Drake trajectory in time by a specified amount.\n", + "\n", + " Args:\n", + " trajectory: The Drake trajectory to be shifted.\n", + " time_shift: The amount of time (in seconds) to shift the trajectory forward.\n", + "\n", + " Returns:\n", + " A new PathParameterizedTrajectory representing the shifted trajectory.\n", + " \"\"\"\n", + " start = trajectory.start_time()\n", + " end = trajectory.end_time()\n", + " time_shift_trajectory = PiecewisePolynomial.FirstOrderHold([start + time_shift, end + time_shift], [[start, end]])\n", + " shifted_trajectory = PathParameterizedTrajectory(trajectory, time_shift_trajectory)\n", + "\n", + " return shifted_trajectory\n", + "\n", "\n", - "for i, path in enumerate(joint_paths):\n", - " print(f\"{path_collisions_as_emojis(collision_checker.CheckConfigsCollisionFree(path))}\")" + "grasp_trajectory_shifted = shift_drake_trajectory_in_time(grasp_trajectory, pregrasp_trajectory.end_time())\n", + "\n", + "print(grasp_trajectory.start_time(), grasp_trajectory.end_time())\n", + "print(grasp_trajectory_shifted.start_time(), grasp_trajectory_shifted.end_time())" ] }, { @@ -454,7 +465,30 @@ "metadata": {}, "outputs": [], "source": [ - "animate_joint_configurations(scene.meshcat, scene.robot_diagram, scene.arm_index, joint_paths[0])" + "def concatenate_drake_trajectories(trajectories: list[Trajectory]) -> CompositeTrajectory:\n", + " \"\"\"Concatenate multiple trajectories in time. The start time will be the start time of the first trajectory.\n", + "\n", + " This is achieved by shifting the times of the trajectories to make them continuous.\n", + "\n", + " Args:\n", + " trajectories: A list of Drake trajectories to be concatenated.\n", + "\n", + " Returns:\n", + " A CompositeTrajectory representing the concatenated sequence.\n", + " \"\"\"\n", + "\n", + " if not trajectories:\n", + " raise ValueError(\"Cannot concatenate an empty list of trajectories.\")\n", + "\n", + " offset = 0.0\n", + " shifted_trajectories = []\n", + " for trajectory in trajectories:\n", + " shifted_trajectories.append(shift_drake_trajectory_in_time(trajectory, offset))\n", + " offset += trajectory.end_time() - trajectory.start_time()\n", + "\n", + " return CompositeTrajectory(shifted_trajectories)\n", + "\n", + "joint_trajectory = concatenate_drake_trajectories([pregrasp_trajectory, grasp_trajectory])" ] }, { @@ -463,15 +497,9 @@ "metadata": {}, "outputs": [], "source": [ - "from functools import partial\n", - "\n", - "from airo_planner.selection.goal_selection import filter_with_distance_to_configurations\n", - "\n", - "path_starts = [path[0] for path in joint_paths]\n", - "\n", - "filter_fn = partial(filter_with_distance_to_configurations, joint_configurations_close=path_starts)\n", + "from airo_drake import animate_joint_trajectory\n", "\n", - "filter_fn(start_configurations)" + "animate_joint_trajectory(scene.meshcat, scene.robot_diagram, scene.arm_index, joint_trajectory)" ] }, { @@ -480,64 +508,32 @@ "metadata": {}, "outputs": [], "source": [ - "# use this a filtering example: we want to filter out the grasp paths that collide with the environment\n", - "\n", - "# def plan_to_grasp(planner: SingleArmPlanner, grasp_pose: HomogeneousMatrixType) -> Trajectory:\n", - "# pass\n", - "# # attempt planning the the grasp pose directly\n", - "# # if all goals in collision try backing-off to a pregrasp pose\n", - "# # time parametrize both paths and concatenate them\n", - "from airo_typing import InverseKinematicsFunctionType, JointConfigurationCheckerType\n", - "\n", - "\n", - "def plan_pregrasp(\n", - " grasp_pose: HomogeneousMatrixType,\n", - " start_configuration: JointConfigurationType,\n", - " inverse_kinematics_fn: InverseKinematicsFunctionType,\n", - " is_state_valid_fn_pregrasp: JointConfigurationCheckerType,\n", - " is_state_valid_fn_grasp: JointConfigurationCheckerType,\n", - "):\n", - "\n", - " pregrasp_distances_to_try = [0.05, 0.1, 0.15, 0.2, 0.25]\n", - "\n", - " planner = SingleArmOmplPlanner(is_state_valid_fn_pregrasp, inverse_kinematics_fn=inverse_kinematics_fn)\n", - "\n", - " for distance in pregrasp_distances_to_try:\n", - " # 1. Compute pregrasp pose\n", - " pregrasp_pose = grasp_pose.copy()\n", - " pregrasp_pose[0:3, 3] -= distance * pregrasp_pose[0:3, 2]\n", - "\n", - " # 2. Compute grasp TCP path\n", - " rigid_transforms = [RigidTransform(pose) for pose in [pregrasp_pose, grasp_pose]]\n", - " times = np.linspace(0, 1, len(rigid_transforms))\n", - " pose_trajectory = PiecewisePose.MakeLinear(times=times, poses=rigid_transforms)\n", - " tcp_path = discretize_drake_pose_trajectory(pose_trajectory).poses\n", - "\n", - " # 3 Compute valid grasp joint paths\n", - " joint_paths = calculate_valid_joint_paths(tcp_path, ur3e_inverse_kinematics, is_state_valid_fn_grasp)\n", - " path_starts = [path[0] for path in joint_paths]\n", - "\n", - " # 4 plan to pregrasp tcp poses, filtering on the valid grasp joint paths\n", - " filter_fn = partial(filter_with_distance_to_configurations, joint_configurations_close=path_starts)\n", - "\n", - " planner.filter_goal_configurations_fn = filter_fn\n", - " path = planner.plan_to_tcp_pose(start_configuration, pregrasp_pose)\n", - "\n", - " if path is not None:\n", - " return path\n", - "\n", - " # Exhausted all pregrasp poses to try\n", - "\n", - "\n", - "path = plan_pregrasp(\n", - " grasp_pose,\n", - " start_joints,\n", - " ur3e_inverse_kinematics,\n", - " collision_checker.CheckConfigCollisionFree,\n", - " collision_checker.CheckConfigCollisionFree,\n", - ")\n", - "\n", - "animate_joint_configurations(scene.meshcat, scene.robot_diagram, scene.arm_index, path)" + "from airo_drake import discretize_drake_joint_trajectory\n", + "import matplotlib.pyplot as plt\n", + "\n", + "\n", + "joint_trajectory_discretized = discretize_drake_joint_trajectory(joint_trajectory, steps=1000)\n", + "joint_velocities_discretized = [joint_trajectory.EvalDerivative(t).squeeze() for t in joint_trajectory_discretized.times]\n", + "joint_accelerations_discretized = [joint_trajectory.EvalDerivative(t, 2).squeeze() for t in joint_trajectory_discretized.times]\n", + "\n", + "\n", + "plt.figure(figsize=(20, 6))\n", + "plt.subplot(1, 3, 1)\n", + "plt.title(\"Joint positions\")\n", + "for row in joint_trajectory_discretized.path.positions.T:\n", + " plt.plot(joint_trajectory_discretized.times, row)\n", + "plt.ylim(-2 * np.pi, 2 * np.pi)\n", + "plt.subplot(1, 3, 2)\n", + "plt.title(\"Joint velocities\")\n", + "for row in np.array(joint_velocities_discretized).T:\n", + " plt.plot(row)\n", + "plt.ylim(-4.0, 4.0)\n", + "plt.subplot(1, 3, 3)\n", + "plt.title(\"Joint accelerations\")\n", + "for row in np.array(joint_accelerations_discretized).T:\n", + " plt.plot(row)\n", + "plt.ylim(-10.0, 10.0)\n", + "plt.show()" ] }, {