Skip to content

Commit

Permalink
add lever path example
Browse files Browse the repository at this point in the history
  • Loading branch information
@Victorlouisdg
Victorlouisdg committed Mar 8, 2024
1 parent d928a5e commit 9b873cd
Showing 1 changed file with 157 additions and 19 deletions.
176 changes: 157 additions & 19 deletions notebooks/03_collision_checking.ipynb
View file
Original file line number Diff line number Diff line change
Expand Up @@ -425,11 +425,7 @@
"outputs": [],
"source": [
"for path in paths:\n",
" print(path_collisions_as_emojis(collision_checker.CheckConfigsCollisionFree(path)))\n",
"\n",
"# for row in np.swapaxes(paths, 0, 1):\n",
"# print(path_collisions_as_emojis(collision_checker.CheckConfigsCollisionFree(row)))\n",
"# np.allclose(np.swapaxes(paths, 0, 1), path_joints_solutions)"
" print(path_collisions_as_emojis(collision_checker.CheckConfigsCollisionFree(path)))"
]
},
{
Expand Down Expand Up @@ -536,7 +532,10 @@
"metadata": {},
"outputs": [],
"source": [
"fastest_trajectory = trajectories[np.argmin(durations)]"
"from airo_drake import animate_joint_trajectory\n",
"\n",
"fastest_trajectory = trajectories[np.argmin(durations)]\n",
"animate_joint_trajectory(scene.meshcat, scene.robot_diagram, scene.arm_index, fastest_trajectory, context)"
]
},
{
Expand All @@ -545,10 +544,9 @@
"metadata": {},
"outputs": [],
"source": [
"from airo_drake import animate_joint_trajectory\n",
"\n",
"\n",
"animate_joint_trajectory(scene.meshcat, scene.robot_diagram, scene.arm_index, fastest_trajectory, context)"
"scene.meshcat.Delete(\"grasp_pose\")\n",
"scene.meshcat.Delete(\"pregrasp_pose\")\n",
"scene.meshcat.Delete(\"tcp_path\")"
]
},
{
Expand All @@ -564,7 +562,14 @@
"metadata": {},
"outputs": [],
"source": [
"lever_axis_point = np.array([0.3, 0.0, 0.5])\n",
"# Colors\n",
"from pydrake.geometry import Rgba\n",
"yellow = Rgba(1, 1, 0, 0.5)\n",
"cyan = Rgba(0, 1, 1, 0.5)\n",
"magenta = Rgba(1, 0, 1, 0.5)\n",
"\n",
"\n",
"lever_axis_middle = np.array([0.35, 0.0, 0.2])\n",
"lever_axis_direction = np.array([0, -1, 0])"
]
},
Expand All @@ -575,8 +580,85 @@
"outputs": [],
"source": [
"from airo_drake.visualization.arrow import visualize_arrow\n",
"from pydrake.geometry import Sphere, Rgba\n",
"from pydrake.math import RigidTransform\n",
"\n",
"\n",
"level_axis_start = lever_axis_middle - 0.1 * lever_axis_direction\n",
"level_axis_end = lever_axis_middle + 0.1 * lever_axis_direction\n",
"visualize_arrow(meshcat, \"lever\", level_axis_start, end=level_axis_end)\n",
"\n",
"\n",
"meshcat.SetTransform(f\"lever/middle\", RigidTransform(p=lever_axis_middle))\n",
"meshcat.SetObject(f\"lever/middle\", Sphere(0.02), yellow)"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"lever_length = 0.15\n",
"lever_tip_opened = lever_axis_middle + lever_length * np.array([0, 0, 1])\n",
"\n",
"meshcat.SetTransform(f\"lever/tip\", RigidTransform(p=lever_tip_opened))\n",
"meshcat.SetObject(f\"lever/tip\", Sphere(0.01), cyan)"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"import numpy as np\n",
"from airo_typing import Vector3DType\n",
"from scipy.spatial.transform import Rotation\n",
"\n",
"# TODO: this is temporarily copied from Linen until it's available on PyPI\n",
"def rotate_point(point: Vector3DType, center: Vector3DType, axis: Vector3DType, angle: float):\n",
" \"\"\"\n",
" Rotate a point around an axis by a given angle.\n",
"\n",
" Args:\n",
" point: The point to rotate.\n",
" center: The center of the rotation.\n",
" axis: The axis to rotate around, which will be normalized.\n",
" angle: The angle in radians to rotate by.\n",
"\n",
" Returns:\n",
" The rotated point.\n",
" \"\"\"\n",
" unit_axis = axis / np.linalg.norm(axis)\n",
" rotation = Rotation.from_rotvec(angle * unit_axis)\n",
" return center + rotation.apply(point - center)\n",
"\n",
"\n",
"lever_tip_opened, rotate_point(lever_tip_opened, lever_axis_middle, lever_axis_direction, 0.03)"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"level_tip_path = []\n",
"\n",
"visualize_arrow(meshcat, \"arrow\", lever_axis_point, lever_axis_direction)"
"max_angle = np.deg2rad(90)\n",
"for angle in np.linspace(0, max_angle, 20):\n",
" tip_position_rotated = rotate_point(lever_tip_opened, lever_axis_middle, lever_axis_direction, angle)\n",
" level_tip_path.append(tip_position_rotated)\n",
"\n",
"\n",
"print(level_tip_path[0])\n",
"print(level_tip_path[1])\n",
"print(level_tip_path[-1])\n",
"\n",
"for i, tip_position in enumerate(level_tip_path):\n",
" meshcat.SetTransform(f\"lever/tip_position_{i}\", RigidTransform(p=tip_position))\n",
" meshcat.SetObject(f\"lever/tip_position_{i}\", Sphere(0.005), magenta)"
]
},
{
Expand All @@ -585,9 +667,21 @@
"metadata": {},
"outputs": [],
"source": [
"start = (1, 0, 0)\n",
"direction = (0, 1, 0)\n",
"visualize_arrow(meshcat, \"arrow2\", start, direction)"
"X = np.array([-1.0, 0.0, 0.0])\n",
"Y = np.array([0.0, 1.0, 0.0])\n",
"Z = np.array([0.0, 0.0, -1.0])\n",
"top_down_orientation = np.column_stack([X, Y, Z])\n",
"\n",
"tcp_path_lever = []\n",
"\n",
"for tip_position in level_tip_path:\n",
" pose = np.identity(4)\n",
" pose[0:3, 3] = tip_position\n",
" pose[0:3, 0:3] = top_down_orientation\n",
" tcp_path_lever.append(pose)\n",
"\n",
"for i, tcp_pose in enumerate(tcp_path_lever):\n",
" visualize_frame(scene.meshcat, f\"tcp_path_lever/pose_{i}\", tcp_pose, length=0.05, opacity=0.1)"
]
},
{
Expand All @@ -596,9 +690,17 @@
"metadata": {},
"outputs": [],
"source": [
"start = (1, 0, 0)\n",
"direction = (0, 0, -1)\n",
"visualize_arrow(meshcat, \"arrow3\", start, direction)"
"path_joints_solutions = []\n",
"\n",
"for tcp_pose in tcp_path_lever:\n",
" joint_solutions = ur5e.inverse_kinematics_with_tcp(tcp_pose, tcp_transform)\n",
" path_joints_solutions.append(np.array(joint_solutions).squeeze())\n",
"\n",
"paths = create_paths_from_closest_solutions(path_joints_solutions)\n",
"paths_without_jumps = [path for path in paths if not joint_path_has_large_jumps(path)]\n",
"\n",
"for path in paths_without_jumps:\n",
" print(path_collisions_as_emojis(collision_checker.CheckConfigsCollisionFree(path)))"
]
},
{
Expand All @@ -607,7 +709,43 @@
"metadata": {},
"outputs": [],
"source": [
"visualize_arrow(meshcat, \"arrow4\", (0, 1, 0), end=(1, 1, 1))"
"paths_with_collisions = [path for path in paths_without_jumps if not np.all(collision_checker.CheckConfigsCollisionFree(path))]\n",
"\n",
"animate_joint_configurations(scene.meshcat, scene.robot_diagram, scene.arm_index, paths_with_collisions[0])"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"paths_without_collisions = [path for path in paths_without_jumps if np.all(collision_checker.CheckConfigsCollisionFree(path))]\n",
"\n",
"for path in paths_without_collisions:\n",
" print(path_collisions_as_emojis(collision_checker.CheckConfigsCollisionFree(path)))"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"trajectories = [time_parametrize_toppra(scene.robot_diagram.plant(), path, joint_acceleration_limit=1.0) for path in paths_without_collisions]\n",
"durations = [trajectory.end_time() for trajectory in trajectories]\n",
"\n",
"print(durations)"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"fastest_trajectory = trajectories[np.argmin(durations)]\n",
"animate_joint_trajectory(scene.meshcat, scene.robot_diagram, scene.arm_index, fastest_trajectory, context)"
]
},
{
Expand Down

0 comments on commit 9b873cd

Please sign in to comment.