diff --git a/morpho_symm/cfg/config_visualization.yaml b/morpho_symm/cfg/config_visualization.yaml index ccb22a9..37999da 100644 --- a/morpho_symm/cfg/config_visualization.yaml +++ b/morpho_symm/cfg/config_visualization.yaml @@ -6,6 +6,7 @@ defaults: # TODO: Make distinctions between. Trainer Args, Model Psecific Args, Program Args debug: False +debug_joints: False make_gif: False make_imgs: False gui: True @@ -19,6 +20,8 @@ hydra: dir: . # disable hydra run directory job: chdir: False + env_set: + HYDRA_FULL_ERROR: '1' job_logging: version: 1 colorlog: True diff --git a/morpho_symm/cfg/robot/aliengo.yaml b/morpho_symm/cfg/robot/aliengo.yaml index 26db72b..b130955 100644 --- a/morpho_symm/cfg/robot/aliengo.yaml +++ b/morpho_symm/cfg/robot/aliengo.yaml @@ -1,12 +1,32 @@ defaults: - base_robot -name: a1 +name: aliengo hip_height: 0.4 group_label: C2 + +# QJ: Joint Space symmetries____________________________________ +# Each leg is parameterized by a: +# Hip joint (1,): 1DoF revolute joint with position constraints +# Thigh joint (2,): 1DoF revolute joint with NO position constraints, thus modeled as a point in the unit circle +# Calf joint (1,): 1DoF revolute joint with position constraints +# The action of the reflection is defined by the permutation of the configurations of the legs, such that: +# FL <---g---> FR and HL <---g---> HR. +# ____FL_______|_____FR______|_________HL______|____HR____________| +# q = [ 0, (1,2), 3, 4, (5,6), 7, 8, (9,10), 11, 12, (13, 14), 15] +# g·q = [ 4, (5,6), 7, 0, (1,2), 3, 12, (13,14), 15, 8, (9,10), 11] # QJ: Joint Space symmetries____________________________________ -permutation_Q_js: [[3, 4, 5, 0, 1, 2, 9, 10, 11, 6, 7, 8]] +permutation_Q_js: [[4, 5, 6, 7, 0, 1, 2, 3, 12, 13, 14, 15, 8, 9, 10, 11]] # Reflections are determined by joint frame predefined orientation. -reflection_Q_js: [[-1, 1, 1, -1, 1, 1, -1, 1, 1, -1, 1, 1]] \ No newline at end of file +reflection_Q_js: [[-1, 1, 1, 1, -1, 1,1, 1, -1, 1, 1, 1, -1, 1, 1, 1,]] + +# The tangent space of a unit circle is a 1D line: +# ____FL______|_______FR_____|__________HL_________|_______HR__________| +# v = [ 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11] +# g·v = [ 3, 4, 5, 0, 1, 2, 9, 10, 11, 6, 7, 8] +permutation_TqQ_js: [[3, 4, 5, 0, 1, 2, 9, 10, 11, 6, 7, 8]] +# Reflections are determined by joint frame predefined orientation. +reflection_TqQ_js: [[-1, 1, 1, -1, 1, 1, -1, 1, 1, -1, 1, 1]] + diff --git a/morpho_symm/cfg/robot/base_robot.yaml b/morpho_symm/cfg/robot/base_robot.yaml index c1d1f4d..d094e54 100644 --- a/morpho_symm/cfg/robot/base_robot.yaml +++ b/morpho_symm/cfg/robot/base_robot.yaml @@ -19,4 +19,8 @@ hip_height: 1.0 endeff_names: null init_q: null q_zero: null # Zero vector of the generalized positions. Can be different from the Zero defined in URDF. -angle_sweep: 0.43 \ No newline at end of file +angle_sweep: 0.43 + +# 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/robot_harmonic_decomposition.py b/morpho_symm/robot_harmonic_decomposition.py index 5050164..922dd27 100644 --- a/morpho_symm/robot_harmonic_decomposition.py +++ b/morpho_symm/robot_harmonic_decomposition.py @@ -43,7 +43,7 @@ def on_key_press(key): def get_motion_trajectory(robot: PinBulletWrapper, recording_name=None, angle_sweep=0.5): # Load a trajectory of motion and measurements from the mini-cheetah robot recordings_path = Path( - morpho_symm.__file__).parent / f'data/{robot.robot_name}/raysim_recordings/flat/forward_minus_0_4/n_trajs=1-frames=7771-train.pkl' + morpho_symm.__file__).parent / f'data/{robot.name}/raysim_recordings/flat/forward_minus_0_4/n_trajs=1-frames=7771-train.pkl' dyn_recordings = DynamicsRecording.load_from_file(recordings_path) return dyn_recordings # diff --git a/morpho_symm/robot_harmonic_decomposition_mini_cheetah.py b/morpho_symm/robot_harmonic_decomposition_mini_cheetah.py index 3dc29b3..52a92d9 100644 --- a/morpho_symm/robot_harmonic_decomposition_mini_cheetah.py +++ b/morpho_symm/robot_harmonic_decomposition_mini_cheetah.py @@ -40,7 +40,7 @@ def on_key_press(key): def generate_dof_motions(robot: PinBulletWrapper, angle_sweep=0.5, recording_name = 'forest'): """TODO: In construction.""" - if robot.robot_name == 'mini_cheetah': + if robot.name == 'mini_cheetah': # / home / danfoa / Projects / MorphoSymm / morpho_symm / data / contact_dataset / training_splitted / mat_test / forest.mat recordings_path = Path(morpho_symm.__file__).parent / 'data/contact_dataset/training_splitted/mat_test' recordings = load_mini_cheetah_trajs(recordings_path) diff --git a/morpho_symm/robot_symmetry_visualization.py b/morpho_symm/robot_symmetry_visualization.py index 326d766..53918aa 100644 --- a/morpho_symm/robot_symmetry_visualization.py +++ b/morpho_symm/robot_symmetry_visualization.py @@ -5,6 +5,8 @@ import hydra import numpy as np from omegaconf import DictConfig +from scipy.spatial.transform import Rotation + from utils.pybullet_visual_utils import ( change_robot_appearance, display_robots_and_vectors, @@ -29,7 +31,7 @@ def main(cfg: DictConfig): np.random.seed(cfg.robot.seed) # Get robot instance, along with representations of the symmetry group on the Euclidean space (in which the robot # base B evolves in) and Joint Space (in which the internal configuration of the robot evolves in). - robot, G = load_symmetric_system(robot_cfg=cfg.robot, debug=cfg.debug) + robot, G = load_symmetric_system(robot_cfg=cfg.robot, debug=cfg.debug_joints) # Get the group representations of joint-state space, and Euclidean space. rep_QJ = G.representations['Q_js'] # rep_QJ(g) is a permutation matrix ∈ R^nqj @@ -37,11 +39,12 @@ 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. offset = max(0.2, 1.8 * robot.hip_height) # Offset between robot base and reflection planes. - pb = configure_bullet_simulation(gui=cfg.gui, debug=cfg.debug) # Load pybullet environment + pb = configure_bullet_simulation(gui=cfg.gui, debug=cfg.debug_joints) # Load pybullet environment robot.configure_bullet_simulation(pb, world=None) # Load robot in pybullet environment change_robot_appearance(pb, robot, change_color=cfg.robot.tint_bodies) # Add color and style to boring grey robots @@ -66,6 +69,8 @@ def main(cfg: DictConfig): # Get the robot's base configuration XB ∈ Ed as a homogenous transformation matrix. XB = robot.get_base_configuration() + base_ori_euler_xyz = Rotation.from_matrix(XB[:3, :3]).as_euler("xyz", degrees=True) + # Get joint space position and velocity coordinates (q_js, v_js) | q_js ∈ QJ, dq_js ∈ TqQJ q_js, v_js = robot.get_joint_space_state() @@ -88,7 +93,7 @@ def main(cfg: DictConfig): orbit_hg_B, orbit_XB_w = {e: hg_B}, {e: XB} orbit_f1, orbit_f2, orbit_rf1, orbit_rf2 = {e: f1}, {e: f2}, {e: rf1}, {e: rf2}, orbit_Rf1, orbit_Rf2 = {e: Rf1}, {e: Rf2} - + orbit_ori_euler_xyz = {e: base_ori_euler_xyz} # 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:]: @@ -101,9 +106,15 @@ def main(cfg: DictConfig): # gXB_w = rep_Ed(g) @ RB_w @ np.linalg.inv(rep_Ed(g)) 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 - # 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 orbit_rf1[g] = (rep_Ed(g) @ np.concatenate((rf1, np.ones(1))))[:3] # using homogenous coordinates orbit_rf2[g] = (rep_Ed(g) @ np.concatenate((rf2, np.ones(1))))[:3] @@ -112,7 +123,9 @@ 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. @@ -134,7 +147,7 @@ def main(cfg: DictConfig): save_path=save_path, anim_time=10, fps=15, periods=1, init_roll_pitch_yaw=(0, 35, 0), invert_roll="dh" in cfg.robot.group_label.lower(), pitch_sin_amplitude=20, - file_name=f"{robot.robot_name}-{G.name}-symmetries_anim_static", + file_name=f"{robot.name}-{G.name}-symmetries_anim_static", gen_gif=True, gen_imgs=False) print("Done enjoy your gif :). I hope you learned something new") elif cfg.make_imgs: diff --git a/morpho_symm/robots/PinBulletWrapper.py b/morpho_symm/robots/PinBulletWrapper.py index edb8961..9398374 100644 --- a/morpho_symm/robots/PinBulletWrapper.py +++ b/morpho_symm/robots/PinBulletWrapper.py @@ -57,8 +57,12 @@ def __init__(self, robot_name: str, endeff_names: Optional[Iterable] = None, fix self.bullet_endeff_ids = {} self.bullet_ids_allowed_floor_contacts = [] - def configure_bullet_simulation(self, bullet_client: BulletClient, world=None, - base_pos=(0, 0, 0), base_ori=(0, 0, 0, 1)): + def configure_bullet_simulation(self, + bullet_client: BulletClient, + world=None, + base_pos=(0, 0, 0), + base_ori=(0, 0, 0, 1) + ): """Configures the bullet simulation and loads this robot URDF description.""" # Load robot to simulation self._pb = bullet_client @@ -66,7 +70,7 @@ def configure_bullet_simulation(self, bullet_client: BulletClient, world=None, self.robot_id = self.load_bullet_robot(base_pos, base_ori) assert self.robot_id is not None - log.debug("Configuring Bullet Robot") + log.debug(f"Configuring Bullet Robot for {self.name}") bullet_joint_map = {} # Key: joint name - Value: joint id auto_end_eff = False @@ -74,7 +78,8 @@ def configure_bullet_simulation(self, bullet_client: BulletClient, world=None, auto_end_eff = True self._endeff_names = [] - for bullet_joint_id in range(self.bullet_client.getNumJoints(self.robot_id)): + n_bullet_joints = self.bullet_client.getNumJoints(self.robot_id) + for bullet_joint_id in range(n_bullet_joints): joint_info = self.bullet_client.getJointInfo(self.robot_id, bullet_joint_id) joint_name = joint_info[1].decode("UTF-8") bullet_joint_map[joint_name] = bullet_joint_id @@ -105,7 +110,7 @@ def configure_bullet_simulation(self, bullet_client: BulletClient, world=None, self.joint_space[joint_name] = pb_pin_joint elif auto_end_eff: - if not np.any([s in joint_name for s in ["base", "imu", "hip", "camera", "accelero"]]): + if not np.any([s in joint_name for s in ["base", "imu", "hip", "camera", "accelero", "rotor"]]): log.debug(f"Adding end-effector {joint_name}") self._endeff_names.append(joint_name) else: @@ -302,7 +307,7 @@ def load_bullet_robot(self, base_pos=None, base_ori=None) -> int: Returns: int: Bullet robot body id. """ - self.robot_id = pb_load_robot_description(f"{self.robot_name}_description", + self.robot_id = pb_load_robot_description(f"{self.name}_description", basePosition=base_pos, baseOrientation=base_ori, flags=self.bullet_client.URDF_USE_INERTIA_FROM_FILE | self.bullet_client.URDF_USE_SELF_COLLISION, @@ -350,7 +355,7 @@ def get_base_acceleration_world(self): def __repr__(self): """.""" bullet_id = f"({self.robot_id})" if hasattr(self, 'robot_id') else "" - return f"{self.robot_name}{bullet_id}-nq:{self.nq}-nv:{self.nv}" + return f"{self.name}{bullet_id}-nq:{self.nq}-nv:{self.nv}" @staticmethod def from_instance(other: 'PinBulletWrapper') -> 'PinBulletWrapper': @@ -366,7 +371,7 @@ def from_instance(other: 'PinBulletWrapper') -> 'PinBulletWrapper': PinBulletWrapper: A new instance of this robot wrapper """ return PinBulletWrapper( - robot_name=other.robot_name, + robot_name=other.name, endeff_names=other.endeff_names, fixed_base=other.fixed_base, reference_robot=other, diff --git a/morpho_symm/robots/PinSimWrapper.py b/morpho_symm/robots/PinSimWrapper.py index 2955ede..8ee3840 100644 --- a/morpho_symm/robots/PinSimWrapper.py +++ b/morpho_symm/robots/PinSimWrapper.py @@ -21,8 +21,14 @@ class PinSimWrapper(ABC): - def __init__(self, robot_name: str, endeff_names: Optional[NameList] = None, fixed_base=False, - reference_robot: Optional['PinSimWrapper'] = None, hip_height=1.0, init_q=None, q_zero=None): + def __init__(self, + robot_name: str, + endeff_names: Optional[NameList] = None, + fixed_base=False, + reference_robot: Optional['PinSimWrapper'] = None, + hip_height=1.0, + init_q=None, + q_zero=None): """Initializes the wrapper. Args: @@ -34,7 +40,7 @@ def __init__(self, robot_name: str, endeff_names: Optional[NameList] = None, fix init_q (List[float]): Initial configuration of the robot of length nq. q_zero (List[float]): Zero configuration. This can be different from the Zero config defined in the URDF. """ - self.robot_name = str.lower(robot_name) + self.name = str.lower(robot_name) self.fixed_base = fixed_base self._endeff_names = endeff_names self.hip_height = hip_height @@ -62,7 +68,6 @@ def __init__(self, robot_name: str, endeff_names: Optional[NameList] = None, fix if joint.idx_q == -1: continue # Ignore universe if joint.nq == 7: continue # Ignore floating-base pin_joint_type = joint.shortname() - log.debug(f"[{pin_joint_type}]:{joint_name} - DoF(nq):{joint.nq}, idx_q:{joint.idx_q}, idx_v:{joint.idx_v}") self.joint_space_names.append(joint_name) low_lim = self.pinocchio_robot.model.lowerPositionLimit[joint.idx_q:joint.idx_q + joint.nq] @@ -79,11 +84,7 @@ def __init__(self, robot_name: str, endeff_names: Optional[NameList] = None, fix self._diff_neutral_conf = diff_neutral_conf self.joint_space_names = sorted(self.joint_space_names, key=lambda x: self.pin_joint_space[x].idx_q) - - # TODO: ensure joint state is within configuration space. - self.joint_space = {} - log.debug(f"Robot loaded {self}") def get_state(self) -> State: """Fetch state of the system from a physics simulator and return pinocchio convention (q, dq). @@ -286,7 +287,15 @@ def load_pinocchio_robot(self, reference_robot: Optional['PinSimWrapper'] = None pin_robot.data = copy.copy(reference_robot.pinocchio_robot.data) assert sys.getrefcount(pin_robot.data) <= 2 else: - pin_robot = pin_load_robot_description(f"{self.robot_name}_description", root_joint=JointModelFreeFlyer()) + pin_robot = pin_load_robot_description(f"{self.name}_description", root_joint=JointModelFreeFlyer()) + log.debug(f"Loaded pinocchio robot model for {self.name}. The robot's joints are:") + n_pin_joints = len(pin_robot.model.joints) + for idx, joint, joint_name in zip(range(n_pin_joints), pin_robot.model.joints, pin_robot.model.names): + pin_joint_type = joint.shortname() + log.debug("\t -{:<3} {:<20} joint_type: {:<20} nq:{:<3} nv:{:<3} idx_q:{:<3} idx_v:{:<3}".format( + idx, joint_name, pin_joint_type, joint.nq, joint.nv, joint.idx_q, joint.idx_v) + ) + self._mass = float(np.sum([i.mass for i in pin_robot.model.inertias])) # [kg] self._pinocchio_robot = pin_robot @@ -407,7 +416,7 @@ def from_instance(other: 'PinSimWrapper') -> 'PinSimWrapper': def __repr__(self): """.""" - return f"{self.robot_name}-nq:{self.nq}-nv:{self.nv}" + return f"{self.name}-nq:{self.nq}-nv:{self.nv}" class JointWrapper: diff --git a/morpho_symm/utils/mysc.py b/morpho_symm/utils/mysc.py index a1a1292..e4beb5a 100644 --- a/morpho_symm/utils/mysc.py +++ b/morpho_symm/utils/mysc.py @@ -27,4 +27,9 @@ def __enter__(self): np.random.seed(self.seed) def __exit__(self, *args): - np.random.set_state(self.state) \ No newline at end of file + np.random.set_state(self.state) + +class ConfigException(Exception): + """Exception raised for errors in the configuration.""" + def __init__(self, message): + super().__init__(message) \ No newline at end of file diff --git a/morpho_symm/utils/pybullet_visual_utils.py b/morpho_symm/utils/pybullet_visual_utils.py index cebf5bd..2293a44 100644 --- a/morpho_symm/utils/pybullet_visual_utils.py +++ b/morpho_symm/utils/pybullet_visual_utils.py @@ -291,10 +291,10 @@ def spawn_robot_instances( assert n_instances == len(base_orientations), "Need to provide a base position and orientation per robot instance" # TODO: Copy error from Pinocchio. To be checked - # robots = [PinBulletWrapper.from_instance(robot) for _ in range(n_instances)] - kwargs = dict(robot_name=robot.robot_name, init_q=robot._init_q, hip_height=robot.hip_height, - endeff_names=robot.endeff_names, q_zero=robot._q0) - robots = [PinBulletWrapper(**kwargs) for _ in range(n_instances)] + robots = [PinBulletWrapper.from_instance(robot) for _ in range(n_instances)] + # kwargs = dict(robot_name=robot.robot_name, init_q=robot._init_q, hip_height=robot.hip_height, + # endeff_names=robot.endeff_names, q_zero=robot._q0) + # robots = [PinBulletWrapper(**kwargs) for _ in range(n_instances)] world = robot.world for r, pos, ori in zip(robots, base_positions, base_orientations): r.configure_bullet_simulation(bullet_client=bullet_client, world=world, base_pos=pos, base_ori=ori) @@ -395,7 +395,7 @@ def get_mock_ground_reaction_forces(pb, robot, robot_cfg): rf2_w, quatf2_w = (np.array(x) for x in pb.getLinkState(robot.robot_id, end_effectors[1])[0:2]) Rf1_w, Rf2_w = quat_xyzw_to_SO3(quatf1_w), quat_xyzw_to_SO3(quatf2_w) - if not np.any([s in robot.robot_name for s in ["atlas"]]): # Ignore + if not np.any([s in robot.name for s in ["atlas"]]): # Ignore Rf1_w, Rf2_w = np.eye(3), np.eye(3) rf1_w -= Rf1_w @ np.array([0, 0, 0.03]) rf2_w -= Rf2_w @ np.array([0, 0, 0.03]) diff --git a/morpho_symm/utils/robot_utils.py b/morpho_symm/utils/robot_utils.py index 1bcff3c..9947d7c 100644 --- a/morpho_symm/utils/robot_utils.py +++ b/morpho_symm/utils/robot_utils.py @@ -3,7 +3,7 @@ # @Time : 11/3/22 # @Author : Daniel Ordonez # @email : daniels.ordonez@gmail.com - +import logging import re from pathlib import Path from typing import Optional @@ -12,6 +12,7 @@ import escnn.group import numpy as np from escnn.group import CyclicGroup, DihedralGroup, DirectProductGroup, Group, Representation +from morpho_symm.utils.mysc import ConfigException from omegaconf import DictConfig, OmegaConf import morpho_symm @@ -23,7 +24,9 @@ configure_bullet_simulation, listen_update_robot_sliders, setup_debug_sliders, -) + ) + +log = logging.getLogger("MorphoSymm") def get_escnn_group(cfg: DictConfig): @@ -114,42 +117,54 @@ def load_symmetric_system( # Select the field for the representations. rep_field = float if robot_cfg.rep_fields.lower() != 'complex' else complex + # Assert the required representations are present in the robot configuration. if 'permutation_Q_js' not in robot_cfg: - raise AttributeError(f"Configuration file for {robot_name} must define the field `permutation_Q_js`, " - f"describing the joint space permutation per each non-trivial group's generator.") + raise ConfigException(f"Configuration file for {robot_name} must define the field `permutation_Q_js`, " + f"describing the joint space permutation per each non-trivial group's generator.") if 'permutation_TqQ_js' not in robot_cfg: - raise AttributeError(f"Configuration file for {robot_name} must define the field `permutation_TqQ_js`, " - f"describing the tangent joint-space permutation per each non-trivial group's generator.") + raise ConfigException(f"Configuration file for {robot_name} must define the field `permutation_TqQ_js`, " + f"describing the tangent joint-space permutation per each non-trivial group's generator.") reps_in_cfg = [k.split('permutation_')[1] for k in robot_cfg if "permutation" in k] + for rep_name in reps_in_cfg: - perm_list = list(robot_cfg[f'permutation_{rep_name}']) - rep_dim = len(perm_list[0]) - reflex_list = list(robot_cfg[f'reflection_{rep_name}']) - assert len(perm_list) == len(reflex_list), \ - f"Found different number of permutations and reflections for {rep_name}" - assert len(perm_list) >= len(G.generators), \ - f"Found {len(perm_list)} element reps for {rep_name}, Expected {len(G.generators)} generators for {G}" - # Generate ESCNN representation of generators - gen_rep = {} - for h, perm, refx in zip(G.generators, perm_list, reflex_list): - assert len(perm) == len(refx) == rep_dim - refx = np.array(refx, dtype=rep_field) - gen_rep[h] = gen_permutation_matrix(oneline_notation=perm, reflections=refx) - # Generate the entire group - rep = group_rep_from_gens(G, rep_H=gen_rep) - rep.name = rep_name - G.representations.update({rep_name: rep}) + try: + perm_list = list(robot_cfg[f'permutation_{rep_name}']) + rep_dim = len(perm_list[0]) + reflex_list = list(robot_cfg[f'reflection_{rep_name}']) + assert len(perm_list) == len(reflex_list), \ + f"Found different number of permutations and reflections for {rep_name}" + assert len(perm_list) >= len(G.generators), \ + f"Found {len(perm_list)} element reps for {rep_name}, Expected {len(G.generators)} generators for {G}" + # Generate ESCNN representation of generators + gen_rep = {} + for h, perm, refx in zip(G.generators, perm_list, reflex_list): + assert len(perm) == len(refx) == rep_dim + refx = np.array(refx, dtype=rep_field) + gen_rep[h] = gen_permutation_matrix(oneline_notation=perm, reflections=refx) + # Generate the entire group + rep = group_rep_from_gens(G, rep_H=gen_rep) + rep.name = rep_name + G.representations.update({rep_name: rep}) + except Exception as e: + raise ConfigException(f"Error in the definition of the representation {rep_name}") from e rep_Q_js = G.representations['Q_js'] rep_TqQ_js = G.representations.get('TqQ_js', None) rep_TqQ_js = rep_Q_js if rep_TqQ_js is None else rep_TqQ_js dimQ_js, dimTqQ_js = robot.nq - 7, robot.nv - 6 - assert dimQ_js == rep_Q_js.size - assert dimTqQ_js == rep_TqQ_js.size + if dimQ_js != rep_Q_js.size: + raise ConfigException( + f"{robot_name}'s Pinocchio joint-space dimension {dimQ_js} does not match the joint-space representation" + f"`Q_js` dimension {rep_Q_js.size}") + if dimTqQ_js != rep_TqQ_js.size: + raise ConfigException( + f"{robot_name}'s Pinocchio joint-space tangent dimension {dimTqQ_js} does not match the joint-space " + f"tangent representation `TqQ_js` dimension {rep_TqQ_js.size}") # Create the representation of isometries on the Euclidean Space in d dimensions. - rep_R3, rep_E3, rep_R3pseudo, rep_E3pseudo = generate_euclidean_space_representations(G) # This adds `O3` and `E3` representations to the group. + # This adds `O3` and `E3` representations to the group. + rep_R3, rep_E3, rep_R3pseudo, rep_E3pseudo = generate_euclidean_space_representations(G) # Define the representation of the rotation matrix R that transforms the base orientation. rep_rot_flat = {} @@ -167,10 +182,13 @@ def load_symmetric_system( E3_pseudo=rep_E3pseudo, SO3_flat=rep_rot_flat) + log.info(f"Loaded robot {robot_name}, with defined group representations:") + for name, rep in G.representations.items(): + log.info(f"\t {name}: dimension: {rep.size}") return robot, G -def generate_euclidean_space_representations(G: Group) -> tuple[Representation]: +def generate_euclidean_space_representations(G: Group) -> tuple[Representation, ...]: """Generate the E3 representation of the group G. This representation is used to transform all members of the Euclidean Space in 3D.