Merge PR #14 add Ergocub to library

[Add] Ergocub humanoid to library

Author: Danfoa

docs/index.html

@@ -184,7 +184,7 @@ <h2 class="title is-3">Video Presentation</h2>
 <section class="hero is-small" id="Introduction">
     <div class="hero-body">
         <div class="container is-centered">
-            <h2 class="title is-3 is-centered">Symmetries of robotic systems</h2>
+            <h2 id="symmetries-of-robotic-systems" class="title is-3 is-centered">Symmetries of robotic systems</h2>
             <p class="content has-text-justified">
                 <p float="left">
                     In the context of dynamical systems, a symmetry is a state transformation that results in another

morpho_symm/cfg/robot/ergocub.yaml

@@ -14,37 +14,63 @@ group_label: C2
 
 # QJ: Joint Space symmetries____________________________________
 #[
-#'l_hip_pitch', 'l_hip_roll', 'l_hip_yaw', 'l_knee', 'l_ankle_pitch', 'l_ankle_roll',    [0... 6)
-#'r_hip_pitch', 'r_hip_roll', 'r_hip_yaw', 'r_knee', 'r_ankle_pitch', 'r_ankle_roll',    [6...12)
-#'torso_roll', 'torso_pitch', 'torso_yaw',                                               [12..15)
-# 'l_shoulder_pitch', 'l_shoulder_roll', 'l_shoulder_yaw', 'l_elbow', 'l_wrist_yaw', 'l_wrist_roll',
-# 'l_wrist_pitch', 'l_index_add', 'l_index_prox', 'l_index_dist', 'l_middle_prox', 'l_middle_dist',
-# 'l_pinkie_prox', 'l_pinkie_dist', 'l_ring_prox', 'l_ring_dist', 'l_thumb_add', 'l_thumb_prox', 'l_thumb_dist',  [15...34)
-#'neck_pitch', 'neck_roll', 'neck_yaw', 'camera_tilt', [34...38)
-# 'r_shoulder_pitch', 'r_shoulder_roll', 'r_shoulder_yaw', 'r_elbow', 'r_wrist_yaw', 'r_wrist_roll',
-# 'r_wrist_pitch', 'r_index_add', 'r_index_prox', 'r_index_dist', 'r_middle_prox', 'r_middle_dist',
-# 'r_pinkie_prox', 'r_pinkie_dist', 'r_ring_prox', 'r_ring_dist', 'r_thumb_add', 'r_thumb_prox', 'r_thumb_dist' [38...57)
+# 'l_hip_pitch', 'l_hip_roll', 'l_hip_yaw', 'l_knee', 'l_ankle_pitch', 'l_ankle_roll',                                [0...5]
+# 'r_hip_pitch', 'r_hip_roll', 'r_hip_yaw', 'r_knee', 'r_ankle_pitch', 'r_ankle_roll',                                [6...11]
+# 'torso_roll', 'torso_pitch', 'torso_yaw',                                                                           [12..14]
+# 'l_shoulder_pitch', 'l_shoulder_roll', 'l_shoulder_yaw', 'l_elbow', 'l_wrist_yaw', 'l_wrist_roll', 'l_wrist_pitch', [15...21]
+# 'l_index_add', 'l_index_prox', 'l_index_dist',                                                                      [22,23, 24]
+# 'l_middle_prox', 'l_middle_dist',                                                                                   [25, 26]
+# 'l_pinkie_prox', 'l_pinkie_dist',                                                                                   [27, 28]
+# 'l_ring_prox', 'l_ring_dist',                                                                                       [29, 30]
+# 'l_thumb_add', 'l_thumb_prox', 'l_thumb_dist',                                                                      [31, 32, 33]
+# 'neck_pitch', 'neck_roll', 'neck_yaw', 'camera_tilt',                                                               [34, 35, 36, 37]
+# 'r_shoulder_pitch', 'r_shoulder_roll', 'r_shoulder_yaw', 'r_elbow', 'r_wrist_yaw', 'r_wrist_roll', 'r_wrist_pitch', [38,..., 44]
+# 'r_index_add', 'r_index_prox', 'r_index_dist',                                                                      [45, 46, 47]
+# 'r_middle_prox', 'r_middle_dist',                                                                                   [48, 49]
+# 'r_pinkie_prox', 'r_pinkie_dist',                                                                                   [50, 51]
+# 'r_ring_prox', 'r_ring_dist',                                                                                       [52, 53]
+# 'r_thumb_add', 'r_thumb_prox', 'r_thumb_dist'                                                                       [54, 55, 56]
 #]
 # QJ: Joint Space symmetries____________________________________
 permutation_Q_js: [
   [
-    6, 7, 8, 9, 10, 11,   # 'r_hip_pitch', 'r_hip_roll', 'r_hip_yaw', 'r_knee', 'r_ankle_pitch', 'r_ankle_roll'
-    0, 1, 2, 3, 4, 5,     # 'l_hip_pitch', 'l_hip_roll', 'l_hip_yaw', 'l_knee', 'l_ankle_pitch', 'l_ankle_roll'
-    12, 13, 14,           # 'torso_roll', 'torso_pitch', 'torso_yaw'
-    38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48, 49, 50, 51, 52, 53, 54, 55, 56,  # 'r_shoulder_pitch', ..., 'r_thumb_dist'
-    34, 35, 36, 37,       # 'neck_pitch', 'neck_roll', 'neck_yaw', 'camera_tilt'
-    15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33   # 'l_shoulder_pitch', ..., 'l_thumb_dist'
+    6, 7, 8, 9, 10, 11,          # 'r_hip_pitch', 'r_hip_roll', 'r_hip_yaw', 'r_knee', 'r_ankle_pitch', 'r_ankle_roll'
+    0, 1, 2, 3, 4, 5,            # 'l_hip_pitch', 'l_hip_roll', 'l_hip_yaw', 'l_knee', 'l_ankle_pitch', 'l_ankle_roll'
+    12, 13, 14,                  # 'torso_roll', 'torso_pitch', 'torso_yaw'
+    38, 39, 40, 41, 42, 43, 44,  # 'r_shoulder_pitch', 'r_shoulder_roll', 'r_shoulder_yaw', 'r_elbow', 'r_wrist_yaw', 'r_wrist_roll',
+    45, 46, 47,                  # 'r_index_add', 'r_index_prox', 'r_index_dist'
+    48, 49,                      # 'r_middle_prox', 'r_middle_dist'
+    50, 51,                      # 'r_pinkie_prox', 'r_pinkie_dist'
+    52, 53,                      # 'r_ring_prox', 'r_ring_dist'
+    54, 55, 56,                  # 'r_thumb_add', 'r_thumb_prox', 'r_thumb_dist'
+    34, 35, 36, 37,              # 'neck_pitch', 'neck_roll', 'neck_yaw', 'camera_tilt'
+    15, 16, 17, 18, 19, 20, 21,  # 'l_shoulder_pitch', 'l_shoulder_roll', 'l_shoulder_yaw', 'l_elbow', 'l_wrist_yaw', 'l_wrist_roll', 'l_wrist_pitch'
+    22, 23, 24,                  # 'l_index_add', 'l_index_prox', 'l_index_dist'
+    25, 26,                      # 'l_middle_prox', 'l_middle_dist'
+    27, 28,                      # 'l_pinkie_prox', 'l_pinkie_dist'
+    29, 30,                      # 'l_ring_prox', 'l_ring_dist'
+    31, 32, 33                   # 'l_thumb_add', 'l_thumb_prox', 'l_thumb_dist'
   ]
 ]
 
 # Reflections are determined by joint frame predefined orientation.
 reflection_Q_js: [
   [
-    1, -1, -1, 1, 1, -1,   # 'l_hip_pitch', 'l_hip_roll', 'l_hip_yaw', 'l_knee', 'l_ankle_pitch', 'l_ankle_roll'
-    1, -1, -1, 1, 1, -1,   # 'r_hip_pitch', 'r_hip_roll', 'r_hip_yaw', 'r_knee', 'r_ankle_pitch', 'r_ankle_roll'
+    1, 1, 1, 1, 1, 1,      # 'l_hip_pitch', 'l_hip_roll', 'l_hip_yaw', 'l_knee', 'l_ankle_pitch', 'l_ankle_roll'
+    1, 1, 1, 1, 1, 1,      # 'r_hip_pitch', 'r_hip_roll', 'r_hip_yaw', 'r_knee', 'r_ankle_pitch', 'r_ankle_roll'
     -1, 1, -1,             # 'torso_roll', 'torso_pitch', 'torso_yaw'
-    -1, -1, 1, -1, 1, -1, -1, -1, 1, -1, -1, -1, -1, 1, -1, -1, -1, -1, -1,      # 'r_shoulder_pitch', ..., 'r_thumb_dist'
-    1, -1, 1, 1,           # 'neck_pitch', 'neck_roll', 'neck_yaw', 'camera_tilt'
-    -1, -1, 1, -1, 1, -1, -1, -1, 1, -1, -1, -1, -1, 1, -1, -1, -1, -1, -1       # 'l_shoulder_pitch', ..., 'l_thumb_dist'
+    1, 1, 1, 1, 1, 1, 1,   # 'shoulder_pitch', 'shoulder_roll', 'shoulder_yaw', 'elbow', 'wrist_yaw', 'wrist_roll', 'wrist_pitch'
+    1, 1, 1,               # 'index_add', 'index_prox', 'index_dist'
+    1, 1,                  # 'middle_prox', 'middle_dist'
+    1, 1,                  # 'pinkie_prox', 'pinkie_dist'
+    1, 1,                  # 'r_ring_prox', 'ring_dist'
+    1, 1, 1,               # 'thumb_add', 'thumb_prox', 'thumb_dist'
+    1, -1, -1, 1,          # 'neck_pitch', 'neck_roll', 'neck_yaw', 'camera_tilt'
+    1, 1, 1, 1, 1, 1, 1,   # 'shoulder_pitch', 'shoulder_roll', 'shoulder_yaw', 'elbow', 'wrist_yaw', 'wrist_roll', 'wrist_pitch'
+    1, 1, 1,               # 'index_add', 'index_prox', 'index_dist'
+    1, 1,                  # 'middle_prox', 'middle_dist'
+    1, 1,                  # 'pinkie_prox', 'pinkie_dist'
+    1, 1,                  # 'r_ring_prox', 'ring_dist'
+    1, 1, 1,               # 'thumb_add', 'thumb_prox', 'thumb_dist'
   ]
 ]

