From 68cbd9fd6a00658c442075d85c00f974a6b5dc35 Mon Sep 17 00:00:00 2001 From: Mansard Date: Wed, 15 Jan 2025 22:04:31 +0100 Subject: [PATCH] TP2: 2024->2025 --- next/2-inv_geom.ipynb => 2-inv_geom.ipynb | 4 +- next/tp2/generated/floating_1 | 55 ---------------------- next/tp2/generated/invgeom3d_1 | 23 --------- next/tp2/generated/invgeom6d_1 | 26 ---------- next/tp2/generated/parallel_robots_0 | 8 ---- next/tp2/generated/parallel_robots_1 | 7 --- next/tp2/generated/parallel_robots_2 | 2 - next/tp2/generated/simple_pick_and_place_1 | 1 - next/tp2/generated/simple_pick_and_place_2 | 8 ---- next/tp2/generated/simple_pick_and_place_3 | 10 ---- next/tp2/generated/simple_pick_and_place_4 | 22 --------- next/tp2/generated/simple_pick_and_place_5 | 9 ---- next/tp2/generated/simple_pick_and_place_6 | 9 ---- next/tp2/generated/simple_pick_and_place_7 | 22 --------- {next/tp2 => tp2}/floating.py | 2 +- {next/tp2 => tp2}/invgeom3d.py | 2 +- {next/tp2 => tp2}/invgeom6d.py | 2 +- {next/tp2 => tp2}/parallel_robots.py | 4 +- {next/tp2 => tp2}/simple_pick_and_place.py | 0 {next/tp2 => tp2}/tests.py | 0 20 files changed, 7 insertions(+), 209 deletions(-) rename next/2-inv_geom.ipynb => 2-inv_geom.ipynb (99%) delete mode 100644 next/tp2/generated/floating_1 delete mode 100644 next/tp2/generated/invgeom3d_1 delete mode 100644 next/tp2/generated/invgeom6d_1 delete mode 100644 next/tp2/generated/parallel_robots_0 delete mode 100644 next/tp2/generated/parallel_robots_1 delete mode 100644 next/tp2/generated/parallel_robots_2 delete mode 100644 next/tp2/generated/simple_pick_and_place_1 delete mode 100644 next/tp2/generated/simple_pick_and_place_2 delete mode 100644 next/tp2/generated/simple_pick_and_place_3 delete mode 100644 next/tp2/generated/simple_pick_and_place_4 delete mode 100644 next/tp2/generated/simple_pick_and_place_5 delete mode 100644 next/tp2/generated/simple_pick_and_place_6 delete mode 100644 next/tp2/generated/simple_pick_and_place_7 rename {next/tp2 => tp2}/floating.py (97%) rename {next/tp2 => tp2}/invgeom3d.py (97%) rename {next/tp2 => tp2}/invgeom6d.py (97%) rename {next/tp2 => tp2}/parallel_robots.py (89%) rename {next/tp2 => tp2}/simple_pick_and_place.py (100%) rename {next/tp2 => tp2}/tests.py (100%) diff --git a/next/2-inv_geom.ipynb b/2-inv_geom.ipynb similarity index 99% rename from next/2-inv_geom.ipynb rename to 2-inv_geom.ipynb index c77084f..fe1cc90 100644 --- a/next/2-inv_geom.ipynb +++ b/2-inv_geom.ipynb @@ -194,7 +194,7 @@ "metadata": {}, "outputs": [], "source": [ - "from supaero2024.meshcat_viewer_wrapper import MeshcatVisualizer, colors # noqa: E402\n", + "from supaero2025.meshcat_viewer_wrapper import MeshcatVisualizer, colors # noqa: E402\n", "\n", "viz = MeshcatVisualizer(robot)" ] @@ -691,7 +691,7 @@ "metadata": {}, "outputs": [], "source": [ - "from supaero2024. load_ur5_parallel import load_ur5_parallel # noqa: E402\n", + "from supaero2025. load_ur5_parallel import load_ur5_parallel # noqa: E402\n", "\n", "robot = load_ur5_parallel()" ] diff --git a/next/tp2/generated/floating_1 b/next/tp2/generated/floating_1 deleted file mode 100644 index 55d9d89..0000000 --- a/next/tp2/generated/floating_1 +++ /dev/null @@ -1,55 +0,0 @@ -robot.feetIndexes = [ - robot.model.getFrameId(frameName) - for frameName in ["HR_FOOT", "HL_FOOT", "FR_FOOT", "FL_FOOT"] -] - -# --- Add box to represent target -# We define 4 targets, one for each leg. -colors = ["red", "blue", "green", "magenta"] -for color in colors: - viz.addSphere("world/%s" % color, 0.05, color) - viz.addSphere("world/%s_des" % color, 0.05, color) - -# -# OPTIM 6D ######################################################### -# - -targets = [ - np.array([-0.7, -0.2, 1.2]), - np.array([-0.3, 0.5, 0.8]), - np.array([0.3, 0.1, -0.1]), - np.array([0.9, 0.9, 0.5]), -] -for i in range(4): - targets[i][2] += 1 - - -def cost(q): - """ - Compute score from a configuration: sum of the 4 reaching - tasks, one for each leg. - """ - cost = 0.0 - for i in range(4): - p_i = robot.framePlacement(q, robot.feetIndexes[i]).translation - cost += norm(p_i - targets[i]) ** 2 - return cost - - -def callback(q): - """ - Diplay the robot, postion a ball of different color for - each foot, and the same ball of same color for the location - of the corresponding target. - """ - for i in range(4): - p_i = robot.framePlacement(q, robot.feetIndexes[i]) - viz.applyConfiguration("world/%s" % colors[i], p_i) - viz.applyConfiguration( - "world/%s_des" % colors[i], list(targets[i]) + [1, 0, 0, 0] - ) - - viz.display(q) - time.sleep(1e-2) - -qopt = fmin_bfgs(cost, robot.q0, callback=callback) diff --git a/next/tp2/generated/invgeom3d_1 b/next/tp2/generated/invgeom3d_1 deleted file mode 100644 index 0068e71..0000000 --- a/next/tp2/generated/invgeom3d_1 +++ /dev/null @@ -1,23 +0,0 @@ -# Add a vizualisation for the tip of the arm. -tipID = "world/blue" -viz.addBox(tipID, [0.08] * 3, [0.2, 0.2, 1.0, 0.5]) -# -# OPTIM 3D ######################################################### -# - - -def cost(q): - """Compute score from a configuration""" - p = robot.placement(q, 6).translation - return norm(p - target) ** 2 - - -def callback(q): - viz.applyConfiguration(ballID, target.tolist() + [0, 1, 0, 0]) - viz.applyConfiguration(tipID, robot.placement(q, 6)) - viz.display(q) - time.sleep(1e-2) - - -target = np.array([0.5, 0.1, 0.2]) # x,y,z -qopt = fmin_bfgs(cost, robot.q0, callback=callback) diff --git a/next/tp2/generated/invgeom6d_1 b/next/tp2/generated/invgeom6d_1 deleted file mode 100644 index 487b1c9..0000000 --- a/next/tp2/generated/invgeom6d_1 +++ /dev/null @@ -1,26 +0,0 @@ -# Add a vizualisation for the tip of the arm. -tipID = "world/blue" -viz.addBox(tipID, [0.08] * 3, [0.2, 0.2, 1.0, 0.5]) - -# -# OPTIM 6D ######################################################### -# - - -def cost(q): - """Compute score from a configuration""" - M = robot.placement(q, 6) - return norm(pin.log(M.inverse() * Mtarget).vector) - - -def callback(q): - viz.applyConfiguration(boxID, Mtarget) - viz.applyConfiguration(tipID, robot.placement(q, 6)) - viz.display(q) - time.sleep(1e-1) - - -Mtarget = pin.SE3(pin.utils.rotate("x", 3.14 / 4), np.array([-0.5, 0.1, 0.2])) # x,y,z -qopt = fmin_bfgs(cost, robot.q0, callback=callback) - -print("The robot finally reached effector placement at\n", robot.placement(qopt, 6)) diff --git a/next/tp2/generated/parallel_robots_0 b/next/tp2/generated/parallel_robots_0 deleted file mode 100644 index 25cbc07..0000000 --- a/next/tp2/generated/parallel_robots_0 +++ /dev/null @@ -1,8 +0,0 @@ -from supaero2024.load_ur5_parallel import load_ur5_parallel -from supaero2024.meshcat_viewer_wrapper import MeshcatVisualizer - -# Load 4 Ur5 robots, placed at 0.3m from origin in the 4 directions x,y,-x,-y. -robot = load_ur5_parallel() - -# Open the viewer -viz = MeshcatVisualizer(robot) diff --git a/next/tp2/generated/parallel_robots_1 b/next/tp2/generated/parallel_robots_1 deleted file mode 100644 index a36be0b..0000000 --- a/next/tp2/generated/parallel_robots_1 +++ /dev/null @@ -1,7 +0,0 @@ -# Add a new object featuring the parallel robot tool plate. -[w, h, d] = [0.5, 0.5, 0.005] -color = [red, green, blue, transparency] = [1, 1, 0.78, 0.3] -viz.addBox("world/robot0/toolplate", [w, h, d], color) -Mtool = pin.SE3(pin.utils.rotate("z", 1.268), np.array([0, 0, 0.75])) -viz.applyConfiguration("world/robot0/toolplate", Mtool) -viz.display(robot.q0) diff --git a/next/tp2/generated/parallel_robots_2 b/next/tp2/generated/parallel_robots_2 deleted file mode 100644 index ff64288..0000000 --- a/next/tp2/generated/parallel_robots_2 +++ /dev/null @@ -1,2 +0,0 @@ -effector_indexes = [robot.model.getFrameId("tool0_#%d" % i) for i in range(4)] -robot.framePlacement(robot.q0, effector_indexes[0]) diff --git a/next/tp2/generated/simple_pick_and_place_1 b/next/tp2/generated/simple_pick_and_place_1 deleted file mode 100644 index 817dce1..0000000 --- a/next/tp2/generated/simple_pick_and_place_1 +++ /dev/null @@ -1 +0,0 @@ -robot = robex.load("ur5") diff --git a/next/tp2/generated/simple_pick_and_place_2 b/next/tp2/generated/simple_pick_and_place_2 deleted file mode 100644 index abf8b02..0000000 --- a/next/tp2/generated/simple_pick_and_place_2 +++ /dev/null @@ -1,8 +0,0 @@ -# Add a red box in the viewer -ballID = "world/ball" -viz.addSphere(ballID, 0.1, colors.red) - -# Place the ball at the position ( 0.5, 0.1, 0.2 ) -# The viewer expect position and rotation, apppend the identity quaternion -q_ball = [0.5, 0.1, 0.2, 1, 0, 0, 0] -viz.applyConfiguration(ballID, q_ball) diff --git a/next/tp2/generated/simple_pick_and_place_3 b/next/tp2/generated/simple_pick_and_place_3 deleted file mode 100644 index 42f8564..0000000 --- a/next/tp2/generated/simple_pick_and_place_3 +++ /dev/null @@ -1,10 +0,0 @@ -q0 = np.zeros(NQ) # set the correct values here -q0[0] = -0.375 -q0[1] = -1.2 -q0[2] = 1.71 -q0[3] = -q0[1] - q0[2] -q0[4] = q0[0] -q0[5] = 0.0 - -viz.display(q0) -q = q0.copy() diff --git a/next/tp2/generated/simple_pick_and_place_4 b/next/tp2/generated/simple_pick_and_place_4 deleted file mode 100644 index b81b382..0000000 --- a/next/tp2/generated/simple_pick_and_place_4 +++ /dev/null @@ -1,22 +0,0 @@ -# Random velocity of the robot driving the movement -vq = np.array([2.0, 0, 0, 4.0, 0, 0]) - -idx = robot.index("wrist_3_joint") -o_eff = robot.placement( - q, idx -).translation # Position of end-eff wrt world at current configuration -o_ball = q_ball[:3] # Position of ball wrt world -eff_ball = o_ball - o_eff # Position of ball wrt eff - -for i in range(50): - # Chose new configuration of the robot - q += vq / 40 - q[2] = 1.71 + math.sin(i * 0.05) / 2 - - # Gets the new position of the ball - o_ball = robot.placement(q, idx) * eff_ball - - # Display new configuration for robot and ball - viz.applyConfiguration(ballID, o_ball.tolist() + [1, 0, 0, 0]) - viz.display(q) - time.sleep(1e-2) diff --git a/next/tp2/generated/simple_pick_and_place_5 b/next/tp2/generated/simple_pick_and_place_5 deleted file mode 100644 index c7fb203..0000000 --- a/next/tp2/generated/simple_pick_and_place_5 +++ /dev/null @@ -1,9 +0,0 @@ -# Add a red box in the viewer -boxID = "world/box" -# viz.delete(ballID) -viz.addBox(boxID, [0.1, 0.2, 0.1], colors.magenta) - -# Place the box at the position (0.5, 0.1, 0.2) -q_box = [0.5, 0.1, 0.2, 1, 0, 0, 0] -viz.applyConfiguration(boxID, q_box) -viz.applyConfiguration(ballID, [2, 2, 2, 1, 0, 0, 0]) diff --git a/next/tp2/generated/simple_pick_and_place_6 b/next/tp2/generated/simple_pick_and_place_6 deleted file mode 100644 index 44c1c44..0000000 --- a/next/tp2/generated/simple_pick_and_place_6 +++ /dev/null @@ -1,9 +0,0 @@ -q0 = np.zeros(NQ) -q0[0] = -0.375 -q0[1] = -1.2 -q0[2] = 1.71 -q0[3] = -q0[1] - q0[2] -q0[4] = q0[0] - -viz.display(q0) -q = q0.copy() diff --git a/next/tp2/generated/simple_pick_and_place_7 b/next/tp2/generated/simple_pick_and_place_7 deleted file mode 100644 index 949c41a..0000000 --- a/next/tp2/generated/simple_pick_and_place_7 +++ /dev/null @@ -1,22 +0,0 @@ -# Random velocity of the robot driving the movement -vq = np.array([2.0, 0, 0, 4.0, 0, 0]) - -idx = robot.index("wrist_3_joint") -oMeff = robot.placement( - q, idx -) # Placement of end-eff wrt world at current configuration -oMbox = pin.XYZQUATToSE3(q_box) # Placement of box wrt world -effMbox = oMeff.inverse() * oMbox # Placement of box wrt eff - -for i in range(100): - # Chose new configuration of the robot - q += vq / 40 - q[2] = 1.71 + math.sin(i * 0.05) / 2 - - # Gets the new position of the box - oMbox = robot.placement(q, idx) * effMbox - - # Display new configuration for robot and box - viz.applyConfiguration(boxID, oMbox) - viz.display(q) - time.sleep(1e-2) diff --git a/next/tp2/floating.py b/tp2/floating.py similarity index 97% rename from next/tp2/floating.py rename to tp2/floating.py index 3c523b0..b6a3cfa 100644 --- a/next/tp2/floating.py +++ b/tp2/floating.py @@ -16,7 +16,7 @@ import numpy as np from numpy.linalg import norm from scipy.optimize import fmin_bfgs -from supaero2024.meshcat_viewer_wrapper import MeshcatVisualizer +from supaero2025.meshcat_viewer_wrapper import MeshcatVisualizer # --- Load robot model # Solo12 is a quadruped robot. Its configuration is composed of: diff --git a/next/tp2/invgeom3d.py b/tp2/invgeom3d.py similarity index 97% rename from next/tp2/invgeom3d.py rename to tp2/invgeom3d.py index d48505d..8d163e5 100644 --- a/next/tp2/invgeom3d.py +++ b/tp2/invgeom3d.py @@ -14,7 +14,7 @@ import numpy as np from numpy.linalg import norm from scipy.optimize import fmin_bfgs -from supaero2024.meshcat_viewer_wrapper import MeshcatVisualizer +from supaero2025.meshcat_viewer_wrapper import MeshcatVisualizer # --- Load robot model robot = robex.load("ur5") diff --git a/next/tp2/invgeom6d.py b/tp2/invgeom6d.py similarity index 97% rename from next/tp2/invgeom6d.py rename to tp2/invgeom6d.py index adac38e..36988c4 100644 --- a/next/tp2/invgeom6d.py +++ b/tp2/invgeom6d.py @@ -16,7 +16,7 @@ import pinocchio as pin from numpy.linalg import norm from scipy.optimize import fmin_bfgs -from supaero2024.meshcat_viewer_wrapper import MeshcatVisualizer +from supaero2025.meshcat_viewer_wrapper import MeshcatVisualizer # --- Load robot model robot = robex.load("ur5") diff --git a/next/tp2/parallel_robots.py b/tp2/parallel_robots.py similarity index 89% rename from next/tp2/parallel_robots.py rename to tp2/parallel_robots.py index d5b4f0c..08b87f5 100644 --- a/next/tp2/parallel_robots.py +++ b/tp2/parallel_robots.py @@ -8,8 +8,8 @@ import pinocchio as pin # %jupyter_snippet 0 -from supaero2024.load_ur5_parallel import load_ur5_parallel -from supaero2024.meshcat_viewer_wrapper import MeshcatVisualizer +from supaero2025.load_ur5_parallel import load_ur5_parallel +from supaero2025.meshcat_viewer_wrapper import MeshcatVisualizer # Load 4 Ur5 robots, placed at 0.3m from origin in the 4 directions x,y,-x,-y. robot = load_ur5_parallel() diff --git a/next/tp2/simple_pick_and_place.py b/tp2/simple_pick_and_place.py similarity index 100% rename from next/tp2/simple_pick_and_place.py rename to tp2/simple_pick_and_place.py diff --git a/next/tp2/tests.py b/tp2/tests.py similarity index 100% rename from next/tp2/tests.py rename to tp2/tests.py