Change 'Rd' for 'R3' in representation name. Fix mini-cheetah dataset…

… reps.

README.md

@@ -35,8 +35,13 @@ As depicted above, each symmetry transformation, represented by a group element
 
 ```python
 from morpho_symm.utils.robot_utils import load_symmetric_system
+from morpho_symm.utils.pybullet_visual_utils import configure_bullet_simulation
 
 robot, G = load_symmetric_system(robot_name='mini_cheetah')
+
+bullet_client = configure_bullet_simulation(gui=True)         # Start pybullet simulation
+robot.configure_bullet_simulation(bullet_client, world=None)  # Load robot in pybullet environment
+
 # Get joint space position and velocity coordinates  (q_js, v_js) | q_js ∈ Qjs, dq_js ∈ TqQjs
 _, v_js = robot.get_joint_space_state()
 
@@ -310,11 +315,17 @@ The function returns:
 
 ### Getting and resetting the state of the system
 
-The system state is defined as $(\mathbf{q}, \mathbf{v}) | \mathbf{q} \in \mathcal{Q},  \mathbf{v} \in T_{q}\mathcal{Q}$, being $\mathcal{Q}$ the space of generalized position coordinates, and $\mathcal{T}_ {q}\mathcal{Q}$ the space of generalized velocity coordinates. Recall from the [paper convention](https://arxiv.org/abs/2302.10433) that the state configuration can be separated into base configuration and joint space configuration $\mathcal{Q} := \mathbb{E}_ d \times \mathcal{Q}_ {js}$. Where, $\mathbb{E}_ d$ is the Euclidean space in which the system evolves, and $\mathcal{Q}_ {js}$ is the joint space position coordinates. This enables to express every system state as $\mathbf{q} := \[\mathbf{X}_ B,  \mathbf{q}_ {js}\]^ T$, where $\mathbf{X}_ B \in  \mathbb{E}_ d$ and $\mathbf{q}_ {js} \in \mathcal{Q}_ {js}$. To access these quantities in code we do:
+The system state is defined as $(\mathbf{q}, \mathbf{v}) | \mathbf{q} \in \mathcal{Q},  \mathbf{v} \in T_{q}\mathcal{Q}$, being $\mathcal{Q}$ the space of generalized position coordinates, and $\mathcal{T}_ {q}\mathcal{Q}$ the space of generalized velocity coordinates. Recall from the [paper convention](https://arxiv.org/abs/2302.10433) that the state configuration can be separated into base configuration and joint space configuration $\mathcal{Q} := \mathbb{E}_ 3 \times \mathcal{Q}_ {js}$. Where, $\mathbb{E}_ 3$ is the Euclidean space in which the system evolves, and $\mathcal{Q}_ {js}$ is the joint space position coordinates. This enables to express every system state as $\mathbf{q} := \[\mathbf{X}_ B,  \mathbf{q}_ {js}\]^ T$, where $\mathbf{X}_ B \in  \mathbb{E}_ 3$ and $\mathbf{q}_ {js} \in \mathcal{Q}_ {js}$. To access these quantities in code we do:
 ```python 
+from morpho_symm.utils.pybullet_visual_utils import configure_bullet_simulation
+
+# Load robot to pybullet simulation environment. Robot state will be the simulation state. 
+bullet_client = configure_bullet_simulation(gui=True)         # Start pybullet simulation
+robot.configure_bullet_simulation(bullet_client, world=None)  # Load robot in pybullet environment
+
 # Get the state of the system
 q, v = robot.get_state()  #  q ∈ Q, v ∈ TqQ
-# Get the robot's base configuration XB ∈ Ed as a homogenous transformation matrix.
+# Get the robot's base configuration XB ∈ E3 as a homogenous transformation matrix.
 XB = robot.get_base_configuration()
 # Get joint space position and velocity coordinates  (q_js, v_js) | q_js ∈ Qjs, dq_js ∈ TqQjs
 q_js, v_js = robot.get_joint_space_state()
@@ -358,20 +369,20 @@ Any observation from this robot has a symmetric equivalent observation. In this
 
 #### Observations evolving in the Euclidean group of $d$ dimensions $\mathbb{E}_d$.
 
