diff --git a/docs/source/content/casefiles.rst b/docs/source/content/casefiles.rst
index da570fc42..36e40f87d 100644
--- a/docs/source/content/casefiles.rst
+++ b/docs/source/content/casefiles.rst
@@ -319,7 +319,99 @@ Item by item:
 
     This is an optional group to add if correcting the aerodynamic forces using airfoil polars is desired. A polar
     should be included for each airfoil defined. Each entry consists of a 4-column table. The first column corresponds
-    to the angle of attack (in radians) and then the ``C_L``, ``C_D`` and ``C_M``. 
+    to the angle of attack (in radians) and then the ``C_L``, ``C_D`` and ``C_M``.
+
+Multibody file
+--------------
+
+All the aerodynamic data is contained in ``case.mb.h5``.
+
+This file encapsulates both the initial conditions for the multiple bodies, and the constraints between them.
+
+Item by item:
+
+* ``num_bodies``: Number of bodies.
+
+* ``num_constraints``: Number of constraints between bodies.
+
+    The initial conditions for each body and the constraint definitions are defined in groups. The body groups are named
+    as ``body_xx``, where the xx is replaced with a two digit body number starting from 00, e.g. ``body_00``. Each of these
+    groups should have the following items:
+
+* ``FoR_acceleration [6]``: Frame of reference initial acceleration.
+
+    An array of the stacked linear and rotational initial accelerations in the inertial frame.
+
+* ``FoR_velocity [6]``: Frame of reference initial velocity.
+
+    An array of the stacked linear and rotational initial velocities in the inertial frame.
+
+* ``FoR_position [6]``: Frame of reference initial position.
+
+    An array of the stacked linear and rotational initial positions in the inertial frame.
+
+* ``quat [4]``: Frame of reference initial orientation.
+
+    A quaternion describing the initial rotation between the body attached and inertial frames.
+
+* ``body_number``: Body number.
+
+    An integer used to identify the body when creating constraints.
+
+* ``FoR_movement``: Type of frame of reference movement.
+
+    Use "free" to include rigid body motion, or "prescribed" for a clamped body.
+
+The constraint groups are named similarly to the bodies, using ``constraint_xx`` where xx is the two digit constraint number
+starting from 00. Each of these groups should have the following items:
+
+* ``scalingFactor``: Scaling factor.
+
+    This value scales the multibody equations, where generally settings to ``dt^2`` provides acceptable results.
+
+* ``behaviour``: Constraint behaviour.
+
+    This string defines the type of constraint applied to a single or multiple bodies. A wide range of standard lower-pair
+    kinematic joints are available, such as hinge and spherical joints, as well as prescribed rotation joints. A list of the
+    available joints can be found in ``sharpy/structure/utils/lagrangeconstraints.py`` and
+    ``sharpy/structure/utils/lagrangeconstraintsjax.py``, depending on the solver being used. Due to every constraint being
+    different, further parameters depend upon the constraint type used. These parameters are added as variables to the constraint
+    group. Some examples which may be included are listed below:
+
+* ``body_FoR``: Body frame of reference number.
+
+    The number of the body which is constrained by its body attached frame of reference. For example for a double pendulum,
+    this would be the lower link.
+
+* ``body``: Body number.
+
+    The number of the body which is constrained by one of its nodes. For example for a double pendulum,
+    this would be the upper link.
+
+* ``node_in_body``: Node in body.
+
+    This is paired to the ``body`` parameter, and this indicates which node within the body is to be constrained.
+
+* ``rot_axisB [3]``: Rotation axis in the B frame.
+
+    For a hinge constraint, this defines a vector for the hinge axis for the ``body``. This is defined in the material frame.
+
+* ``rot_axisA2 [3]``: Rotation axis in the A frame.
+
+    For a hinge constraint, this defines a vector for the hinge axis for the ``body_FoR``. This is defined in the body attached frame.
+
+* ``controller_id``: Controller ID for using an actuated constraint.
+
+    This should use the same ID as the ``MultibodyController`` used in the ``DynamicCoupled`` simulation, allowing the rotation to be controlled over time.
+
+* ``aerogrid_warp_factor [num_elem, 3]``: Aerodynamic grid warping factor.
+
+    For simulating wings with dynamic sweep by a local rotation around the z axis, the aerodynamic grid needs to warp to
+    account for this. To create a smooth warp, it can be preferred to gradually change the sweep (and therefore chord to
+    maintain the "same" geometry) at elements around the discontinuity. This parameter sets the sweep per node using this
+    parameter to scale the input angle. If not included, it will be ignored when generating the aerodynamic grid and have
+    no effect.
+
 
 Nonlifting Body file
 -----------------
@@ -354,7 +446,7 @@ Item by item:
 
 * ``z_0_ellipse [num_node]``: Vertical offset of the ellipse center from the beam node.
 
-    Is an array with the vertical offset of the center of the elliptical cross-sectoin from the fuselage node.
+    Is an array with the vertical offset of the center of the elliptical cross-section from the fuselage node.
 
 *  ``surface_m [num_surfaces]``: Radial panelling.
 
