diff --git a/README.md b/README.md index ec5e79c..7bea277 100644 --- a/README.md +++ b/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 diff --git a/morpho_symm/cfg/robot/mini_cheetah.yaml b/morpho_symm/cfg/robot/mini_cheetah.yaml index 5ec51ea..79da22e 100644 --- a/morpho_symm/cfg/robot/mini_cheetah.yaml +++ b/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]] \ No newline at end of file +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]] diff --git a/morpho_symm/data/mini_cheetah/read_recordings.py b/morpho_symm/data/mini_cheetah/read_recordings.py index c05a2fd..ff12a12 100644 --- a/morpho_symm/data/mini_cheetah/read_recordings.py +++ b/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__": diff --git a/morpho_symm/robot_symmetry_visualization.py b/morpho_symm/robot_symmetry_visualization.py index c017499..75b7da6 100644 --- a/morpho_symm/robot_symmetry_visualization.py +++ b/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 ========================================================== diff --git a/morpho_symm/robot_symmetry_visualization_dynamic.py b/morpho_symm/robot_symmetry_visualization_dynamic.py index bd1734a..da45c8e 100644 --- a/morpho_symm/robot_symmetry_visualization_dynamic.py +++ b/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]) diff --git a/morpho_symm/utils/rep_theory_utils.py b/morpho_symm/utils/rep_theory_utils.py index 54129c9..81434bb 100644 --- a/morpho_symm/utils/rep_theory_utils.py +++ b/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"