-The homogenous matrix describing the configuration of any rigid body, including the system's base configuration $\mathbf{X}_ B$ is an observation evolving in $\mathbb{E}_ d$. To obtain the set of symmetric base configurations, i.e. the observation group orbit: $\mathbb{G} \cdot \mathbf{X}_ B = \\{g \cdot \mathbf{X}_ B := \rho_ {\mathbb{E}_ d}(g) \\; \mathbf{X}_ B \\; \rho_ {\mathbb{E}_ d}(g)^{-1} | \forall \\; g \in \mathbb{G}\\}$ [(1)](https://danfoa.github.io/MorphoSymm/), you can do the following:
+The homogenous matrix describing the configuration of any rigid body, including the system's base configuration $\mathbf{X}_ B$ is an observation evolving in $\mathbb{E}_ 3$. To obtain the set of symmetric base configurations, i.e. the observation group orbit: $\mathbb{G} \cdot \mathbf{X}_ B = \\{g \cdot \mathbf{X}_ B := \rho_ {\mathbb{E}_ 3}(g) \\; \mathbf{X}_ B \\; \rho_ {\mathbb{E}_ 3}(g)^{-1} | \forall \\; g \in \mathbb{G}\\}$ [(1)](https://danfoa.github.io/MorphoSymm/), you can do the following:
 
 ```python 
-rep_Ed = G.representations['Ed']  # rep_Ed(g) ∈ R^(d+1)x(d+1) is a homogenous transformation matrix 
-# The orbit of the base configuration XB is a map from group elements g ∈ G to base configurations g·XB ∈ Ed
-orbit_X_B = {g: rep_Ed(g) @ XB @ rep_Ed(g).T for g in G.elements()} 
+rep_E3 = G.representations['E3']  # rep_E3(g) ∈ R^(3+1)x(3+1) is a homogenous transformation matrix 
+# The orbit of the base configuration XB is a map from group elements g ∈ G to base configurations g·XB ∈ E3
+orbit_X_B = {g: rep_E3(g) @ XB @ rep_E3(g).T for g in G.elements()} 
 ```
 
-Another example of an observation transfromed by $\rho_ {\mathbb{E}_ d}$ are **points** in $\mathbb{R}^d$. These can represent contact locations, object/body positions, etc. To obtain the point orbit, $\mathbb{G} \cdot \mathbf{r} = \\{g \cdot \mathbf{r} := \rho_ {\mathbb{E}_ d}(g) \\; \mathbf{r} | \forall \\; g \in \mathbb{G} \\}$, you can do:
+Another example of an observation transfromed by $\rho_ {\mathbb{E}_ 3}$ are **points** in $\mathbb{R}^d$. These can represent contact locations, object/body positions, etc. To obtain the point orbit, $\mathbb{G} \cdot \mathbf{r} = \\{g \cdot \mathbf{r} := \rho_ {\mathbb{E}_ 3}(g) \\; \mathbf{r} | \forall \\; g \in \mathbb{G} \\}$, you can do:
 ```python
-r = np.random.rand(3)   # Example point in Ed, assuming d=3.
-r_hom = np.concatenate((r, np.ones(1)))  # Use homogenous coordinates to represent a point in Ed
+r = np.random.rand(3)   # Example point in E3, assuming d=3.
+r_hom = np.concatenate((r, np.ones(1)))  # Use homogenous coordinates to represent a point in E3
 # The orbit of the point is a map from group elements g ∈ G to the set of symmetric points g·r ∈ R^d
-orbit_r = {g: (rep_Ed(g) @ r_hom)[:3] for g in G.elements}
+orbit_r = {g: (rep_E3(g) @ r_hom)[:3] for g in G.elements}
 ```
 
 #### Observations evolving in Joint Space (e.g. joint positions, velocity, torques). 
