Aliengo bug fix.

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

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]]
+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]]
+

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
+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]]

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
     #

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)

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,19 +31,20 @@ 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
     rep_TqQJ = G.representations['TqQ_js']  # rep_TqQJ(g) is a permutation matrix ∈ R^nvj
     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:

morpho_symm/robots/PinBulletWrapper.py

@@ -57,24 +57,29 @@ 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
         self.world = world
         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
         if self.endeff_names is 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,

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:

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)
+        np.random.set_state(self.state)
+
+class ConfigException(Exception):
+    """Exception raised for errors in the configuration."""
+    def __init__(self, message):
+        super().__init__(message)

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])