diff --git a/docs/source/content/example_notebooks/cantilever/model_static_cantilever.py b/docs/source/content/example_notebooks/cantilever/model_static_cantilever.py
index 1c36b528c..52422ddad 100644
--- a/docs/source/content/example_notebooks/cantilever/model_static_cantilever.py
+++ b/docs/source/content/example_notebooks/cantilever/model_static_cantilever.py
@@ -16,8 +16,7 @@ def generate_fem_file(route, case_name, num_elem, deadforce=600e3, followerforce
     length = 5
     num_node_elem=3
     num_node = (num_node_elem - 1)*num_elem + 1
-    # import pdb; pdb.set_trace()
-    angle = 0*np.pi/180.0       # Angle of the beam reference line within the x-y plane of the B frame.
+    angle = 0. * np.pi / 180.       # Angle of the beam reference line within the x-y plane of the B frame.
     x = (np.linspace(0, length, num_node))*np.cos(angle)
     y = (np.linspace(0, length, num_node))*np.sin(angle)
     z = np.zeros((num_node,))
@@ -41,7 +40,6 @@ def generate_fem_file(route, case_name, num_elem, deadforce=600e3, followerforce
                           + [0, 2, 1])
 
     # stiffness array
-    # import pdb; pdb.set_trace()
     num_stiffness = 1
     ea = 4.8e8
     ga = 3.231e8
@@ -49,7 +47,6 @@ def generate_fem_file(route, case_name, num_elem, deadforce=600e3, followerforce
     ei = 9.346e6
     base_stiffness = np.diag([ea, ga, ga, gj, ei, ei])
     stiffness = np.zeros((num_stiffness, 6, 6))
-    # import pdb; pdb.set_trace()
     for i in range(num_stiffness):
         stiffness[i, :, :] = base_stiffness
 
diff --git a/scripts/optimiser/optimiser.py b/scripts/optimiser/optimiser.py
index d4d583298..13690da1f 100755
--- a/scripts/optimiser/optimiser.py
+++ b/scripts/optimiser/optimiser.py
@@ -265,8 +265,6 @@ def optimiser(in_dict, previous_x, previous_y):
 
     print('FINISHED')
 
-    import pdb; pdb.set_trace()
-
 def local_optimisation(opt, yaml_dict=None, min_method='Powell'):
     x_in = opt.X
     y_in = opt.Y
@@ -280,7 +278,6 @@ def local_optimisation(opt, yaml_dict=None, min_method='Powell'):
                'gtol': 1e-3}
     # scipy.optimize
     local_opt = optimize.minimize(
-                                  # lambda x: rbf_constrained(x, rbf, yaml_dict, opt),
                                   lambda x: gp_constrained(x, opt, yaml_dict),
                                   x0=opt.x_opt,
                                   method=min_method,
diff --git a/setup.py b/setup.py
index 500fb1bad..034aa82be 100644
--- a/setup.py
+++ b/setup.py
@@ -145,7 +145,8 @@ def run(self):
         "openpyxl>=3.0.10",
         "lxml>=4.4.1",
         "PySocks",
-        "PyYAML"
+        "PyYAML",
+        "jax",
     ],
     extras_require={
         "docs": [
diff --git a/sharpy/aero/models/aerogrid.py b/sharpy/aero/models/aerogrid.py
index 72794ce01..5053d21a7 100644
--- a/sharpy/aero/models/aerogrid.py
+++ b/sharpy/aero/models/aerogrid.py
@@ -24,6 +24,7 @@ class Aerogrid(Grid):
     It is created by the solver :class:`sharpy.solvers.aerogridloader.AerogridLoader`
 
     """
+
     def __init__(self):
         super().__init__()
         self.dimensions_star = None
@@ -47,7 +48,7 @@ def generate(self, data_dict, beam, settings, ts):
                                          self.dimensions_star)
 
         # Initial panel orientation, used when aligned grid is off
-        self.initial_strip_z_rot = np.zeros([self.n_elem, 3]) 
+        self.initial_strip_z_rot = np.zeros([self.n_elem, 3])
         if not settings['aligned_grid'] and settings['initial_align']:
             for i_elem in range(self.n_elem):
                 for i_local_node in range(3):
@@ -62,7 +63,8 @@ def generate(self, data_dict, beam, settings, ts):
                 try:
                     self.airfoil_db[self.data_dict['airfoil_distribution'][i_elem, i_local_node]]
                 except KeyError:
-                    airfoil_coords = self.data_dict['airfoils'][str(self.data_dict['airfoil_distribution'][i_elem, i_local_node])]
+                    airfoil_coords = self.data_dict['airfoils'][
+                        str(self.data_dict['airfoil_distribution'][i_elem, i_local_node])]
                     self.airfoil_db[self.data_dict['airfoil_distribution'][i_elem, i_local_node]] = (
                         scipy.interpolate.interp1d(airfoil_coords[:, 0],
                                                    airfoil_coords[:, 1],
@@ -80,14 +82,13 @@ def generate(self, data_dict, beam, settings, ts):
         try:
             settings['control_surface_deflection']
         except KeyError:
-            settings.update({'control_surface_deflection': ['']*self.n_control_surfaces})
+            settings.update({'control_surface_deflection': [''] * self.n_control_surfaces})
 
         # pad ctrl surfaces dict with empty strings if not defined
         if len(settings['control_surface_deflection']) != self.n_control_surfaces:
-            undef_ctrl_sfcs = ['']*(self.n_control_surfaces - len(settings['control_surface_deflection']))
+            undef_ctrl_sfcs = [''] * (self.n_control_surfaces - len(settings['control_surface_deflection']))
             settings['control_surface_deflection'].extend(undef_ctrl_sfcs)
 
-
         # initialise generators
         with_error_initialising_cs = False
         for i_cs in range(self.n_control_surfaces):
@@ -113,7 +114,6 @@ def generate(self, data_dict, beam, settings, ts):
         if with_error_initialising_cs:
             raise KeyError('Unable to locate settings for at least one control surface.')
 
-
         self.add_timestep()
         self.generate_mapping()
         self.generate_zeta(self.beam, self.aero_settings, ts)
@@ -136,13 +136,13 @@ def output_info(self):
             cout.cout_wrap('     Wake %u, M=%u, N=%u' % (i_surf,
                                                          self.dimensions_star[i_surf, 0],
                                                          self.dimensions_star[i_surf, 1]))
-        cout.cout_wrap('  In total: %u bound panels' % (sum(self.dimensions[:, 0]*
+        cout.cout_wrap('  In total: %u bound panels' % (sum(self.dimensions[:, 0] *
                                                             self.dimensions[:, 1])))
-        cout.cout_wrap('  In total: %u wake panels' % (sum(self.dimensions_star[:, 0]*
+        cout.cout_wrap('  In total: %u wake panels' % (sum(self.dimensions_star[:, 0] *
                                                            self.dimensions_star[:, 1])))
-        cout.cout_wrap('  Total number of panels = %u' % (sum(self.dimensions[:, 0]*
+        cout.cout_wrap('  Total number of panels = %u' % (sum(self.dimensions[:, 0] *
                                                               self.dimensions[:, 1]) +
-                                                          sum(self.dimensions_star[:, 0]*
+                                                          sum(self.dimensions_star[:, 0] *
                                                               self.dimensions_star[:, 1])))
 
     def calculate_dimensions(self):
@@ -172,9 +172,9 @@ def generate_zeta_timestep_info(self, structure_tstep, aero_tstep, beam, setting
             self.data_dict['sweep'] = np.zeros_like(self.data_dict['twist'])
 
         # Define first_twist for backwards compatibility
-        if 'first_twist' not in self.data_dict:     
-            self.data_dict['first_twist'] = [True]*self.data_dict['surface_m'].shape[0]
-        
+        if 'first_twist' not in self.data_dict:
+            self.data_dict['first_twist'] = [True] * self.data_dict['surface_m'].shape[0]
+
         # one surface per element
         for i_elem in range(self.n_elem):
             i_surf = self.data_dict['surface_distribution'][i_elem]
@@ -193,11 +193,6 @@ def generate_zeta_timestep_info(self, structure_tstep, aero_tstep, beam, setting
                 else:
                     global_node_in_surface[i_surf].append(i_global_node)
 
-                # master_elem, master_elem_node = beam.master[i_elem, i_local_node, :]
-                # if master_elem < 0:
-                    # master_elem = i_elem
-                    # master_elem_node = i_local_node
-
                 # find the i_surf and i_n data from the mapping
                 i_n = -1
                 ii_surf = -1
@@ -213,24 +208,27 @@ def generate_zeta_timestep_info(self, structure_tstep, aero_tstep, beam, setting
                 # control surface implementation
                 control_surface_info = None
                 if with_control_surfaces:
-                # 1) check that this node and elem have a control surface
+                    # 1) check that this node and elem have a control surface
                     if self.data_dict['control_surface'][i_elem, i_local_node] >= 0:
                         i_control_surface = self.data_dict['control_surface'][i_elem, i_local_node]
-                # 2) type of control surface + write info
+                        # 2) type of control surface + write info
                         control_surface_info = dict()
                         if self.data_dict['control_surface_type'][i_control_surface] == 0:
                             control_surface_info['type'] = 'static'
-                            control_surface_info['deflection'] = self.data_dict['control_surface_deflection'][i_control_surface]
+                            control_surface_info['deflection'] = self.data_dict['control_surface_deflection'][
+                                i_control_surface]
                             control_surface_info['chord'] = self.data_dict['control_surface_chord'][i_control_surface]
                             try:
-                                control_surface_info['hinge_coords'] = self.data_dict['control_surface_hinge_coords'][i_control_surface]
+                                control_surface_info['hinge_coords'] = self.data_dict['control_surface_hinge_coords'][
+                                    i_control_surface]
                             except KeyError:
                                 control_surface_info['hinge_coords'] = None
                         elif self.data_dict['control_surface_type'][i_control_surface] == 1:
                             control_surface_info['type'] = 'dynamic'
                             control_surface_info['chord'] = self.data_dict['control_surface_chord'][i_control_surface]
                             try:
-                                control_surface_info['hinge_coords'] = self.data_dict['control_surface_hinge_coords'][i_control_surface]
+                                control_surface_info['hinge_coords'] = self.data_dict['control_surface_hinge_coords'][
+                                    i_control_surface]
                             except KeyError:
                                 control_surface_info['hinge_coords'] = None
 
@@ -242,7 +240,8 @@ def generate_zeta_timestep_info(self, structure_tstep, aero_tstep, beam, setting
                             control_surface_info['type'] = 'controlled'
 
                             try:
-                                old_deflection = self.data.aero.timestep_info[-1].control_surface_deflection[i_control_surface]
+                                old_deflection = self.data.aero.timestep_info[-1].control_surface_deflection[
+                                    i_control_surface]
                             except AttributeError:
                                 try:
                                     old_deflection = aero_tstep.control_surface_deflection[i_control_surface]
@@ -250,35 +249,50 @@ def generate_zeta_timestep_info(self, structure_tstep, aero_tstep, beam, setting
                                     old_deflection = self.data_dict['control_surface_deflection'][i_control_surface]
 
                             try:
-                                control_surface_info['deflection'] = aero_tstep.control_surface_deflection[i_control_surface]
+                                control_surface_info['deflection'] = aero_tstep.control_surface_deflection[
+                                    i_control_surface]
                             except IndexError:
-                                control_surface_info['deflection'] = self.data_dict['control_surface_deflection'][i_control_surface]
+                                control_surface_info['deflection'] = self.data_dict['control_surface_deflection'][
+                                    i_control_surface]
 
                             if dt is not None:
                                 control_surface_info['deflection_dot'] = (
-                                        (control_surface_info['deflection'] - old_deflection)/dt)
+                                        (control_surface_info['deflection'] - old_deflection) / dt)
                             else:
                                 control_surface_info['deflection_dot'] = 0.0
 
                             control_surface_info['chord'] = self.data_dict['control_surface_chord'][i_control_surface]
 
                             try:
-                                control_surface_info['hinge_coords'] = self.data_dict['control_surface_hinge_coords'][i_control_surface]
+                                control_surface_info['hinge_coords'] = self.data_dict['control_surface_hinge_coords'][
+                                    i_control_surface]
                             except KeyError:
                                 control_surface_info['hinge_coords'] = None
                         else:
                             raise NotImplementedError(str(self.data_dict['control_surface_type'][i_control_surface]) +
-                                ' control surfaces are not yet implemented')
-
-
+                                                      ' control surfaces are not yet implemented')
+
+                # add sweep for aerogrid warping in constraint defintition
+                # if no constraint_xx.aerogrid warp factor is provided or the constraint is not an actuated type,
+                # this will be ignored
+                ang_warp = 0.
+                if structure_tstep.mb_dict is not None and structure_tstep.mb_prescribed_dict is not None:
+                    for i_constraint in range(structure_tstep.mb_dict['num_constraints']):
+                        cst_name = f"constraint_{i_constraint:02d}"
+                        if ('controller_id' in structure_tstep.mb_dict[cst_name]
+                                and 'aerogrid_warp_factor' in structure_tstep.mb_dict[cst_name]):
+                            ctrl_id = structure_tstep.mb_dict[cst_name]['controller_id'].decode('UTF-8')
+                            f_warp = structure_tstep.mb_dict[cst_name]['aerogrid_warp_factor'][i_elem, i_local_node]
+                            ang_z = structure_tstep.mb_prescribed_dict[ctrl_id]['delta_psi'][2]
+                            ang_warp += f_warp * ang_z
 
                 node_info = dict()
                 node_info['i_node'] = i_global_node
                 node_info['i_local_node'] = i_local_node
-                node_info['chord'] = self.data_dict['chord'][i_elem, i_local_node]
+                node_info['chord'] = self.data_dict['chord'][i_elem, i_local_node] / np.cos(ang_warp)
                 node_info['eaxis'] = self.data_dict['elastic_axis'][i_elem, i_local_node]
                 node_info['twist'] = self.data_dict['twist'][i_elem, i_local_node]
-                node_info['sweep'] = self.data_dict['sweep'][i_elem, i_local_node]
+                node_info['sweep'] = self.data_dict['sweep'][i_elem, i_local_node] + ang_warp
                 node_info['M'] = self.dimensions[i_surf, 0]
                 node_info['M_distribution'] = self.data_dict['m_distribution'].decode('ascii')
                 node_info['airfoil'] = self.data_dict['airfoil_distribution'][i_elem, i_local_node]
@@ -293,7 +307,8 @@ def generate_zeta_timestep_info(self, structure_tstep, aero_tstep, beam, setting
                 node_info['cga'] = structure_tstep.cga()
                 if node_info['M_distribution'].lower() == 'user_defined':
                     ielem_in_surf = i_elem - np.sum(self.surface_distribution < i_surf)
-                    node_info['user_defined_m_distribution'] = self.data_dict['user_defined_m_distribution'][str(i_surf)][:, ielem_in_surf, i_local_node]
+                    node_info['user_defined_m_distribution'] = self.data_dict['user_defined_m_distribution'][
+                                                                   str(i_surf)][:, ielem_in_surf, i_local_node]
                 (aero_tstep.zeta[i_surf][:, :, i_n],
                  aero_tstep.zeta_dot[i_surf][:, :, i_n]) = (
                     generate_strip(node_info,
@@ -307,10 +322,9 @@ def generate_zeta_timestep_info(self, structure_tstep, aero_tstep, beam, setting
             if np.any(self.data_dict["junction_boundary_condition"] >= 0):
                 self.generate_phantom_panels_at_junction(aero_tstep)
 
-
     def generate_phantom_panels_at_junction(self, aero_tstep):
         for i_surf in range(self.n_surf):
-                aero_tstep.flag_zeta_phantom[0, i_surf] = self.data_dict["junction_boundary_condition"][0,i_surf] 
+            aero_tstep.flag_zeta_phantom[0, i_surf] = self.data_dict["junction_boundary_condition"][0, i_surf]
 
     @staticmethod
     def compute_gamma_dot(dt, tstep, previous_tsteps):
@@ -351,40 +365,19 @@ def compute_gamma_dot(dt, tstep, previous_tsteps):
         if len(previous_tsteps) == 0:
             for i_surf in range(tstep.n_surf):
                 tstep.gamma_dot[i_surf].fill(0.0)
-        # elif len(previous_tsteps) == 1:
-            # # first order
-            # # f'(n) = (f(n) - f(n - 1))/dx
-            # for i_surf in range(tstep.n_surf):
-                # tstep.gamma_dot[i_surf] = (tstep.gamma[i_surf] - previous_tsteps[-1].gamma[i_surf])/dt
-        # else:
-            # # second order
-            # for i_surf in range(tstep.n_surf):
-                # if (not np.isfinite(tstep.gamma[i_surf]).any()) or \
-                    # (not np.isfinite(previous_tsteps[-1].gamma[i_surf]).any()) or \
-                        # (not np.isfinite(previous_tsteps[-2].gamma[i_surf]).any()):
-                    # raise ArithmeticError('NaN found in gamma')
-
-                # if part_of_fsi:
-                    # tstep.gamma_dot[i_surf] = (3.0*tstep.gamma[i_surf]
-                                               # - 4.0*previous_tsteps[-1].gamma[i_surf]
-                                               # + previous_tsteps[-2].gamma[i_surf])/(2.0*dt)
-                # else:
-                    # tstep.gamma_dot[i_surf] = (3.0*tstep.gamma[i_surf]
-                                               # - 4.0*previous_tsteps[-2].gamma[i_surf]
-                                               # + previous_tsteps[-3].gamma[i_surf])/(2.0*dt)
+
         if part_of_fsi:
             for i_surf in range(tstep.n_surf):
-                tstep.gamma_dot[i_surf] = (tstep.gamma[i_surf] - previous_tsteps[-1].gamma[i_surf])/dt
+                tstep.gamma_dot[i_surf] = (tstep.gamma[i_surf] - previous_tsteps[-1].gamma[i_surf]) / dt
         else:
             for i_surf in range(tstep.n_surf):
-                tstep.gamma_dot[i_surf] = (tstep.gamma[i_surf] - previous_tsteps[-2].gamma[i_surf])/dt
-
+                tstep.gamma_dot[i_surf] = (tstep.gamma[i_surf] - previous_tsteps[-2].gamma[i_surf]) / dt
 
 
 def generate_strip(node_info, airfoil_db, aligned_grid,
                    initial_strip_z_rot,
                    orientation_in=np.array([1, 0, 0]),
-                   calculate_zeta_dot = False,
+                   calculate_zeta_dot=False,
                    first_twist=True):
     """
     Returns a strip of panels in ``A`` frame of reference, it has to be then rotated to
@@ -401,14 +394,14 @@ def generate_strip(node_info, airfoil_db, aligned_grid,
         strip_coordinates_b_frame[1, :] = np.linspace(0.0, 1.0, node_info['M'] + 1)
     elif node_info['M_distribution'] == '1-cos':
         domain = np.linspace(0, 1.0, node_info['M'] + 1)
-        strip_coordinates_b_frame[1, :] = 0.5*(1.0 - np.cos(domain*np.pi))
+        strip_coordinates_b_frame[1, :] = 0.5 * (1.0 - np.cos(domain * np.pi))
     elif node_info['M_distribution'].lower() == 'user_defined':
-        strip_coordinates_b_frame[1,:] = node_info['user_defined_m_distribution']
+        strip_coordinates_b_frame[1, :] = node_info['user_defined_m_distribution']
     else:
         raise NotImplemented('M_distribution is ' + node_info['M_distribution'] +
                              ' and it is not yet supported')
     strip_coordinates_b_frame[2, :] = airfoil_db[node_info['airfoil']](
-                                            strip_coordinates_b_frame[1, :])
+        strip_coordinates_b_frame[1, :])
 
     # elastic axis correction
     for i_M in range(node_info['M'] + 1):
@@ -426,7 +419,7 @@ def generate_strip(node_info, airfoil_db, aligned_grid,
             if not node_info['M'] - node_info['control_surface']['chord'] == 0:
                 node_info['control_surface']['hinge_coords'] = None
             else:
-                b_frame_hinge_coords =  node_info['control_surface']['hinge_coords']
+                b_frame_hinge_coords = node_info['control_surface']['hinge_coords']
 
         for i_M in range(node_info['M'] - node_info['control_surface']['chord'], node_info['M'] + 1):
             relative_coords = strip_coordinates_b_frame[:, i_M] - b_frame_hinge_coords
@@ -436,7 +429,7 @@ def generate_strip(node_info, airfoil_db, aligned_grid,
             # deflection velocity
             try:
                 cs_velocity[:, i_M] += np.cross(np.array([-node_info['control_surface']['deflection_dot'], 0.0, 0.0]),
-                                            relative_coords)
+                                                relative_coords)
             except KeyError:
                 pass
 
@@ -472,10 +465,12 @@ def generate_strip(node_info, airfoil_db, aligned_grid,
     for i_M in range(node_info['M'] + 1):
         if first_twist:
             strip_coordinates_b_frame[:, i_M] = np.dot(c_sweep, np.dot(Crot,
-                                                   np.dot(Ctwist, strip_coordinates_b_frame[:, i_M])))
+                                                                       np.dot(Ctwist,
+                                                                              strip_coordinates_b_frame[:, i_M])))
         else:
             strip_coordinates_b_frame[:, i_M] = np.dot(Ctwist, np.dot(Crot,
-                                                   np.dot(c_sweep, strip_coordinates_b_frame[:, i_M])))
+                                                                      np.dot(c_sweep,
+                                                                             strip_coordinates_b_frame[:, i_M])))
         strip_coordinates_a_frame[:, i_M] = np.dot(Cab, strip_coordinates_b_frame[:, i_M])
 
         cs_velocity[:, i_M] = np.dot(Cab, cs_velocity[:, i_M])
@@ -509,10 +504,10 @@ def generate_strip(node_info, airfoil_db, aligned_grid,
         strip_coordinates_a_frame[:, i_M] += node_info['beam_coord']
 
     # add quarter-chord disp
-    delta_c = (strip_coordinates_a_frame[:, -1] - strip_coordinates_a_frame[:, 0])/node_info['M']
+    delta_c = (strip_coordinates_a_frame[:, -1] - strip_coordinates_a_frame[:, 0]) / node_info['M']
     if node_info['M_distribution'] == 'uniform':
         for i_M in range(node_info['M'] + 1):
-                strip_coordinates_a_frame[:, i_M] += 0.25*delta_c
+            strip_coordinates_a_frame[:, i_M] += 0.25 * delta_c
     else:
         warnings.warn("No quarter chord disp of grid for non-uniform grid distributions implemented", UserWarning)
 
diff --git a/sharpy/aero/utils/airfoilpolars.py b/sharpy/aero/utils/airfoilpolars.py
index 514faadd1..35559a2f8 100644
--- a/sharpy/aero/utils/airfoilpolars.py
+++ b/sharpy/aero/utils/airfoilpolars.py
@@ -11,6 +11,9 @@ class Polar:
 
     def __init__(self):
 
+        self.cm_interp = None
+        self.cd_interp = None
+        self.cl_interp = None
         self.table = None
         self.aoa_cl0_deg = None
 
@@ -34,15 +37,11 @@ def initialise(self, table):
         for ipoint in range(npoints - 1):
             if self.table[ipoint, 1] == 0.:
                 matches.append(self.table[ipoint, 0])
-            elif (self.table[ipoint, 1] < 0. and self.table[ipoint + 1, 1] > 0):
-            # elif ((self.table[ipoint, 1] < 0. and self.table[ipoint + 1, 1] > 0) or
-            #       (self.table[ipoint, 1] > 0. and self.table[ipoint + 1, 1] < 0)):
-                if (self.table[ipoint, 0] <= 0.):
+            elif self.table[ipoint, 1] < 0. and self.table[ipoint + 1, 1] > 0:
+                if self.table[ipoint, 0] <= 0.:
                     matches.append(np.interp(0,
                                              self.table[ipoint:ipoint+2, 1],
                                              self.table[ipoint:ipoint+2, 0]))
-                # else:
-                #     print("WARNING: Be careful negative camber airfoil not supported")
 
         iaoacl0 = 0
         aux = np.abs(matches[0])
@@ -62,7 +61,7 @@ def get_coefs(self, aoa_deg):
         cd = self.cd_interp(aoa_deg)
         cm = self.cm_interp(aoa_deg)
 
-        return cl, cd, cm
+        return cl[0], cd[0], cm[0]
 
     def get_aoa_deg_from_cl_2pi(self, cl):
 
@@ -116,7 +115,7 @@ def get_cdcm_from_cl(self, cl):
                 cd = np.interp(cl, self.table[i:i+2, 1], self.table[i:i+2, 2])
                 cm = np.interp(cl, self.table[i:i+2, 1], self.table[i:i+2, 3])
         
-        return cd, cm
+        return float(cd), float(cm)
 
     
 def interpolate(polar1, polar2, coef=0.5):
diff --git a/sharpy/controllers/multibodycontroller.py b/sharpy/controllers/multibodycontroller.py
new file mode 100644
index 000000000..e7b13a817
--- /dev/null
+++ b/sharpy/controllers/multibodycontroller.py
@@ -0,0 +1,178 @@
+import numpy as np
+import os
+
+import sharpy.utils.controller_interface as controller_interface
+import sharpy.utils.settings as settings
+
+
+@controller_interface.controller
+class MultibodyController(controller_interface.BaseController):
+    r""" """
+
+    controller_id = "MultibodyController"
+
+    settings_types = dict()
+    settings_default = dict()
+    settings_description = dict()
+    settings_options = dict()
+
+    settings_types["ang_history_input_file"] = "str"
+    settings_default["ang_history_input_file"] = None
+    settings_description["ang_history_input_file"] = "Route and file name of the time history of desired CRV rotation"
+
+    settings_types["ang_vel_history_input_file"] = "str"
+    settings_default["ang_vel_history_input_file"] = ""
+    settings_description["ang_vel_history_input_file"] = ("Route and file name of the time history of desired CRV "
+                                                          "velocity")
+
+    settings_types["psi_dot_init"] = "list(float)"
+    settings_default["psi_dot_init"] = [0., 0., 0.]
+    settings_description["psi_dot_init"] = "Initial rotation velocity of hinge"
+
+    settings_types["dt"] = "float"
+    settings_default["dt"] = None
+    settings_description["dt"] = "Time step of the simulation"
+
+    settings_types["write_controller_log"] = "bool"
+    settings_default["write_controller_log"] = True
+    settings_description["write_controller_log"] = (
+        "Write a time history of input, required input, " + "and control"
+    )
+
+    settings_table = settings.SettingsTable()
+    __doc__ += settings_table.generate(
+        settings_types, settings_default, settings_description, settings_options
+    )
+
+    def __init__(self):
+        self.in_dict = None     # this also holds the settings dict, kept to be consistent with other controllers
+        self.data = None
+        self.settings = None
+
+        self.prescribed_ang_time_history = None
+        self.prescribed_ang_vel_time_history = None
+
+        # Time histories are ordered such that the [i]th element of each
+        # is the state of the controller at the time of returning.
+        # That means that for the timestep i,
+        # state_input_history[i] == input_time_history_file[i] + error[i]
+
+        self.real_state_input_history = list()
+        self.control_history = list()
+
+        self.controller_implementation = None
+        self.log = None
+
+    def initialise(self, data, in_dict, controller_id=None, restart=False):
+        self.in_dict = in_dict
+        settings.to_custom_types(
+            self.in_dict, self.settings_types, self.settings_default
+        )
+
+        self.settings = self.in_dict
+        self.controller_id = controller_id
+
+        # whilst PID control is not here implemented, I have left the remains for if it gets implemented in future
+        if self.settings["write_controller_log"]:
+            folder = data.output_folder + "/controllers/"
+            if not os.path.exists(folder):
+                os.makedirs(folder)
+            self.log = open(folder + self.controller_id + ".log.csv", "w+")
+            self.log.write(
+                ("#" + 1 * "{:>2}," + 6 * "{:>12}," + "{:>12}\n").format(
+                    "tstep",
+                    "time",
+                    "Ref. state",
+                    "state",
+                    "Pcontrol",
+                    "Icontrol",
+                    "Dcontrol",
+                    "control",
+                )
+            )
+            self.log.flush()
+
+        # save input time history
+        try:
+            self.prescribed_ang_time_history = np.loadtxt(
+                self.settings["ang_history_input_file"], delimiter=","
+            )
+        except:
+            try:
+                self.prescribed_ang_time_history = np.load(
+                    self.settings["ang_history_input_file"]
+                )
+            except:
+                raise OSError(
+                    "File {} not found in Controller".format(
+                        self.settings["ang_history_input_file"]
+                    )
+                )
+
+        if self.settings["ang_vel_history_input_file"]:
+            try:
+                self.prescribed_ang_vel_time_history = np.loadtxt(
+                    self.settings["ang_vel_history_input_file"], delimiter=","
+                )
+            except:
+                try:
+                    self.prescribed_ang_vel_time_history = np.load(
+                        self.settings["ang_vel_history_input_file"]
+                    )
+                except:
+                    raise OSError(
+                        "File {} not found in Controller".format(
+                            self.settings["ang_vel_history_input_file"]
+                        )
+                    )
+
+    def control(self, data, controlled_state):
+        r"""
+        Main routine of the controller.
+        Input is `data` (the self.data in the solver), and
+        `currrent_state` which is a dictionary with ['structural', 'aero']
+        time steps for the current iteration.
+
+        :param data: problem data containing all the information.
+        :param controlled_state: `dict` with two vars: `structural` and `aero`
+            containing the `timestep_info` that will be returned with the
+            control variables.
+
+        :returns: A `dict` with `structural` and `aero` time steps and control
+            input included.
+        """
+
+        control_command = self.prescribed_ang_time_history[data.ts - 1, :]
+
+        if self.prescribed_ang_vel_time_history is None:
+            if data.ts == 1:
+                psi_dot = self.settings["psi_dot_init"]
+            else:
+                psi_dot = (
+                    self.prescribed_ang_time_history[data.ts - 1, :]
+                    - self.prescribed_ang_time_history[data.ts - 2, :]
+                ) / self.settings["dt"]
+        else:
+            psi_dot = self.prescribed_ang_vel_time_history[data.ts - 1, :]
+
+        if controlled_state["structural"].mb_prescribed_dict is None:
+            controlled_state["structural"].mb_prescribed_dict = dict()
+
+        controlled_state["structural"].mb_prescribed_dict[self.controller_id] = {
+            "psi": control_command,
+            "psi_dot": psi_dot,
+            "delta_psi": control_command - self.prescribed_ang_time_history[0, :]}
+
+        return controlled_state, control_command
+
+    def controller_wrapper(
+        self, required_input, current_input, control_param, i_current
+    ):
+        self.controller_implementation.set_point(required_input[i_current - 1])
+        control_param, detailed_control_param = self.controller_implementation(
+            current_input[-1]
+        )
+        return control_param, detailed_control_param
+
+    def __exit__(self, *args):
+        self.log.close()
diff --git a/sharpy/controllers/takeofftrajectorycontroller.py b/sharpy/controllers/takeofftrajectorycontroller.py
index 80b1112e0..92218ef7c 100644
--- a/sharpy/controllers/takeofftrajectorycontroller.py
+++ b/sharpy/controllers/takeofftrajectorycontroller.py
@@ -143,9 +143,6 @@ def control(self, data, controlled_state):
                     mb_dict[self.settings['controlled_constraint']]
         except KeyError:
             return controlled_state
-        except TypeError:
-            import pdb
-            pdb.set_trace()
 
         if self.controlled_body is None or self.controlled_node is None:
             self.controlled_body = constraint['body_number']
diff --git a/sharpy/generators/polaraeroforces.py b/sharpy/generators/polaraeroforces.py
index b94c368d7..9670690e5 100644
--- a/sharpy/generators/polaraeroforces.py
+++ b/sharpy/generators/polaraeroforces.py
@@ -102,6 +102,9 @@ class PolarCorrection(generator_interface.BaseGenerator):
                                        header_line='This generator takes in the following settings.')
 
     def __init__(self):
+        self.folder = None
+        self.cd_from_cl = None
+        self.list_aoa_cl0 = None
         self.settings = None
 
         self.aero = None
@@ -202,11 +205,9 @@ def generate(self, **params):
                     c_bs = local_stability_axes(cgb.T.dot(dir_urel), cgb.T.dot(dir_chord))
                     forces_s = c_bs.T.dot(struct_forces[inode, :3])
                     moment_s = c_bs.T.dot(struct_forces[inode, 3:])
-                    drag_force = forces_s[0]
                     lift_force = forces_s[2]
                     # Compute the associated lift
                     cl = np.sign(lift_force) * np.linalg.norm(lift_force) / coef
-                    cd_sharpy = np.linalg.norm(drag_force) / coef
 
                     if self.cd_from_cl:
                         # Compute the drag from the UVLM computed lift
diff --git a/sharpy/solvers/beamloader.py b/sharpy/solvers/beamloader.py
index a30ee6086..22398324a 100644
--- a/sharpy/solvers/beamloader.py
+++ b/sharpy/solvers/beamloader.py
@@ -136,9 +136,4 @@ def run(self, **kwargs):
         self.data.structure.generate(self.fem_data_dict, self.settings)
         self.data.structure.dyn_dict = self.dyn_data_dict
 
-        # Change the beam description to the local FoR for multibody
-        # if (self.data.structure.num_bodies > 1):
-        #     self.data.structure.ini_info.whole_structure_to_local_AFoR(self.data.structure)
-        #     self.data.structure.timestep_info[0].whole_structure_to_local_AFoR(self.data.structure)
-
         return self.data
diff --git a/sharpy/solvers/dynamiccoupled.py b/sharpy/solvers/dynamiccoupled.py
index 719a58380..13412b420 100644
--- a/sharpy/solvers/dynamiccoupled.py
+++ b/sharpy/solvers/dynamiccoupled.py
@@ -543,7 +543,7 @@ def time_loop(self, in_queue=None, out_queue=None, finish_event=None, solvers=No
                 state = {'structural': structural_kstep,
                          'aero': aero_kstep}
                 for k, v in self.controllers.items():
-                    state = v.control(self.data, state)
+                    state, control = v.control(self.data, state)
                     # this takes care of the changes in options for the solver
                     structural_kstep, aero_kstep = self.process_controller_output(
                         state)
@@ -751,8 +751,6 @@ def convergence(self, k, tstep, previous_tstep,
         """
         # check for non-convergence
         if not all(np.isfinite(tstep.q)):
-            import pdb
-            pdb.set_trace()
             raise Exception(
                 '***Not converged! There is a NaN value in the forces!')
 
diff --git a/sharpy/solvers/initialaeroelasticloader.py b/sharpy/solvers/initialaeroelasticloader.py
index 5adabf534..7ea4aaba2 100644
--- a/sharpy/solvers/initialaeroelasticloader.py
+++ b/sharpy/solvers/initialaeroelasticloader.py
@@ -45,17 +45,18 @@ def initialise(self, data, custom_settings=None, restart=False):
         else:
             self.settings = custom_settings
         settings_utils.to_custom_types(self.settings,
-                           self.settings_types,
-                           self.settings_default,
-                           no_ctype=True)
+                                       self.settings_types,
+                                       self.settings_default,
+                                       no_ctype=True)
         # Load simulation data
         self.file_info = h5utils.readh5(self.settings['input_file'])
 
     def run(self, **kwargs):
 
         aero_step = settings_utils.set_value_or_default(kwargs, 'aero_step', self.data.aero.timestep_info[-1])
-        structural_step = settings_utils.set_value_or_default(kwargs, 'structural_step', self.data.structure.timestep_info[-1])
-        
+        structural_step = settings_utils.set_value_or_default(kwargs, 'structural_step',
+                                                              self.data.structure.timestep_info[-1])
+
         # Copy structural information
         attributes = ['pos', 'pos_dot', 'pos_ddot',
                       'psi', 'psi_dot', 'psi_ddot',
@@ -64,9 +65,9 @@ def run(self, **kwargs):
 
         if self.settings['include_forces']:
             attributes.extend(['runtime_steady_forces',
-                      'runtime_unsteady_forces',
-                      'steady_applied_forces',
-                      'unsteady_applied_forces'])
+                               'runtime_unsteady_forces',
+                               'steady_applied_forces',
+                               'unsteady_applied_forces'])
 
         for att in attributes:
             new_attr = getattr(structural_step, att)
@@ -86,19 +87,18 @@ def run(self, **kwargs):
                                                        self.data.aero.aero_settings)
             # generate the wake because the solid shape might change
             self.data.aero.wake_shape_generator.generate({'zeta': aero_step.zeta,
-                                            'zeta_star': aero_step.zeta_star,
-                                            'gamma': aero_step.gamma,
-                                            'gamma_star': aero_step.gamma_star,
-                                            'dist_to_orig': aero_step.dist_to_orig})
+                                                          'zeta_star': aero_step.zeta_star,
+                                                          'gamma': aero_step.gamma,
+                                                          'gamma_star': aero_step.gamma_star,
+                                                          'dist_to_orig': aero_step.dist_to_orig})
 
         else:
             attributes = ['zeta', 'zeta_star', 'normals',
                           'gamma', 'gamma_star',
-                          'u_ext', 'u_ext_star',]
-                          # 'dist_to_orig', 'gamma_dot', 'zeta_dot',
+                          'u_ext', 'u_ext_star', ]
 
             if self.settings['include_forces']:
-                attributes.extend(['dynamic_forces', 'forces',])
+                attributes.extend(['dynamic_forces', 'forces', ])
 
             for att in attributes:
                 for isurf in range(aero_step.n_surf):
diff --git a/sharpy/solvers/lindynamicsim.py b/sharpy/solvers/lindynamicsim.py
index 8538e0bab..cd84cf425 100644
--- a/sharpy/solvers/lindynamicsim.py
+++ b/sharpy/solvers/lindynamicsim.py
@@ -283,8 +283,6 @@ def state_to_timestep(data, x, u=None, y=None):
         modal = False
 
     aero_state = x[:-data.linear.linear_system.beam.ss.states]
-    beam_state = x[-data.linear.linear_system.beam.ss.states:] # after aero all the rest is beam, but should not use
-    # in case it is in DT the state variables do not mean the same!
 
     # Beam output
     y_beam = y[-data.linear.linear_system.beam.ss.outputs:]
@@ -340,15 +338,9 @@ def state_to_timestep(data, x, u=None, y=None):
     current_aero_tstep.zeta_dot = zeta_dot
     current_aero_tstep.u_ext = u_ext
 
-    # self.data.aero.timestep_info.append(current_aero_tstep)
-
     aero_forces_vec = np.concatenate([forces[i_surf][:3, :, :].reshape(-1, order='C') for i_surf in range(len(forces))])
     beam_forces = data.linear.linear_system.couplings['Ksa'].dot(aero_forces_vec)
 
-    if u is not None:
-        u_struct = u[-data.linear.linear_system.beam.ss.inputs:]
-    # y_struct = y[:self.data.linear.lsys[sys_id].lsys['LinearBeam'].ss.outputs]
-
     # Reconstruct the state if modal
     if modal:
         phi = data.linear.linear_system.beam.sys.U
@@ -356,9 +348,7 @@ def state_to_timestep(data, x, u=None, y=None):
     else:
         x_s = y_beam
     y_s = beam_forces #+ phi.dot(u_struct)
-    # y_s = self.data.linear.lsys['LinearBeam'].sys.U.T.dot(y_struct)
 
     current_struct_step = data.linear.linear_system.beam.unpack_ss_vector(x_s, y_s, data.linear.tsstruct0)
-    # data.structure.timestep_info.append(current_struct_step)
 
     return current_aero_tstep, current_struct_step
diff --git a/sharpy/solvers/linearassembler.py b/sharpy/solvers/linearassembler.py
index 69f4908f4..64a4e316a 100644
--- a/sharpy/solvers/linearassembler.py
+++ b/sharpy/solvers/linearassembler.py
@@ -118,7 +118,6 @@ def initialise(self, data, custom_settings=None, restart=False):
         if custom_settings:
             self.data.settings[self.solver_id] = custom_settings
             self.settings = self.data.settings[self.solver_id]
-        # else:custom_settings
 
         else:
             self.settings = data.settings[self.solver_id]
@@ -143,12 +142,6 @@ def initialise(self, data, custom_settings=None, restart=False):
         # Create data.linear
         self.data.linear = Linear(tsaero0, tsstruct0)
 
-        # Load available systems
-        import sharpy.linear.assembler
-
-        # Load roms
-        import sharpy.rom
-
         lsys = ss_interface.initialise_system(self.settings['linear_system'])
         lsys.initialise(data)
         self.data.linear.linear_system = lsys
diff --git a/sharpy/solvers/modal.py b/sharpy/solvers/modal.py
index 3fc053a8e..790ce7392 100644
--- a/sharpy/solvers/modal.py
+++ b/sharpy/solvers/modal.py
@@ -268,14 +268,6 @@ def run(self, **kwargs):
                     zero_FullCglobal = False
                     warnings.warn('Projecting a system with damping on undamped modal shapes')
                     break
-        # Check if the damping matrix is skew-symmetric
-        # skewsymmetric_FullCglobal = True
-        # for i in range(num_dof):
-        #     for j in range(i:num_dof):
-        #         if((i==j) and (np.absolute(FullCglobal[i, j]) > np.finfo(float).eps)):
-        #             skewsymmetric_FullCglobal = False
-        #         elif(np.absolute(FullCglobal[i, j] + FullCglobal[j, i]) > np.finfo(float).eps):
-        #             skewsymmetric_FullCglobal = False
 
         NumLambda = min(num_dof, self.settings['NumLambda'])
 
@@ -289,7 +281,6 @@ def run(self, **kwargs):
             freq_natural = np.sqrt(eigenvalues)
             order = np.argsort(freq_natural)[:NumLambda]
             freq_natural = freq_natural[order]
-            #freq_damped = freq_natural
             eigenvalues = eigenvalues[order]
             eigenvectors = eigenvectors[:,order]
             damping = np.zeros((NumLambda,))
@@ -533,16 +524,12 @@ def free_free_modes(self, phi, M):
                 bc_here = self.data.structure.boundary_conditions[node_glob]
 
                 if bc_here == 1:  # clamp (only rigid-body)
-                    dofs_here = 0
-                    jj_tra, jj_rot = [], []
                     continue
 
                 elif bc_here == -1 or bc_here == 0:  # (rigid+flex body)
                     dofs_here = 6
                     jj_tra = 6 * self.data.structure.vdof[node_glob] + np.array([0, 1, 2], dtype=int)
                     jj_rot = 6 * self.data.structure.vdof[node_glob] + np.array([3, 4, 5], dtype=int)
-                # jj_tra=[jj  ,jj+1,jj+2]
-                # jj_rot=[jj+3,jj+4,jj+5]
                 else:
                     raise NameError('Invalid boundary condition (%d) at node %d!' \
                                     % (bc_here, node_glob))
@@ -567,7 +554,6 @@ def free_free_modes(self, phi, M):
 
             # Assemble transformed modes
             phirr = Krr.dot(phi[-10:, :10])
-            phiss = K_vec.dot(phi[:, 10:])
 
             # Get rigid body modes to be positive in translation and rotation
             for i in range(10):
diff --git a/sharpy/solvers/noaero.py b/sharpy/solvers/noaero.py
index 562bbe3a4..4621a2d7e 100644
--- a/sharpy/solvers/noaero.py
+++ b/sharpy/solvers/noaero.py
@@ -42,9 +42,9 @@ def initialise(self, data, custom_settings=None, restart=False):
         else:
             self.settings = custom_settings
         settings_utils.to_custom_types(self.settings,
-                           self.settings_types,
-                           self.settings_default,
-                           no_ctype=True)
+                                       self.settings_types,
+                                       self.settings_default,
+                                       no_ctype=True)
 
         if len(self.data.aero.timestep_info) == 0:  # initialise with zero timestep for static sims
             self.update_step()
@@ -52,18 +52,14 @@ def initialise(self, data, custom_settings=None, restart=False):
     def run(self, **kwargs):
 
         aero_tstep = settings_utils.set_value_or_default(kwargs, 'aero_step', self.data.aero.timestep_info[-1])
-        structure_tstep = settings_utils.set_value_or_default(kwargs, 'structural_step', self.data.structure.timestep_info[-1])
-        convect_wake = settings_utils.set_value_or_default(kwargs, 'convect_wake', True)
-        dt= settings_utils.set_value_or_default(kwargs, 'dt', self.settings['dt'])                                                                                                                             
-        t = settings_utils.set_value_or_default(kwargs, 't', self.data.ts*dt)
-        unsteady_contribution = settings_utils.set_value_or_default(kwargs, 'unsteady_contribution', False)
+        dt = settings_utils.set_value_or_default(kwargs, 'dt', self.settings['dt'])
 
         # generate the wake because the solid shape might change
         self.data.aero.wake_shape_generator.generate({'zeta': aero_tstep.zeta,
-                                            'zeta_star': aero_tstep.zeta_star,
-                                            'gamma': aero_tstep.gamma,
-                                            'gamma_star': aero_tstep.gamma_star,
-                                            'dist_to_orig': aero_tstep.dist_to_orig})
+                                                      'zeta_star': aero_tstep.zeta_star,
+                                                      'gamma': aero_tstep.gamma,
+                                                      'gamma_star': aero_tstep.gamma_star,
+                                                      'dist_to_orig': aero_tstep.dist_to_orig})
 
         return self.data
 
@@ -78,7 +74,7 @@ def update_grid(self, beam):
                                          -1,
                                          beam_ts=-1)
 
-    def update_custom_grid(self, structure_tstep, aero_tstep, nl_body_tstep = None):
+    def update_custom_grid(self, structure_tstep, aero_tstep, nl_body_tstep=None):
         # called by DynamicCoupled
         if self.settings['update_grid']:
             self.data.aero.generate_zeta_timestep_info(structure_tstep,
diff --git a/sharpy/solvers/nonlineardynamic.py b/sharpy/solvers/nonlineardynamic.py
index 953305ffe..321cca8e0 100644
--- a/sharpy/solvers/nonlineardynamic.py
+++ b/sharpy/solvers/nonlineardynamic.py
@@ -66,7 +66,6 @@ def run(self, **kwargs):
             pass
         if prescribed_motion is True:
             cout.cout_wrap('Running non linear dynamic solver...', 2)
-            # raise NotImplementedError
             xbeamlib.cbeam3_solv_nlndyn(self.data.structure, self.settings)
         else:
             cout.cout_wrap('Running non linear dynamic solver with RB...', 2)
diff --git a/sharpy/solvers/nonlineardynamicmultibodyjax.py b/sharpy/solvers/nonlineardynamicmultibodyjax.py
new file mode 100644
index 000000000..92801edca
--- /dev/null
+++ b/sharpy/solvers/nonlineardynamicmultibodyjax.py
@@ -0,0 +1,368 @@
+import numpy as np
+import typing
+
+from sharpy.utils.solver_interface import solver, solver_from_string
+import sharpy.utils.settings as settings_utils
+import sharpy.utils.solver_interface as solver_interface
+import sharpy.structure.utils.xbeamlib as xbeamlib
+import sharpy.utils.multibodyjax as mb
+import sharpy.structure.utils.lagrangeconstraintsjax as lagrangeconstraints
+
+_BaseStructural = solver_from_string('_BaseStructural')
+
+
+@solver
+class NonLinearDynamicMultibodyJAX(_BaseStructural):
+    """
+    Nonlinear dynamic multibody
+
+    Nonlinear dynamic step solver for multibody structures.
+
+    """
+    solver_id = 'NonLinearDynamicMultibodyJAX'
+    solver_classification = 'structural'
+
+    settings_types = _BaseStructural.settings_types.copy()
+    settings_default = _BaseStructural.settings_default.copy()
+    settings_description = _BaseStructural.settings_description.copy()
+    settings_options = dict()
+
+    settings_types['time_integrator'] = 'str'
+    settings_default['time_integrator'] = 'NewmarkBetaJAX'
+    settings_description['time_integrator'] = 'Method to perform time integration'
+    settings_options['time_integrator'] = ['NewmarkBetaJAX', 'GeneralisedAlphaJAX']
+
+    settings_types['time_integrator_settings'] = 'dict'
+    settings_default['time_integrator_settings'] = dict()
+    settings_description['time_integrator_settings'] = 'Settings for the time integrator'
+
+    settings_types['jacobian_method'] = 'str'
+    settings_default['jacobian_method'] = 'forward'
+    settings_description['jacobian_method'] = 'Autodifferentiation method used to calculate system jacobians'
+    settings_options['jacobian_method'] = ['forward', 'reverse']
+
+    settings_types['write_lm'] = 'bool'
+    settings_default['write_lm'] = False
+    settings_description['write_lm'] = 'Write lagrange multipliers to file'
+
+    settings_types['relax_factor_lm'] = 'float'
+    settings_default['relax_factor_lm'] = 0.
+    settings_description['relax_factor_lm'] = ('Relaxation factor for Lagrange Multipliers. '
+                                               '0 no relaxation. 1 full relaxation')
+
+    settings_types['allow_skip_step'] = 'bool'
+    settings_default['allow_skip_step'] = False
+    settings_description['allow_skip_step'] = 'Allow skip step when NaN is found while solving the system'
+
+    settings_types['zero_ini_dot_ddot'] = 'bool'
+    settings_default['zero_ini_dot_ddot'] = False
+    settings_description['zero_ini_dot_ddot'] = 'Set to zero the position and crv derivatives at the first time step'
+
+    settings_types['fix_prescribed_quat_ini'] = 'bool'
+    settings_default['fix_prescribed_quat_ini'] = False
+    settings_description['fix_prescribed_quat_ini'] = 'Set to initial the quaternion for prescibed bodies'
+
+    # initial speed direction is given in inertial FOR!!! also in a lot of cases coincident with global A frame
+    settings_types['initial_velocity_direction'] = 'list(float)'
+    settings_default['initial_velocity_direction'] = [-1., 0., 0.]
+    settings_description[
+        'initial_velocity_direction'] = 'Initial velocity of the reference node given in the inertial FOR'
+
+    settings_types['initial_velocity'] = 'float'
+    settings_default['initial_velocity'] = 0
+    settings_description['initial_velocity'] = 'Initial velocity magnitude of the reference node'
+
+    # restart sim after dynamictrim
+    settings_types['dyn_trim'] = 'bool'
+    settings_default['dyn_trim'] = False
+    settings_description['dyn_trim'] = 'flag for dyntrim prior to dyncoup'
+
+    settings_table = settings_utils.SettingsTable()
+    __doc__ += settings_table.generate(settings_types, settings_default, settings_description)
+
+    def __init__(self):
+        self.data = None
+        self.settings = None
+
+        self.sys_size: typing.Optional[int] = None  # total number of unknowns in the system
+        self.num_lm_tot: typing.Optional[int] = None  # total number of LMs in the system
+        self.num_eq_tot: typing.Optional[int] = None
+
+        # Total number of equations associated to the Lagrange multipliers
+        self.lc_list: typing.Optional[list[lagrangeconstraints.Constraint]] = None
+        self.num_lm_eq: typing.Optional[int] = None
+        self.lambda_h: typing.Optional[np.ndarray] = None
+        self.lambda_n: typing.Optional[np.ndarray] = None
+
+        # function called to generate contributions of all LMs to equations
+        self.lc_all_run: typing.Optional[typing.Callable] = None
+
+        self.gamma: typing.Optional[int] = None
+        self.beta: typing.Optional[int] = None
+        self.prev_dq: typing.Optional[np.ndarray] = None
+
+        self.time_integrator = None
+
+        self.out_files = None  # dict: containing output_variable:file_path if desired to write output
+
+    def initialise(self, data, custom_settings=None, restart=False):
+        self.data = data
+        if custom_settings is None:
+            self.settings = data.settings[self.solver_id]
+        else:
+            self.settings = custom_settings
+        settings_utils.to_custom_types(self.settings, self.settings_types, self.settings_default,
+                                       self.settings_options, no_ctype=True)
+
+        # load info from dyn dictionary
+        self.data.structure.add_unsteady_information(self.data.structure.dyn_dict, self.settings['num_steps'])
+
+        if self.settings['initial_velocity']:
+            new_direction = (self.data.structure.timestep_info[-1].cag()
+                             @ self.settings['initial_velocity_direction'] * self.settings['initial_velocity'])
+            self.data.structure.timestep_info[-1].for_vel[:3] = new_direction
+            num_body = self.data.structure.timestep_info[0].mb_FoR_vel.shape[0]
+            for ibody in range(num_body):
+                self.data.structure.timestep_info[-1].mb_FoR_vel[ibody, :] \
+                    = self.data.structure.timestep_info[-1].for_vel
+
+        # find total number of LC equations
+        dict_of_lc = lagrangeconstraints.DICT_OF_LC
+        lc_cls_list: list[typing.Type[lagrangeconstraints.Constraint]] = []
+        lc_settings: list[dict] = []
+        self.num_lm_tot = 0
+        for i in range(self.data.structure.ini_mb_dict['num_constraints']):
+            lc_settings.append(self.data.structure.ini_mb_dict[f'constraint_{i:02d}'])
+            lc_cls_list.append(dict_of_lc[lc_settings[-1]['behaviour']])
+            self.num_lm_tot += lc_cls_list[-1].get_n_lm()
+
+        # find total number of equations
+        mb_dict = self.data.structure.ini_mb_dict
+        self.sys_size = self.data.structure.num_dof.value
+        for ibody in range(self.data.structure.num_bodies):
+            if mb_dict[f'body_{ibody:02d}']['FoR_movement'] == 'free':
+                self.sys_size += 10
+        self.num_eq_tot = self.sys_size + self.num_lm_tot
+
+        i_start_lc = 0
+        self.lc_list = []
+        for i_lc, lc_cls in enumerate(lc_cls_list):
+            self.lc_list.append(lc_cls(self, i_start_lc, lc_settings[i_lc]))
+            i_start_lc += self.lc_list[-1].num_lm
+
+        # create a single function for all constraints
+        self.lc_all_run = lagrangeconstraints.combine_constraints(self.lc_list)
+
+        # lambda values
+        self.lambda_h = np.zeros(self.num_lm_tot)
+        self.lambda_n = np.zeros(self.num_lm_tot)
+
+        self.prev_dq = np.zeros(self.sys_size + self.num_lm_tot)
+
+        try:
+            self.num_lm_eq = self.lc_list[0].num_lm_tot
+        except IndexError:
+            self.num_lm_eq = 0
+
+        self.settings['time_integrator_settings']['sys_size'] = self.sys_size
+        self.settings['time_integrator_settings']['num_LM_eq'] = self.num_lm_eq
+
+        # Initialise time integrator
+        if not restart:
+            self.time_integrator = solver_interface.initialise_solver(self.settings['time_integrator'])
+
+        self.time_integrator.initialise(self.data, self.settings['time_integrator_settings'], restart=restart)
+
+
+    def assembly_mb_eq_system(self, mb_beam, mb_tstep, ts, dt, lambda_h, lambda_n, mb_dict, mb_prescribed_dict):
+        """
+        This function generates the matrix and vector associated to the linear system to solve a structural iteration
+        It usses a Newmark-beta scheme for time integration. Being M, C and K the mass, damping
+        and stiffness matrices of the system:
+
+        .. math::
+            MB_Asys = mb_k + mb_c \frac{\gamma}{\beta dt} + \frac{1}{\beta dt^2} mb_m
+
+        Args:
+            mb_beam (list(:class:`~sharpy.structure.models.beam.Beam`)): each entry represents a body
+            mb_tstep (list(:class:`~sharpy.utils.datastructures.StructTimeStepInfo`)): each entry represents a body
+            ts (int): Time step number
+            dt(int): time step
+            lambda_ (np.ndarray): Lagrange Multipliers array
+            lambda_dot (np.ndarray): Time derivarive of ``Lambda``
+            mb_dict (dict): Dictionary including the multibody information
+
+        Returns:
+            MB_Asys (np.ndarray): Matrix of the systems of equations
+            mb_q (np.ndarray): Vector of the systems of equations
+        """
+
+        mb_m = np.zeros((self.num_eq_tot, self.num_eq_tot))
+        mb_c = np.zeros((self.num_eq_tot, self.num_eq_tot))
+        mb_k = np.zeros((self.num_eq_tot, self.num_eq_tot))
+        mb_rhs = np.zeros(self.num_eq_tot)
+
+        first_dof = 0
+        for ibody in range(len(mb_beam)):
+            # Generate the matrices for each body
+            if mb_beam[ibody].FoR_movement in ('prescribed', 'prescribed_trim'):
+                last_dof = first_dof + mb_beam[ibody].num_dof.value
+                m, c, k, rhs = xbeamlib.cbeam3_asbly_dynamic(mb_beam[ibody], mb_tstep[ibody], self.settings)
+            elif mb_beam[ibody].FoR_movement == 'free':
+                last_dof = first_dof + mb_beam[ibody].num_dof.value + 10
+                m, c, k, rhs = xbeamlib.xbeam3_asbly_dynamic(mb_beam[ibody], mb_tstep[ibody], self.settings)
+            else:
+                raise KeyError(f"Body FoR movement {mb_beam[ibody].FoR_movement} is invalid")
+
+            mb_m[first_dof:last_dof, first_dof:last_dof] = m
+            mb_c[first_dof:last_dof, first_dof:last_dof] = c
+            mb_k[first_dof:last_dof, first_dof:last_dof] = k
+            mb_rhs[first_dof:last_dof] = rhs
+
+            first_dof = last_dof
+
+        q = np.hstack([beam.q for beam in mb_tstep])
+        q_dot = np.hstack([beam.dqdt for beam in mb_tstep])
+
+        u = []
+        u_dot = []
+        for i_lc in range(len(self.lc_list)):
+            try:
+                ctrl_id = self.lc_list[i_lc].settings['controller_id'].decode('UTF-8')
+                u.append(mb_prescribed_dict[ctrl_id]['psi'])
+                u_dot.append(mb_prescribed_dict[ctrl_id]['psi_dot'])
+            except KeyError:
+                u.append(None)
+                u_dot.append(None)
+
+        lc_c, lc_k, lc_rhs = self.call_lm_generate(q, q_dot, u, u_dot, lambda_h, lambda_n)
+
+        mb_c += lc_c
+        mb_k += lc_k
+        mb_rhs += lc_rhs
+
+        return mb_m, mb_c, mb_k, mb_rhs
+
+    # added to make profiling easier
+    def call_lm_generate(self, *args):
+        return self.lc_all_run(*args)
+
+    def integrate_position(self, mb_beam, mb_tstep, dt):
+        """
+        This function integrates the position of each local A FoR after the
+        structural iteration has been solved.
+
+        It uses a Newmark-beta approximation.
+
+        Args:
+            mb_beam (list(:class:`~sharpy.structure.models.beam.Beam`)): each entry represents a body
+            mb_tstep (list(:class:`~sharpy.utils.datastructures.StructTimeStepInfo`)): each entry represents a body
+            dt(int): time step
+        """
+        vel = np.zeros(6)
+        acc = np.zeros(6)
+        for ibody in range(len(mb_tstep)):
+            acc[:3] = ((0.5 - self.beta) * mb_beam[ibody].timestep_info.cga()
+                        @ mb_beam[ibody].timestep_info.for_acc[:3] + self.beta * mb_tstep[ibody].cga()
+                        @ mb_tstep[ibody].for_acc[:3])
+            vel[:3] = mb_beam[ibody].timestep_info.cga() @ mb_beam[ibody].timestep_info.for_vel[:3]
+            mb_tstep[ibody].for_pos[:3] += dt * (vel[:3] + dt * acc[:3])
+
+    def extract_resultants(self, tstep):
+        if tstep is None:
+            tstep = self.data.structure.timestep_info[self.data.ts]
+        steady, unsteady, grav = tstep.extract_resultants(self.data.structure,
+                                                          force_type=['steady', 'unsteady', 'grav'])
+        totals = steady + unsteady + grav
+        return totals[:3], totals[3:6]
+
+    def run(self, **kwargs):
+        structural_step = settings_utils.set_value_or_default(kwargs, 'structural_step',
+                                                              self.data.structure.timestep_info[-1])
+        dt = settings_utils.set_value_or_default(kwargs, 'dt', self.settings['dt'])
+
+        if structural_step.mb_dict is not None:
+            mb_dict = structural_step.mb_dict
+        else:
+            mb_dict = self.data.structure.ini_mb_dict
+
+        mb_prescribed_dict = structural_step.mb_prescribed_dict
+        mb_beam, mb_tstep = mb.split_multibody(self.data.structure, structural_step, mb_dict, self.data.ts)
+
+        if self.data.ts == 1 and self.settings['zero_ini_dot_ddot']:
+            for ibody in range(len(mb_tstep)):
+                mb_beam[ibody].ini_info.pos_dot.fill(0.)
+                mb_beam[ibody].ini_info.pos_ddot.fill(0.)
+                mb_beam[ibody].ini_info.psi_dot.fill(0.)
+                mb_beam[ibody].ini_info.psi_dot_local.fill(0.)
+                mb_beam[ibody].ini_info.psi_ddot.fill(0.)
+                mb_tstep[ibody].pos_dot.fill(0.)
+                mb_tstep[ibody].pos_ddot.fill(0.)
+                mb_tstep[ibody].psi_dot.fill(0.)
+                mb_tstep[ibody].psi_dot_local.fill(0.)
+                mb_tstep[ibody].psi_ddot.fill(0.)
+
+        # Predictor step
+        q, dqdt, dqddt = mb.disp_and_accel2state(mb_beam, mb_tstep, self.lambda_h, self.lambda_n,
+                                                 self.sys_size, self.num_lm_eq)
+        self.time_integrator.predictor(q, dqdt, dqddt)
+
+        res_sys = 0.
+        res_lm = 0.
+
+        for iteration in range(self.settings['max_iterations']):
+            if iteration == self.settings['max_iterations'] - 1:   # Check if the maximum of iterations has been reached
+                print(f'Solver did not converge in {iteration} iterations.\n res_sys = {res_sys} \n res_lm = {res_lm}')
+                break
+
+            # Update positions and velocities
+            lambda_h, lambda_n = mb.state2disp_and_accel(q, dqdt, dqddt, mb_beam, mb_tstep, self.num_lm_eq)
+
+            mb_m, mb_c, mb_k, mb_rhs = self.assembly_mb_eq_system(mb_beam, mb_tstep, self.data.ts, dt, lambda_h,
+                                                                  lambda_n, mb_dict, mb_prescribed_dict)
+
+            a_sys = self.time_integrator.build_matrix(mb_m, mb_c, mb_k)
+
+            dq = np.linalg.solve(a_sys, -mb_rhs)
+
+            # Relaxation
+            relax_dq = np.zeros_like(dq)
+            relax_dq[:self.sys_size] = dq[:self.sys_size].copy()
+            relax_dq[self.sys_size:] = ((1. - self.settings['relax_factor_lm']) * dq[self.sys_size:] +
+                                        self.settings['relax_factor_lm'] * self.prev_dq[self.sys_size:])
+            self.prev_dq = dq.copy()
+
+            # Corrector step
+            self.time_integrator.corrector(q, dqdt, dqddt, relax_dq)
+
+            res_sys = np.max(np.abs(dq[:self.sys_size]))
+            try:
+                res_lm = np.max(np.abs(dq[self.sys_size:]))
+            except ValueError:
+                res_lm = 0.
+            if iteration > 0 and (res_sys < self.settings['min_delta']) and (res_lm < self.settings['min_delta']):
+                break
+
+        lambda_h, lambda_n = mb.state2disp_and_accel(q, dqdt, dqddt, mb_beam, mb_tstep, self.num_lm_eq)
+
+        for lc in self.lc_list:
+            lc.postprocess(mb_beam, mb_tstep)
+
+        # End of Newmark-beta iterations
+        if self.settings['gravity_on']:
+            for ibody in range(len(mb_beam)):
+                xbeamlib.cbeam3_correct_gravity_forces(mb_beam[ibody], mb_tstep[ibody], self.settings)
+        mb.merge_multibody(mb_tstep, mb_beam, self.data.structure, structural_step, mb_dict, dt)
+
+        self.lambda_h = lambda_h
+        self.lambda_n = lambda_n
+        self.data.Lambda = lambda_h
+        self.data.Lambda_dot = lambda_n
+
+        return self.data
+
+    def add_step(self):
+        self.data.structure.next_step()
+
+    def next_step(self):
+        raise NotImplementedError
diff --git a/sharpy/solvers/nonlineardynamicprescribedstep.py b/sharpy/solvers/nonlineardynamicprescribedstep.py
index 535a5d225..216d26c30 100644
--- a/sharpy/solvers/nonlineardynamicprescribedstep.py
+++ b/sharpy/solvers/nonlineardynamicprescribedstep.py
@@ -8,6 +8,7 @@
 
 _BaseStructural = solver_from_string('_BaseStructural')
 
+
 @solver
 class NonLinearDynamicPrescribedStep(_BaseStructural):
     """
@@ -46,10 +47,10 @@ def initialise(self, data, custom_settings=None, restart=False):
         # load info from dyn dictionary
         self.data.structure.add_unsteady_information(self.data.structure.dyn_dict, self.settings['num_steps'])
 
-
     def run(self, **kwargs):
-        structural_step = settings_utils.set_value_or_default(kwargs, 'structural_step', self.data.structure.timestep_info[-1])
-        dt = settings_utils.set_value_or_default(kwargs, 'dt', self.settings['dt'])  
+        structural_step = settings_utils.set_value_or_default(kwargs, 'structural_step',
+                                                              self.data.structure.timestep_info[-1])
+        dt = settings_utils.set_value_or_default(kwargs, 'dt', self.settings['dt'])
 
         if self.data.ts > 0:
             try:
@@ -64,7 +65,6 @@ def run(self, **kwargs):
                                     structural_step,
                                     dt=dt)
 
-        # self.extract_resultants(structural_step)
         self.data.structure.integrate_position(structural_step, dt)
         return self.data
 
@@ -73,22 +73,15 @@ def add_step(self):
 
     def next_step(self):
         pass
-        # self.data.structure.next_step()
-        # ts = len(self.data.structure.timestep_info) - 1
-        # if ts > 0:
-        #     self.data.structure.timestep_info[ts].for_vel[:] = self.data.structure.dynamic_input[ts - 1]['for_vel']
-        #     self.data.structure.timestep_info[ts].for_acc[:] = self.data.structure.dynamic_input[ts - 1]['for_acc']
-        #     self.data.structure.timestep_info[ts].unsteady_applied_forces[:] = self.data.structure.dynamic_input[ts - 1]['dynamic_forces']
-
 
     def extract_resultants(self, tstep=None):
         if tstep is None:
             tstep = self.data.structure.timestep_info[self.data.ts]
-        steady, unsteady, grav = tstep.extract_resultants(self.data.structure, force_type=['steady', 'unsteady', 'grav'])
+        steady, unsteady, grav = tstep.extract_resultants(self.data.structure,
+                                                          force_type=['steady', 'unsteady', 'grav'])
         totals = steady + unsteady + grav
         return totals[0:3], totals[3:6]
 
-
     def update(self, tstep=None):
         self.create_q_vector(tstep)
 
diff --git a/sharpy/solvers/prescribeduvlm.py b/sharpy/solvers/prescribeduvlm.py
index cbd7acdb8..49692b5c4 100644
--- a/sharpy/solvers/prescribeduvlm.py
+++ b/sharpy/solvers/prescribeduvlm.py
@@ -136,19 +136,6 @@ def run(self, **kwargs):
                 self.data.structure.timestep_info[ts].for_vel[:] = self.data.structure.dynamic_input[ts - 1]['for_vel']
                 self.data.structure.timestep_info[ts].for_acc[:] = self.data.structure.dynamic_input[ts - 1]['for_acc']
 
-
-            # # # generate new grid (already rotated)
-            # self.aero_solver.update_custom_grid(structural_kstep, aero_kstep)
-            #
-            # # run the solver
-            # self.data = self.aero_solver.run(aero_kstep,
-            #                                  structural_kstep,
-            #                                  self.data.aero.timestep_info[-1],
-            #                                  convect_wake=True)
-            #
-            # self.residual_table.print_line([self.data.ts,
-            #                                 self.data.ts*self.dt])
-
             self.data.structure.next_step()
             self.data.structure.integrate_position(self.data.ts, self.settings['dt'])
 
@@ -170,68 +157,3 @@ def run(self, **kwargs):
                     self.data = self.postprocessors[postproc].run(online=True)
 
         return self.data
-
-#
-#     def map_forces(self, aero_kstep, structural_kstep, unsteady_forces_coeff=1.0):
-#         # set all forces to 0
-#         structural_kstep.steady_applied_forces.fill(0.0)
-#         structural_kstep.unsteady_applied_forces.fill(0.0)
-#
-#         # aero forces to structural forces
-#         struct_forces = mapping.aero2struct_force_mapping(
-#             aero_kstep.forces,
-#             self.data.aero.struct2aero_mapping,
-#             aero_kstep.zeta,
-#             structural_kstep.pos,
-#             structural_kstep.psi,
-#             self.data.structure.node_master_elem,
-#             self.data.structure.master,
-#             structural_kstep.cag())
-#         dynamic_struct_forces = unsteady_forces_coeff*mapping.aero2struct_force_mapping(
-#             aero_kstep.dynamic_forces,
-#             self.data.aero.struct2aero_mapping,
-#             aero_kstep.zeta,
-#             structural_kstep.pos,
-#             structural_kstep.psi,
-#             self.data.structure.node_master_elem,
-#             self.data.structure.master,
-#             structural_kstep.cag())
-#
-#         # prescribed forces + aero forces
-#         structural_kstep.steady_applied_forces = (
-#             (struct_forces + self.data.structure.ini_info.steady_applied_forces).
-#                 astype(dtype=ct.c_double, order='F', copy=True))
-#         structural_kstep.unsteady_applied_forces = (
-#             (dynamic_struct_forces + self.data.structure.dynamic_input[max(self.data.ts - 1, 0)]['dynamic_forces']).
-#                 astype(dtype=ct.c_double, order='F', copy=True))
-#
-#     def relaxation_factor(self, k):
-#         initial = self.settings['relaxation_factor']
-#         if not self.settings['dynamic_relaxation']:
-#             return initial
-#
-#         final = self.settings['final_relaxation_factor']
-#         if k >= self.settings['relaxation_steps']:
-#             return final
-#
-#         value = initial + (final - initial)/self.settings['relaxation_steps']*k
-#         return value
-#
-#
-# def relax(beam, timestep, previous_timestep, coeff):
-#     if coeff > 0.0:
-#         timestep.steady_applied_forces[:] = ((1.0 - coeff)*timestep.steady_applied_forces
-#                                              + coeff*previous_timestep.steady_applied_forces)
-#         timestep.unsteady_applied_forces[:] = ((1.0 - coeff)*timestep.unsteady_applied_forces
-#                                                + coeff*previous_timestep.unsteady_applied_forces)
-#         # timestep.pos_dot[:] = (1.0 - coeff)*timestep.pos_dot + coeff*previous_timestep.pos_dot
-#         # timestep.psi[:] = (1.0 - coeff)*timestep.psi + coeff*previous_timestep.psi
-#         # timestep.psi_dot[:] = (1.0 - coeff)*timestep.psi_dot + coeff*previous_timestep.psi_dot
-#
-#         # normalise_quaternion(timestep)
-#         # xbeam_solv_state2disp(beam, timestep)
-#
-#
-#
-#
-#
diff --git a/sharpy/solvers/rigiddynamicprescribedstep.py b/sharpy/solvers/rigiddynamicprescribedstep.py
index ddff02f6d..91fb2cc62 100644
--- a/sharpy/solvers/rigiddynamicprescribedstep.py
+++ b/sharpy/solvers/rigiddynamicprescribedstep.py
@@ -63,11 +63,6 @@ def run(self, **kwargs):
 
         xbeamlib.cbeam3_solv_disp2state(self.data.structure, structural_step)
 
-        # xbeamlib.cbeam3_step_nlndyn(self.data.structure,
-        #                             self.settings,
-        #                             self.data.ts,
-        #                             structural_step,
-        #                             dt=dt)
         self.extract_resultants(structural_step)
         if self.data.ts > 0:
             self.data.structure.integrate_position(structural_step, self.settings['dt'])
@@ -78,12 +73,6 @@ def add_step(self):
 
     def next_step(self):
         pass
-        # self.data.structure.next_step()
-        # ts = len(self.data.structure.timestep_info) - 1
-        # if ts > 0:
-        #     self.data.structure.timestep_info[ts].for_vel[:] = self.data.structure.dynamic_input[ts - 1]['for_vel']
-        #     self.data.structure.timestep_info[ts].for_acc[:] = self.data.structure.dynamic_input[ts - 1]['for_acc']
-        #     self.data.structure.timestep_info[ts].unsteady_applied_forces[:] = self.data.structure.dynamic_input[ts - 1]['dynamic_forces']
 
     def extract_resultants(self, tstep=None):
         if tstep is None:
diff --git a/sharpy/solvers/staticcoupled.py b/sharpy/solvers/staticcoupled.py
index e3fde1bae..a3e61f630 100644
--- a/sharpy/solvers/staticcoupled.py
+++ b/sharpy/solvers/staticcoupled.py
@@ -345,7 +345,6 @@ def change_trim(self, alpha, thrust, thrust_nodes, tail_deflection, tail_cs_inde
             for i_node, node in enumerate(thrust_nodes):
                 self.force_orientation[i_node, :] = (
                     algebra.unit_vector(self.data.structure.ini_info.steady_applied_forces[node, 0:3]))
-            # print(self.force_orientation)
 
         # thrust
         # thrust is scaled so that the direction of the forces is conserved
@@ -354,10 +353,7 @@ def change_trim(self, alpha, thrust, thrust_nodes, tail_deflection, tail_cs_inde
         # if there are two or more nodes in thrust_nodes, the total forces
         # is n_nodes_in_thrust_nodes*thrust
         # thrust forces have to be indicated in structure.ini_info
-        # print(algebra.unit_vector(self.data.structure.ini_info.steady_applied_forces[0, 0:3])*thrust)
         for i_node, node in enumerate(thrust_nodes):
-            # self.data.structure.ini_info.steady_applied_forces[i_node, 0:3] = (
-            #     algebra.unit_vector(self.data.structure.ini_info.steady_applied_forces[i_node, 0:3])*thrust)
             self.data.structure.ini_info.steady_applied_forces[node, 0:3] = (
                     self.force_orientation[i_node, :]*thrust)
             self.data.structure.timestep_info[0].steady_applied_forces[node, 0:3] = (
diff --git a/sharpy/solvers/statictrim.py b/sharpy/solvers/statictrim.py
index a76947edf..cab135c82 100644
--- a/sharpy/solvers/statictrim.py
+++ b/sharpy/solvers/statictrim.py
@@ -317,17 +317,12 @@ def trim_algorithm(self):
 
     def evaluate(self, alpha, deflection_gamma, thrust):
         if not np.isfinite(alpha):
-            import pdb; pdb.set_trace()
+            raise ValueError("Alpha trim gradient is zero, resulting in division by zero.")
         if not np.isfinite(deflection_gamma):
-            import pdb; pdb.set_trace()
+            raise ValueError("Delta trim gradient is zero, resulting in division by zero.")
         if not np.isfinite(thrust):
-            import pdb; pdb.set_trace()
+            raise ValueError("Thrust trim gradient is zero, resulting in division by zero.")
 
-        # cout.cout_wrap('--', 2)
-        # cout.cout_wrap('Trying trim: ', 2)
-        # cout.cout_wrap('Alpha: ' + str(alpha*180/np.pi), 2)
-        # cout.cout_wrap('CS deflection: ' + str((deflection_gamma - alpha)*180/np.pi), 2)
-        # cout.cout_wrap('Thrust: ' + str(thrust), 2)
         # modify the trim in the static_coupled solver
         self.solver.change_trim(alpha,
                                 thrust,
@@ -342,10 +337,6 @@ def evaluate(self, alpha, deflection_gamma, thrust):
         forcez = forces[2]
         forcex = forces[0]
         moment = moments[1]
-        # cout.cout_wrap('Forces and moments:', 2)
-        # cout.cout_wrap('fx = ' + str(forces[0]) + ' mx = ' + str(moments[0]), 2)
-        # cout.cout_wrap('fy = ' + str(forces[1]) + ' my = ' + str(moments[1]), 2)
-        # cout.cout_wrap('fz = ' + str(forces[2]) + ' mz = ' + str(moments[2]), 2)
 
         self.table.print_line([self.i_iter,
                                alpha*180/np.pi,
diff --git a/sharpy/solvers/steplinearuvlm.py b/sharpy/solvers/steplinearuvlm.py
index 047773b4f..f271dc0b4 100644
--- a/sharpy/solvers/steplinearuvlm.py
+++ b/sharpy/solvers/steplinearuvlm.py
@@ -107,12 +107,13 @@ class StepLinearUVLM(BaseSolver):
 
     settings_types['vortex_radius_wake_ind'] = 'float'
     settings_default['vortex_radius_wake_ind'] = vortex_radius_def
-    settings_description['vortex_radius_wake_ind'] = 'Distance between points below which induction is not computed in the wake convection'
+    settings_description[
+        'vortex_radius_wake_ind'] = 'Distance between points below which induction is not computed in the wake convection'
 
     settings_types['cfl1'] = 'bool'
     settings_default['cfl1'] = True
     settings_description['cfl1'] = 'If it is ``True``, it assumes that the discretisation complies with CFL=1'
-    
+
     settings_table = settings_utils.SettingsTable()
     __doc__ += settings_table.generate(settings_types, settings_default, settings_description)
 
@@ -173,7 +174,7 @@ def initialise(self, data, custom_settings=None, restart=False):
             self.settings = custom_settings
         settings_utils.to_custom_types(self.settings, self.settings_types, self.settings_default, no_ctype=True)
         settings_utils.to_custom_types(self.settings['ScalingDict'], self.scaling_settings_types,
-                                 self.scaling_settings_default, no_ctype=True)
+                                       self.scaling_settings_default, no_ctype=True)
 
         # Initialise velocity generator
         velocity_generator_type = gen_interface.generator_from_string(self.settings['velocity_field_generator'])
@@ -198,11 +199,11 @@ def initialise(self, data, custom_settings=None, restart=False):
                 if self.num_body_track == -1:
                     self.quat0 = self.data.structure.timestep_info[-1].quat.copy()
                     self.for_vel0 = self.data.structure.timestep_info[-1].for_vel.copy()
-                else: # track a specific body
+                else:  # track a specific body
                     self.quat0 = \
-                        self.data.structure.timestep_info[-1].mb_quat[self.num_body_track,:].copy()
+                        self.data.structure.timestep_info[-1].mb_quat[self.num_body_track, :].copy()
                     self.for_vel0 = \
-                        self.data.structure.timestep_info[-1].mb_FoR_vel[self.num_body_track ,:].copy()
+                        self.data.structure.timestep_info[-1].mb_FoR_vel[self.num_body_track, :].copy()
 
                 # convert to G frame
                 self.Cga0 = algebra.quat2rotation(self.quat0)
@@ -210,7 +211,7 @@ def initialise(self, data, custom_settings=None, restart=False):
                 self.for_vel0[:3] = self.Cga0.dot(self.for_vel0[:3])
                 self.for_vel0[3:] = self.Cga0.dot(self.for_vel0[3:])
 
-            else: # check/record initial rotation speed
+            else:  # check/record initial rotation speed
                 self.num_body_track = None
                 self.quat0 = None
                 self.Cag0 = None
@@ -223,12 +224,7 @@ def initialise(self, data, custom_settings=None, restart=False):
             # Generate instance of linuvlm.Dynamic()
             lin_uvlm_system = linuvlm.DynamicBlock(aero_tstep,
                                                    dynamic_settings=self.settings,
-                                              # dt=self.settings['dt'],
-                                              # integr_order=self.settings['integr_order'],
-                                              # ScalingDict=self.settings['ScalingDict'],
-                                              # RemovePredictor=self.settings['remove_predictor'],
-                                              # UseSparse=self.settings['use_sparse'],
-                                              for_vel=self.for_vel0)
+                                                   for_vel=self.for_vel0)
 
             # add rotational speed
             for ii in range(lin_uvlm_system.MS.n_surf):
@@ -249,8 +245,8 @@ def initialise(self, data, custom_settings=None, restart=False):
             # Assemble the state space system
             wake_prop_settings = {'dt': self.settings['dt'],
                                   'ts': self.data.ts,
-                                  't': self.data.ts*self.settings['dt'],
-                                  'for_pos':self.data.structure.timestep_info[-1].for_pos,
+                                  't': self.data.ts * self.settings['dt'],
+                                  'for_pos': self.data.structure.timestep_info[-1].for_pos,
                                   'cfl1': self.settings['cfl1'],
                                   'vel_gen': self.velocity_generator}
             lin_uvlm_system.assemble_ss(wake_prop_settings=wake_prop_settings)
@@ -259,14 +255,7 @@ def initialise(self, data, custom_settings=None, restart=False):
             self.data.aero.linear['x_0'] = x_0
             self.data.aero.linear['u_0'] = u_0
             self.data.aero.linear['y_0'] = f_0
-            # self.data.aero.linear['gamma_0'] = gamma
-            # self.data.aero.linear['gamma_star_0'] = gamma_star
-            # self.data.aero.linear['gamma_dot_0'] = gamma_dot
-
             # TODO: Implement in AeroTimeStepInfo a way to store the state vectors
-            # aero_tstep.linear.x = x_0
-            # aero_tstep.linear.u = u_0
-            # aero_tstep.linear.y = f_0
 
     def run(self, **kwargs):
         r"""
@@ -323,11 +312,10 @@ def run(self, **kwargs):
         """
 
         aero_tstep = settings_utils.set_value_or_default(kwargs, 'aero_step', self.data.aero.timestep_info[-1])
-        structure_tstep = settings_utils.set_value_or_default(kwargs, 'structural_step', self.data.structure.timestep_info[-1])
-        convect_wake = settings_utils.set_value_or_default(kwargs, 'convect_wake', False)
-        dt= settings_utils.set_value_or_default(kwargs, 'dt', self.settings['dt'])                                                                                                    
-        t = settings_utils.set_value_or_default(kwargs, 't', self.data.ts*dt)
-        unsteady_contribution = settings_utils.set_value_or_default(kwargs, 'unsteady_contribution', False)
+        structure_tstep = settings_utils.set_value_or_default(kwargs, 'structural_step',
+                                                              self.data.structure.timestep_info[-1])
+        dt = settings_utils.set_value_or_default(kwargs, 'dt', self.settings['dt'])
+        t = settings_utils.set_value_or_default(kwargs, 't', self.data.ts * dt)
 
         integr_order = self.settings['integr_order']
 
@@ -346,11 +334,11 @@ def run(self, **kwargs):
         # - proj happens in self.pack_input_vector and unpack_ss_vectors
         if self.settings['track_body']:
             # track A frame
-            if self.num_body_track  == -1:
-                self.Cga = algebra.quat2rotation( structure_tstep.quat )
-            else: # track a specific body
+            if self.num_body_track == -1:
+                self.Cga = algebra.quat2rotation(structure_tstep.quat)
+            else:  # track a specific body
                 self.Cga = algebra.quat2rotation(
-                                structure_tstep.mb_quat[self.num_body_track,:] )
+                    structure_tstep.mb_quat[self.num_body_track, :])
 
         # Column vector that will be the input to the linearised UVLM system
         # Input is at time step n, since it is updated in the aeroelastic solver prior to aerodynamic solver
@@ -395,7 +383,8 @@ def update_grid(self, beam):
         self.data.aero.generate_zeta(beam, self.data.aero.aero_settings, -1, beam_ts=-1)
 
     def update_custom_grid(self, structure_tstep, aero_tstep):
-        self.data.aero.generate_zeta_timestep_info(structure_tstep, aero_tstep, self.data.structure, self.data.aero.aero_settings)
+        self.data.aero.generate_zeta_timestep_info(structure_tstep, aero_tstep, self.data.structure,
+                                                   self.data.aero.aero_settings)
 
     def unpack_ss_vectors(self, y_n, x_n, u_n, aero_tstep):
         r"""
@@ -449,7 +438,7 @@ def unpack_ss_vectors(self, y_n, x_n, u_n, aero_tstep):
 
         ### project forces from uvlm FoR to FoR G
         if self.settings['track_body']:
-            Cg_uvlm = np.dot( self.Cga, self.Cga0.T )
+            Cg_uvlm = np.dot(self.Cga, self.Cga0.T)
 
         f_aero = y_n
 
@@ -478,7 +467,7 @@ def unpack_ss_vectors(self, y_n, x_n, u_n, aero_tstep):
             panels_in_wake = aero_tstep.gamma_star[i_surf].size
 
             # Append reshaped forces to each entry in list (one for each surface)
-            forces.append(f_aero[worked_points:worked_points+points_in_surface].reshape(dimensions, order='C'))
+            forces.append(f_aero[worked_points:worked_points + points_in_surface].reshape(dimensions, order='C'))
 
             ### project forces.
             # - forces are in UVLM linearisation frame. Hence, these  are projected
@@ -486,19 +475,19 @@ def unpack_ss_vectors(self, y_n, x_n, u_n, aero_tstep):
             if self.settings['track_body']:
                 for mm in range(dimensions[1]):
                     for nn in range(dimensions[2]):
-                        forces[i_surf][:,mm,nn] = np.dot(Cg_uvlm, forces[i_surf][:,mm,nn])
+                        forces[i_surf][:, mm, nn] = np.dot(Cg_uvlm, forces[i_surf][:, mm, nn])
 
             # Add the null bottom 3 rows to to the forces entry
             forces[i_surf] = np.concatenate((forces[i_surf], np.zeros(dimensions)))
 
             # Reshape bound circulation terms
-            gamma.append(gamma_vec[worked_panels:worked_panels+panels_in_surface].reshape(
+            gamma.append(gamma_vec[worked_panels:worked_panels + panels_in_surface].reshape(
                 dimensions_gamma, order='C'))
-            gamma_dot.append(gamma_dot_vec[worked_panels:worked_panels+panels_in_surface].reshape(
+            gamma_dot.append(gamma_dot_vec[worked_panels:worked_panels + panels_in_surface].reshape(
                 dimensions_gamma, order='C'))
 
             # Reshape wake circulation terms
-            gamma_star.append(gamma_star_vec[worked_wake_panels:worked_wake_panels+panels_in_wake].reshape(
+            gamma_star.append(gamma_star_vec[worked_wake_panels:worked_wake_panels + panels_in_wake].reshape(
                 dimensions_wake, order='C'))
 
             worked_points += points_in_surface
@@ -507,7 +496,6 @@ def unpack_ss_vectors(self, y_n, x_n, u_n, aero_tstep):
 
         return forces, gamma, gamma_dot, gamma_star
 
-
     def pack_input_vector(self):
         r"""
         Transform a SHARPy AeroTimestep instance into a column vector containing the input to the linear UVLM system.
@@ -531,25 +519,25 @@ def pack_input_vector(self):
         # using rotation matrix aat time 0 (as if FoR A was not rotating).
         if self.settings['track_body']:
 
-            Cuvlm_g = np.dot( self.Cga0, self.Cga.T )
+            Cuvlm_g = np.dot(self.Cga0, self.Cga.T)
             zeta_uvlm, zeta_dot_uvlm, u_ext_uvlm = [], [], []
 
             for i_surf in range(aero_tstep.n_surf):
 
                 Mp1, Np1 = aero_tstep.dimensions[i_surf] + 1
 
-                zeta_uvlm.append( np.empty((3,Mp1,Np1)) )
-                zeta_dot_uvlm.append( np.empty((3,Mp1,Np1)) )
-                u_ext_uvlm.append( np.empty((3,Mp1,Np1)) )
+                zeta_uvlm.append(np.empty((3, Mp1, Np1)))
+                zeta_dot_uvlm.append(np.empty((3, Mp1, Np1)))
+                u_ext_uvlm.append(np.empty((3, Mp1, Np1)))
 
                 for mm in range(Mp1):
                     for nn in range(Np1):
-                        zeta_uvlm[i_surf][:,mm,nn] = \
-                            np.dot(Cuvlm_g, aero_tstep.zeta[i_surf][:,mm,nn])
-                        zeta_dot_uvlm[i_surf][:,mm,nn] = \
-                            np.dot(Cuvlm_g, aero_tstep.zeta_dot[i_surf][:,mm,nn])
-                        u_ext_uvlm[i_surf][:,mm,nn] = \
-                            np.dot(Cuvlm_g, aero_tstep.u_ext[i_surf][:,mm,nn])
+                        zeta_uvlm[i_surf][:, mm, nn] = \
+                            np.dot(Cuvlm_g, aero_tstep.zeta[i_surf][:, mm, nn])
+                        zeta_dot_uvlm[i_surf][:, mm, nn] = \
+                            np.dot(Cuvlm_g, aero_tstep.zeta_dot[i_surf][:, mm, nn])
+                        u_ext_uvlm[i_surf][:, mm, nn] = \
+                            np.dot(Cuvlm_g, aero_tstep.u_ext[i_surf][:, mm, nn])
 
             zeta = np.concatenate([zeta_uvlm[i_surf].reshape(-1, order='C')
                                    for i_surf in range(aero_tstep.n_surf)])
@@ -565,7 +553,7 @@ def pack_input_vector(self):
             zeta_dot = np.concatenate([aero_tstep.zeta_dot[i_surf].reshape(-1, order='C')
                                        for i_surf in range(aero_tstep.n_surf)])
             u_ext = np.concatenate([aero_tstep.u_ext[i_surf].reshape(-1, order='C')
-                                   for i_surf in range(aero_tstep.n_surf)])
+                                    for i_surf in range(aero_tstep.n_surf)])
 
         u = np.concatenate((zeta, zeta_dot, u_ext))
 
@@ -613,7 +601,7 @@ def pack_state_vector(aero_tstep, aero_tstep_m1, dt, integr_order):
         gamma = np.concatenate([aero_tstep.gamma[ss].reshape(-1, order='C')
                                 for ss in range(aero_tstep.n_surf)])
         gamma_star = np.concatenate([aero_tstep.gamma_star[ss].reshape(-1, order='C')
-                                    for ss in range(aero_tstep.n_surf)])
+                                     for ss in range(aero_tstep.n_surf)])
         gamma_dot = np.concatenate([aero_tstep.gamma_dot[ss].reshape(-1, order='C')
                                     for ss in range(aero_tstep.n_surf)])
 
@@ -623,7 +611,7 @@ def pack_state_vector(aero_tstep, aero_tstep_m1, dt, integr_order):
         else:
             if aero_tstep_m1:
                 gamma_m1 = np.concatenate([aero_tstep_m1.gamma[ss].reshape(-1, order='C')
-                                    for ss in range(aero_tstep.n_surf)])
+                                           for ss in range(aero_tstep.n_surf)])
             else:
                 gamma_m1 = gamma - dt * gamma_dot
 