morpho_symm/robot_symmetry_visualization.py

@@ -38,7 +38,6 @@ def main(cfg: DictConfig):
     rep_Ed = G.representations["E3"]  # rep_Ed(g) is a homogenous transformation matrix ∈ R^(3+1)x(3+1)
     rep_Rd = G.representations["R3"]  # rep_Rd(g) is an orthogonal matrix ∈ R^3x3
     rep_Rd_pseudo = G.representations["R3_pseudo"]  # rep_Rd_pseudo(g) is an orthogonal matrix ∈ R^3x3
-    rep_euler_xyz = G.representations["euler_xyz"]  # rep_euler_xyz(g) is an euler angle vector ∈ R^3
 
     # Configuration of the 3D visualization -------------------------------------------------------------------------
     # Not really relevant to understand.
@@ -111,12 +110,6 @@ def main(cfg: DictConfig):
         gXB = rep_Ed(g) @ XB @ rep_Ed(g).T
         orbit_XB_w[g] = gXB  # Add new robot base configuration (homogenous matrix) to the orbit of base configs.
         orbit_ori_euler_xyz[g] = Rotation.from_matrix(gXB[:3, :3]).as_euler("xyz", degrees=True)
-        # If people use euler xyz angles to represent the orientation of the robot base, we can also compute the
-        # symmetric states of the robot base orientation:
-        g_euler_xyz = rep_euler_xyz(g) @ base_ori_euler_xyz
-        # Check the analytic transformation to elements of SO(3) is equivalent to the transformation in euler xyz angles
-        g_euler_xyz_true = Rotation.from_matrix(gXB[:3, :3]).as_euler("xyz", degrees=True)
-        assert np.allclose(g_euler_xyz, g_euler_xyz_true, rtol=1e-6, atol=1e-6)
 
         # Use symmetry representations to get symmetric versions of Euclidean vectors, representing measurements of data
         orbit_f1[g], orbit_f2[g] = rep_Rd(g) @ f1, rep_Rd(g) @ f2
