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/DynamicsRecording.py b/morpho_symm/data/DynamicsRecording.py index 6809c74..2e05649 100644 --- a/morpho_symm/data/DynamicsRecording.py +++ b/morpho_symm/data/DynamicsRecording.py @@ -20,18 +20,18 @@ class DynamicsRecording: """Data structure to store recordings of a Markov Dynamics.""" description: Optional[str] = None - info: Dict[str, object] = field(default_factory=dict) - dynamics_parameters: Dict = field(default_factory=lambda: {'dt': None}) + info: dict[str, object] = field(default_factory=dict) + dynamics_parameters: dict = field(default_factory=lambda: {'dt': None}) # Ordered list of observations composing to the state and action space of the Markov Process. - state_obs: Tuple[str] = field(default_factory=list) - action_obs: Tuple[str] = field(default_factory=list) + state_obs: tuple[str, ...] = field(default_factory=list) + action_obs: tuple[str, ...] = field(default_factory=list) # Map from observation name to the observation representation name. This name should be in `group_representations`. - obs_representations: Dict[str, Representation] = field(default_factory=dict) - recordings: Dict[str, Iterable] = field(default_factory=dict) + obs_representations: dict[str, Representation] = field(default_factory=dict) + recordings: dict[str, Iterable] = field(default_factory=dict) # Map from observation name to the observation moments (mean, var) of the recordings. - obs_moments: Dict[str, tuple] = field(default_factory=dict) + obs_moments: dict[str, tuple] = field(default_factory=dict) @property def obs_dims(self): 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/nn/MLP.py b/morpho_symm/nn/MLP.py index 8b78abc..d6d55c5 100644 --- a/morpho_symm/nn/MLP.py +++ b/morpho_symm/nn/MLP.py @@ -102,7 +102,7 @@ def reset_parameters(self, init_mode=None): try: torch.nn.init.kaiming_uniform_(tensor, mode=self.init_mode, nonlinearity=activation.lower()) except ValueError as e: - log.warning(f"Could not initialize {module.__class__.__name__} with {self.init_mode} mode. " + log.debug(f"Could not initialize {module.__class__.__name__} with {self.init_mode} mode. " f"Using default Pytorch initialization") elif 'normal' in self.init_mode.lower(): split = self.init_mode.split('l') diff --git a/morpho_symm/robot_symmetry_visualization.py b/morpho_symm/robot_symmetry_visualization.py index c017499..326d766 100644 --- a/morpho_symm/robot_symmetry_visualization.py +++ b/morpho_symm/robot_symmetry_visualization.py @@ -111,6 +111,7 @@ 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 + # ============================================================================================================= # 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"