diff --git a/sharpy/solvers/stepuvlm.py b/sharpy/solvers/stepuvlm.py
index ba4b2a686..d39f45ff5 100644
--- a/sharpy/solvers/stepuvlm.py
+++ b/sharpy/solvers/stepuvlm.py
@@ -101,7 +101,8 @@ class StepUvlm(BaseSolver):
 
     settings_types['vortex_radius_wake_ind'] = 'float'
     settings_default['vortex_radius_wake_ind'] = vortex_radius_def
-    settings_description['vortex_radius_wake_ind'] = 'Distance between points below which induction is not computed in the wake convection'
+    settings_description[
+        'vortex_radius_wake_ind'] = 'Distance between points below which induction is not computed in the wake convection'
 
     settings_types['interp_coords'] = 'int'
     settings_default['interp_coords'] = 0
@@ -116,7 +117,8 @@ class StepUvlm(BaseSolver):
 
     settings_types['interp_method'] = 'int'
     settings_default['interp_method'] = 0
-    settings_description['interp_method'] = 'Method of interpolation: linear(0), parabolic(1), splines(2), slerp around z(3), slerp around yaw_slerp(4)'
+    settings_description[
+        'interp_method'] = 'Method of interpolation: linear(0), parabolic(1), splines(2), slerp around z(3), slerp around yaw_slerp(4)'
     settings_options['interp_method'] = [0, 1, 2, 3, 4]
 
     settings_types['yaw_slerp'] = 'float'
