Final versiomn of TP2.

1-forward_geom.ipynb

@@ -687,7 +687,7 @@
    "name": "python",
    "nbconvert_exporter": "python",
    "pygments_lexer": "ipython3",
-   "version": "3.11.6"
+   "version": "3.10.12"
   }
  },
  "nbformat": 4,

2-inv_geom.ipynb

@@ -791,7 +791,7 @@
    "name": "python",
    "nbconvert_exporter": "python",
    "pygments_lexer": "ipython3",
-   "version": "3.8.10"
+   "version": "3.10.12"
   }
  },
  "nbformat": 4,

tp2/floating.py

@@ -16,6 +16,7 @@
 import numpy as np
 from numpy.linalg import norm
 from scipy.optimize import fmin_bfgs
+
 from supaero2025.meshcat_viewer_wrapper import MeshcatVisualizer
 
 # --- Load robot model

tp2/generated/floating_1

@@ -0,0 +1,56 @@
+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)

tp2/generated/invgeom3d_1

@@ -0,0 +1,23 @@
+# 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)

tp2/generated/invgeom6d_1

@@ -0,0 +1,26 @@
+# 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))

tp2/generated/parallel_robots_0

@@ -0,0 +1,8 @@
+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()
+
+# Open the viewer
+viz = MeshcatVisualizer(robot)

tp2/generated/parallel_robots_1

@@ -0,0 +1,7 @@
+# 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)

tp2/generated/parallel_robots_2

@@ -0,0 +1,2 @@
+effector_indexes = [robot.model.getFrameId("tool0_#%d" % i) for i in range(4)]
+robot.framePlacement(robot.q0, effector_indexes[0])

tp2/generated/simple_pick_and_place_1

@@ -0,0 +1 @@
+robot = robex.load("ur5")

tp2/generated/simple_pick_and_place_2

@@ -0,0 +1,8 @@
+# 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)

tp2/generated/simple_pick_and_place_3

@@ -0,0 +1,10 @@
+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()

tp2/generated/simple_pick_and_place_4

@@ -0,0 +1,22 @@
+# 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)

tp2/generated/simple_pick_and_place_5

@@ -0,0 +1,9 @@
+# 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])

tp2/generated/simple_pick_and_place_6

@@ -0,0 +1,9 @@
+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()

tp2/generated/simple_pick_and_place_7

@@ -0,0 +1,22 @@
+# 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)

tp2/invgeom3d.py

@@ -1,10 +1,10 @@
 """
-Stand-alone inverse geom in 3D (translation only). Given a reference translation <target>,
-it computes the configuration of the UR5 so that the end-effector position (3D)
-matches the target. This is done using BFGS solver. While iterating to compute
-the optimal configuration, the script also display the successive candidate
-solution, hence moving the robot from the initial guess configuaration to the
-reference target.
+Stand-alone inverse geom in 3D (translation only). Given a reference
+translation <target>, it computes the configuration of the UR5 so that the
+end-effector position (3D) matches the target. This is done using BFGS
+solver. While iterating to compute the optimal configuration, the script also
+display the successive candidate solution, hence moving the robot from the
+initial guess configuaration to the reference target.
 """
 
 import time
@@ -14,6 +14,7 @@
 import numpy as np
 from numpy.linalg import norm
 from scipy.optimize import fmin_bfgs
+
 from supaero2025.meshcat_viewer_wrapper import MeshcatVisualizer
 
 # --- Load robot model

tp2/invgeom6d.py

@@ -16,6 +16,7 @@
 import pinocchio as pin
 from numpy.linalg import norm
 from scipy.optimize import fmin_bfgs
+
 from supaero2025.meshcat_viewer_wrapper import MeshcatVisualizer
 
 # --- Load robot model

tp2/simple_pick_and_place.py

@@ -14,7 +14,8 @@
 import example_robot_data as robex
 import numpy as np
 import pinocchio as pin
-from supaero2024.meshcat_viewer_wrapper import MeshcatVisualizer, colors
+
+from supaero2025.meshcat_viewer_wrapper import MeshcatVisualizer, colors
 
 # %jupyter_snippet 1
 robot = robex.load("ur5")