@@ -387,19 +398,19 @@ q_js, v_js = robot.get_joint_space_state()
 orbit_js_state = {g: (rep_Qjs(g) @ q_js, rep_TqQjs(g) @ v_js) for g in G.elements}
 ```
 
-#### Obervations evolving $\mathbb{R}_ d$ (e.g. vectors, pseudo-vectors).
+#### Obervations evolving $\mathbb{R}_ 3$ (e.g. vectors, pseudo-vectors).
 
-Observations evolving in $\mathbb{R}_ d$ include contact and ground reaction forces, linear and angular velocity of rigid bodies, distance vectors to target positons/locations, etc. To tranform vectors we use the representation $\rho_ {\mathbb{R}_ {d}}$. While to transform [pseudo-vectors](https://en.wikipedia.org/wiki/Pseudovector#:~:text=In%20physics%20and%20mathematics%2C%20a,of%20the%20space%20is%20changed) (or axial-vectors) we use the representation $\rho_ {\mathbb{R}_ {d,pseudo}}$ (these can represent angular velocities, angular accelerations, etc.). To obtain the orbit of these observations you can do:
+Observations evolving in $\mathbb{R}_ 3$ include contact and ground reaction forces, linear and angular velocity of rigid bodies, distance vectors to target positons/locations, etc. To tranform vectors we use the representation $\rho_ {\mathbb{R}_ {3}}$. While to transform [pseudo-vectors](https://en.wikipedia.org/wiki/Pseudovector#:~:text=In%20physics%20and%20mathematics%2C%20a,of%20the%20space%20is%20changed) (or axial-vectors) we use the representation $\rho_ {\mathbb{R}_ {3,pseudo}}$ (these can represent angular velocities, angular accelerations, etc.). To obtain the orbit of these observations you can do:
 ```python
-rep_Rd = G.representations['Rd'] # rep_Rd(g) is an orthogonal matrix ∈ R^dxd
-rep_Rd_pseudo = G.representations['Rd_pseudo'] 
+rep_R3 = G.representations['R3'] # rep_R3(g) is an orthogonal matrix ∈ R^dxd
+rep_R3_pseudo = G.representations['R3_pseudo'] 
 
 v = np.random.rand(3)  # Example vector in R3, E.g. linear velocity of the base frame.
-w = np.random.rand(3)  # Example pseudo-vector in Ed, assuming d=3. E.g. angular velocity of the base frame.
+w = np.random.rand(3)  # Example pseudo-vector in E3, assuming d=3. E.g. angular velocity of the base frame.
 # The orbit of the vector is a map from group elements g ∈ G to the set of symmetric vectors g·v ∈ R^d
-orbit_v = {g: rep_Rd(g) @ v for g in G.elements}
+orbit_v = {g: rep_R3(g) @ v for g in G.elements}
 # The orbit of the pseudo-vector is a map from group elements g ∈ G to the set of symmetric pseudo-vectors g·w ∈ R^d