@@ -125,12 +127,13 @@ class StepUvlm(BaseSolver):
 
     settings_types['centre_rot'] = 'list(float)'
     settings_default['centre_rot'] = [0., 0., 0.]
-    settings_description['centre_rot'] = 'Coordinates of the centre of rotation to perform slerp interpolation or cylindrical coordinates'
+    settings_description[
+        'centre_rot'] = 'Coordinates of the centre of rotation to perform slerp interpolation or cylindrical coordinates'
 
     settings_types['quasi_steady'] = 'bool'
     settings_default['quasi_steady'] = False
     settings_description['quasi_steady'] = 'Use quasi-steady approximation in UVLM'
-    
+
     settings_types['only_nonlifting'] = 'bool'
     settings_default['only_nonlifting'] = False
     settings_description['only_nonlifting'] = 'Consider nonlifting body interactions'
@@ -149,7 +152,8 @@ class StepUvlm(BaseSolver):
 
     settings_types['ignore_first_x_nodes_in_force_calculation'] = 'int'
     settings_default['ignore_first_x_nodes_in_force_calculation'] = 0
-    settings_description['ignore_first_x_nodes_in_force_calculation'] = 'Ignores the forces on the first user-specified number of nodes of all surfaces.'
+    settings_description[
+        'ignore_first_x_nodes_in_force_calculation'] = 'Ignores the forces on the first user-specified number of nodes of all surfaces.'
 
     settings_table = settings_utils.SettingsTable()
     __doc__ += settings_table.generate(settings_types, settings_default, settings_description, settings_options)
@@ -169,9 +173,9 @@ def initialise(self, data, custom_settings=None, restart=False):
         else:
             self.settings = custom_settings
         settings_utils.to_custom_types(self.settings,
-                           self.settings_types,
-                           self.settings_default,
-                           self.settings_options)
+                                       self.settings_types,
+                                       self.settings_default,
+                                       self.settings_options)
 
         self.data.structure.add_unsteady_information(
             self.data.structure.dyn_dict,
@@ -209,10 +213,11 @@ def run(self, **kwargs):
 
         # Default values
         aero_tstep = settings_utils.set_value_or_default(kwargs, 'aero_step', self.data.aero.timestep_info[-1])
-        structure_tstep = settings_utils.set_value_or_default(kwargs, 'structural_step', self.data.structure.timestep_info[-1])
+        structure_tstep = settings_utils.set_value_or_default(kwargs, 'structural_step',
+                                                              self.data.structure.timestep_info[-1])
         convect_wake = settings_utils.set_value_or_default(kwargs, 'convect_wake', True)
-        dt= settings_utils.set_value_or_default(kwargs, 'dt', self.settings['dt'])
-        t = settings_utils.set_value_or_default(kwargs, 't', self.data.ts*dt)
+        dt = settings_utils.set_value_or_default(kwargs, 'dt', self.settings['dt'])
+        t = settings_utils.set_value_or_default(kwargs, 't', self.data.ts * dt)
         unsteady_contribution = settings_utils.set_value_or_default(kwargs, 'unsteady_contribution', False)
 
         if not aero_tstep.zeta:
@@ -228,7 +233,7 @@ def run(self, **kwargs):
                                           'is_wake': False},
                                          aero_tstep.u_ext)
         if ((self.settings['convection_scheme'] > 1 and convect_wake) or
-           (not self.settings['cfl1'])):
+                (not self.settings['cfl1'])):
             # generate uext_star
             self.velocity_generator.generate({'zeta': aero_tstep.zeta_star,
                                               'override': True,
@@ -240,7 +245,8 @@ def run(self, **kwargs):
                                              aero_tstep.u_ext_star)
         if self.settings['nonlifting_body_interactions']:
 
-            nl_body_tstep = settings_utils.set_value_or_default(kwargs, 'nl_body_tstep', self.data.nonlifting_body.timestep_info[-1])
+            nl_body_tstep = settings_utils.set_value_or_default(kwargs, 'nl_body_tstep',
+                                                                self.data.nonlifting_body.timestep_info[-1])
             self.velocity_generator.generate({'zeta': nl_body_tstep.zeta,
                                               'override': True,
                                               'ts': self.data.ts,
@@ -249,15 +255,15 @@ def run(self, **kwargs):
                                               'for_pos': structure_tstep.for_pos,
                                               'is_wake': False},
                                              nl_body_tstep.u_ext)
-            
+
             uvlmlib.uvlm_solver_lifting_and_nonlifting(self.data.ts,
-                                                       aero_tstep, 
+                                                       aero_tstep,
                                                        nl_body_tstep,
-                                                       structure_tstep, 
+                                                       structure_tstep,
                                                        self.settings,
-                                                       convect_wake=convect_wake, 
+                                                       convect_wake=convect_wake,
                                                        dt=dt)
-        else:            
+        else:
             uvlmlib.uvlm_solver(self.data.ts,
                                 aero_tstep,
                                 structure_tstep,
@@ -291,7 +297,7 @@ def run(self, **kwargs):
 
     def add_step(self):
         self.data.aero.add_timestep()
-        if self.settings['nonlifting_body_interactions']:              
+        if self.settings['nonlifting_body_interactions']:
             self.data.nonlifting_body.add_timestep()
 
     def update_grid(self, beam):
@@ -299,13 +305,13 @@ def update_grid(self, beam):
                                      self.data.aero.aero_settings,
                                      -1,
                                      beam_ts=-1)
-        if self.settings['nonlifting_body_interactions']:                            
+        if self.settings['nonlifting_body_interactions']:
             self.data.nonlifting_body.generate_zeta(beam,
                                                     self.data.aero.aero_settings,
                                                     -1,
                                                     beam_ts=-1)
 
-    def update_custom_grid(self, structure_tstep, aero_tstep, nl_body_tstep = None):
+    def update_custom_grid(self, structure_tstep, aero_tstep, nl_body_tstep=None):
         self.data.aero.generate_zeta_timestep_info(structure_tstep,
                                                    aero_tstep,
                                                    self.data.structure,
@@ -313,12 +319,12 @@ def update_custom_grid(self, structure_tstep, aero_tstep, nl_body_tstep = None):
                                                    dt=self.settings['dt'])
         if self.settings['nonlifting_body_interactions']:
             if nl_body_tstep is None:
-                nl_body_tstep  =  self.data.nonlifting_body.timestep_info[-1] 
+                nl_body_tstep = self.data.nonlifting_body.timestep_info[-1]
             self.data.nonlifting_body.generate_zeta_timestep_info(structure_tstep,
                                                                   nl_body_tstep,
                                                                   self.data.structure,
                                                                   self.data.nonlifting_body.aero_settings,
-                                                                  dt = self.settings['dt'])
+                                                                  dt=self.settings['dt'])
 
     @staticmethod
     def filter_gamma_dot(tstep, history, filter_param):
diff --git a/sharpy/solvers/timeintegrators.py b/sharpy/solvers/timeintegrators.py
index a11a96e5e..2067ac3c5 100644
--- a/sharpy/solvers/timeintegrators.py
+++ b/sharpy/solvers/timeintegrators.py
@@ -71,6 +71,8 @@ class NewmarkBeta(_BaseTimeIntegrator):
 
     def __init__(self):
 
+        self.sys_size = None
+        self.num_LM_eq = None
         self.dt = None
         self.beta = None
         self.gamma = None
@@ -168,6 +170,10 @@ class GeneralisedAlpha(_BaseTimeIntegrator):
 
     def __init__(self):
 
+        self.num_LM_eq = None
+        self.sys_size = None
+        self.om_af = None
+        self.om_am = None
         self.dt = None
         self.am = None
         self.af = None
diff --git a/sharpy/solvers/timeintegratorsjax.py b/sharpy/solvers/timeintegratorsjax.py
new file mode 100644
index 000000000..f740c7036
--- /dev/null
+++ b/sharpy/solvers/timeintegratorsjax.py
@@ -0,0 +1,209 @@
+import numpy as np
+from abc import abstractmethod
+import typing
+
+import sharpy.utils.settings as settings_utils
+from sharpy.utils.solver_interface import solver
+
+arr: typing.Type = np.ndarray
+
+
+@solver
+class _BaseTimeIntegrator:
+    """
+    Base structure for time integrators
+    """
+
+    solver_id = '_BaseTimeIntegrator'
+    solver_classification = 'time_integrator'
+
+    settings_types = dict()
+    settings_default = dict()
+    settings_description = dict()
+    settings_options = dict()
+
+    def __init__(self):
+        pass
+
+    @abstractmethod
+    def initialise(self, data, custom_settings=None, restart=False):
+        pass
+
+    @abstractmethod
+    def predictor(self, q: arr, dqdt: arr, dqddt: arr):
+        pass
+
+    @abstractmethod
+    def build_matrix(self, m: arr, c: arr, k: arr):
+        pass
+
+    @abstractmethod
+    def corrector(self, q: arr, dqdt: arr, dqddt: arr, dq: arr):
+        pass
+
+
+@solver
+class NewmarkBetaJAX(_BaseTimeIntegrator):
+    """
+    Time integration according to the Newmark-beta scheme
+    """
+
+    solver_id = 'NewmarkBetaJAX'
+    solver_classification = 'time_integrator'
+
+    settings_types = _BaseTimeIntegrator.settings_types.copy()
+    settings_default = _BaseTimeIntegrator.settings_default.copy()
+    settings_description = _BaseTimeIntegrator.settings_description.copy()
+    settings_options = _BaseTimeIntegrator.settings_options.copy()
+
+    settings_types['dt'] = 'float'
+    settings_default['dt'] = None
+    settings_description['dt'] = 'Time step'
+
+    settings_types['newmark_damp'] = 'float'
+    settings_default['newmark_damp'] = 1e-4
+    settings_description['newmark_damp'] = 'Newmark damping coefficient'
+
+    settings_types['sys_size'] = 'int'
+    settings_default['sys_size'] = 0
+    settings_description['sys_size'] = 'Size of the system without constraints'
+
+    settings_types['num_LM_eq'] = 'int'
+    settings_default['num_LM_eq'] = 0
+    settings_description['num_LM_eq'] = 'Number of constraint equations'
+
+    def __init__(self):
+        super().__init__()  # I know the base class has no function here, but this makes Pycharm leave me alone
+        self.dt = None
+        self.beta = None
+        self.gamma = None
+        self.sys_size = None
+        self.num_lm_eq = None
+        self.settings = None
+
+    def initialise(self, data, custom_settings=None, restart=False) -> None:
+
+        if custom_settings is None:
+            self.settings = data.input_settings[self.solver_id]
+        else:
+            self.settings = custom_settings
+        settings_utils.to_custom_types(self.settings,
+                                       self.settings_types,
+                                       self.settings_default,
+                                       no_ctype=True)
+
+        self.dt = self.settings['dt']
+        self.gamma = 0.5 + self.settings['newmark_damp']
+        self.beta = 0.25 * (self.gamma + 0.5) * (self.gamma + 0.5)
+
+        self.sys_size = self.settings['sys_size']
+        self.num_lm_eq = self.settings['num_LM_eq']
+
+    def predictor(self, q, dqdt, dqddt):
+        q[:self.sys_size] += (self.dt * dqdt[:self.sys_size]
+                              + (0.5 - self.beta) * self.dt * self.dt * dqddt[:self.sys_size])
+        dqdt[:self.sys_size] += (1. - self.gamma) * self.dt * dqddt[:self.sys_size]
+        dqddt.fill(0.)
+
+    def build_matrix(self, m: arr, c: arr, k: arr) -> arr:
+        a_sys = np.zeros((self.sys_size + self.num_lm_eq, self.sys_size + self.num_lm_eq))
+        a_sys[:, :self.sys_size] = (k[:, :self.sys_size]
+                                    + c[:, :self.sys_size] * self.gamma / (self.beta * self.dt)
+                                    + m[:, :self.sys_size] / (self.beta * self.dt * self.dt))
+        a_sys[:, self.sys_size:] = k[:, self.sys_size:] + c[:, self.sys_size:]
+        return a_sys
+
+    def corrector(self, q: arr, dqdt: arr, dqddt: arr, dq: arr) -> None:
+        q[:self.sys_size] += dq[:self.sys_size]
+        dqdt[:self.sys_size] += self.gamma / (self.beta * self.dt) * dq[:self.sys_size]
+        dqddt[:self.sys_size] += 1. / (self.beta * self.dt * self.dt) * dq[:self.sys_size]
+        dqdt[self.sys_size:] += dq[self.sys_size:]
+
+
+@solver
+class GeneralisedAlphaJAX(_BaseTimeIntegrator):
+    """
+    Time integration according to the Generalised-Alpha scheme
+    """
+
+    solver_id = 'GeneralisedAlphaJAX'
+    solver_classification = 'time_integrator'
+
+    settings_types = _BaseTimeIntegrator.settings_types.copy()
+    settings_default = _BaseTimeIntegrator.settings_default.copy()
+    settings_description = _BaseTimeIntegrator.settings_description.copy()
+    settings_options = _BaseTimeIntegrator.settings_options.copy()
+
+    settings_types['dt'] = 'float'
+    settings_default['dt'] = None
+    settings_description['dt'] = 'Time step'
+
+    settings_types['am'] = 'float'
+    settings_default['am'] = 0.
+    settings_description['am'] = 'alpha_M coefficient'
+
+    settings_types['af'] = 'float'
+    settings_default['af'] = 0.1
+    settings_description['af'] = 'alpha_F coefficient'
+
+    settings_types['sys_size'] = 'int'
+    settings_default['sys_size'] = 0
+    settings_description['sys_size'] = 'Size of the system without constraints'
+
+    settings_types['num_LM_eq'] = 'int'
+    settings_default['num_LM_eq'] = 0
+    settings_description['num_LM_eq'] = 'Number of contraint equations'
+
+    def __init__(self):
+        super().__init__()
+        self.dt = None
+        self.am = None
+        self.af = None
+        self.gamma = None
+        self.beta = None
+        self.om_am = None
+        self.om_af = None
+        self.sys_size = None
+        self.num_lm_eq = None
+
+    def initialise(self, data, custom_settings=None, restart=False) -> None:
+
+        if custom_settings is None:
+            self.settings = data.input_settings[self.solver_id]
+        else:
+            self.settings = custom_settings
+        settings_utils.to_custom_types(self.settings,
+                                       self.settings_types,
+                                       self.settings_default,
+                                       no_ctype=True)
+
+        self.dt = self.settings['dt']
+        self.am = self.settings['am']
+        self.af = self.settings['af']
+        self.om_am = 1. - self.am
+        self.om_af = 1. - self.af
+        self.gamma = 0.5 - self.am + self.af
+        self.beta = 0.25 * (1. - self.am + self.af) ** 2
+        self.sys_size = self.settings['sys_size']
+        self.num_lm_eq = self.settings['num_LM_eq']
+
+    def predictor(self, q: arr, dqdt: arr, dqddt: arr):
+        q[:self.sys_size] += (self.dt * dqdt[:self.sys_size]
+                              + (0.5 - self.beta) * self.dt * self.dt * dqddt[:self.sys_size])
+        dqdt[:self.sys_size] += (1. - self.gamma) * self.dt * dqddt[:self.sys_size]
+        dqddt.fill(0.)
+
+    def build_matrix(self, m: arr, c: arr, k: arr) -> arr:
+        a_sys = np.zeros((self.sys_size + self.num_lm_eq, self.sys_size + self.num_lm_eq))
+        a_sys[:, :self.sys_size] = (self.om_af * k
+                                    + self.gamma * self.om_af / (self.beta * self.dt) * c
+                                    + self.om_am / (self.beta * self.dt * self.dt) * m)
+
+        a_sys[:, self.sys_size:] = k[:, self.sys_size:] + c[:, self.sys_size:]
+        return a_sys
+
+    def corrector(self, q: arr, dqdt: arr, dqddt: arr, dq: arr) -> None:
+        q[:self.sys_size] += self.om_af * dq[:self.sys_size]
+        dqdt[:self.sys_size] += self.gamma * self.om_af / self.beta / self.dt * dq[:self.sys_size]
+        dqddt[:self.sys_size] += self.om_am / self.beta / self.dt ** 2 * dq[:self.sys_size]
+        dqdt[self.sys_size:] += dq[self.sys_size:]
diff --git a/sharpy/solvers/trim.py b/sharpy/solvers/trim.py
index 4cf0a5a05..933e9b73d 100644
--- a/sharpy/solvers/trim.py
+++ b/sharpy/solvers/trim.py
@@ -261,21 +261,8 @@ def optimise(self, func, tolerance, print_info, method, refine):
 
         cout.cout_wrap('Solution = ')
         cout.cout_wrap(solution.x)
-        # pretty_print_x(x, x_info)
         return solution
 
-
-# def pretty_print_x(x, x_info):
-#     cout.cout_wrap('X vector:', 1)
-#     for k, v in x_info:
-#         if k.startswith('i_'):
-#             if isinstance(v, list):
-#                 for i, vv in v:
-#                     cout.cout_wrap(k + ' ' + str(i) + ': ', vv)
-#             else:
-#                 cout.cout_wrap(k + ': ', v)
-
-
 def solver_wrapper(x, x_info, solver_data, i_dim=-1):
     if solver_data.settings['print_info']:
         cout.cout_wrap('x = ' + str(x), 1)
@@ -286,7 +273,6 @@ def solver_wrapper(x, x_info, solver_data, i_dim=-1):
     # change input data
     solver_data.data.structure.timestep_info[solver_data.data.ts] = solver_data.data.structure.ini_info.copy()
     tstep = solver_data.data.structure.timestep_info[solver_data.data.ts]
-    aero_tstep = solver_data.data.aero.timestep_info[solver_data.data.ts]
     orientation_quat = algebra.euler2quat(np.array([roll, alpha, beta]))
     tstep.quat[:] = orientation_quat
     # control surface deflection
@@ -323,21 +309,13 @@ def solver_wrapper(x, x_info, solver_data, i_dim=-1):
     totals[3:6] = moments
     if solver_data.settings['print_info']:
         cout.cout_wrap(' forces = ' + str(totals), 1)
-    # print('total forces = ', totals)
-    # try:
-    #     totals += x[x_info['i_none']]
-    # except KeyError:
-    #     pass
-    # return resultant forces and moments
-    # return np.linalg.norm(totals)
+
     if i_dim >= 0:
         return totals[i_dim]
     elif i_dim == -1:
-        # return [np.sum(totals[0:3]**2), np.sum(totals[4:6]**2)]
         return totals
     elif i_dim == -2:
         coeffs = np.array([1.0, 1.0, 1.0, 2, 2, 2])
-        # print('return = ', np.dot(coeffs*totals, coeffs*totals))
         if solver_data.settings['print_info']:
             cout.cout_wrap(' val = ' + str(np.dot(coeffs*totals, coeffs*totals)), 1)
         return np.dot(coeffs*totals, coeffs*totals)
diff --git a/sharpy/structure/models/beam.py b/sharpy/structure/models/beam.py
index 5cfba2d1a..cde302931 100644
--- a/sharpy/structure/models/beam.py
+++ b/sharpy/structure/models/beam.py
@@ -132,8 +132,6 @@ def generate(self, in_data, settings):
         self.frame_of_reference_delta = in_data['frame_of_reference_delta'].copy()
         # structural twist
         self.structural_twist = in_data['structural_twist'].copy()
-        # boundary conditions
-        # self.boundary_conditions = in_data['boundary_conditions'].copy()
         # beam number for every elem
         try:
             self.beam_number = in_data['beam_number'].copy()
@@ -250,21 +248,6 @@ def add_unsteady_information(self, dyn_dict, num_steps):
             for it in range(num_steps):
                 self.dynamic_input[it]['for_acc'] = np.zeros((6, ), dtype=ct.c_double, order='F')
 
-        # try:
-        #     for it in range(num_steps):
-        #         self.dynamic_input[it]['trayectories'] = dyn_dict['trayectories'][it, :, :]
-        # except KeyError:
-        #     for it in range(num_steps):
-        #         self.dynamic_input[it]['trayectories'] = None
-
-# TODO ADC: necessary? I don't think so
-        # try:
-            # for it in range(num_steps):
-                # self.dynamic_input[it]['enforce_trajectory'] = dyn_dict['enforce_trayectory'][it, :, :]
-        # except KeyError:
-            # for it in range(num_steps):
-                # self.dynamic_input[it]['enforce_trajectory'] = np.zeros((self.num_node, 3), dtype=bool)
-
     def generate_dof_arrays(self):
         self.vdof = np.zeros((self.num_node,), dtype=ct.c_int, order='F') - 1
         self.fdof = np.zeros((self.num_node,), dtype=ct.c_int, order='F') - 1