@@ -127,10 +120,6 @@ def main(cfg: DictConfig):
         orbit_Rf1[g] = rep_Rd(g) @ Rf1 @ rep_Rd(g).T
         orbit_Rf2[g] = rep_Rd(g) @ Rf2 @ rep_Rd(g).T
 
-    for g in G.elements:
-        print(f"Element: {g} euler_xyz(g): \n{rep_euler_xyz(g)}")
-        print(f"Element: {g} Rd_pseudo(g): \n{rep_Rd_pseudo(g)}")
-
     # Visualization of orbits of robot states and of data ==========================================================
     # Use Ctrl and mouse-click+drag to rotate the 3D environment.
     display_robots_and_vectors(

morpho_symm/utils/robot_utils.py

@@ -216,7 +216,8 @@ def load_symmetric_system(
                 )
             pb = configure_bullet_simulation(gui=True, debug=debug)
             robot.configure_bullet_simulation(pb, world=None)
-            change_robot_appearance(pb, robot)
+            if robot_cfg.tint_bodies:
+                change_robot_appearance(pb, robot)
             setup_debug_sliders(pb, robot)
             listen_update_robot_sliders(pb, robot)
 

pyproject.toml

@@ -6,7 +6,7 @@ allow-direct-references = true   # We use custom branch of escnn with some devel
 
 [project]
 name = "morpho-symm"
-version = "0.1.10"
+version = "0.1.11"
 keywords = ["morphological symmetry", "locomotion", "dynamical systems", "robot symmetries", "symmetry"]
 description = "Tools for the identification, study, and exploitation of morphological symmetries in robotics"
 readme = "README.md"