-orbit_w = {g: rep_Rd_pseudo(g) @ w for g in G.elements}
+orbit_w = {g: rep_R3_pseudo(g) @ w for g in G.elements}
 ```
 
 ### Equivariant/Invariant Neural Networks
@@ -445,8 +456,8 @@ gspace = escnn.gspaces.no_base_space(G)
 # Get the relevant group representations.
 rep_Qjs = G.representations["Q_js"]  # Used to transform joint space position coordinates q_js ∈ Q_js
 rep_TqQjs = G.representations["TqQ_js"]  # Used to transform joint space velocity coordinates v_js ∈ TqQ_js
-rep_R3 = G.representations["Rd"]  # Used to transform the linear momentum l ∈ R3
-rep_R3_pseudo = G.representations["Rd_pseudo"]  # Used to transform the angular momentum k ∈ R3
+rep_R3 = G.representations["R3"]  # Used to transform the linear momentum l ∈ R3
+rep_R3_pseudo = G.representations["R3_pseudo"]  # Used to transform the angular momentum k ∈ R3
 
 # Define the input and output FieldTypes using the representations of each geometric object.
 # Representation of x := [q_js, v_js] ∈ Q_js x TqQ_js =>  ρ_X_js(g) := ρ_Q_js(g) ⊕ ρ_TqQ_js(g)  | g ∈ G

morpho_symm/cfg/robot/mini_cheetah.yaml

@@ -39,4 +39,8 @@ reflection_TqQ_js:  [[-1, 1, 1, -1, 1, 1, -1, 1, 1, -1, 1, 1], [1, -1, -1, 1, -1
 
 # 4D-Representation permuting the set of elements, associated with the legs kinematic chains
 permutation_kin_chain: [[1, 0, 3, 2], [2, 3, 0, 1], [0, 1, 2, 3]]
-reflection_kin_chain:  [[1, 1, 1, 1], [1, 1, 1, 1], [1, 1, 1, 1]]
+reflection_kin_chain:  [[1, 1, 1, 1], [1, 1, 1, 1], [1, 1, 1, 1]]
+
+# Transformation for Euler angles in 'xyz' convention
+permutation_euler_xyz: [[0, 1, 2], [0, 1, 2], [0, 1, 2]]
+reflection_euler_xyz: [[-1, 1, -1], [1, -1, -1], [-1, -1, 1]]

morpho_symm/data/mini_cheetah/read_recordings.py

@@ -26,7 +26,7 @@ def get_kinematic_three_rep(G: Group):
 
 
 def get_ground_reaction_forces_rep(G: Group, rep_kin_three: Representation):
-    rep_R3 = G.representations['Rd']
+    rep_R3 = G.representations['R3']
     rep_F = {G.identity: np.eye(12, dtype=int)}
     gens = [np.kron(rep_kin_three(g), rep_R3(g)) for g in G.generators]
     for h, rep_h in zip(G.generators, gens):
@@ -70,14 +70,16 @@ def convert_mini_cheetah_raysim_recordings(data_path: Path):
     robot, G = load_symmetric_system(robot_name='mini_cheetah')
     rep_Q_js = G.representations['Q_js']  # Representation on joint space position coordinates
     rep_TqQ_js = G.representations['TqQ_js']  # Representation on joint space velocity coordinates
-    rep_Rd = G.representations['Rd']  # Representation on vectors in R^d
-    rep_Rd_pseudo = G.representations['Rd_pseudo']  # Representation on pseudo vectors in R^d
+    rep_Rd = G.representations['R3']  # Representation on vectors in R^d
+    rep_Rd_pseudo = G.representations['R3_pseudo']  # Representation on pseudo vectors in R^d
+    rep_euler_xyz = G.representations['euler_xyz']  # Representation on Euler angles
+    rep_z = group_rep_from_gens(G, rep_H={h: rep_Rd(h)[2,2].reshape((1,1)) for h in G.elements if h != G.identity})
 
     # Define observation variables and their group representations z
     base_pos, base_pos_rep = state[:, :3], rep_Rd
-    base_z, base_z_rep = state[:, [2]], G.trivial_representation
+    base_z, base_z_rep = state[:, [2]], rep_z
     base_vel, base_vel_rep = state[:, 3:6], rep_Rd
-    base_ori, base_ori_rep = state[:, 6:9], rep_Rd
+    base_ori, base_ori_rep = state[:, 6:9], rep_euler_xyz
     base_ang_vel, base_ang_vel_rep = state[:, 9:12], rep_Rd_pseudo  # Pseudo vector
     feet_pos, feet_pos_rep = state[:, 12:24], directsum([rep_Rd] * 4, name='Rd^4')
     joint_vel, joint_vel_rep = state[:, 36:48], rep_TqQ_js
@@ -148,9 +150,6 @@ def convert_mini_cheetah_raysim_recordings(data_path: Path):
     base_vel_error = base_vel_error[::dt_subsample]
     base_ang_vel_error = base_ang_vel_error[::dt_subsample]
 
-
-
-
     data_recording = DynamicsRecording(
         description=f"Mini Cheetah {data_path.parent.parent.stem}",
         info=dict(num_traj=1,
@@ -175,7 +174,7 @@ def convert_mini_cheetah_raysim_recordings(data_path: Path):
                         base_vel_error=base_vel_error[None, ...].astype(np.float32),
                         base_ang_vel_error=base_ang_vel_error[None, ...].astype(np.float32),
                         ),
-        state_obs=('joint_pos', 'joint_vel', 'base_ori_R_flat' ,'base_z_error', 'base_vel_error', 'base_ang_vel_error'),
+        state_obs=('joint_pos', 'joint_vel', 'base_ori_R_flat', 'base_z_error', 'base_vel_error', 'base_ang_vel_error'),
         action_obs=('joint_torques',),
         obs_representations=dict(base_pos=base_pos_rep,
                                  base_z=G.trivial_representation,
@@ -236,6 +235,8 @@ def convert_mini_cheetah_raysim_recordings(data_path: Path):
     # file_path = data_path.parent.parent / "recording"
     # data_recording.save_to_file(file_path)
     print(f"Dynamics Recording saved to")
+
+
 #
 
 if __name__ == "__main__":

morpho_symm/robot_symmetry_visualization.py

@@ -91,7 +91,7 @@ def main(cfg: DictConfig):
 
     # For each symmetry action g ∈ G, we get the representations of the action in the relevant vector spaces to
     # compute the symmetric states of robot configuration and measurements.
-    for g in G.elements[1:]:
+    for g in G.generators:
         # Get symmetric joint-space state (g.q_js, g.v_js), and CoM momentum g·h=[g·l, g·k] ---------------------------
         # gx_w, gh_B = (rep_x(g) @ x).astype(x.dtype), (rep_h(g) @ hg_B).astype(hg_B.dtype)
         orbit_q_js[g], orbit_v_js[g] = rep_QJ(g) @ q_js, rep_TqQJ(g) @ v_js
@@ -102,6 +102,8 @@ 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.
 
+
+
         # Use symmetry representations to get symmetric versions of Euclidean vectors, representing measurements of data
         # We could also add some pseudo-vectors e.g. torque, and augment them as we did with `k`
         orbit_f1[g], orbit_f2[g] = rep_Rd(g) @ f1, rep_Rd(g) @ f2
@@ -111,6 +113,16 @@ def main(cfg: DictConfig):
         # Apply transformations to the terrain elevation estimations/measurements
         orbit_Rf1[g] = rep_Rd(g) @ Rf1 @ rep_Rd(g).T
         orbit_Rf2[g] = rep_Rd(g) @ Rf2 @ rep_Rd(g).T
+    # Iterate over all symmetric states and print the base position XB[:3, 3] and orientation XB[:3, :3] in euler
+    # angles. This is useful to understand how the robot base is transformed by the group elements.
+    from scipy.spatial.transform import Rotation
+    for g, XB in orbit_XB_w.items():
+        rB, RB = XB[:3, 3], XB[:3, :3]
+        rpy = Rotation.from_matrix(RB).as_euler('xyz', degrees=True)
+        print(f"Group Element: {g}, Base Position: {rB}, Base Orientation: {rpy}")
+        print(f"Rd rep of g: \n{rep_Rd(g)}")
+    raise ValueError("Stop here")
+
     # =============================================================================================================
 
     # Visualization of orbits of robot states and of data ==========================================================

morpho_symm/robot_symmetry_visualization_dynamic.py

@@ -57,7 +57,7 @@ def get_kinematic_three_rep(G: Group):
 
 def get_ground_reaction_forces_rep(G: Group, rep_kin_three: Representation):
 
-    rep_R3 = G.representations['Rd']
+    rep_R3 = G.representations['R3']
     rep_F = {G.identity: np.eye(12, dtype=int)}
     gens = [np.kron(rep_kin_three(g), rep_R3(g)) for g in G.generators]
     for h, rep_h in zip(G.generators, gens):
@@ -173,7 +173,7 @@ def main(cfg: DictConfig):
     rep_Q_js = G.representations['Q_js']
     # rep_TqJ = G.representations['TqQ_js']
     rep_E3 = G.representations['Ed']
-    rep_R3 = G.representations['Rd']
+    rep_R3 = G.representations['R3']
 
     offset = max(0.2, 1.8 * robot.hip_height)
     base_pos = np.array([-offset if G.order() != 2 else 0, -offset] + [robot.hip_height * 5.5])

morpho_symm/utils/rep_theory_utils.py

@@ -16,7 +16,7 @@ def generate_cyclic_rep(G: CyclicGroup, rep):
     """Generate cylic froup form a representation of its generator."""
     h = G.generators[0]
     # Check the given matrix representations comply with group axioms
-    assert not np.allclose(rep[h], rep[G.identity]), "Invalid generator: h=e"
+    # assert not np.allclose(rep[h], rep[G.identity]), "Invalid generator: h=e"
     assert np.allclose(np.linalg.matrix_power(rep[h], G.order()), rep[G.identity]), \
         f"Invalid rotation generator h_ref^{G.order()} != I"