@@ -333,23 +316,6 @@ def add_lumped_mass_to_element(self, i_lumped_node, inertia_tensor, replace=Fals
                 self.elements[i_lumped_master_elem].rbmass[i_lumped_master_node_local, :, :] += (
                     inertia_tensor) # += necessary in case multiple masses defined per node
 
-    # def generate_master_structure(self):
-    #     self.master = np.zeros((self.num_elem, self.num_node_elem, 2), dtype=int) - 1
-    #     for i_elem in range(self.num_elem):
-    #         for i_node_local in range(self.elements[i_elem].n_nodes):
-    #             if not i_elem and not i_node_local:
-    #                 continue
-    #             j_elem = 0
-    #             while self.master[i_elem, i_node_local, 0] == -1 and j_elem <= i_elem:
-    #                 # for j_node_local in self.elements[j_elem].ordering:
-    #                 for j_node_local in range(self.elements[j_elem].n_nodes):
-    #                     if (self.connectivities[i_elem, i_node_local] ==
-    #                             self.connectivities[j_elem, j_node_local]):
-    #                         self.master[i_elem, i_node_local, :] = [j_elem, j_node_local]
-    #                 j_elem += 1
-
-    #     self.generate_node_master_elem()
-    #     a = 1
     def generate_master_structure(self):
         self.master = np.zeros((self.num_elem, self.num_node_elem, 2), dtype=int) - 1
         for i_elem in range(self.num_elem):
@@ -380,17 +346,6 @@ def add_timestep(self, timestep_info):
     def next_step(self):
         self.add_timestep(self.timestep_info)
 
-    # def generate_node_master_elem(self):
-    #     """
-    #     Returns a matrix indicating the master element for a given node
-    #     :return:
-    #     """
-    #     self.node_master_elem = np.zeros((self.num_node, 2), dtype=ct.c_int, order='F') - 1
-    #     for i_elem in range(self.num_elem):
-    #         for i_node_local in range(self.elements[i_elem].n_nodes):
-    #             if self.master[i_elem, i_node_local, 0] == -1:
-    #                 self.node_master_elem[self.connectivities[i_elem, i_node_local], 0] = i_elem
-    #                 self.node_master_elem[self.connectivities[i_elem, i_node_local], 1] = i_node_local
     def generate_node_master_elem(self):
         """
         Returns a matrix indicating the master element for a given node
@@ -456,28 +411,6 @@ def generate_fortran(self):
 
         if self.settings['unsteady']:
             pass
-            # TODO
-            # if self.dynamic_forces_amplitude is not None:
-            #     self.dynamic_forces_amplitude_fortran = self.dynamic_forces_amplitude.astype(dtype=ct.c_double, order='F')
-            #     self.dynamic_forces_time_fortran = self.dynamic_forces_time.astype(dtype=ct.c_double, order='F')
-            # else:
-            #     self.dynamic_forces_amplitude_fortran = np.zeros((self.num_node, 6), dtype=ct.c_double, order='F')
-            #     self.dynamic_forces_time_fortran = np.zeros((self.n_tsteps, 1), dtype=ct.c_double, order='F')
-            #
-            # if self.forced_vel is not None:
-            #     self.forced_vel_fortran = self.forced_vel.astype(dtype=ct.c_double, order='F')
-            # else:
-            #     self.forced_vel_fortran = np.zeros((self.n_tsteps, 6), dtype=ct.c_double, order='F')
-            #
-            # if self.forced_acc is not None:
-            #     self.forced_acc_fortran = self.forced_acc.astype(dtype=ct.c_double, order='F')
-            # else:
-            #     self.forced_acc_fortran = np.zeros((self.n_tsteps, 6), dtype=ct.c_double, order='F')
-
-    # def update_orientation(self, quat=None, ts=-1):
-    #     if quat is None:
-    #         quat = algebra.euler2quat(self.timestep_info[ts].for_pos[3:6])
-    #     self.timestep_info[ts].update_orientation(quat)  # Cga going in here
 
     def integrate_position(self, ts, dt):
         try:
@@ -490,7 +423,6 @@ def integrate_position(self, ts, dt):
                           self.timestep_info[ts].for_vel[0:3]))
 
     def nodal_premultiply_inv_T_transpose(self, nodal, tstep, filter=np.array([True]*6)):
-        # nodal_t = np.zeros_like(nodal, dtype=ct.c_double, order='F')
         nodal_t = nodal.copy(order='F')
         for i_node in range(self.num_node):
             # get master elem and i_local_node
@@ -544,7 +476,8 @@ def get_body(self, ibody):
         int_list_nodes = np.arange(0, ibody_beam.num_node, 1)
         for ielem in range(ibody_beam.num_elem):
             for inode_in_elem in range(ibody_beam.num_node_elem):
-                ibody_beam.connectivities[ielem, inode_in_elem] = int_list_nodes[ibody_nodes == ibody_beam.connectivities[ielem, inode_in_elem]]
+                ibody_beam.connectivities[ielem, inode_in_elem] = int_list_nodes[
+                    np.argwhere(ibody_nodes == ibody_beam.connectivities[ielem, inode_in_elem])[0][0]]
 
         # TODO: I could copy only the needed stiffness and masses to save storage
         ibody_beam.elem_stiffness = self.elem_stiffness[ibody_elements].astype(dtype=ct.c_int, order='F', copy=True)
diff --git a/sharpy/structure/utils/lagrangeconstraintsjax.py b/sharpy/structure/utils/lagrangeconstraintsjax.py
new file mode 100644
index 000000000..7cc2438c3
--- /dev/null
+++ b/sharpy/structure/utils/lagrangeconstraintsjax.py
@@ -0,0 +1,498 @@
+from typing import Callable, Any, Optional, Type, cast
+from abc import ABC
+import numpy as np
+import scipy as sp
+
+import jax
+import jax.scipy.spatial.transform
+import jax.numpy as jnp
+from jax.numpy import ndarray as jarr
+
+# global constants
+jax.config.update("jax_enable_x64", True)
+DICT_OF_LC = dict()
+USE_JIT = True
+
+# type definition for b subfunctions (Constraint, i_lm, b_mat, q, q_dot, u, u_dot)
+b_type: Type = Optional[Callable[[Any, slice, jarr, jarr, jarr, jarr, jarr], jarr]]
+
+# type definitions for  b functions, ordering is (q, q_dot, u, u_dot, lmh, lmn)
+func_type: Type = Callable[[jarr, jarr, jarr, jarr, jarr, jarr], jarr]
+
+
+# redefine jax.scipy rotation class to use [real, i, j, k] ordering by default
+class Rot(jax.scipy.spatial.transform.Rotation):
+    @classmethod
+    def from_quat(cls, quat: jarr):
+        return super().from_quat(jnp.array((*quat[1:4], quat[0])))
+
+    def as_quat(self, canonical=True) -> jarr:
+        return super().as_quat(canonical=canonical)
+
+
+# decorator populating DICT_OF_LC as {lc_id: Constraint}, not type hinted as preceeds Constraint declaration
+def constraint(constraint_):
+    global DICT_OF_LC
+    if constraint_.lc_id is not None:
+        DICT_OF_LC[constraint_.lc_id] = constraint_
+    else:
+        raise AttributeError('Class defined as lagrange constraint has no lc_id attribute')
+    return constraint_
+
+
+# JAX vector skew
+def skew(vec: jarr) -> jarr:
+    if vec.shape != (3,) or jnp.iscomplexobj(vec):
+        raise ValueError("Incompatible input vector dimention or data type")
+    return jnp.array(((0., -vec[2], vec[1]),
+                      (vec[2], 0., -vec[0]),
+                      (-vec[1], vec[0], 0.)))
+
+
+# JAX cartesian rotation vector to rotation matrix as version from SciPy's Rotation class doesn't differentiate well
+def crv2rot(crv: jarr) -> jarr:
+    ang = jnp.linalg.norm(crv)
+    crv_skew = skew(crv)
+    return jax.lax.cond(ang > 1e-15,
+                        lambda: jnp.eye(3) + jnp.sin(ang) / ang * crv_skew
+                                + (1 - jnp.cos(ang)) / ang ** 2 * crv_skew @ crv_skew,
+                        lambda: jnp.eye(3) + crv_skew + 0.5 * crv_skew @ crv_skew)
+
+
+# JAX cartesian rotation vector tangent operator
+def crv2tan(psi: jarr) -> jarr:
+    ang = jnp.linalg.norm(psi)
+    psi_skew = skew(psi)
+    return jax.lax.cond(ang > 1e-8,
+                        lambda: jnp.eye(3) + (jnp.cos(ang) - 1.) / ang ** 2 * psi_skew + (ang - jnp.sin(ang))
+                                / ang ** 3 * psi_skew @ psi_skew,
+                        lambda: jnp.eye(3) - 0.5 * psi_skew + psi_skew @ psi_skew / 6.)
+
+
+@constraint
+class Constraint(ABC):
+    # this might make it a bit faster, might be overkill
+    __slots__ = ('settings', 's_fact', 'p_fact', 'use_p_fact', 'jac_func', 'num_h_funcs', 'num_n_funcs', 'i_first_lm',
+                 'num_lmh', 'num_lmn', 'num_lm', 'num_lm_tot', 'num_bodys', 'is_free', 'num_elem_body', 'num_node_body',
+                 'num_dof_body', 'num_dof_tot', 'num_eq_tot', 'i_sys', 'i_lm_tot', 'i_lmh_eq', 'i_lmn_eq', 'i_lm_eq',
+                 'i_v0', 'i_omega0', 'i_quat0', 'i_v1', 'i_omega1', 'i_quat1', 'i_r0', 'i_psi0', 'q_i_global',
+                 'num_active_q', 'bh', 'bn', 'gn', 'c', 'd', 'bht', 'bnt', 'bht_lmh', 'bnt_lmn', 'p_func_h', 'p_func_n',
+                 'run')
+
+    lc_id = '_base_constraint'
+    required_params: tuple[str, ...] = tuple()  # required parameters for the given constraint
+    bh_funcs: tuple[b_type, ...] = list()  # tuple of holonomic B matrix funcs
+    bn_funcs: tuple[b_type | None, ...] = tuple()  # tuple of non holonomic B matrix funcs, or None for zero matrix
+    gn_funcs: tuple[b_type | None, ...] = tuple()  # tuple of non holonomic g matrix funcs, or None for zero matrix
+    num_lmh_eq: tuple[int, ...] = tuple()  # number of LMs for each holonomic constraint
+    num_lmn_eq: tuple[int, ...] = tuple()  # number of LMs for each nonholonomic constraint
+    postproc_funcs: tuple[Callable, ...] = tuple()  # postprocessing functions to be run
+
+    def __init__(self, data, i_lm: int, constraint_settings: dict): # case data, index of first LM, input settings
+        self.settings = constraint_settings  # input constraint parameters
+
+        # scaling factor, with 1/dt^2 as default
+        self.s_fact = constraint_settings.get('scaling_factor', data.data.settings['DynamicCoupled']['dt'] ** -2)
+        self.p_fact = constraint_settings.get('penalty_factor', 0.)  # penalty factor for constraint
+        self.use_p_fact = abs(self.p_fact) > 1e-6   # if true, penalty equations are included in run function
+
+        # choose either forward or reverse mode autodifferentiation
+        # I believe that only forward works as of current, and for this case the difference should be negligible
+        match data.settings['jacobian_method']:
+            case 'forward':
+                self.jac_func = jax.jacfwd
+            case 'reverse':
+                self.jac_func = jax.jacrev
+
+        self.num_h_funcs = len(self.bh_funcs)           # number of holonomic functions (this constraint)
+        self.num_n_funcs = len(self.bn_funcs)           # number of non holonomic functions (this constraint)
+        self.i_first_lm = i_lm                          # index of first LM (this constraint)
+        self.num_lmh = sum(self.num_lmh_eq)             # total number of holonomic LMs (this constraint)
+        self.num_lmn = sum(self.num_lmn_eq)             # total number of non holonomic LMs (this constraint)
+        self.num_lm = self.num_lmh + self.num_lmn       # total number of LMs (this constraint)
+        self.num_lm_tot = data.num_lm_tot               # total numer of LMs (all constraints)
+
+        self.num_bodys = data.data.structure.ini_mb_dict['num_bodies']  # number of bodies
+        self.is_free = [data.data.structure.ini_mb_dict[f'body_{i:02d}']['FoR_movement'] == 'free'
+                        for i in range(self.num_bodys)]  # boolean for beam freedom condition
+
+        self.num_elem_body = [list(data.data.structure.body_number).count(i)
+                              for i in range(self.num_bodys)]  # number of elements per body
+        self.num_node_body = [self.num_elem_body[i] * (data.data.structure.num_node_elem - 1) + 1
+                              for i in range(self.num_bodys)]  # number of nodes per body
+        self.num_dof_body = [(self.num_node_body[i] - 1) * 6 + self.is_free[i] * 10
+                             for i in range(self.num_bodys)]  # number of DoFs per body
+
+        self.num_dof_tot = data.sys_size                     # number of equations representing the unconstrained system
+        self.num_eq_tot = self.num_dof_tot + self.num_lm_tot  # number of equations representing the constrainted system
+
+        self.i_sys = slice(0, self.num_dof_tot)  # index of equations representing the unconstrained system
+
+        self.i_lm_tot = jnp.arange(self.num_dof_tot, self.num_dof_tot + self.num_lm_tot)  # index of LMs in full system
+
+        self.i_lmh_eq: list[slice] = []  # list of slices of holonomic LMs for this constraint
+        self.i_lmn_eq: list[slice] = []  # list of slices of non holonomic LMs for this constraint
+
+        i_eq = self.i_first_lm
+        for i in range(len(self.num_lmh_eq)):
+            self.i_lmh_eq.append(slice(i_eq, i_eq + self.num_lmh_eq[i]))
+            i_eq += self.num_lmh_eq[i]
+        for i in range(len(self.num_lmn_eq)):
+            self.i_lmn_eq.append(slice(i_eq, i_eq + self.num_lmn_eq[i]))
+            i_eq += self.num_lmn_eq[i]
+        self.i_lm_eq = self.i_lmh_eq + self.i_lmn_eq  # list of slices of LMs for this constraint, holonomic first
+
+        # the below indexes are slices of q and q_dot in the subset of active q terms
+        self.i_v0: Optional[slice] = None        # index of body 0 linear velocity
+        self.i_omega0: Optional[slice] = None    # index of body 0 rotational velocity
+        self.i_quat0: Optional[slice] = None     # index of body 0 orientation quaternion
+        self.i_v1: Optional[slice] = None        # index of body 1 linear velocity
+        self.i_omega1: Optional[slice] = None    # index of body 1 rotational velocity
+        self.i_quat1: Optional[slice] = None     # index of body 1 orientation quaternion
+        self.i_r0: Optional[slice] = None        # index of body 0 node position
+        self.i_psi0: Optional[slice] = None      # index of body 0 node orientation
+        self.q_i_global: Optional[jnp.ndarray] = None     # index of q used in constraints (global)
+        self.num_active_q: Optional[int] = None     # number of elements of q required in constraint
+        self.create_index()                      # assign all required indexes
+
+        self.bh: func_type = self.create_bh()                                          # overall holonomic b function
+        self.bn: func_type = self.create_bn()                                          # overall nonholonomic b function
+        self.gn: func_type = self.create_gn()                                          # overall nonholonomic g function
+
+        # cannot type hint lambda expressions, so casting the type will do
+        self.c: func_type = cast(func_type, lambda *args: self.bh(*args) @ args[0])                    # C function
+        self.d: func_type = cast(func_type, lambda *args: self.bn(*args) @ args[1] + self.gn(*args))   # D function
+        self.bht: func_type = cast(func_type, lambda *args: self.bh(*args).T)                          # B_h^T
+        self.bnt: func_type = cast(func_type, lambda *args: self.bn(*args).T)                          # B_n^T
+        self.bht_lmh: func_type = cast(func_type, lambda *args: self.bht(*args) @ args[4])             # B_h^T @ lm_h
+        self.bnt_lmn: func_type = cast(func_type, lambda *args: self.bnt(*args) @ args[5])             # B_n^T @ lm_n
+        self.p_func_h: func_type = cast(func_type, lambda *args: self.bht(*args) @ self.c(*args))
+        self.p_func_n: func_type = cast(func_type, lambda *args: self.bnt(*args) @ self.d(*args))
+
+        # create function which returns contribution to structural equations
+        self.run: Callable[[jarr, jarr, jarr, jarr, jarr, jarr], tuple[jarr, jarr, jarr]] = self.create_run()
+
+    def postprocess(self, mb_beam, mb_tstep) -> None:
+        for func in self.postproc_funcs:
+            func(self, mb_beam, mb_tstep)
+
+    @classmethod
+    def get_n_lm(cls) -> int:
+        return sum(cls.num_lmh_eq) + sum(cls.num_lmn_eq)
+
+    def create_index(self) -> None:
+        # check all required parameters are in settings
+        for param in self.required_params:
+            if param not in self.settings.keys():
+                raise KeyError(f"Parameter {param} is undefined in constraint settings")
+
+        i_global = []   # index in full q
+        i_count = 0     # start of current slice
+
+        if 'body' in self.required_params:
+            i_for0 = self.settings['body']
+            i_start_for0 = sum(self.num_dof_body[:i_for0 + 1]) - 10
+            self.i_v0 = np.arange(0, 3, dtype=int)
+            self.i_omega0 = np.arange(3, 6, dtype=int)
+            self.i_quat0 = np.arange(6, 10, dtype=int)
+            i_global.append(np.arange(i_start_for0, i_start_for0 + 10))
+            i_count += 10
+
+        if 'node_in_body' in self.required_params:
+            i_node = self.settings['node_in_body']
+            self.i_r0 = np.arange(i_count, i_count + 3, dtype=int)
+            self.i_psi0 = np.arange(i_count + 3, i_count + 6, dtype=int)
+            i_global.append(jnp.arange((i_node - 1) * 6, (i_node - 1) * 6 + 6))
+            i_count += 6
+
+        if 'body_FoR' in self.required_params:
+            i_for1 = self.settings['body_FoR']
+            i_start_for1 = sum(self.num_dof_body[:i_for1 + 1]) - 10
+            self.i_v1 = np.arange(i_count, i_count + 3, dtype=int)
+            self.i_omega1 = np.arange(i_count + 3, i_count + 6, dtype=int)
+            self.i_quat1 = np.arange(i_count + 6, i_count + 10, dtype=int)
+            i_global.append(np.arange(i_start_for1, i_start_for1 + 10))
+            i_count += 10
+
+        self.q_i_global = jnp.hstack(i_global)
+        self.num_active_q = self.q_i_global.shape[0]
+
+    def create_bh(self) -> func_type:
+        def bh(*args: jarr) -> jarr:
+            bh_mat = jnp.zeros((self.num_lm_tot, self.num_active_q))
+            for i_func, bh_func in enumerate(self.bh_funcs):
+                bh_mat = bh_func(self, self.i_lmh_eq[i_func], bh_mat, *args[:4])
+            return bh_mat
+
+        return bh
+
+    def create_bn(self) -> func_type:
+        def bn(*args: jarr) -> jarr:
+            bn_mat = jnp.zeros((self.num_lm_tot, self.num_active_q))
+            for i_func, bn_func in enumerate(self.bn_funcs):
+                if bn_func is not None:
+                    bn_mat = bn_func(self, self.i_lmn_eq[i_func], bn_mat, *args[:4])
+            return bn_mat
+
+        return bn
+
+    def create_gn(self) -> func_type:
+        def gn(*args: jarr) -> jarr:
+            gn_mat = jnp.zeros(self.num_lm_tot)
+            for i_func, gn_func in enumerate(self.gn_funcs):
+                if gn_func is not None:
+                    gn_mat = gn_func(self, self.i_lmn_eq[i_func], gn_mat, *args[:4])
+            return gn_mat
+
+        return gn
+
+    def create_run(self):
+        def run(q: jarr, q_dot: jarr, u: jarr, u_dot: jarr, lmh: jarr, lmn: jarr) -> tuple[jarr, jarr, jarr]:
+            args = (q, q_dot, u, u_dot, lmh, lmn)
+
+            c = jnp.zeros((self.num_eq_tot, self.num_eq_tot))
+            c = c.at[jnp.ix_(self.q_i_global, self.q_i_global)].add(self.jac_func(self.bht_lmh, 1)(*args))
+            c = c.at[jnp.ix_(self.q_i_global, self.q_i_global)].add(self.jac_func(self.bnt_lmn, 1)(*args))
+            c = c.at[jnp.ix_(self.q_i_global, self.i_lm_tot)].add(self.bnt(*args))
+            c = c.at[jnp.ix_(self.i_lm_tot, self.q_i_global)].add(self.s_fact * (self.jac_func(self.c, 1)(*args)))
+            c = c.at[jnp.ix_(self.i_lm_tot, self.q_i_global)].add(self.s_fact * (self.jac_func(self.d, 1)(*args)))
+
+            k = jnp.zeros((self.num_eq_tot, self.num_eq_tot))
+            k = k.at[jnp.ix_(self.q_i_global, self.q_i_global)].add(self.jac_func(self.bht_lmh, 0)(*args))
+            k = k.at[jnp.ix_(self.q_i_global, self.q_i_global)].add(self.jac_func(self.bnt_lmn, 0)(*args))
+            k = k.at[jnp.ix_(self.q_i_global, self.i_lm_tot)].add(self.bht(*args))
+            k = k.at[jnp.ix_(self.i_lm_tot, self.q_i_global)].add(self.s_fact * (self.jac_func(self.c, 0)(*args)))
+            k = k.at[jnp.ix_(self.i_lm_tot, self.q_i_global)].add(self.s_fact * (self.jac_func(self.d, 0)(*args)))
+
+            rhs = jnp.zeros(self.num_eq_tot)
+            rhs = rhs.at[self.q_i_global].add(self.bht_lmh(*args) + self.bnt_lmn(*args))
+            rhs = rhs.at[self.i_lm_tot].add(self.s_fact * (self.c(*args) + self.d(*args)))
+
+            if self.use_p_fact:
+                c = c.at[jnp.ix_(self.q_i_global, self.q_i_global)].add(self.p_fact * (self.jac_func(self.p_func_h, 1)(*args)))
+                c = c.at[jnp.ix_(self.q_i_global, self.q_i_global)].add(self.p_fact * (self.jac_func(self.p_func_n, 1)(*args)))
+                k = k.at[jnp.ix_(self.q_i_global, self.q_i_global)].add(self.p_fact * (self.jac_func(self.p_func_h, 0)(*args)))
+                k = k.at[jnp.ix_(self.q_i_global, self.q_i_global)].add(self.p_fact * (self.jac_func(self.p_func_n, 0)(*args)))
+                rhs = rhs.at[self.q_i_global].add(self.p_fact * (self.p_func_h(*args) + self.p_func_n(*args)))
+            return c, k, rhs
+
+        return run
+
+
+def combine_constraints(csts: list[Constraint]) -> Callable:
+    def combined_run(q: jarr, q_dot: jarr, u: list[None | jarr, ...], u_dot: list[None | jarr, ...], lmh: jarr,
+                     lmn: jarr) -> tuple[jarr, jarr, jarr]:
+        q_i = [cst.q_i_global for cst in csts]
+
+        n_cst = len(csts)
+        out_mats = [csts[i_cst].run(q[q_i[i_cst]], q_dot[q_i[i_cst]], u[i_cst], u_dot[i_cst], lmh, lmn)
+                    for i_cst in range(n_cst)]
+        c = jnp.sum(jnp.array([out_mats[i][0] for i in range(n_cst)]), axis=0)
+        k = jnp.sum(jnp.array([out_mats[i][1] for i in range(n_cst)]), axis=0)
+        rhs = jnp.sum(jnp.array([out_mats[i][2] for i in range(n_cst)]), axis=0)
+        return c, k, rhs
+
+    return jax.jit(combined_run) if USE_JIT else combined_run
+
+
+class BaseFunc(ABC):
+    n_eq = 3
+
+
+class CstFuncs(ABC):
+    class EqualNodeFoR(BaseFunc):
+        @staticmethod
+        def b_lin_vel(constraint_: Constraint, i_lm: slice, b: jarr, q: jarr, q_dot: jarr, u: jarr, u_dot: jarr) \
+                -> jarr:
+            b = b.at[i_lm, constraint_.i_r0].add(
+                -Rot.from_quat(q_dot[constraint_.i_quat0]).as_matrix())
+            b = b.at[i_lm, constraint_.i_v0].add(
+                -Rot.from_quat(q_dot[constraint_.i_quat0]).as_matrix())
+            b = b.at[i_lm, constraint_.i_v1].add(
+                Rot.from_quat(q_dot[constraint_.i_quat1]).as_matrix())
+            b = b.at[i_lm, constraint_.i_omega0].add(
+                Rot.from_quat(q_dot[constraint_.i_quat0]).as_matrix() @ skew(q[constraint_.i_r0]))
+            return b
+
+        @staticmethod
+        def b_ang_vel(constraint_: Constraint, i_lm: slice, b: jarr, q: jarr, q_dot: jarr, u: jarr, u_dot: jarr) \
+                -> jarr:
+            b = b.at[i_lm, constraint_.i_psi0].add(crv2tan(q[constraint_.i_psi0]))
+            b = b.at[i_lm, constraint_.i_omega1].add(-crv2rot(q[constraint_.i_psi0]).T
+                                                     @ Rot.from_quat(q_dot[constraint_.i_quat0]).as_matrix().T
+                                                     @ Rot.from_quat(q_dot[constraint_.i_quat1]).as_matrix())
+            b = b.at[i_lm, constraint_.i_omega0].add(crv2rot(q[constraint_.i_psi0]).T)
+            return b
+
+    class ControlFoR(BaseFunc):
+        @staticmethod
+        def b_ang_vel(constraint_: Constraint, i_lm: slice, b: jarr, q: jarr, q_dot: jarr, u: jarr, u_dot: jarr) \
+                -> jarr:
+            return b.at[i_lm, constraint_.i_omega1].add(jnp.eye(3))
+
+        @staticmethod
+        def g_ang_vel(constraint_: Constraint, i_lm: slice, g: jarr, q: jarr, q_dot: jarr, u: jarr, u_dot: jarr) \
+                -> jarr:
+            return g.at[i_lm].add(-crv2tan(u) @ u_dot)
+
+    class ControlNodeFoR(EqualNodeFoR):
+        @staticmethod
+        def g_ang_vel(constraint_: Constraint, i_lm: slice, g: jarr, q: jarr, q_dot: jarr, u: jarr, u_dot: jarr) \
+                -> jarr:
+            return g.at[i_lm].add(crv2rot(u).T @ crv2tan(u) @ u_dot)
+
+    class ZeroFoR(BaseFunc):
+        @staticmethod
+        def b_lin_vel(constraint_: Constraint, i_lm: slice, b: jarr, q: jarr, q_dot: jarr, u: jarr, u_dot: jarr) \
+                -> jarr:
+            return b.at[i_lm, constraint_.i_v1].add(jnp.eye(3))
+
+        @staticmethod
+        def b_ang_vel(constraint_: Constraint, i_lm: slice, b: jarr, q: jarr, q_dot: jarr, u: jarr, u_dot: jarr) \
+                -> jarr:
+            return b.at[i_lm, constraint_.i_omega1].add(jnp.eye(3))
+
+    class HingeNodeFoR(BaseFunc):
+        n_eq = 2
+
+        @staticmethod
+        def b_ang_vel(constraint_: Constraint, i_lm: slice, b: jarr, q: jarr, q_dot: jarr, u: jarr, u_dot: jarr) \
+                -> jarr:
+            r_ax_a = jnp.array(constraint_.settings['rot_axisA2'])
+            r_ax_a /= jnp.linalg.norm(r_ax_a)
+            a_skew = skew(r_ax_a)
+
+            r_ax_b = jnp.array(constraint_.settings['rot_axisB'])
+            r_ax_b /= jnp.linalg.norm(r_ax_b)
+            b_skew = skew(r_ax_b)
+
+            rot_psi = crv2rot(q[constraint_.i_psi0])
+
+            aux = (rot_psi.T @ Rot.from_quat(q[constraint_.i_quat0]).as_matrix().T
+                   @ Rot.from_quat(q[constraint_.i_quat1]).as_matrix()) @ a_skew
+
+            aux_norms = jnp.linalg.norm(aux, axis=1)
+            dirs = jnp.array(((1, 2), (0, 2), (0, 1)))[jnp.argmin(aux_norms)]
+
+            b = b.at[i_lm, constraint_.i_omega0].add((b_skew @ rot_psi.T)[dirs, :])
+            b = b.at[i_lm, constraint_.i_psi0].add((b_skew @ crv2tan(q[constraint_.i_psi0]))[dirs, :])
+            b = b.at[i_lm, constraint_.i_omega1].add(-aux[dirs, :])
+            return b
+
+    class HingeFoR(BaseFunc):
+        n_eq = 2
+
+        @classmethod
+        def b_ang_vel(cls, constraint_: Constraint, i_lm: slice, b: jarr, q: jarr, q_dot: jarr, u: jarr, u_dot: jarr) \
+                -> jarr:
+            axis = jnp.array(constraint_.settings['rot_axis_AFoR'])
+            axis = axis / jnp.linalg.norm(axis)
+            axis_skew = skew(axis)
+            dirs = jnp.array(((1, 2), (0, 2), (0, 1)))[jnp.argmax(axis)]  # directions for axis matrix
+            i_omega1_indep = jnp.arange(constraint_.i_omega1[0], constraint_.i_omega1[2] + 1)[dirs]
+
+            b = jax.lax.cond(jnp.abs(jnp.linalg.norm(axis) - 1.) < 1e-6,
+                             lambda: b.at[i_lm, i_omega1_indep].add(jnp.eye(2)),
+                             lambda: b.at[i_lm, constraint_.i_omega1].add(axis_skew[dirs, :]))
+            return b
+
+
+def move_for_to_node(constraint_: Constraint, mb_beam, mb_tstep) -> None:
+    i_body_node = constraint_.settings['body']  # beam number which contains node
+    i_node = constraint_.settings['node_in_body']  # node number in beam
+    i_body_for = constraint_.settings['body_FoR']
+    rel_pos_b = constraint_.settings.get('rel_posB', np.zeros(3))
+
+    i_elem, i_node_in_elem = mb_beam[i_body_node].node_master_elem[i_node]
+    c_ga = mb_tstep[i_body_node].cga()
+    c_ab = sp.spatial.transform.Rotation.from_rotvec(mb_tstep[i_body_node].psi[i_elem, i_node_in_elem, :]).as_matrix()
+
+    mb_tstep[i_body_for].for_pos[:3] = (c_ga @ (mb_tstep[i_body_node].pos[i_node, :] + c_ab @ rel_pos_b)
+                                        + mb_tstep[i_body_node].for_pos[:3])
+
+
+@constraint
+class FullyConstrainedNodeFoR(Constraint):
+    lc_id = 'fully_constrained_node_FoR'
+    required_params = ('node_in_body', 'body', 'body_FoR')
+    bn_funcs = (CstFuncs.EqualNodeFoR.b_lin_vel, CstFuncs.EqualNodeFoR.b_ang_vel)
+    gn_funcs = (None, None)
+    num_lmn_eq = (CstFuncs.EqualNodeFoR.n_eq, CstFuncs.EqualNodeFoR.n_eq)
+    postproc_funcs = (move_for_to_node,)
+
+
+@constraint
+class FullyConstrainedFoR(Constraint):
+    lc_id = 'fully_constrained_FoR'
+    required_params = ('body_FoR',)
+    bn_funcs = (CstFuncs.ZeroFoR.b_lin_vel, CstFuncs.ZeroFoR.b_ang_vel)
+    gn_funcs = (None, None)
+    num_lmn_eq = (CstFuncs.ZeroFoR.n_eq, CstFuncs.ZeroFoR.n_eq)
+
+
+@constraint
+class SphericalFoR(Constraint):
+    lc_id = 'spherical_FoR'
+    required_params = ('body_FoR',)
+    bn_funcs = (CstFuncs.ZeroFoR.b_lin_vel,)
+    gn_funcs = (None,)
+    num_lmn_eq = (CstFuncs.ZeroFoR.n_eq,)
+
+
+@constraint
+class Free(Constraint):
+    lc_id = 'free'
+
+
+@constraint
+class ControlledRotNodeFoR(Constraint):
+    lc_id = 'control_node_FoR_rot_vel'
+    required_params = ('controller_id', 'node_in_body', 'body', 'body_FoR')
+    bn_funcs = (CstFuncs.ControlNodeFoR.b_lin_vel, CstFuncs.ControlNodeFoR.b_ang_vel)
+    gn_funcs = (None, CstFuncs.ControlNodeFoR.g_ang_vel)
+    num_lmn_eq = (CstFuncs.ControlNodeFoR.n_eq, CstFuncs.ControlNodeFoR.n_eq)
+    postproc_funcs = (move_for_to_node,)
+
+
+@constraint
+class ControlledRotFoR(Constraint):
+    lc_id = 'control_rot_vel_FoR'
+    required_params = ('controller_id', 'body_FoR')
+    bn_funcs = (CstFuncs.ZeroFoR.b_lin_vel, CstFuncs.ControlFoR.b_ang_vel)
+    gn_funcs = (None, CstFuncs.ControlFoR.g_ang_vel)
+    num_lmn_eq = (CstFuncs.ZeroFoR.n_eq, CstFuncs.ControlFoR.n_eq)
+
+
+@constraint
+class HingeFoR(Constraint):
+    lc_id = 'hinge_FoR'
+    required_params = ('body_FoR', 'rot_axis_AFoR')
+    bn_funcs = (CstFuncs.ZeroFoR.b_lin_vel, CstFuncs.HingeFoR.b_ang_vel)
+    gn_funcs = (None, None)
+    num_lmn_eq = (CstFuncs.ZeroFoR.n_eq, CstFuncs.HingeFoR.n_eq)
+
+
+@constraint
+class SphericalNodeFor(Constraint):
+    lc_id = 'spherical_node_FoR'
+    required_params = ('body', 'body_FoR', 'node_in_body')
+    bn_funcs = (CstFuncs.EqualNodeFoR.b_lin_vel, )
+    gn_funcs = (None, )
+    num_lmn_eq = (CstFuncs.EqualNodeFoR.n_eq, )
+    postproc_funcs = (move_for_to_node,)
+
+
+@constraint
+class HingeNodeFoR(Constraint):
+    lc_id = 'hinge_node_FoR'
+    required_params = ('body', 'body_FoR', 'node_in_body', 'rot_axisA2', 'rot_axisB')
+    bn_funcs = (CstFuncs.EqualNodeFoR.b_lin_vel, CstFuncs.HingeNodeFoR.b_ang_vel)
+    gn_funcs = (None, None)
+    num_lmn_eq = (CstFuncs.EqualNodeFoR.n_eq, CstFuncs.HingeNodeFoR.n_eq)
+    postproc_funcs = (move_for_to_node,)
+
+
+if __name__ == '__main__':
+    print("Available constraints:")
+    for constraint in DICT_OF_LC.values():
+        print(constraint.lc_id)
diff --git a/sharpy/utils/datastructures.py b/sharpy/utils/datastructures.py
index 2f90153df..c734cfedc 100644
--- a/sharpy/utils/datastructures.py
+++ b/sharpy/utils/datastructures.py
@@ -739,6 +739,7 @@ def __init__(self, num_node, num_elem, num_node_elem=3, num_dof=None, num_bodies
         self.forces_constraints_nodes = np.zeros((self.num_node, 6), dtype=ct.c_double, order='F')
         self.forces_constraints_FoR = np.zeros((num_bodies, 10), dtype=ct.c_double, order='F')
         self.mb_dict = None
+        self.mb_prescribed_dict = None
 
     def copy(self):
         """
@@ -794,6 +795,7 @@ def copy(self):
         copied.forces_constraints_FoR = self.forces_constraints_FoR.astype(dtype=ct.c_double, order='F', copy=True)
 
         copied.mb_dict = copy.deepcopy(self.mb_dict)
+        copied.mb_prescribed_dict = copy.deepcopy(self.mb_prescribed_dict)
 
         return copied
 
diff --git a/sharpy/utils/exceptions.py b/sharpy/utils/exceptions.py
index 59a55c061..cdff0ed12 100644
--- a/sharpy/utils/exceptions.py
+++ b/sharpy/utils/exceptions.py
@@ -6,8 +6,8 @@
 class DefaultValueBaseException(Exception):
     def __init__(self, variable, value, message=''):
         super().__init__(message)
-        
-    def output_message(self, message, color_id = 3):
+
+    def output_message(self, message, color_id=3):
         if cout.cout_wrap is None:
             print(message)
         else:
@@ -18,8 +18,8 @@ def output_message(self, message, color_id = 3):
 
 class NoDefaultValueException(DefaultValueBaseException):
     def __init__(self, variable, value=None, message=''):
-        super().__init__(message, value)        
-        self.output_message("The variable " + variable + " has no default value, please indicate one") 
+        super().__init__(message, value)
+        self.output_message("The variable " + variable + " has no default value, please indicate one")
 
 
 class NotValidInputFile(Exception):
@@ -31,9 +31,12 @@ class NotImplementedSolver(Exception):
     def __init__(self, solver_name, message=''):
         super().__init__(message)
         if cout.cout_wrap is None:
-            print("The solver " + solver_name + " is not implemented. Check the list of available solvers when starting SHARPy")
+            print(
+                "The solver " + solver_name + " is not implemented. Check the list of available solvers when starting SHARPy")
         else:
-            cout.cout_wrap("The solver " + solver_name + " is not implemented. Check the list of available solvers when starting SHARPy", 3)
+            cout.cout_wrap(
+                "The solver " + solver_name + " is not implemented. Check the list of available solvers when starting SHARPy",
+                3)
 
 
 class NotConvergedStructuralSolver(Exception):
@@ -69,18 +72,18 @@ class NotValidSetting(DefaultValueBaseException):
     def __init__(self, setting, variable, options, value=None, message=''):
         message = 'The setting %s with entry %s is not one of the valid options: %s' % (setting, variable, options)
         super().__init__(variable, value, message=message)
-        self.output_message(message, color_id = 4)
-        
-    
+        self.output_message(message, color_id=4)
+
+
 class NotValidSettingType(DefaultValueBaseException):
     """
     Raised when a user gives a setting with an invalid type
     """
 
-    def __init__(self, setting, variable,  data_types, value=None, message=''):
+    def __init__(self, setting, variable, data_types, value=None, message=''):
         message = 'The setting %s with entry %s is not one of the valid types: %s' % (setting, variable, data_types)
         super().__init__(variable, value, message=message)
-        self.output_message(message, color_id = 4)
+        self.output_message(message, color_id=4)
 
 
 class SolverNotFound(Exception):
@@ -94,7 +97,8 @@ class NotRecognisedSetting(DefaultValueBaseException):
     """
     Raised when a setting is not recognised
     """
+
     def __init__(self, setting, value=None, message=''):
         message = 'Unrecognised setting {:s}. Please check input file and/or documentation'.format(setting)
         super().__init__(variable=None, value=None, message=message)
-        self.output_message(message, color_id = 4)
+        self.output_message(message, color_id=4)
diff --git a/sharpy/utils/generate_cases.py b/sharpy/utils/generate_cases.py
index a5299e116..4d726eaa4 100644
--- a/sharpy/utils/generate_cases.py
+++ b/sharpy/utils/generate_cases.py
@@ -32,6 +32,8 @@
 import sharpy.structure.utils.lagrangeconstraints as lagrangeconstraints
 import sharpy.utils.cout_utils as cout
 
+from sharpy.structure.utils.lagrangeconstraintsjax import DICT_OF_LC
+
 
 if not cout.check_running_unittest():
     cout.cout_wrap.print_screen = True
@@ -270,7 +272,7 @@ def get_aoacl0_from_camber(x, y):
     f1 = 1./(np.pi*(1-xc)*np.sqrt(xc*(1-xc)))
     int = yc*f1
 
-    return -scipy.integrate.trapz(int, xc)
+    return -scipy.integrate.trapezoid(int, xc)
 
 
 def get_mu0_from_camber(x, y):
@@ -1992,13 +1994,15 @@ def check(self):
             raise RuntimeError(("'behaviour' parameter is required in '%s' lagrange constraint" % self.behaviour))
 
 
-def generate_multibody_file(list_LagrangeConstraints, list_Bodies, route, case_name):
+def generate_multibody_file(list_LagrangeConstraints, list_Bodies, route, case_name, use_jax=False):
 
     # Check
     for body in list_Bodies:
         body.check()
-    for lc in list_LagrangeConstraints:
-        lc.check()
+
+    if not use_jax:
+        for lc in list_LagrangeConstraints:
+            lc.check()
 
     with h5.File(route + '/' + case_name + '.mb.h5', 'a') as h5file:
 
@@ -2013,27 +2017,34 @@ def generate_multibody_file(list_LagrangeConstraints, list_Bodies, route, case_n
             # Write general parameters
             constraint_id.create_dataset("behaviour", data=constraint.behaviour.encode('ascii', 'ignore'))
 
-            # Write parameters associated to the specific type of boundary condition
-            default = lagrangeconstraints.lc_from_string(constraint.behaviour)
-            default.__init__(default)
-            required_parameters = default.required_parameters
+            # I branch depending on if you use the JAX solver or not
+            # this is beneficial where a constraint is implemented in one solver but not the other
+            if use_jax:
+                required_parameters = DICT_OF_LC[constraint.behaviour].required_params
+            else:
+                # Write parameters associated to the specific type of boundary condition
+                default = lagrangeconstraints.lc_from_string(constraint.behaviour)
+                default.__init__(default)
+                required_parameters = default.required_parameters
             for param in required_parameters:
                 constraint_id.create_dataset(param, data=getattr(constraint, param))
             try:
-                constraint_id.create_dataset("scalingFactor",
-                                             data=getattr(constraint, "scalingFactor"))
-            except:
+                constraint_id.create_dataset("scalingFactor", data=constraint.scalingFactor)
+            except AttributeError:
                 pass
             try:
-                constraint_id.create_dataset("penaltyFactor",
-                                             data=getattr(constraint, "penaltyFactor"))
-            except:
+                constraint_id.create_dataset("penaltyFactor", data=constraint.penaltyFactor)
+            except AttributeError:
                 pass
             try:
-                constraint_id.create_dataset("rot_axisA2",
-                                             data=getattr(constraint, "rot_axisA2"))
-            except:
+                constraint_id.create_dataset("aerogrid_warp_factor", data=constraint.aerogrid_warp_factor)
+            except AttributeError:
+                pass
+            try:
+                constraint_id.create_dataset("rot_axisA2", data=constraint.rot_axisA2)
+            except AttributeError:
                 pass
+
             iconstraint += 1
 
         # Write the body information
diff --git a/sharpy/utils/h5utils.py b/sharpy/utils/h5utils.py
index 5f3549f83..1f335a6b7 100644
--- a/sharpy/utils/h5utils.py
+++ b/sharpy/utils/h5utils.py
@@ -59,7 +59,6 @@ def load_attributes(handle, path):
 
 def check_fem_dict(fem_dict):
     print('\tRunning tests for the FEM input file...', end='')
-    # import pdb; pdb.set_trace()
     (num_elem_dict, num_node_elem_dict) = np.shape(fem_dict['connectivities'])
     num_elem = fem_dict['num_elem']
     num_node_elem = fem_dict['num_node_elem']
diff --git a/sharpy/utils/multibodyjax.py b/sharpy/utils/multibodyjax.py
new file mode 100644
index 000000000..a10d6c596
--- /dev/null
+++ b/sharpy/utils/multibodyjax.py
@@ -0,0 +1,325 @@
+"""
+Multibody library for the NonlinearDynamicMultibodyJAX solver
+Library used to manipulate multibody systems
+To use this library: import sharpy.utils.multibodyjax as mb
+
+"""
+import numpy as np
+import sharpy.structure.utils.xbeamlib as xbeamlib
+import ctypes as ct
+
+
+def split_multibody(beam, tstep, mb_data_dict, ts):
+    """
+    split_multibody
+
+    This functions splits a structure at a certain time step in its different bodies
+
+    Args:
+    	beam (:class:`~sharpy.structure.models.beam.Beam`): structural information of the multibody system
+    	tstep (:class:`~sharpy.utils.datastructures.StructTimeStepInfo`): timestep information of the multibody system
+        mb_data_dict (dict): Dictionary including the multibody information
+        ts (int): time step number
+
+    Returns:
+        MB_beam (list(:class:`~sharpy.structure.models.beam.Beam`)): each entry represents a body
+        MB_tstep (list(:class:`~sharpy.utils.datastructures.StructTimeStepInfo`)): each entry represents a body
+    """
+
+    MB_beam = []
+    MB_tstep = []
+
+    quat0 = tstep.quat.astype(dtype=ct.c_double, order='F', copy=True)
+    for0_pos = tstep.for_pos.astype(dtype=ct.c_double, order='F', copy=True)
+    for0_vel = tstep.for_vel.astype(dtype=ct.c_double, order='F', copy=True)
+
+    ini_quat0 = beam.ini_info.quat.astype(dtype=ct.c_double, order='F', copy=True)
+    ini_for0_pos = beam.ini_info.for_pos.astype(dtype=ct.c_double, order='F', copy=True)
+    ini_for0_vel = beam.ini_info.for_vel.astype(dtype=ct.c_double, order='F', copy=True)
+
+    for ibody in range(beam.num_bodies):
+        ibody_beam = beam.get_body(ibody=ibody)
+        ibody_tstep = tstep.get_body(beam, ibody_beam.num_dof, ibody=ibody)
+
+        ibody_beam.FoR_movement = mb_data_dict['body_%02d' % ibody]['FoR_movement']
+
+        ibody_beam.ini_info.compute_psi_local_AFoR(ini_for0_pos, ini_for0_vel, ini_quat0)
+        ibody_beam.ini_info.change_to_local_AFoR(ini_for0_pos, ini_for0_vel, ini_quat0)
+        if ts == 1:
+            ibody_tstep.compute_psi_local_AFoR(for0_pos, for0_vel, quat0)
+        ibody_tstep.change_to_local_AFoR(for0_pos, for0_vel, quat0)
+
+        MB_beam.append(ibody_beam)
+        MB_tstep.append(ibody_tstep)
+
+    return MB_beam, MB_tstep
+
+
+def merge_multibody(MB_tstep, MB_beam, beam, tstep, mb_data_dict, dt):
+    """
+    merge_multibody
+
+    This functions merges a series of bodies into a multibody system at a certain time step
+
+    Longer description
+
+    Args:
+        MB_beam (list(:class:`~sharpy.structure.models.beam.Beam`)): each entry represents a body
+        MB_tstep (list(:class:`~sharpy.utils.datastructures.StructTimeStepInfo`)): each entry represents a body
+    	beam (:class:`~sharpy.structure.models.beam.Beam`): structural information of the multibody system
+    	tstep (:class:`~sharpy.utils.datastructures.StructTimeStepInfo`): timestep information of the multibody system
+        mb_data_dict (dict): Dictionary including the multibody information
+        dt(int): time step
+
+    Returns:
+        beam (:class:`~sharpy.structure.models.beam.Beam`): structural information of the multibody system
+    	tstep (:class:`~sharpy.utils.datastructures.StructTimeStepInfo`): timestep information of the multibody system
+    """
+
+    update_mb_dB_before_merge(tstep, MB_tstep)
+
+    quat0 = MB_tstep[0].quat.astype(dtype=ct.c_double, order='F', copy=True)
+    for0_pos = MB_tstep[0].for_pos.astype(dtype=ct.c_double, order='F', copy=True)
+    for0_vel = MB_tstep[0].for_vel.astype(dtype=ct.c_double, order='F', copy=True)
+
+    for ibody in range(beam.num_bodies):
+        MB_tstep[ibody].change_to_global_AFoR(for0_pos, for0_vel, quat0)
+
+    first_dof = 0
+    for ibody in range(beam.num_bodies):
+        # Renaming for clarity
+        ibody_elems = MB_beam[ibody].global_elems_num
+        ibody_nodes = MB_beam[ibody].global_nodes_num
+
+        # Merge tstep
+        tstep.pos[ibody_nodes, :] = MB_tstep[ibody].pos.astype(dtype=ct.c_double, order='F', copy=True)
+        tstep.pos_dot[ibody_nodes, :] = MB_tstep[ibody].pos_dot.astype(dtype=ct.c_double, order='F', copy=True)
+        tstep.pos_ddot[ibody_nodes, :] = MB_tstep[ibody].pos_ddot.astype(dtype=ct.c_double, order='F', copy=True)
+        tstep.psi[ibody_elems, :, :] = MB_tstep[ibody].psi.astype(dtype=ct.c_double, order='F', copy=True)
+        tstep.psi_local[ibody_elems, :, :] = MB_tstep[ibody].psi_local.astype(dtype=ct.c_double, order='F', copy=True)
+        tstep.psi_dot[ibody_elems, :, :] = MB_tstep[ibody].psi_dot.astype(dtype=ct.c_double, order='F', copy=True)
+        tstep.psi_dot_local[ibody_elems, :, :] = MB_tstep[ibody].psi_dot_local.astype(dtype=ct.c_double, order='F',
+                                                                                      copy=True)
+        tstep.psi_ddot[ibody_elems, :, :] = MB_tstep[ibody].psi_ddot.astype(dtype=ct.c_double, order='F', copy=True)
+        tstep.gravity_forces[ibody_nodes, :] = MB_tstep[ibody].gravity_forces.astype(dtype=ct.c_double, order='F',
+                                                                                     copy=True)
+        tstep.steady_applied_forces[ibody_nodes, :] = MB_tstep[ibody].steady_applied_forces.astype(dtype=ct.c_double,
+                                                                                                   order='F', copy=True)
+        tstep.unsteady_applied_forces[ibody_nodes, :] = MB_tstep[ibody].unsteady_applied_forces.astype(
+            dtype=ct.c_double, order='F', copy=True)
+        tstep.runtime_steady_forces[ibody_nodes, :] = MB_tstep[ibody].runtime_steady_forces.astype(dtype=ct.c_double,
+                                                                                                   order='F', copy=True)
+        tstep.runtime_unsteady_forces[ibody_nodes, :] = MB_tstep[ibody].runtime_unsteady_forces.astype(
+            dtype=ct.c_double, order='F', copy=True)
+        # TODO: Do I need a change in FoR for the following variables? Maybe for the FoR ones.
+        tstep.forces_constraints_nodes[ibody_nodes, :] = MB_tstep[ibody].forces_constraints_nodes.astype(
+            dtype=ct.c_double, order='F', copy=True)
+        tstep.forces_constraints_FoR[ibody, :] = MB_tstep[ibody].forces_constraints_FoR[ibody, :].astype(
+            dtype=ct.c_double, order='F', copy=True)
+
+        # Merge states
+        ibody_num_dof = MB_beam[ibody].num_dof.value
+        tstep.q[first_dof:first_dof + ibody_num_dof] = MB_tstep[ibody].q[:-10].astype(dtype=ct.c_double, order='F',
+                                                                                      copy=True)
+        tstep.dqdt[first_dof:first_dof + ibody_num_dof] = MB_tstep[ibody].dqdt[:-10].astype(dtype=ct.c_double,
+                                                                                            order='F', copy=True)
+        tstep.dqddt[first_dof:first_dof + ibody_num_dof] = MB_tstep[ibody].dqddt[:-10].astype(dtype=ct.c_double,
+                                                                                              order='F', copy=True)
+
+        tstep.mb_dquatdt[ibody, :] = MB_tstep[ibody].dqddt[-4:].astype(dtype=ct.c_double, order='F', copy=True)
+
+        first_dof += ibody_num_dof
+
+    tstep.q[-10:] = MB_tstep[0].q[-10:].astype(dtype=ct.c_double, order='F', copy=True)
+    tstep.dqdt[-10:] = MB_tstep[0].dqdt[-10:].astype(dtype=ct.c_double, order='F', copy=True)
+    tstep.dqddt[-10:] = MB_tstep[0].dqddt[-10:].astype(dtype=ct.c_double, order='F', copy=True)
+
+    # Define the new FoR information
+    tstep.for_pos = MB_tstep[0].for_pos.astype(dtype=ct.c_double, order='F', copy=True)
+    tstep.for_vel = MB_tstep[0].for_vel.astype(dtype=ct.c_double, order='F', copy=True)
+    tstep.for_acc = MB_tstep[0].for_acc.astype(dtype=ct.c_double, order='F', copy=True)
+    tstep.quat = MB_tstep[0].quat.astype(dtype=ct.c_double, order='F', copy=True)
+
+
+def update_mb_dB_before_merge(tstep, MB_tstep):
+    """
+    update_mb_db_before_merge
+
+    Updates the FoR information database before merging bodies
+
+    Args:
+    	tstep (:class:`~sharpy.utils.datastructures.StructTimeStepInfo`): timestep information of the multibody system
+        MB_tstep (list(:class:`~sharpy.utils.datastructures.StructTimeStepInfo`)): each entry represents a body
+    """
+
+    for ibody in range(len(MB_tstep)):
+        tstep.mb_FoR_pos[ibody, :] = MB_tstep[ibody].for_pos.astype(dtype=ct.c_double, order='F', copy=True)
+        tstep.mb_FoR_vel[ibody, :] = MB_tstep[ibody].for_vel.astype(dtype=ct.c_double, order='F', copy=True)
+        tstep.mb_FoR_acc[ibody, :] = MB_tstep[ibody].for_acc.astype(dtype=ct.c_double, order='F', copy=True)
+        tstep.mb_quat[ibody, :] = MB_tstep[ibody].quat.astype(dtype=ct.c_double, order='F', copy=True)
+        assert (MB_tstep[ibody].mb_dquatdt[ibody, :] == MB_tstep[ibody].dqddt[-4:]).all(), "Error in multibody storage"
+        tstep.mb_dquatdt[ibody, :] = MB_tstep[ibody].dqddt[-4:].astype(dtype=ct.c_double, order='F', copy=True)
+
+
+def disp_and_accel2state(MB_beam, MB_tstep, Lambda, Lambda_dot, sys_size, num_LM_eq):
+    """
+    disp2state
+
+    Fills the vector of states according to the displacements information
+
+    Args:
+        MB_beam (list(:class:`~sharpy.structure.models.beam.Beam`)): each entry represents a body
+        MB_tstep (list(:class:`~sharpy.utils.datastructures.StructTimeStepInfo`)): each entry represents a body
+        Lambda(np.ndarray): Lagrange multipliers of holonomic constraints
+        Lambda_dot(np.ndarray): Lagrange multipliers of non-holonomic constraints
+
+        sys_size(int): number of degrees of freedom of the system of equations not accounting for lagrange multipliers
+        num_LM_eq(int): Number of equations associated to the Lagrange Multipliers
+
+        q(np.ndarray): Vector of states
+    	dqdt(np.ndarray): Time derivatives of states
+        dqddt(np.ndarray): Second time derivatives of states
+    """
+    q = np.zeros((sys_size + num_LM_eq,), dtype=ct.c_double, order='F')
+    dqdt = np.zeros((sys_size + num_LM_eq,), dtype=ct.c_double, order='F')
+    dqddt = np.zeros((sys_size + num_LM_eq,), dtype=ct.c_double, order='F')
+
+    first_dof = 0
+    for ibody in range(len(MB_beam)):
+
+        ibody_num_dof = MB_beam[ibody].num_dof.value
+        if (MB_beam[ibody].FoR_movement == 'prescribed'):
+            MB_tstep[ibody].for_vel = np.zeros(6)
+            MB_tstep[ibody].for_acc = np.zeros(6)
+            MB_tstep[ibody].quat = MB_beam[ibody].ini_info.quat
+            xbeamlib.cbeam3_solv_disp2state(MB_beam[ibody], MB_tstep[ibody])
+            xbeamlib.cbeam3_solv_accel2state(MB_beam[ibody], MB_tstep[ibody])
+            q[first_dof:first_dof + ibody_num_dof] = MB_tstep[ibody].q[:-10].astype(dtype=ct.c_double, order='F',
+                                                                                    copy=True)
+            dqdt[first_dof:first_dof + ibody_num_dof] = MB_tstep[ibody].dqdt[:-10].astype(dtype=ct.c_double, order='F',
+                                                                                          copy=True)
+            dqddt[first_dof:first_dof + ibody_num_dof] = MB_tstep[ibody].dqddt[:-10].astype(dtype=ct.c_double,
+                                                                                            order='F', copy=True)
+            first_dof += ibody_num_dof
+
+        elif (MB_beam[ibody].FoR_movement == 'prescribed_trim'):
+            MB_tstep[ibody].for_vel = np.zeros(6)
+            MB_tstep[ibody].for_acc = np.zeros(6)
+            xbeamlib.cbeam3_solv_disp2state(MB_beam[ibody], MB_tstep[ibody])
+            xbeamlib.cbeam3_solv_accel2state(MB_beam[ibody], MB_tstep[ibody])
+            q[first_dof:first_dof + ibody_num_dof] = MB_tstep[ibody].q[:-10].astype(dtype=ct.c_double, order='F',
+                                                                                    copy=True)
+            dqdt[first_dof:first_dof + ibody_num_dof] = MB_tstep[ibody].dqdt[:-10].astype(dtype=ct.c_double, order='F',
+                                                                                          copy=True)
+            dqddt[first_dof:first_dof + ibody_num_dof] = MB_tstep[ibody].dqddt[:-10].astype(dtype=ct.c_double,
+                                                                                            order='F', copy=True)
+            first_dof += ibody_num_dof
+
+        elif (MB_beam[ibody].FoR_movement == 'free'):
+            dquatdt = MB_tstep[ibody].mb_dquatdt[ibody, :].astype(dtype=ct.c_double, order='F', copy=True)
+            xbeamlib.xbeam_solv_disp2state(MB_beam[ibody], MB_tstep[ibody])
+            xbeamlib.xbeam_solv_accel2state(MB_beam[ibody], MB_tstep[ibody])
+            q[first_dof:first_dof + ibody_num_dof + 10] = MB_tstep[ibody].q.astype(dtype=ct.c_double, order='F',
+                                                                                   copy=True)
+            dqdt[first_dof:first_dof + ibody_num_dof + 10] = MB_tstep[ibody].dqdt.astype(dtype=ct.c_double, order='F',
+                                                                                         copy=True)
+            dqddt[first_dof:first_dof + ibody_num_dof + 10] = MB_tstep[ibody].dqddt.astype(dtype=ct.c_double, order='F',
+                                                                                           copy=True)
+            dqddt[first_dof + ibody_num_dof + 6:first_dof + ibody_num_dof + 10] = dquatdt.astype(dtype=ct.c_double,
+                                                                                                 order='F', copy=True)
+            first_dof += ibody_num_dof + 10
+
+    if num_LM_eq > 0:
+        q[first_dof:] = Lambda.astype(dtype=ct.c_double, order='F', copy=True)
+        dqdt[first_dof:] = Lambda_dot.astype(dtype=ct.c_double, order='F', copy=True)
+
+    return q, dqdt, dqddt
+
+
+def state2disp_and_accel(q, dqdt, dqddt, MB_beam, MB_tstep, num_LM_eq):
+    """
+    state2disp
+
+    Recovers the displacements from the states
+
+    Longer description
+
+    Args:
+        MB_beam (list(:class:`~sharpy.structure.models.beam.Beam`)): each entry represents a body
+        MB_tstep (list(:class:`~sharpy.utils.datastructures.StructTimeStepInfo`)): each entry represents a body
+        q(np.ndarray): Vector of states
+    	dqdt(np.ndarray): Time derivatives of states
+        dqddt(np.ndarray): Second time derivatives of states
+
+        num_LM_eq(int): Number of equations associated to the Lagrange Multipliers
+
+        Lambda(np.ndarray): Lagrange multipliers of holonomic constraints
+        Lambda_dot(np.ndarray): Lagrange multipliers of non-holonomic constraints
+
+    """
+
+    first_dof = 0
+    for ibody in range(len(MB_beam)):
+
+        ibody_num_dof = MB_beam[ibody].num_dof.value
+        if (MB_beam[ibody].FoR_movement == 'prescribed'):
+            MB_tstep[ibody].q[:-10] = q[first_dof:first_dof + ibody_num_dof].astype(dtype=ct.c_double, order='F',
+                                                                                    copy=True)
+            MB_tstep[ibody].dqdt[:-10] = dqdt[first_dof:first_dof + ibody_num_dof].astype(dtype=ct.c_double, order='F',
+                                                                                          copy=True)
+            MB_tstep[ibody].dqddt[:-10] = dqddt[first_dof:first_dof + ibody_num_dof].astype(dtype=ct.c_double,
+                                                                                            order='F', copy=True)
+            xbeamlib.cbeam3_solv_state2disp(MB_beam[ibody], MB_tstep[ibody])
+            xbeamlib.cbeam3_solv_state2accel(MB_beam[ibody], MB_tstep[ibody])
+            first_dof += ibody_num_dof
+
+        elif (MB_beam[ibody].FoR_movement == 'prescribed_trim'):
+            MB_tstep[ibody].q[:-10] = q[first_dof:first_dof + ibody_num_dof].astype(dtype=ct.c_double, order='F',
+                                                                                    copy=True)
+            MB_tstep[ibody].dqdt[:-10] = dqdt[first_dof:first_dof + ibody_num_dof].astype(dtype=ct.c_double, order='F',
+                                                                                          copy=True)
+            MB_tstep[ibody].dqddt[:-10] = dqddt[first_dof:first_dof + ibody_num_dof].astype(dtype=ct.c_double,
+                                                                                            order='F', copy=True)
+            xbeamlib.cbeam3_solv_state2disp(MB_beam[ibody], MB_tstep[ibody])
+            xbeamlib.cbeam3_solv_state2accel(MB_beam[ibody], MB_tstep[ibody])
+            first_dof += ibody_num_dof
+
+        elif (MB_beam[ibody].FoR_movement == 'free'):
+            MB_tstep[ibody].q = q[first_dof:first_dof + ibody_num_dof + 10].astype(dtype=ct.c_double, order='F',
+                                                                                   copy=True)
+            MB_tstep[ibody].dqdt = dqdt[first_dof:first_dof + ibody_num_dof + 10].astype(dtype=ct.c_double, order='F',
+                                                                                         copy=True)
+            MB_tstep[ibody].dqddt = dqddt[first_dof:first_dof + ibody_num_dof + 10].astype(dtype=ct.c_double, order='F',
+                                                                                           copy=True)
+            MB_tstep[ibody].mb_dquatdt[ibody, :] = MB_tstep[ibody].dqddt[-4:]
+            xbeamlib.xbeam_solv_state2disp(MB_beam[ibody], MB_tstep[ibody])
+            xbeamlib.xbeam_solv_state2accel(MB_beam[ibody], MB_tstep[ibody])
+            first_dof += ibody_num_dof + 10
+
+    lm_h = q[first_dof:].astype(dtype=ct.c_double, order='F', copy=True)
+    lm_n = dqdt[first_dof:].astype(dtype=ct.c_double, order='F', copy=True)
+
+    return lm_h, lm_n
+
+
+def get_elems_nodes_list(beam, ibody):
+    """
+        get_elems_nodes_list
+
+        This function returns the elements (``ibody_elements``) and the nodes
+        (``ibody_nodes``) that belong to the body number ``ibody``
+
+        Args:
+    	   beam (:class:`~sharpy.structure.models.beam.Beam`): structural information of the multibody system
+           ibody (int): Body number about which the information is required
+
+        Returns:
+            ibody_elements (list): List of elements that belong the ``ibody``
+            ibody_nodes (list): List of nodes that belong the ``ibody``
+
+    """
+    int_list = np.arange(beam.num_elem)
+    ibody_elements = int_list[beam.body_number == ibody]
+    ibody_nodes = np.sort(np.unique(beam.connectivities[ibody_elements, :].reshape(-1)))
+
+    return ibody_elements, ibody_nodes
diff --git a/tests/coupled/multibody/double_prescribed_pendulum/test_double_prescribed_pendulum.py b/tests/coupled/multibody/double_prescribed_pendulum/test_double_prescribed_pendulum.py
new file mode 100644
index 000000000..57dfcbe76
--- /dev/null
+++ b/tests/coupled/multibody/double_prescribed_pendulum/test_double_prescribed_pendulum.py
@@ -0,0 +1,368 @@
+import numpy as np
+import os
+import shutil
+from matplotlib import pyplot as plt
+import unittest
+
+import sharpy.sharpy_main
+import sharpy.utils.algebra as ag
+import sharpy.utils.generate_cases as gc
+
+
+class TestDoublePrescribedPendulum(unittest.TestCase):
+    # this case is not convenient for using a setup method, as the second case definition depends upon the first
+    # we'll just shove it all in the run method
+    @staticmethod
+    def run_and_assert():
+        # Simulation inputs
+        case_name_free = 'free_double_pendulum_jax'
+        case_name_prescribed = 'prescribed_double_pendulum_jax'
+
+        try:
+            shutil.rmtree('' + str(os.path.dirname(os.path.realpath(__file__))) + '/cases/')
+        except:
+            pass
+
+        try:
+            shutil.rmtree('' + str(os.path.dirname(os.path.realpath(__file__))) + '/output/')
+        except:
+            pass
+
+        os.makedirs('' + str(os.path.dirname(os.path.realpath(__file__))) + '/cases/')
+        os.makedirs('' + str(os.path.dirname(os.path.realpath(__file__))) + '/output/')
+
+        case_route = os.path.abspath(os.path.dirname(os.path.realpath(__file__))) + '/cases/'
+        case_out_folder = case_route + '/output/'
+
+        total_time = 0.05  # physical time (s)
+        dt = 0.002  # time step length (s)
+        n_tstep = int(total_time / dt)  # number of time steps
+        hinge_ang = np.deg2rad(30.)
+
+        # Beam structural properties
+        beam_l = 0.5  # beam length (m)
+        beam_yz = 0.02  # cross-section width/height (m)
+        beam_A = beam_yz ** 2  # square cross-section area`
+        beam_rho = 2700.0  # beam material density (kg/m^3)
+        beam_m = beam_rho * beam_A  # beam mass per unit length (kg/m)
+        beam_iner = beam_m * beam_yz ** 2 / 6.  # beam inertia per unit length (kg m)
+
+        EA = 2.800e7  # axial stiffness
+        GA = 1.037e7  # shear stiffness
+        GJ = 6.914e2  # torsional stiffness
+        EI = 9.333e2  # bending stiffness
+
+        # Toggle on to make spaghetti
+        # GJ *= 5e-2
+        # EI *= 5e-2
+
+        # Airfoils (required as the NoAero solver still requires an aero grid)
+        airfoil = np.zeros((1, 20, 2))
+        airfoil[0, :, 0] = np.linspace(0., 1., 20)
+
+        # Beam 0
+        beam0_n_nodes = 11  # number of nodes
+        beam0_l = beam_l  # beam length
+        beam0_ml = 0.0  # lumped mass
+        beam0_theta_ini = np.deg2rad(90.)  # initial beam angle
+
+        # Beam 1
+        beam1_n_nodes = 11  # number of nodes
+        beam1_l = beam_l  # beam length
+        beam1_ml = 0.0  # lumped mass
+        beam1_theta_ini = np.deg2rad(90.)  # initial beam angle
+
+        for hinge_dir in ('y',):
+            # Create beam 0 structure
+            beam0 = gc.AeroelasticInformation()  # use aeroelastic as controllers implemented here
+            beam0_r = np.linspace(0., beam0_l, beam0_n_nodes)  # local node placement
+            beam0_pos_ini = np.zeros((beam0_n_nodes, 3))  # initial global node placement
+            match hinge_dir:
+                case 'x':
+                    beam0_pos_ini[:, 1] = beam0_r * np.sin(beam0_theta_ini)
+                    beam0_pos_ini[:, 2] = -beam0_r * np.cos(beam0_theta_ini)
+                    y_BFoR = 'x_AFoR'
+                case 'y':
+                    beam0_pos_ini[:, 0] = beam0_r * np.sin(beam0_theta_ini)
+                    beam0_pos_ini[:, 2] = -beam0_r * np.cos(beam0_theta_ini)
+                    y_BFoR = 'y_AFoR'
+                case _:
+                    raise KeyError
+
+            beam0.StructuralInformation.generate_uniform_beam(beam0_pos_ini, beam_m, beam_iner, beam_iner / 2.0,
+                                                              beam_iner / 2.0,
+                                                              np.zeros(3), EA, GA, GA, GJ, EI, EI,
+                                                              num_node_elem=3, y_BFoR=y_BFoR, num_lumped_mass=1)
+
+            beam0.StructuralInformation.body_number = np.zeros(beam0.StructuralInformation.num_elem, dtype=int)
+            beam0.StructuralInformation.boundary_conditions[0] = 1
+            beam0.StructuralInformation.boundary_conditions[-1] = -1
+            beam0.StructuralInformation.lumped_mass_nodes = np.array([beam0_n_nodes - 1], dtype=int)
+            beam0.StructuralInformation.lumped_mass = np.ones(1) * beam0_ml
+            beam0.StructuralInformation.lumped_mass_inertia = np.zeros((1, 3, 3))
+            beam0.StructuralInformation.lumped_mass_position = np.zeros((1, 3))
+            beam0.AerodynamicInformation.create_one_uniform_aerodynamics(
+                beam0.StructuralInformation, chord=1., twist=0., sweep=0.,
+                num_chord_panels=4, m_distribution='uniform', elastic_axis=0.25,
+                num_points_camber=20, airfoil=airfoil)
+
+            # Create beam 1 structure
+            beam1 = gc.AeroelasticInformation()
+            beam1_r = np.linspace(0., beam1_l, beam0_n_nodes)  # local node placement
+
+            beam1_pos_ini = np.zeros((beam0_n_nodes, 3))  # initial global node placement
+            match hinge_dir:
+                case 'x':
+                    beam1_pos_ini[:, 1] = beam1_r * np.sin(beam1_theta_ini) + beam0_pos_ini[-1, 1]
+                    beam1_pos_ini[:, 2] = -beam0_r * np.cos(beam1_theta_ini) + beam0_pos_ini[-1, 2]
+                case 'y':
+                    beam1_pos_ini[:, 0] = beam1_r * np.sin(beam1_theta_ini) + beam0_pos_ini[-1, 0]
+                    beam1_pos_ini[:, 2] = -beam1_r * np.cos(beam1_theta_ini) + beam0_pos_ini[-1, 2]
+                case _:
+                    raise KeyError
+
+            beam1.StructuralInformation.generate_uniform_beam(beam1_pos_ini, beam_m, beam_iner, beam_iner / 2.0,
+                                                              beam_iner / 2.0,
+                                                              np.zeros(3), EA, GA, GA, GJ, EI, EI,
+                                                              num_node_elem=3, y_BFoR=y_BFoR, num_lumped_mass=1)
+
+            beam1.StructuralInformation.body_number = np.zeros(beam1.StructuralInformation.num_elem, dtype=int)
+            beam1.StructuralInformation.boundary_conditions[0] = 1
+            beam1.StructuralInformation.boundary_conditions[-1] = -1
+            beam1.StructuralInformation.lumped_mass_nodes = np.array([beam1_n_nodes - 1], dtype=int)
+            beam1.StructuralInformation.lumped_mass = np.ones(1) * beam1_ml
+            beam1.StructuralInformation.lumped_mass_inertia = np.zeros((1, 3, 3))
+            beam1.StructuralInformation.lumped_mass_position = np.zeros((1, 3))
+            beam1.AerodynamicInformation.create_one_uniform_aerodynamics(
+                beam1.StructuralInformation, chord=1., twist=0., sweep=0.,
+                num_chord_panels=4, m_distribution='uniform', elastic_axis=0.25,
+                num_points_camber=20, airfoil=airfoil)
+
+            # Combine beam1 into beam0
+            beam0.assembly(beam1)
+
+            # Create the MB and BC parameters
+            # Free hinge
+            LC0_free = gc.LagrangeConstraint()
+            LC0_free.behaviour = 'hinge_FoR'
+            LC0_free.body_FoR = 0
+            match hinge_dir:
+                case 'x':
+                    LC0_free.rot_axis_AFoR = np.array((1., 0., 0.))
+                case 'y':
+                    LC0_free.rot_axis_AFoR = np.array((0., 1., 0.))
+            LC0_free.scalingFactor = dt ** -2
+
+            # Free hinge between beams
+            LC1_free = gc.LagrangeConstraint()
+            LC1_free.behaviour = 'hinge_node_FoR'
+            LC1_free.node_in_body = beam0_n_nodes - 1
+            LC1_free.body = 0
+            LC1_free.body_FoR = 1
+            match hinge_dir:
+                case 'x':
+                    LC1_free.rot_axisB = np.array((np.sin(hinge_ang), np.cos(hinge_ang), 0.))
+                    LC1_free.rot_axisA2 = np.array((np.cos(hinge_ang), np.sin(hinge_ang), 0.))
+                case 'y':
+                    LC1_free.rot_axisB = np.array((np.sin(hinge_ang), np.cos(hinge_ang), 0.))
+                    LC1_free.rot_axisA2 = np.array((np.sin(hinge_ang), np.cos(hinge_ang), 0.))
+            LC1_free.scalingFactor = dt ** -2
+
+            LC_free = [LC0_free, LC1_free]  # List of LCs
+
+            MB0 = gc.BodyInformation()
+            MB0.body_number = 0
+            MB0.FoR_position = np.zeros(6)
+            MB0.FoR_velocity = np.zeros(6)
+            MB0.FoR_acceleration = np.zeros(6)
+            MB0.FoR_movement = 'free'
+            MB0.quat = np.array([1., 0., 0., 0.])
+
+            MB1 = gc.BodyInformation()
+            MB1.body_number = 1
+            MB1.FoR_position = np.array([*beam1_pos_ini[0, :], 0., 0., 0.])
+            MB1.FoR_velocity = np.zeros(6)
+            MB1.FoR_acceleration = np.zeros(6)
+            MB1.FoR_movement = 'free'
+            MB1.quat = np.array((1., 0., 0., 0.))
+
+            MB = [MB0, MB1]  # List of MBs
+
+            # Simulation details
+            SimInfo = gc.SimulationInformation()
+            SimInfo.set_default_values()
+            SimInfo.with_forced_vel = False
+            SimInfo.with_dynamic_forces = False
+
+            SimInfo.solvers['SHARPy']['flow'] = [
+                'BeamLoader',
+                'AerogridLoader',
+                'DynamicCoupled'
+            ]
+
+            SimInfo.solvers['SHARPy']['case'] = case_name_free
+            SimInfo.solvers['SHARPy']['write_screen'] = False
+            SimInfo.solvers['SHARPy']['route'] = case_route
+            SimInfo.solvers['SHARPy']['log_folder'] = case_out_folder
+            SimInfo.set_variable_all_dicts('dt', dt)
+            SimInfo.define_num_steps(n_tstep)
+            SimInfo.set_variable_all_dicts('output', case_out_folder)
+
+            SimInfo.solvers['BeamLoader']['unsteady'] = 'on'
+
+            SimInfo.solvers['AerogridLoader']['unsteady'] = 'on'
+            SimInfo.solvers['AerogridLoader']['mstar'] = 2
+            SimInfo.solvers['AerogridLoader']['wake_shape_generator'] = 'StraightWake'
+            SimInfo.solvers['AerogridLoader']['wake_shape_generator_input'] = {'u_inf': 1.,
+                                                                               'u_inf_direction': np.array(
+                                                                                   (0., 1., 0.)),
+                                                                               'dt': dt}
+
+            SimInfo.solvers['NonLinearDynamicMultibodyJAX']['write_lm'] = False
+            SimInfo.solvers['NonLinearDynamicMultibodyJAX']['gravity_on'] = True
+            SimInfo.solvers['NonLinearDynamicMultibodyJAX']['gravity'] = 9.81
+            SimInfo.solvers['NonLinearDynamicMultibodyJAX']['time_integrator'] = 'NewmarkBetaJAX'
+            SimInfo.solvers['NonLinearDynamicMultibodyJAX']['time_integrator_settings'] = {'newmark_damp': 0.0,
+                                                                                           'dt': dt}
+
+            SimInfo.solvers['BeamPlot']['include_FoR'] = False
+
+            SimInfo.solvers['DynamicCoupled']['structural_solver'] = 'NonLinearDynamicMultibodyJAX'
+            SimInfo.solvers['DynamicCoupled']['structural_solver_settings'] = SimInfo.solvers[
+                'NonLinearDynamicMultibodyJAX']
+            SimInfo.solvers['DynamicCoupled']['aero_solver'] = 'NoAero'
+            SimInfo.solvers['DynamicCoupled']['aero_solver_settings'] = SimInfo.solvers['NoAero']
+            SimInfo.solvers['DynamicCoupled']['postprocessors'] = ['BeamPlot']
+            SimInfo.solvers['DynamicCoupled']['postprocessors_settings'] = {'BeamPlot': {}}
+
+            # Write files
+            gc.clean_test_files(case_route, case_name_free)
+            SimInfo.generate_solver_file()
+            SimInfo.generate_dyn_file(n_tstep)
+            beam0.generate_h5_files(case_route, case_name_free)
+            gc.generate_multibody_file(LC_free, MB, case_route, case_name_free, use_jax=True)
+
+            # Run SHARPy for free case
+            case_data_free = sharpy.sharpy_main.main(['', case_route + case_name_free + '.sharpy'])
+
+            # Calculate constraint angles
+            quat0 = [case_data_free.structure.timestep_info[i].mb_quat[0, :] for i in range(n_tstep)]
+            quat1 = [case_data_free.structure.timestep_info[i].mb_quat[1, :] for i in range(n_tstep)]
+            psi_ga = [case_data_free.structure.timestep_info[i].psi[int((beam0_n_nodes - 3) / 2), 1, :] for i in
+                      range(n_tstep)]
+
+            psi_a = [ag.quat2crv(quat0[i]) for i in range(n_tstep)]
+
+            psi_hg = [ag.rotation2crv(ag.quat2rotation(quat0[i])
+                                      @ ag.crv2rotation(psi_ga[i])
+                                      @ ag.quat2rotation(quat1[i]).T) for i in range(n_tstep)]
+
+            # Calculate constraint velocities
+            omega_a = [case_data_free.structure.timestep_info[i].mb_FoR_vel[0, 3:] for i in range(n_tstep)]
+            omega_h = [case_data_free.structure.timestep_info[i].mb_FoR_vel[1, 3:] for i in range(n_tstep)]
+            psi_ga_dot = [case_data_free.structure.timestep_info[i].psi_dot[int((beam0_n_nodes - 3) / 2), 1, :] for i in
+                          range(n_tstep)]
+
+            psi_a_dot = [np.linalg.solve(ag.crv2tan(psi_a[i]), omega_a[i]) for i in range(n_tstep)]
+
+            term1 = [-ag.crv2tan(psi_ga[i]) @ psi_ga_dot[i]
+                     - ag.crv2rotation(psi_ga[i]).T @ omega_a[i]
+                     + ag.crv2rotation(psi_ga[i]).T @ ag.quat2rotation(quat0[i]).T @ ag.quat2rotation(quat1[i]) @
+                     omega_h[i]
+                     for i in range(n_tstep)]
+
+            psi_hg_dot = [np.linalg.inv(ag.crv2tan(psi_hg[i])) @ ag.crv2rotation(psi_hg[i]) @ term1[i] for i in
+                          range(n_tstep)]
+
+            # Controller inputs
+            angle_input_file0 = case_out_folder + f'angle_input0_{hinge_dir}.npy'
+            angle_input_file1 = case_out_folder + f'angle_input1_{hinge_dir}.npy'
+            vel_input_file0 = case_out_folder + f'vel_input0_{hinge_dir}.npy'
+            vel_input_file1 = case_out_folder + f'vel_input1_{hinge_dir}.npy'
+
+            angle_input0 = np.array(psi_a)
+            angle_input1 = np.array(psi_hg)
+            vel_input0 = np.array(psi_a_dot)
+            vel_input1 = np.array(psi_hg_dot)
+
+            # Uncomment to stop prescribed pendulum tracking after a certain timestep (prove both cases aren't actually free!)
+            # angle_input1[int(n_tstep/2):, :] = angle_input1[int(n_tstep/2) - 1, :]
+            # vel_input1[int(n_tstep/2):, :] = 0.
+
+            np.save(angle_input_file0, angle_input0)
+            np.save(angle_input_file1, angle_input1)
+            np.save(vel_input_file0, vel_input0)
+            np.save(vel_input_file1, vel_input1)
+
+            SimInfo.define_num_steps(n_tstep - 1)
+
+            # Prescribe top joint angular velocity
+            LC0_prescribed = gc.LagrangeConstraint()
+            LC0_prescribed.behaviour = 'control_rot_vel_FoR'
+            LC0_prescribed.controller_id = 'controller0'
+            LC0_prescribed.body_FoR = 0
+            LC0_prescribed.scalingFactor = dt ** -2
+
+            # Actuated joint between beams
+            LC1_prescribed = gc.LagrangeConstraint()
+            LC1_prescribed.behaviour = 'control_node_FoR_rot_vel'
+            LC1_prescribed.controller_id = 'controller1'
+            LC1_prescribed.node_in_body = beam1_n_nodes - 1
+            LC1_prescribed.body = 0
+            LC1_prescribed.body_FoR = 1
+            LC1_prescribed.scalingFactor = dt ** -2
+            LC1_prescribed.rel_posB = np.zeros(3)
+
+            LC_prescribed = [LC0_prescribed, LC1_prescribed]  # List of LCs
+
+            SimInfo.solvers['SHARPy']['case'] = case_name_prescribed
+            SimInfo.solvers['DynamicCoupled']['controller_id'] = {
+                'controller0': 'MultibodyController',
+                'controller1': 'MultibodyController',
+            }
+            SimInfo.solvers['DynamicCoupled']['controller_settings']['controller0'] = {
+                'ang_history_input_file': angle_input_file0,
+                'ang_vel_history_input_file': vel_input_file0,
+                'dt': dt,
+            }
+            SimInfo.solvers['DynamicCoupled']['controller_settings']['controller1'] = {
+                'ang_history_input_file': angle_input_file1,
+                'ang_vel_history_input_file': vel_input_file1,
+                'dt': dt,
+            }
+
+            # Write files
+            gc.clean_test_files(case_route, case_name_prescribed)
+            SimInfo.generate_solver_file()
+            SimInfo.generate_dyn_file(n_tstep)
+            beam0.generate_h5_files(case_route, case_name_prescribed)
+            gc.generate_multibody_file(LC_prescribed, MB, case_route, case_name_prescribed, use_jax=True)
+
+            # Run SHARPy for prescribed case
+            case_data_prescribed = sharpy.sharpy_main.main(['', case_route + case_name_prescribed + '.sharpy'])
+
+            # compare (controller framework operates a timestep behind)
+            # comparing y tip displacement
+            pos_free = case_data_free.structure.timestep_info[-3].pos[-1, 1]
+            pos_prescribed = case_data_prescribed.structure.timestep_info[-1].pos[-1, 1]
+
+            diff = np.abs((pos_free / pos_prescribed) - 1.)
+
+            if diff > 1e-3:
+                raise ValueError(f"Free and prescribed pendulum results no not closely match: diff={diff}")
+
+
+    def test_double_prescribed_pendulum(self):
+        self.run_and_assert()
+
+    def tearDown(self):
+        solver_path = os.path.abspath(os.path.dirname(os.path.realpath(__file__))) + '/'
+
+        shutil.rmtree(solver_path + 'cases/')
+        shutil.rmtree(solver_path + 'output/')
+
+    def teardown_method(self):
+        pass
+
+if __name__ == '__main__':
+    unittest.main()
diff --git a/tests/coupled/multibody/double_slanted_pendulum/__init__.py b/tests/coupled/multibody/double_slanted_pendulum/__init__.py
new file mode 100644
index 000000000..e69de29bb
diff --git a/tests/coupled/multibody/test_double_pendulum_slanted.py b/tests/coupled/multibody/double_slanted_pendulum/test_double_pendulum_slanted.py
similarity index 93%
rename from tests/coupled/multibody/test_double_pendulum_slanted.py
rename to tests/coupled/multibody/double_slanted_pendulum/test_double_pendulum_slanted.py
index 1e2c8eb35..9af4f08d1 100644
--- a/tests/coupled/multibody/test_double_pendulum_slanted.py
+++ b/tests/coupled/multibody/double_slanted_pendulum/test_double_pendulum_slanted.py
@@ -255,27 +255,6 @@ def _setUp(self, lateral):
         beam1.generate_h5_files(SimInfo.solvers['SHARPy']['route'], SimInfo.solvers['SHARPy']['case'])
         gc.generate_multibody_file(LC, MB, SimInfo.solvers['SHARPy']['route'], SimInfo.solvers['SHARPy']['case'])
 
-        #     # Same case with spherical joints
-        #     global name_spherical
-        #     name_spherical = 'dpg_spherical'
-        #     SimInfo.solvers['SHARPy']['case'] = name_spherical
-
-        #     SimInfo.solvers['NonLinearDynamicMultibody']['time_integrator'] = 'NewmarkBeta'
-        #     SimInfo.solvers['NonLinearDynamicMultibody']['time_integrator_settings'] = {'newmark_damp': 0.15,
-        #                                                                                     'dt': dt}
-
-        #     LC1 = gc.LagrangeConstraint()
-        #     LC1.behaviour = 'spherical_FoR'
-        #     LC1.body_FoR = 0
-        #     LC1.scalingFactor = 1e6
-
-        #     LC2 = gc.LagrangeConstraint()
-        #     LC2.behaviour = 'spherical_node_FoR'
-        #     LC2.node_in_body = nnodes1-1
-        #     LC2.body = 0
-        #     LC2.body_FoR = 1
-        #     LC2.scalingFactor = 1e6
-
         gc.clean_test_files(SimInfo.solvers['SHARPy']['route'], SimInfo.solvers['SHARPy']['case'])
         SimInfo.generate_solver_file()
         SimInfo.generate_dyn_file(numtimesteps)
@@ -322,7 +301,7 @@ def test_doublependulum_hinge_slanted_lateralrot(self):
     def tearDown(self):
         solver_path = os.path.abspath(os.path.dirname(os.path.realpath(__file__)))
         solver_path += '/'
-        for name in [name_hinge_slanted, name_hinge_slanted_pen, name_hinge_slanted_lateralrot]:
+        for name in ['name_hinge_slanted', 'name_hinge_slanted_pen', 'name_hinge_slanted_lateralrot']:
             files_to_delete = [name + '.aero.h5',
                                name + '.dyn.h5',
                                name + '.fem.h5',
diff --git a/tests/coupled/static/smith_g_2deg/generate_smith_g_2deg.py b/tests/coupled/static/smith_g_2deg/generate_smith_g_2deg.py
index c0630ae26..8af8b1800 100644
--- a/tests/coupled/static/smith_g_2deg/generate_smith_g_2deg.py
+++ b/tests/coupled/static/smith_g_2deg/generate_smith_g_2deg.py
@@ -67,14 +67,19 @@ def generate_fem_file():
     x = np.zeros((num_node, ))
     y = np.zeros((num_node, ))
     z = np.zeros((num_node, ))
+
     # struct twist
     structural_twist = np.zeros((num_elem, num_node_elem))
+
     # beam number
     beam_number = np.zeros((num_elem, ), dtype=int)
+
     # frame of reference delta
     frame_of_reference_delta = np.zeros((num_elem, num_node_elem, 3))
+
     # connectivities
     conn = np.zeros((num_elem, num_node_elem), dtype=int)
+
     # stiffness
     num_stiffness = 1
     ea = 1e5
@@ -87,6 +92,7 @@ def generate_fem_file():
     stiffness = np.zeros((num_stiffness, 6, 6))
     stiffness[0, :, :] = main_sigma*base_stiffness
     elem_stiffness = np.zeros((num_elem,), dtype=int)
+
     # mass
     num_mass = 1
     m_base = 0.75
@@ -95,30 +101,19 @@ def generate_fem_file():
     mass = np.zeros((num_mass, 6, 6))
     mass[0, :, :] = base_mass
     elem_mass = np.zeros((num_elem,), dtype=int)
+
     # boundary conditions
     boundary_conditions = np.zeros((num_node, ), dtype=int)
     boundary_conditions[0] = 1
+
     # applied forces
-    # n_app_forces = 2
-    # node_app_forces = np.zeros((n_app_forces,), dtype=int)
     app_forces = np.zeros((num_node, 6))
 
-    spacing_param = 4
-
     # right wing (beam 0) --------------------------------------------------------------
     working_elem = 0
     working_node = 0
     beam_number[working_elem:working_elem + num_elem_main] = 0
-    domain = np.linspace(0, 1.0, num_node_main)
-    # 16 - (np.geomspace(20, 4, 10) - 4)
-    # x[working_node:working_node + num_node_main] = np.sin(sweep)*(main_span - (np.geomspace(main_span + spacing_param,
-    #                                                                                         0 + spacing_param,
-    #                                                                                         num_node_main)
-    #                                                                            - spacing_param))
-    # y[working_node:working_node + num_node_main] = np.abs(np.cos(sweep)*(main_span - (np.geomspace(main_span + spacing_param,
-    #                                                                                         0 + spacing_param,
-    #                                                                                         num_node_main)
-    #                                                                            - spacing_param)))
+
     y[0] = 0
     y[working_node:working_node + num_node_main] = np.cos(sweep)*np.linspace(0.0, main_span, num_node_main)
     x[working_node:working_node + num_node_main] = np.sin(sweep)*np.linspace(0.0, main_span, num_node_main)
@@ -142,14 +137,7 @@ def generate_fem_file():
     tempy = np.linspace(-main_span, 0.0, num_node_main)
     x[working_node:working_node + num_node_main - 1] = -np.sin(sweep)*tempy[0:-1]
     y[working_node:working_node + num_node_main - 1] = np.cos(sweep)*tempy[0:-1]
-    # x[working_node:working_node + num_node_main - 1] = -np.sin(sweep)*(main_span - (np.geomspace(0 + spacing_param,
-    #                                                                                         main_span + spacing_param,
-    #                                                                                         num_node_main)[:-1]
-    #                                                                            - spacing_param))
-    # y[working_node:working_node + num_node_main - 1] = -np.abs(np.cos(sweep)*(main_span - (np.geomspace(0 + spacing_param,
-    #                                                                                                main_span + spacing_param,
-    #                                                                                                num_node_main)[:-1]
-    #                                                                                   - spacing_param)))
+
     for ielem in range(num_elem_main):
         for inode in range(num_node_elem):
             frame_of_reference_delta[working_elem + ielem, inode, :] = [-1, 0, 0]
@@ -208,6 +196,7 @@ def generate_aero_file():
 
     working_elem = 0
     working_node = 0
+
     # right wing (surface 0, beam 0)
     i_surf = 0
     airfoil_distribution[working_elem:working_elem + num_elem_main, :] = 0
@@ -219,15 +208,13 @@ def generate_aero_file():
     working_elem += num_elem_main
     working_node += num_node_main
 
-    # left wing (surface 1, beam 1)
+    # right wing (surface 1, beam 1)
     i_surf = 1
-    # airfoil_distribution[working_node:working_node + num_node_main - 1] = 0
     airfoil_distribution[working_elem:working_elem + num_elem_main, :] = 0
     surface_distribution[working_elem:working_elem + num_elem_main] = i_surf
     surface_m[i_surf] = m_main
     aero_node[working_node:working_node + num_node_main - 1] = True
-    # chord[working_node:working_node + num_node_main - 1] = main_chord
-    # elastic_axis[working_node:working_node + num_node_main - 1] = main_ea
+
     working_elem += num_elem_main
     working_node += num_node_main - 1
 
@@ -274,14 +261,14 @@ def naca(x, m, p):
 
 def generate_solver_file(horseshoe=False):
     file_name = route + '/' + case_name + '.sharpy'
-    # config = configparser.ConfigParser()
+
     import configobj
     config = configobj.ConfigObj()
     config.filename = file_name
     config['SHARPy'] = {'case': case_name,
                         'route': route,
-                        'flow': ['BeamLoader', 'AerogridLoader', 'StaticCoupled', 'AerogridPlot', 'BeamPlot', 'AeroForcesCalculator', 'WriteVariablesTime'],
-                        # 'flow': ['BeamLoader', 'NonLinearStatic', 'BeamPlot'],
+                        'flow': ['BeamLoader', 'AerogridLoader', 'StaticCoupled',
+                                 'AeroForcesCalculator', 'WriteVariablesTime'],
                         'write_screen': 'off',
                         'write_log': 'on',
                         'log_folder': route + '/output/',
@@ -312,8 +299,6 @@ def generate_solver_file(horseshoe=False):
                                                                                  'u_inf_direction': [1., 0, 0]},
                                                         'rho': rho},
                                'max_iter': 50,
-                               # 'n_load_steps': 1,
-                               # 'n_load_steps': 5,
                                'tolerance': 1e-9,
                                'relaxation_factor': 0.0}
     config['WriteVariablesTime'] = {'cleanup_old_solution': 'on',
@@ -338,16 +323,10 @@ def generate_solver_file(horseshoe=False):
                                     'wake_shape_generator_input': {'u_inf': u_inf,
                                                                    'u_inf_direction': np.array([1., 0., 0.]),
                                                                    'dt': main_chord/m_main/u_inf}}
-    config['AerogridPlot'] = {'include_rbm': 'off',
-                              'include_applied_forces': 'on',
-                              'minus_m_star': 0
-                              }
     config['AeroForcesCalculator'] = {'write_text_file': 'on',
                                       'text_file_name': case_name + '_aeroforces.csv',
                                       'screen_output': 'on',
                                       }
-    config['BeamPlot'] = {'include_rbm': 'off',
-                          'include_applied_forces': 'on'}
     config.write()
 
 
diff --git a/tests/coupled/static/smith_g_4deg/generate_smith_g_4deg.py b/tests/coupled/static/smith_g_4deg/generate_smith_g_4deg.py
index 28a516974..45ba45b31 100644
--- a/tests/coupled/static/smith_g_4deg/generate_smith_g_4deg.py
+++ b/tests/coupled/static/smith_g_4deg/generate_smith_g_4deg.py
@@ -66,14 +66,19 @@ def generate_fem_file():
     x = np.zeros((num_node, ))
     y = np.zeros((num_node, ))
     z = np.zeros((num_node, ))
+
     # struct twist
     structural_twist = np.zeros((num_elem, num_node_elem))
+
     # beam number
     beam_number = np.zeros((num_elem, ), dtype=int)
+
     # frame of reference delta
     frame_of_reference_delta = np.zeros((num_elem, num_node_elem, 3))
+
     # connectivities
     conn = np.zeros((num_elem, num_node_elem), dtype=int)
+
     # stiffness
     num_stiffness = 1
     ea = 1e5
@@ -86,6 +91,7 @@ def generate_fem_file():
     stiffness = np.zeros((num_stiffness, 6, 6))
     stiffness[0, :, :] = main_sigma*base_stiffness
     elem_stiffness = np.zeros((num_elem,), dtype=int)
+
     # mass
     num_mass = 1
     m_base = 0.75
@@ -94,12 +100,11 @@ def generate_fem_file():
     mass = np.zeros((num_mass, 6, 6))
     mass[0, :, :] = base_mass
     elem_mass = np.zeros((num_elem,), dtype=int)
+
     # boundary conditions
     boundary_conditions = np.zeros((num_node, ), dtype=int)
     boundary_conditions[0] = 1
-    # applied forces
-    # n_app_forces = 2
-    # node_app_forces = np.zeros((n_app_forces,), dtype=int)
+
     app_forces = np.zeros((num_node, 6))
 
     spacing_param = 4
@@ -108,16 +113,7 @@ def generate_fem_file():
     working_elem = 0
     working_node = 0
     beam_number[working_elem:working_elem + num_elem_main] = 0
-    domain = np.linspace(0, 1.0, num_node_main)
-    # 16 - (np.geomspace(20, 4, 10) - 4)
-    # x[working_node:working_node + num_node_main] = np.sin(sweep)*(main_span - (np.geomspace(main_span + spacing_param,
-    #                                                                                         0 + spacing_param,
-    #                                                                                         num_node_main)
-    #                                                                            - spacing_param))
-    # y[working_node:working_node + num_node_main] = np.abs(np.cos(sweep)*(main_span - (np.geomspace(main_span + spacing_param,
-    #                                                                                         0 + spacing_param,
-    #                                                                                         num_node_main)
-    #                                                                            - spacing_param)))
+
     y[0] = 0
     y[working_node:working_node + num_node_main] = np.cos(sweep)*np.linspace(0.0, main_span, num_node_main)
     x[working_node:working_node + num_node_main] = np.sin(sweep)*np.linspace(0.0, main_span, num_node_main)
@@ -137,18 +133,10 @@ def generate_fem_file():
 
     # left wing (beam 1) --------------------------------------------------------------
     beam_number[working_elem:working_elem + num_elem_main] = 1
-    domain = np.linspace(-1.0, 0.0, num_node_main)
     tempy = np.linspace(-main_span, 0.0, num_node_main)
     x[working_node:working_node + num_node_main - 1] = -np.sin(sweep)*tempy[0:-1]
     y[working_node:working_node + num_node_main - 1] = np.cos(sweep)*tempy[0:-1]
-    # x[working_node:working_node + num_node_main - 1] = -np.sin(sweep)*(main_span - (np.geomspace(0 + spacing_param,
-    #                                                                                         main_span + spacing_param,
-    #                                                                                         num_node_main)[:-1]
-    #                                                                            - spacing_param))
-    # y[working_node:working_node + num_node_main - 1] = -np.abs(np.cos(sweep)*(main_span - (np.geomspace(0 + spacing_param,
-    #                                                                                                main_span + spacing_param,
-    #                                                                                                num_node_main)[:-1]
-    #                                                                                   - spacing_param)))
+
     for ielem in range(num_elem_main):
         for inode in range(num_node_elem):
             frame_of_reference_delta[working_elem + ielem, inode, :] = [-1, 0, 0]
@@ -190,8 +178,6 @@ def generate_fem_file():
             'beam_number', data=beam_number)
         app_forces_handle = h5file.create_dataset(
             'app_forces', data=app_forces)
-        # node_app_forces_handle = h5file.create_dataset(
-        #     'node_app_forces', data=node_app_forces)
 
 
 def generate_aero_file():
@@ -207,6 +193,7 @@ def generate_aero_file():
 
     working_elem = 0
     working_node = 0
+
     # right wing (surface 0, beam 0)
     i_surf = 0
     airfoil_distribution[working_elem:working_elem + num_elem_main, :] = 0
@@ -224,8 +211,7 @@ def generate_aero_file():
     surface_distribution[working_elem:working_elem + num_elem_main] = i_surf
     surface_m[i_surf] = m_main
     aero_node[working_node:working_node + num_node_main - 1] = True
-    # chord[working_node:working_node + num_node_main - 1] = main_chord
-    # elastic_axis[working_node:working_node + num_node_main - 1] = main_ea
+
     working_elem += num_elem_main
     working_node += num_node_main - 1
 
@@ -272,14 +258,14 @@ def naca(x, m, p):
 
 def generate_solver_file(horseshoe=False):
     file_name = route + '/' + case_name + '.sharpy'
-    # config = configparser.ConfigParser()
+
     import configobj
     config = configobj.ConfigObj()
     config.filename = file_name
     config['SHARPy'] = {'case': case_name,
                         'route': route,
-                        'flow': ['BeamLoader', 'AerogridLoader', 'StaticCoupled', 'AerogridPlot', 'BeamPlot', 'AeroForcesCalculator', 'WriteVariablesTime'],
-                        # 'flow': ['BeamLoader', 'NonLinearStatic', 'BeamPlot'],
+                        'flow': ['BeamLoader', 'AerogridLoader', 'StaticCoupled',
+                                 'AeroForcesCalculator', 'WriteVariablesTime'],
                         'write_screen': 'off',
                         'write_log': 'on',
                         'log_folder': route + '/output/',
@@ -336,16 +322,11 @@ def generate_solver_file(horseshoe=False):
                                                                    'u_inf_direction': np.array([1., 0., 0.]),
                                                                    'dt': main_chord/m_main/u_inf}}
 
-    config['AerogridPlot'] = {'include_rbm': 'off',
-                              'include_applied_forces': 'on',
-                              'minus_m_star': 0
-                              }
     config['AeroForcesCalculator'] = {'write_text_file': 'on',
                                       'text_file_name': case_name + '_aeroforces.csv',
                                       'screen_output': 'on',
                                       }
-    config['BeamPlot'] = {'include_rbm': 'off',
-                          'include_applied_forces': 'on'}
+
     config.write()
 
 
diff --git a/tests/coupled/static/smith_nog_2deg/generate_smith_nog_2deg.py b/tests/coupled/static/smith_nog_2deg/generate_smith_nog_2deg.py
index f824569c6..9c2fb93d3 100644
--- a/tests/coupled/static/smith_nog_2deg/generate_smith_nog_2deg.py
+++ b/tests/coupled/static/smith_nog_2deg/generate_smith_nog_2deg.py
@@ -66,14 +66,19 @@ def generate_fem_file():
     x = np.zeros((num_node, ))
     y = np.zeros((num_node, ))
     z = np.zeros((num_node, ))
+
     # struct twist
     structural_twist = np.zeros((num_elem, num_node_elem))
+
     # beam number
     beam_number = np.zeros((num_elem, ), dtype=int)
+
     # frame of reference delta
     frame_of_reference_delta = np.zeros((num_elem, num_node_elem, 3))
+
     # connectivities
     conn = np.zeros((num_elem, num_node_elem), dtype=int)
+
     # stiffness
     num_stiffness = 1
     ea = 1e5
@@ -86,6 +91,7 @@ def generate_fem_file():
     stiffness = np.zeros((num_stiffness, 6, 6))
     stiffness[0, :, :] = main_sigma*base_stiffness
     elem_stiffness = np.zeros((num_elem,), dtype=int)
+
     # mass
     num_mass = 1
     m_base = 0.75
@@ -94,30 +100,19 @@ def generate_fem_file():
     mass = np.zeros((num_mass, 6, 6))
     mass[0, :, :] = base_mass
     elem_mass = np.zeros((num_elem,), dtype=int)
+
     # boundary conditions
     boundary_conditions = np.zeros((num_node, ), dtype=int)
     boundary_conditions[0] = 1
+
     # applied forces
-    # n_app_forces = 2
-    # node_app_forces = np.zeros((n_app_forces,), dtype=int)
     app_forces = np.zeros((num_node, 6))
 
-    spacing_param = 4
-
     # right wing (beam 0) --------------------------------------------------------------
     working_elem = 0
     working_node = 0
     beam_number[working_elem:working_elem + num_elem_main] = 0
-    domain = np.linspace(0, 1.0, num_node_main)
-    # 16 - (np.geomspace(20, 4, 10) - 4)
-    # x[working_node:working_node + num_node_main] = np.sin(sweep)*(main_span - (np.geomspace(main_span + spacing_param,
-    #                                                                                         0 + spacing_param,
-    #                                                                                         num_node_main)
-    #                                                                            - spacing_param))
-    # y[working_node:working_node + num_node_main] = np.abs(np.cos(sweep)*(main_span - (np.geomspace(main_span + spacing_param,
-    #                                                                                         0 + spacing_param,
-    #                                                                                         num_node_main)
-    #                                                                            - spacing_param)))
+
     y[0] = 0
     y[working_node:working_node + num_node_main] = np.cos(sweep)*np.linspace(0.0, main_span, num_node_main)
     x[working_node:working_node + num_node_main] = np.sin(sweep)*np.linspace(0.0, main_span, num_node_main)
@@ -137,18 +132,10 @@ def generate_fem_file():
 
     # left wing (beam 1) --------------------------------------------------------------
     beam_number[working_elem:working_elem + num_elem_main] = 1
-    domain = np.linspace(-1.0, 0.0, num_node_main)
     tempy = np.linspace(-main_span, 0.0, num_node_main)
     x[working_node:working_node + num_node_main - 1] = -np.sin(sweep)*tempy[0:-1]
     y[working_node:working_node + num_node_main - 1] = np.cos(sweep)*tempy[0:-1]
-    # x[working_node:working_node + num_node_main - 1] = -np.sin(sweep)*(main_span - (np.geomspace(0 + spacing_param,
-    #                                                                                         main_span + spacing_param,
-    #                                                                                         num_node_main)[:-1]
-    #                                                                            - spacing_param))
-    # y[working_node:working_node + num_node_main - 1] = -np.abs(np.cos(sweep)*(main_span - (np.geomspace(0 + spacing_param,
-    #                                                                                                main_span + spacing_param,
-    #                                                                                                num_node_main)[:-1]
-    #                                                                                   - spacing_param)))
+
     for ielem in range(num_elem_main):
         for inode in range(num_node_elem):
             frame_of_reference_delta[working_elem + ielem, inode, :] = [-1, 0, 0]
@@ -190,8 +177,6 @@ def generate_fem_file():
             'beam_number', data=beam_number)
         app_forces_handle = h5file.create_dataset(
             'app_forces', data=app_forces)
-        # node_app_forces_handle = h5file.create_dataset(
-        #     'node_app_forces', data=node_app_forces)
 
 
 def generate_aero_file():
@@ -220,13 +205,10 @@ def generate_aero_file():
 
     # left wing (surface 1, beam 1)
     i_surf = 1
-    # airfoil_distribution[working_node:working_node + num_node_main - 1] = 0
     airfoil_distribution[working_elem:working_elem + num_elem_main, :] = 0
     surface_distribution[working_elem:working_elem + num_elem_main] = i_surf
     surface_m[i_surf] = m_main
     aero_node[working_node:working_node + num_node_main - 1] = True
-    # chord[working_node:working_node + num_node_main - 1] = main_chord
-    # elastic_axis[working_node:working_node + num_node_main - 1] = main_ea
     working_elem += num_elem_main
     working_node += num_node_main - 1
 
@@ -279,8 +261,8 @@ def generate_solver_file(horseshoe=False):
     config.filename = file_name
     config['SHARPy'] = {'case': case_name,
                         'route': route,
-                        'flow': ['BeamLoader', 'AerogridLoader', 'StaticCoupled', 'AerogridPlot', 'BeamPlot', 'AeroForcesCalculator', 'WriteVariablesTime'],
-                        # 'flow': ['BeamLoader', 'NonLinearStatic', 'BeamPlot'],
+                        'flow': ['BeamLoader', 'AerogridLoader', 'StaticCoupled',
+                                 'AeroForcesCalculator', 'WriteVariablesTime'],
                         'write_screen': 'off',
                         'write_log': 'on',
                         'log_folder': route + '/output/',
@@ -311,8 +293,6 @@ def generate_solver_file(horseshoe=False):
                                                                                  'u_inf_direction': [1., 0, 0]},
                                                         'rho': rho},
                                'max_iter': 50,
-                               # 'n_load_steps': 1,
-                               # 'n_load_steps': 5,
                                'tolerance': 1e-9,
                                'relaxation_factor': 0.0}
     config['WriteVariablesTime'] = {'cleanup_old_solution': 'on',
@@ -337,16 +317,11 @@ def generate_solver_file(horseshoe=False):
                                     'wake_shape_generator_input': {'u_inf': u_inf,
                                                                    'u_inf_direction': np.array([1., 0., 0.]),
                                                                    'dt': main_chord/m_main/u_inf}}
-    config['AerogridPlot'] = {'include_rbm': 'off',
-                              'include_applied_forces': 'on',
-                              'minus_m_star': 0
-                              }
+
     config['AeroForcesCalculator'] = {'write_text_file': 'on',
                                       'text_file_name': case_name + '_aeroforces.csv',
                                       'screen_output': 'on',
                                       }
-    config['BeamPlot'] = {'include_rbm': 'off',
-                          'include_applied_forces': 'on'}
     config.write()
 
 
diff --git a/tests/coupled/static/smith_nog_4deg/generate_smith_nog_4deg.py b/tests/coupled/static/smith_nog_4deg/generate_smith_nog_4deg.py
index a1bf6e85d..c146b9292 100644
--- a/tests/coupled/static/smith_nog_4deg/generate_smith_nog_4deg.py
+++ b/tests/coupled/static/smith_nog_4deg/generate_smith_nog_4deg.py
@@ -66,14 +66,19 @@ def generate_fem_file():
     x = np.zeros((num_node, ))
     y = np.zeros((num_node, ))
     z = np.zeros((num_node, ))
+
     # struct twist
     structural_twist = np.zeros((num_elem, num_node_elem))
+
     # beam number
     beam_number = np.zeros((num_elem, ), dtype=int)
+
     # frame of reference delta
     frame_of_reference_delta = np.zeros((num_elem, num_node_elem, 3))
+
     # connectivities
     conn = np.zeros((num_elem, num_node_elem), dtype=int)
+
     # stiffness
     num_stiffness = 1
     ea = 1e5
@@ -86,6 +91,7 @@ def generate_fem_file():
     stiffness = np.zeros((num_stiffness, 6, 6))
     stiffness[0, :, :] = main_sigma*base_stiffness
     elem_stiffness = np.zeros((num_elem,), dtype=int)
+
     # mass
     num_mass = 1
     m_base = 0.75
@@ -94,30 +100,18 @@ def generate_fem_file():
     mass = np.zeros((num_mass, 6, 6))
     mass[0, :, :] = base_mass
     elem_mass = np.zeros((num_elem,), dtype=int)
+
     # boundary conditions
     boundary_conditions = np.zeros((num_node, ), dtype=int)
     boundary_conditions[0] = 1
+
     # applied forces
-    # n_app_forces = 2
-    # node_app_forces = np.zeros((n_app_forces,), dtype=int)
     app_forces = np.zeros((num_node, 6))
 
-    spacing_param = 4
-
     # right wing (beam 0) --------------------------------------------------------------
     working_elem = 0
     working_node = 0
     beam_number[working_elem:working_elem + num_elem_main] = 0
-    domain = np.linspace(0, 1.0, num_node_main)
-    # 16 - (np.geomspace(20, 4, 10) - 4)
-    # x[working_node:working_node + num_node_main] = np.sin(sweep)*(main_span - (np.geomspace(main_span + spacing_param,
-    #                                                                                         0 + spacing_param,
-    #                                                                                         num_node_main)
-    #                                                                            - spacing_param))
-    # y[working_node:working_node + num_node_main] = np.abs(np.cos(sweep)*(main_span - (np.geomspace(main_span + spacing_param,
-    #                                                                                         0 + spacing_param,
-    #                                                                                         num_node_main)
-    #                                                                            - spacing_param)))
     y[0] = 0
     y[working_node:working_node + num_node_main] = np.cos(sweep)*np.linspace(0.0, main_span, num_node_main)
     x[working_node:working_node + num_node_main] = np.sin(sweep)*np.linspace(0.0, main_span, num_node_main)
@@ -137,18 +131,10 @@ def generate_fem_file():
 
     # left wing (beam 1) --------------------------------------------------------------
     beam_number[working_elem:working_elem + num_elem_main] = 1
-    domain = np.linspace(-1.0, 0.0, num_node_main)
     tempy = np.linspace(-main_span, 0.0, num_node_main)
     x[working_node:working_node + num_node_main - 1] = -np.sin(sweep)*tempy[0:-1]
     y[working_node:working_node + num_node_main - 1] = np.cos(sweep)*tempy[0:-1]
-    # x[working_node:working_node + num_node_main - 1] = -np.sin(sweep)*(main_span - (np.geomspace(0 + spacing_param,
-    #                                                                                         main_span + spacing_param,
-    #                                                                                         num_node_main)[:-1]
-    #                                                                            - spacing_param))
-    # y[working_node:working_node + num_node_main - 1] = -np.abs(np.cos(sweep)*(main_span - (np.geomspace(0 + spacing_param,
-    #                                                                                                main_span + spacing_param,
-    #                                                                                                num_node_main)[:-1]
-    #                                                                                   - spacing_param)))
+
     for ielem in range(num_elem_main):
         for inode in range(num_node_elem):
             frame_of_reference_delta[working_elem + ielem, inode, :] = [-1, 0, 0]
@@ -190,8 +176,6 @@ def generate_fem_file():
             'beam_number', data=beam_number)
         app_forces_handle = h5file.create_dataset(
             'app_forces', data=app_forces)
-        # node_app_forces_handle = h5file.create_dataset(
-        #     'node_app_forces', data=node_app_forces)
 
 
 def generate_aero_file():
@@ -224,8 +208,6 @@ def generate_aero_file():
     surface_distribution[working_elem:working_elem + num_elem_main] = i_surf
     surface_m[i_surf] = m_main
     aero_node[working_node:working_node + num_node_main - 1] = True
-    # chord[working_node:working_node + num_node_main - 1] = main_chord
-    # elastic_axis[working_node:working_node + num_node_main - 1] = main_ea
     working_elem += num_elem_main
     working_node += num_node_main - 1
 
@@ -272,14 +254,14 @@ def naca(x, m, p):
 
 def generate_solver_file(horseshoe=False):
     file_name = route + '/' + case_name + '.sharpy'
-    # config = configparser.ConfigParser()
+
     import configobj
     config = configobj.ConfigObj()
     config.filename = file_name
     config['SHARPy'] = {'case': case_name,
                         'route': route,
-                        'flow': ['BeamLoader', 'AerogridLoader', 'StaticCoupled', 'AerogridPlot', 'BeamPlot', 'AeroForcesCalculator', 'WriteVariablesTime'],
-                        # 'flow': ['BeamLoader', 'NonLinearStatic', 'BeamPlot'],
+                        'flow': ['BeamLoader', 'AerogridLoader', 'StaticCoupled',
+                                 'AeroForcesCalculator', 'WriteVariablesTime'],
                         'write_screen': 'off',
                         'write_log': 'on',
                         'log_folder': route + '/output/',
@@ -337,16 +319,10 @@ def generate_solver_file(horseshoe=False):
                                                                    'u_inf_direction': np.array([1., 0., 0.]),
                                                                    'dt': main_chord/m_main/u_inf}}
 
-    config['AerogridPlot'] = {'include_rbm': 'off',
-                              'include_applied_forces': 'on',
-                              'minus_m_star': 0
-                              }
     config['AeroForcesCalculator'] = {'write_text_file': 'on',
                                       'text_file_name': case_name + '_aeroforces.csv',
                                       'screen_output': 'on',
                                       }
-    config['BeamPlot'] = {'include_rbm': 'off',
-                          'include_applied_forces': 'on'}
     config.write()
 
 
diff --git a/tests/uvlm/static/polars/test_polars.py b/tests/uvlm/static/polars/test_polars.py
index 43269a915..44def2bc5 100644
--- a/tests/uvlm/static/polars/test_polars.py
+++ b/tests/uvlm/static/polars/test_polars.py
@@ -10,8 +10,8 @@
 
 
 class InfiniteWing:
-    area = 90000000.0
-    chord = 3.
+    area = 9e7
+    chord = 3
 
     def force_coef(self, rho, uinf):
         return 0.5 * rho * uinf ** 2 * self.area
diff --git a/tests/xbeam/geradin/generate_geradin.py b/tests/xbeam/geradin/generate_geradin.py
index b29175f84..9c22951c7 100644
--- a/tests/xbeam/geradin/generate_geradin.py
+++ b/tests/xbeam/geradin/generate_geradin.py
@@ -21,7 +21,6 @@ def generate_fem_file(route, case_name, num_elem, num_node_elem=3):
     length = 5
 
     num_node = (num_node_elem - 1)*num_elem + 1
-    # import pdb; pdb.set_trace()
     angle = 0*np.pi/180.0
     x = (np.linspace(0, length, num_node))*np.cos(angle)
     y = (np.linspace(0, length, num_node))*np.sin(angle)
@@ -32,7 +31,6 @@ def generate_fem_file(route, case_name, num_elem, num_node_elem=3):
     frame_of_reference_delta = np.zeros((num_elem, num_node_elem, 3))
     for ielem in range(num_elem):
         for inode in range(num_node_elem):
-            # frame_of_reference_delta[inode, :] = [0, 1, 0]
             frame_of_reference_delta[ielem, inode, :] = [-np.sin(angle), np.cos(angle), 0]
 
     scale = 1
@@ -47,7 +45,6 @@ def generate_fem_file(route, case_name, num_elem, num_node_elem=3):
                           + [0, 2, 1])
 
     # stiffness array
-    # import pdb; pdb.set_trace()
     num_stiffness = 1
     ea = 4.8e8
     ga = 3.231e8
@@ -55,7 +52,6 @@ def generate_fem_file(route, case_name, num_elem, num_node_elem=3):
     ei = 9.346e6
     base_stiffness = np.diag([ea, ga, ga, gj, ei, ei])
     stiffness = np.zeros((num_stiffness, 6, 6))
-    # import pdb; pdb.set_trace()
     for i in range(num_stiffness):
         stiffness[i, :, :] = base_stiffness
 
@@ -83,7 +79,6 @@ def generate_fem_file(route, case_name, num_elem, num_node_elem=3):
 
     # new app forces scheme (only follower)
     app_forces = np.zeros((num_node, 6))
-    # app_forces[0, :] = [0, 0, 3000000, 0, 0, 0]
 
     # lumped masses input
     n_lumped_mass = 1
@@ -133,7 +128,7 @@ def generate_fem_file(route, case_name, num_elem, num_node_elem=3):
 
 def generate_solver_file():
     file_name = route + '/' + case_name + '.sharpy'
-    # config = configparser.ConfigParser()
+
     import configobj
     config = configobj.ConfigObj()
     config.filename = file_name
@@ -160,8 +155,6 @@ def generate_solver_file():
                           'include_applied_forces': 'on'}
 
     config.write()
-    # with open(file_name, 'w') as configfile:
-    #     config.write(configfile)
 
 
 # run everything
diff --git a/tests/xbeam/modal/rotating_beam/generate_bielawa_baromega2_1e3.py b/tests/xbeam/modal/rotating_beam/generate_bielawa_baromega2_1e3.py
index 9eb7109d3..7e7bd71a2 100644
--- a/tests/xbeam/modal/rotating_beam/generate_bielawa_baromega2_1e3.py
+++ b/tests/xbeam/modal/rotating_beam/generate_bielawa_baromega2_1e3.py
@@ -7,7 +7,6 @@
 import sys
 import sharpy.utils.cout_utils as cout
 
-
 case_name = 'bielawa_baromega2_1e3'
 route = os.path.dirname(os.path.realpath(__file__)) + '/'
 
@@ -17,21 +16,22 @@
 length = 1
 
 # linear_factor: scaling factor to make the non linear solver behave as a linear one
-linear_factor=1
-E=1e6*linear_factor
-A=1e4
-I=1e-4
-ei = E*I
-m_bar = 1*linear_factor
+linear_factor = 1
+E = 1e6 * linear_factor
+A = 1e4
+I = 1e-4
+ei = E * I
+m_bar = 1 * linear_factor
 
-rot_speed=np.sqrt(1e3*ei/m_bar/length**4)
+rot_speed = np.sqrt(1e3 * ei / m_bar / length ** 4)
 
 steps_per_revolution = 180
-dt = 2.0*np.pi/rot_speed/steps_per_revolution
-n_tstep = 1*steps_per_revolution+1
+dt = 2.0 * np.pi / rot_speed / steps_per_revolution
+n_tstep = 1 * steps_per_revolution + 1
 
 n_tstep = 90
 
+
 def clean_test_files():
     fem_file_name = route + '/' + case_name + '.fem.h5'
     if os.path.isfile(fem_file_name):
@@ -49,14 +49,13 @@ def clean_test_files():
     if os.path.isfile(solver_file_name):
         os.remove(solver_file_name)
 
-def generate_fem_file(route, case_name, num_elem, num_node_elem=3):
 
+def generate_fem_file(route, case_name, num_elem, num_node_elem=3):
     global num_node
-    num_node = (num_node_elem - 1)*num_elem + 1
-    # import pdb; pdb.set_trace()
-    angle = 0*np.pi/180.0
-    x = (np.linspace(0, length, num_node))*np.cos(angle)
-    y = (np.linspace(0, length, num_node))*np.sin(angle)
+    num_node = (num_node_elem - 1) * num_elem + 1
+    angle = 0 * np.pi / 180.0
+    x = (np.linspace(0, length, num_node)) * np.cos(angle)
+    y = (np.linspace(0, length, num_node)) * np.sin(angle)
     z = np.zeros((num_node,))
 
     structural_twist = np.zeros((num_elem, num_node_elem))
@@ -77,25 +76,21 @@ def generate_fem_file(route, case_name, num_elem, num_node_elem=3):
         conn[ielem, :] = (np.ones((3,)) * ielem * (num_node_elem - 1)
                           + [0, 2, 1])
 
-
-
     # stiffness array
-    # import pdb; pdb.set_trace()
     num_stiffness = 1
 
-    ea = E*A
+    ea = E * A
     # APPROXIMATION!!!
     cout.cout_wrap("Assuming isotropic material", 2)
-    G = E / 2.0 / (1.0+0.3)
+    G = E / 2. / (1. + 0.3)
     cout.cout_wrap("Using total cross-section area as shear area", 2)
-    ga = G*A
+    ga = G * A
     cout.cout_wrap("Assuming planar cross-sections", 2)
-    J = 2.0* I
-    gj = G*J
+    J = 2. * I
+    gj = G * J
 
     base_stiffness = np.diag([ea, ga, ga, gj, ei, ei])
     stiffness = np.zeros((num_stiffness, 6, 6))
-    # import pdb; pdb.set_trace()
     for i in range(num_stiffness):
         stiffness[i, :, :] = base_stiffness
     # element stiffness
@@ -103,11 +98,11 @@ def generate_fem_file(route, case_name, num_elem, num_node_elem=3):
 
     # mass array
     num_mass = 1
-    base_mass = m_bar*np.diag([1.0, 1.0, 1.0, J/A, I/A, I/A])
-    # base_mass = m_bar*np.diag([1.0, 1.0, 1.0, 1.0,1.0,1.0])
+    base_mass = m_bar * np.diag([1., 1., 1., J / A, I / A, I / A])
     mass = np.zeros((num_mass, 6, 6))
     for i in range(num_mass):
         mass[i, :, :] = base_mass
+
     # element masses
     elem_mass = np.zeros((num_elem,), dtype=int)
 
@@ -121,63 +116,55 @@ def generate_fem_file(route, case_name, num_elem, num_node_elem=3):
 
     # new app forces scheme (only follower)
     app_forces = np.zeros((num_node, 6))
-    # app_forces[0, :] = [0, 0, 3000000, 0, 0, 0]
 
     # lumped masses input
     n_lumped_mass = 1
     lumped_mass_nodes = np.array([num_node - 1], dtype=int)
-    lumped_mass = np.zeros((n_lumped_mass, ))
+    lumped_mass = np.zeros((n_lumped_mass,))
     lumped_mass[0] = 0.0
     lumped_mass_inertia = np.zeros((n_lumped_mass, 3, 3))
     lumped_mass_position = np.zeros((n_lumped_mass, 3))
 
-    #n_lumped_mass = 1
-    #lumped_mass_nodes = np.ones((num_node,), dtype=int)
-    #lumped_mass = np.zeros((n_lumped_mass, ))
-    #lumped_mass[0] = m_bar*length/num_elem/(num_node_elem-1)
-    #lumped_mass_inertia[0,:,:] = np.diag([J, I, I])
-    #lumped_mass_position = np.zeros((n_lumped_mass, 3))
-
     with h5.File(route + '/' + case_name + '.fem.h5', 'a') as h5file:
 
         # CHECKING
-        if(elem_stiffness.shape[0]!=num_elem):
+        if (elem_stiffness.shape[0] != num_elem):
             sys.exit("ERROR: Element stiffness must be defined for each element")
-        if(elem_mass.shape[0]!=num_elem):
+        if (elem_mass.shape[0] != num_elem):
             sys.exit("ERROR: Element mass must be defined for each element")
-        if(frame_of_reference_delta.shape[0]!=num_elem):
+        if (frame_of_reference_delta.shape[0] != num_elem):
             sys.exit("ERROR: The first dimension of FoR does not match the number of elements")
-        if(frame_of_reference_delta.shape[1]!=num_node_elem):
+        if (frame_of_reference_delta.shape[1] != num_node_elem):
             sys.exit("ERROR: The second dimension of FoR does not match the number of nodes element")
-        if(frame_of_reference_delta.shape[2]!=3):
+        if (frame_of_reference_delta.shape[2] != 3):
             sys.exit("ERROR: The third dimension of FoR must be 3")
-        if(structural_twist.shape[0]!=num_node):
+        if (structural_twist.shape[0] != num_node):
             sys.exit("ERROR: The structural twist must be defined for each node")
-        if(boundary_conditions.shape[0]!=num_node):
+        if (boundary_conditions.shape[0] != num_node):
             sys.exit("ERROR: The boundary conditions must be defined for each node")
-        if(beam_number.shape[0]!=num_node):
+        if (beam_number.shape[0] != num_node):
             sys.exit("ERROR: The beam number must be defined for each node")
-        if(app_forces.shape[0]!=num_node):
+        if (app_forces.shape[0] != num_node):
             sys.exit("ERROR: The first dimension of the applied forces matrix does not match the number of nodes")
-        if(app_forces.shape[1]!=6):
+        if (app_forces.shape[1] != 6):
             sys.exit("ERROR: The second dimension of the applied forces matrix must be 6")
 
-        coordinates = h5file.create_dataset('coordinates', data = np.column_stack((x, y, z)))
-        conectivities = h5file.create_dataset('connectivities', data = conn)
+        coordinates = h5file.create_dataset('coordinates', data=np.column_stack((x, y, z)))
+        conectivities = h5file.create_dataset('connectivities', data=conn)
         num_nodes_elem_handle = h5file.create_dataset(
-            'num_node_elem', data = num_node_elem)
+            'num_node_elem', data=num_node_elem)
         num_nodes_handle = h5file.create_dataset(
-            'num_node', data = num_node)
+            'num_node', data=num_node)
         num_elem_handle = h5file.create_dataset(
-            'num_elem', data = num_elem)
+            'num_elem', data=num_elem)
         stiffness_db_handle = h5file.create_dataset(
-            'stiffness_db', data = stiffness)
+            'stiffness_db', data=stiffness)
         stiffness_handle = h5file.create_dataset(
-            'elem_stiffness', data = elem_stiffness)
+            'elem_stiffness', data=elem_stiffness)
         mass_db_handle = h5file.create_dataset(
-            'mass_db', data = mass)
+            'mass_db', data=mass)
         mass_handle = h5file.create_dataset(
-            'elem_mass', data = elem_mass)
+            'elem_mass', data=elem_mass)
         frame_of_reference_delta_handle = h5file.create_dataset(
             'frame_of_reference_delta', data=frame_of_reference_delta)
         structural_twist_handle = h5file.create_dataset(
@@ -198,17 +185,14 @@ def generate_fem_file(route, case_name, num_elem, num_node_elem=3):
             'lumped_mass_position', data=lumped_mass_position)
     return num_node, coordinates
 
+
 def generate_dyn_file():
     global num_node
 
     forced_for_vel = np.zeros((n_tstep, 6))
-    dynamic_forces_time = np.zeros((n_tstep, num_node,6))
+    dynamic_forces_time = np.zeros((n_tstep, num_node, 6))
     for it in range(n_tstep):
-        # forced_for_vel[it, 3:6] = it/n_tstep*angular_velocity
         forced_for_vel[it, 5] = rot_speed
-        # dynamic_forces_time[it,-1,2] = 100
-
-
 
     with h5.File(route + '/' + case_name + '.dyn.h5', 'a') as h5file:
         h5file.create_dataset(
@@ -218,33 +202,37 @@ def generate_dyn_file():
         h5file.create_dataset(
             'num_steps', data=n_tstep)
 
+
 def generate_aero_file():
     global num_node
     with h5.File(route + '/' + case_name + '.aero.h5', 'a') as h5file:
         airfoils_group = h5file.create_group('airfoils')
         # add the airfoils
-        airfoils_group.create_dataset("0", data = np.column_stack( (np.linspace( 0.0, 1.0, 10), np.zeros(10) )) )
+        airfoils_group.create_dataset("0", data=np.column_stack((np.linspace(0.0, 1.0, 10), np.zeros(10))))
 
         # chord
-        chord_input = h5file.create_dataset('chord', data= np.ones((num_elem,num_node_elem),))
-        dim_attr = chord_input .attrs['units'] = 'm'
+        chord_input = h5file.create_dataset('chord', data=np.ones((num_elem, num_node_elem), ))
+        dim_attr = chord_input.attrs['units'] = 'm'
 
         # twist
-        twist_input = h5file.create_dataset('twist', data=np.zeros((num_elem,num_node_elem),))
+        twist_input = h5file.create_dataset('twist', data=np.zeros((num_elem, num_node_elem), ))
         dim_attr = twist_input.attrs['units'] = 'rad'
 
         # airfoil distribution
-        airfoil_distribution_input = h5file.create_dataset('airfoil_distribution', data=np.zeros((num_elem,num_node_elem),dtype=int))
+        airfoil_distribution_input = h5file.create_dataset('airfoil_distribution',
+                                                           data=np.zeros((num_elem, num_node_elem), dtype=int))
 
-        surface_distribution_input = h5file.create_dataset('surface_distribution', data=np.zeros((num_elem,),dtype=int))
-        surface_m_input = h5file.create_dataset('surface_m', data = np.ones((1,),dtype=int))
+        surface_distribution_input = h5file.create_dataset('surface_distribution',
+                                                           data=np.zeros((num_elem,), dtype=int))
+        surface_m_input = h5file.create_dataset('surface_m', data=np.ones((1,), dtype=int))
         m_distribution = 'uniform'
         m_distribution_input = h5file.create_dataset('m_distribution', data=m_distribution.encode('ascii', 'ignore'))
 
-        aero_node = np.zeros((num_node,),dtype=bool)
-        aero_node[-3:] = np.ones((3,),dtype=bool)
+        aero_node = np.zeros((num_node,), dtype=bool)
+        aero_node[-3:] = np.ones((3,), dtype=bool)
         aero_node_input = h5file.create_dataset('aero_node', data=aero_node)
-        elastic_axis_input = h5file.create_dataset('elastic_axis', data=0.5*np.ones((num_elem,num_node_elem),))
+        elastic_axis_input = h5file.create_dataset('elastic_axis', data=0.5 * np.ones((num_elem, num_node_elem), ))
+
 
 def generate_solver_file():
     file_name = route + '/' + case_name + '.sharpy'
@@ -253,12 +241,13 @@ def generate_solver_file():
     settings = dict()
 
     settings['SHARPy'] = {'case': case_name,
-                        'route': route,
-                        'flow': ['BeamLoader', 'AerogridLoader', 'StaticCoupled', 'BeamPlot', 'AerogridPlot',  'DynamicPrescribedCoupled', 'Modal'],
-                        'write_screen': 'off',
-                        'write_log': 'on',
-                        'log_folder': route + '/output/',
-                        'log_file': case_name + '.log'}
+                          'route': route,
+                          'flow': ['BeamLoader', 'AerogridLoader', 'StaticCoupled', 'BeamPlot', 'AerogridPlot',
+                                   'DynamicPrescribedCoupled', 'Modal'],
+                          'write_screen': 'off',
+                          'write_log': 'on',
+                          'log_folder': route + '/output/',
+                          'log_file': case_name + '.log'}
 
     # AUX DICTIONARIES
     aux_settings['velocity_field_input'] = {'u_inf': 100.0,
@@ -266,7 +255,7 @@ def generate_solver_file():
 
     # LOADERS
     settings['BeamLoader'] = {'unsteady': 'on',
-                              'orientation': algebra.euler2quat(np.array([0.0,0.0,0.0]))}
+                              'orientation': algebra.euler2quat(np.array([0.0, 0.0, 0.0]))}
 
     settings['AerogridLoader'] = {'unsteady': 'on',
                                   'aligned_grid': 'on',
@@ -291,7 +280,6 @@ def generate_solver_file():
 
     settings['BeamLoads'] = {}
 
-
     # STATIC COUPLED
 
     settings['NonLinearStatic'] = {'print_info': 'on',
@@ -326,14 +314,14 @@ def generate_solver_file():
     # DYNAMIC PRESCRIBED COUPLED
 
     settings['NonLinearDynamicPrescribedStep'] = {'print_info': 'on',
-                                               'max_iterations': 95000,
-                                               'delta_curved': 1e-9,
-                                               'min_delta': 1e-6,
-                                               'newmark_damp': 1e-3,
-                                               'gravity_on': 'off',
-                                               'gravity': 9.81,
-                                               'num_steps': n_tstep,
-                                               'dt': dt}
+                                                  'max_iterations': 95000,
+                                                  'delta_curved': 1e-9,
+                                                  'min_delta': 1e-6,
+                                                  'newmark_damp': 1e-3,
+                                                  'gravity_on': 'off',
+                                                  'gravity': 9.81,
+                                                  'num_steps': n_tstep,
+                                                  'dt': dt}
 
     settings['StepUvlm'] = {'print_info': 'on',
                             'horseshoe': 'off',
@@ -366,9 +354,9 @@ def generate_solver_file():
                                                                         'AerogridPlot': settings['AerogridPlot']}}
 
     settings['Modal'] = {'include_rbm': 'on',
-                          'NumLambda': 10000,
-                          'num_steps': 1,
-                          'print_matrices': 'on'}
+                         'NumLambda': 10000,
+                         'num_steps': 1,
+                         'print_matrices': 'on'}
 
     import configobj
     config = configobj.ConfigObj()
@@ -377,6 +365,7 @@ def generate_solver_file():
         config[k] = v
     config.write()
 
+
 # run everything
 clean_test_files()
 generate_fem_file(route, case_name, num_elem, num_node_elem)
@@ -384,4 +373,6 @@ def generate_solver_file():
 generate_dyn_file()
 generate_solver_file()
 
-cout.cout_wrap('Reference for validation: "Rotary wing structural dynamics and aeroelasticity", R.L. Bielawa. AIAA education series. Second edition', 1)
+cout.cout_wrap(
+    'Reference for validation: "Rotary wing structural dynamics and aeroelasticity", R.L. Bielawa. AIAA education series. Second edition',
+    1)