Repository: ImperialCollegeLondon/sharpy (language: python)

Issue:
Include gravity direction as input for structural solvers
Right now gravity always acts on the z direction in SHARPy.

However, xbeam allows gravity to be applied in any direction. Thus, it seems that we have ready the most laborious part of this feature and we are just missing the interface. 

Include gravity direction as input for structural solvers
Right now gravity always acts on the z direction in SHARPy.

However, xbeam allows gravity to be applied in any direction. Thus, it seems that we have ready the most laborious part of this feature and we are just missing the interface. 



Discussion / hints:

Fix patch:
diff --git a/cases/templates/template_wt.py b/cases/templates/template_wt.py
index 1e85e70a4..e18db01d6 100644
--- a/cases/templates/template_wt.py
+++ b/cases/templates/template_wt.py
@@ -718,8 +718,7 @@ def generate_from_excel_type02(chord_panels,
     LC1.node_in_body = tower.StructuralInformation.num_node - 1
     LC1.body = 0
     LC1.body_FoR = 1
-    LC1.rot_axisB = np.array([1., 0., 0.0])
-    LC1.rot_vel = -rotation_velocity
+    LC1.rot_vect = np.array([-1., 0., 0.])*rotation_velocity
 
     LC = []
     LC.append(LC1)
diff --git a/sharpy/postproc/beamplot.py b/sharpy/postproc/beamplot.py
index 2995be48e..ff832aa6c 100644
--- a/sharpy/postproc/beamplot.py
+++ b/sharpy/postproc/beamplot.py
@@ -132,11 +132,7 @@ def write_beam(self, it):
         forces_constraints_nodes = np.zeros((num_nodes, 3))
         moments_constraints_nodes = np.zeros((num_nodes, 3))
 
-        if self.data.structure.timestep_info[it].in_global_AFoR:
-            tstep = self.data.structure.timestep_info[it]
-        else:
-            tstep = self.data.structure.timestep_info[it].copy()
-            tstep.whole_structure_to_global_AFoR(self.data.structure)
+        tstep = self.data.structure.timestep_info[it]
 
         # aero2inertial rotation
         aero2inertial = tstep.cga()
diff --git a/sharpy/postproc/liftdistribution.py b/sharpy/postproc/liftdistribution.py
index 6f6fd26b1..69908c26c 100644
--- a/sharpy/postproc/liftdistribution.py
+++ b/sharpy/postproc/liftdistribution.py
@@ -55,11 +55,7 @@ def run(self, online=False):
 
     def lift_distribution(self):
         # add entry to dictionary for postproc
-        if self.data.structure.timestep_info[self.data.ts].in_global_AFoR:
-            tstep = self.data.structure.timestep_info[self.data.ts]
-        else:
-            tstep = self.data.structure.timestep_info[self.data.ts].copy()
-            tstep.whole_structure_to_global_AFoR(self.data.structure)
+        tstep = self.data.structure.timestep_info[self.data.ts]
         tstep.postproc_cell['lift_distribution'] = init_matrix_structure(dimensions=tstep.dimensions,
                                                                            with_dim_dimension=False)
 
diff --git a/sharpy/postproc/pickledata.py b/sharpy/postproc/pickledata.py
index 91ec56797..2f6b536e2 100644
--- a/sharpy/postproc/pickledata.py
+++ b/sharpy/postproc/pickledata.py
@@ -45,9 +45,6 @@ def initialise(self, data, custom_settings=None, caller=None):
     def run(self, online=False):
         for it in range(len(self.data.structure.timestep_info)):
             tstep_p = self.data.structure.timestep_info[it]
-            if tstep_p is not None:
-                if not tstep_p.in_global_AFoR:
-                    tstep_p.whole_structure_to_global_AFoR(self.data.structure)
         with open(self.filename, 'wb') as f:
             pickle.dump(self.data, f, protocol=pickle.HIGHEST_PROTOCOL)
 
diff --git a/sharpy/postproc/savedata.py b/sharpy/postproc/savedata.py
index cafc06daf..feb1f89a5 100644
--- a/sharpy/postproc/savedata.py
+++ b/sharpy/postproc/savedata.py
@@ -117,7 +117,7 @@ def initialise(self, data, custom_settings=None, caller=None):
 
         settings.to_custom_types(self.settings,
                                  self.settings_types, self.settings_default, options=self.settings_options)
-        
+
         # Add these anyway - therefore if you add your own skip_attr you don't have to retype all of these
         self.settings['skip_attr'].extend(['fortran',
                                             'airfoils',
@@ -296,11 +296,7 @@ def save_timestep(data, settings, ts, hdfile):
                                SkipAttr=settings['skip_attr'],
                                compress_float=settings['compress_float'])
         if settings['save_struct']:
-            if data.structure.timestep_info[ts].in_global_AFoR:
-                tstep = data.structure.timestep_info[ts]
-            else:
-                tstep = data.structure.timestep_info[data.ts].copy()
-                tstep.whole_structure_to_global_AFoR(data.structure)
+            tstep = data.structure.timestep_info[ts]
 
             h5utils.add_as_grp(tstep,
                                hdfile['data']['structure']['timestep_info'],
diff --git a/sharpy/postproc/writevariablestime.py b/sharpy/postproc/writevariablestime.py
index b59acbe83..17b6355d4 100644
--- a/sharpy/postproc/writevariablestime.py
+++ b/sharpy/postproc/writevariablestime.py
@@ -212,11 +212,7 @@ def write(self, it):
         else:
             self.settings['FoR_number'] = np.array([0], dtype=int)
 
-        if self.data.structure.timestep_info[it].in_global_AFoR:
-            tstep = self.data.structure.timestep_info[it]
-        else:
-            tstep = self.data.structure.timestep_info[it].copy()
-            tstep.whole_structure_to_global_AFoR(self.data.structure)
+        tstep = self.data.structure.timestep_info[it]
 
         for ivariable in range(len(self.settings['FoR_variables'])):
             if self.settings['FoR_variables'][ivariable] == '':
diff --git a/sharpy/solvers/beamloader.py b/sharpy/solvers/beamloader.py
index 5e0513537..e13339022 100644
--- a/sharpy/solvers/beamloader.py
+++ b/sharpy/solvers/beamloader.py
@@ -134,8 +134,8 @@ def run(self):
         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)
+        # 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/nonlineardynamicmultibody.py b/sharpy/solvers/nonlineardynamicmultibody.py
index 20d448391..772208a33 100644
--- a/sharpy/solvers/nonlineardynamicmultibody.py
+++ b/sharpy/solvers/nonlineardynamicmultibody.py
@@ -1,9 +1,12 @@
 import ctypes as ct
 import numpy as np
+import os
 
 from sharpy.utils.solver_interface import solver, BaseSolver, solver_from_string
 import sharpy.utils.settings as settings
+import sharpy.utils.solver_interface as solver_interface
 
+import sharpy.utils.cout_utils as cout
 import sharpy.structure.utils.xbeamlib as xbeamlib
 import sharpy.utils.multibody as mb
 import sharpy.structure.utils.lagrangeconstraints as lagrangeconstraints
@@ -27,6 +30,24 @@ class NonLinearDynamicMultibody(_BaseStructural):
     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'] = 'NewmarkBeta'
+    settings_description['time_integrator'] = 'Method to perform time integration'
+    settings_options['time_integrator'] = ['NewmarkBeta']
+
+    settings_types['time_integrator_settings'] = 'dict'
+    settings_default['time_integrator_settings'] = dict()
+    settings_description['time_integrator_settings'] = 'Settings for the time integrator'
+
+    settings_types['write_lm'] = 'bool'
+    settings_default['write_lm'] = False
+    settings_description['write_lm'] = 'Write lagrange multipliers'
+
+    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_table = settings.SettingsTable()
     __doc__ += settings_table.generate(settings_types, settings_default, settings_description)
@@ -48,6 +69,8 @@ def __init__(self):
         self.gamma = None
         self.beta = None
 
+        self.prev_Dq = None
+
     def initialise(self, data, custom_settings=None):
 
         self.data = data
@@ -55,16 +78,12 @@ def initialise(self, data, custom_settings=None):
             self.settings = data.settings[self.solver_id]
         else:
             self.settings = custom_settings
-        settings.to_custom_types(self.settings, self.settings_types, self.settings_default)
+        settings.to_custom_types(self.settings, self.settings_types, self.settings_default, no_ctype=True)
 
         # load info from dyn dictionary
         self.data.structure.add_unsteady_information(
             self.data.structure.dyn_dict, self.settings['num_steps'])
 
-        # Define Newmark constants
-        self.gamma = 0.5 + self.settings['newmark_damp']
-        self.beta = 0.25*(self.gamma + 0.5)*(self.gamma + 0.5)
-
         # Define the number of equations
         self.lc_list = lagrangeconstraints.initialize_constraints(self.data.structure.ini_mb_dict)
         self.num_LM_eq = lagrangeconstraints.define_num_LM_eq(self.lc_list)
@@ -73,9 +92,29 @@ def initialise(self, data, custom_settings=None):
         self.Lambda_dot = np.zeros((self.num_LM_eq,), dtype=ct.c_double, order='F')
         self.Lambda_ddot = np.zeros((self.num_LM_eq,), dtype=ct.c_double, order='F')
 
+        if self.settings['write_lm']:
+            dire = './output/' + self.data.settings['SHARPy']['case'] + '/NonLinearDynamicMultibody/'
+            if not os.path.isdir(dire):
+                os.makedirs(dire)
+            self.fid_lambda = open(dire + 'lambda.dat', "w")
+            self.fid_lambda_dot = open(dire + 'lambda_dot.dat', "w")
+            self.fid_lambda_ddot = open(dire + 'lambda_ddot.dat', "w")
+            self.fid_cond_num = open(dire + 'cond_num.dat', "w")
+
         # Define the number of dofs
         self.define_sys_size()
 
+        self.prev_Dq = np.zeros((self.sys_size + self.num_LM_eq))
+
+        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
+        self.time_integrator = solver_interface.initialise_solver(
+            self.settings['time_integrator'])
+        self.time_integrator.initialise(
+            self.data, self.settings['time_integrator_settings'])
+
     def add_step(self):
         self.data.structure.next_step()
 
@@ -120,11 +159,10 @@ def assembly_MB_eq_system(self, MB_beam, MB_tstep, ts, dt, Lambda, Lambda_dot, M
         """
         self.num_LM_eq = lagrangeconstraints.define_num_LM_eq(self.lc_list)
 
-        MB_M = np.zeros((self.sys_size+self.num_LM_eq, self.sys_size+self.num_LM_eq), dtype=ct.c_double, order='F')
-        MB_C = np.zeros((self.sys_size+self.num_LM_eq, self.sys_size+self.num_LM_eq), dtype=ct.c_double, order='F')
-        MB_K = np.zeros((self.sys_size+self.num_LM_eq, self.sys_size+self.num_LM_eq), dtype=ct.c_double, order='F')
-        MB_Asys = np.zeros((self.sys_size+self.num_LM_eq, self.sys_size+self.num_LM_eq), dtype=ct.c_double, order='F')
-        MB_Q = np.zeros((self.sys_size+self.num_LM_eq,), dtype=ct.c_double, order='F')
+        MB_M = np.zeros((self.sys_size, self.sys_size), dtype=ct.c_double, order='F')
+        MB_C = np.zeros((self.sys_size, self.sys_size), dtype=ct.c_double, order='F')
+        MB_K = np.zeros((self.sys_size, self.sys_size), dtype=ct.c_double, order='F')
+        MB_Q = np.zeros((self.sys_size,), dtype=ct.c_double, order='F')
         first_dof = 0
         last_dof = 0
 
@@ -173,13 +211,15 @@ def assembly_MB_eq_system(self, MB_beam, MB_tstep, ts, dt, Lambda, Lambda_dot, M
             "dynamic")
 
         # Include the matrices associated to Lagrange Multipliers
-        MB_C += LM_C
-        MB_K += LM_K
-        MB_Q += LM_Q
+        MB_C += LM_C[:self.sys_size, :self.sys_size]
+        MB_K += LM_K[:self.sys_size, :self.sys_size]
+        MB_Q += LM_Q[:self.sys_size]
 
-        MB_Asys = MB_K + MB_C*self.gamma/(self.beta*dt) + MB_M/(self.beta*dt*dt)
+        # Only working for non-holonomic constratints
+        kBnh = LM_C[self.sys_size:, :self.sys_size]
+        strict_LM_Q = LM_Q[self.sys_size:]
 
-        return MB_Asys, MB_Q
+        return MB_M, MB_C, MB_K, MB_Q, kBnh, strict_LM_Q
 
     def integrate_position(self, MB_beam, MB_tstep, dt):
         """
@@ -204,12 +244,6 @@ def integrate_position(self, MB_beam, MB_tstep, dt):
             else:
                 MB_tstep[ibody].for_pos[0:3] += dt*np.dot(MB_tstep[ibody].cga(),MB_tstep[ibody].for_vel[0:3])
 
-        # Use next line for double pendulum (fix position of the second FoR)
-        # MB_tstep[ibody].for_pos[0:3] = np.dot(algebra.quat2rotation(MB_tstep[0].quat), MB_tstep[0].pos[-1,:])
-        # print("tip final pos: ", np.dot(algebra.quat2rotation(MB_tstep[0].quat), MB_tstep[0].pos[-1,:]))
-        # print("FoR final pos: ", MB_tstep[ibody].for_pos[0:3])
-        # print("pause")
-
     def extract_resultants(self):
         # TODO: code
         pass
@@ -255,6 +289,22 @@ def compute_forces_constraints(self, MB_beam, MB_tstep, ts, dt, Lambda, Lambda_d
             first_dof = last_dof
         # TODO: right now, these forces are only used as an output, they are not read when the multibody is splitted
 
+    def write_lm_cond_num(self, iteration, Lambda, Lambda_dot, Lambda_ddot, cond_num, cond_num_lm):
+
+        self.fid_lambda.write("%d %d " % (self.data.ts, iteration))
+        self.fid_lambda_dot.write("%d %d " % (self.data.ts, iteration))
+        self.fid_lambda_ddot.write("%d %d " % (self.data.ts, iteration))
+        self.fid_cond_num.write("%d %d " % (self.data.ts, iteration))
+        for ilm in range(self.num_LM_eq):
+            self.fid_lambda.write("%f " % Lambda[ilm])
+            self.fid_lambda_dot.write("%f " % Lambda_dot[ilm])
+            self.fid_lambda_ddot.write("%f " % Lambda_ddot[ilm])
+        self.fid_lambda.write("\n")
+        self.fid_lambda_dot.write("\n")
+        self.fid_lambda_ddot.write("\n")
+        self.fid_cond_num.write("%e %e\n" % (cond_num, cond_num_lm))
+
+
     def run(self, structural_step=None, dt=None):
         if structural_step is None:
             structural_step = self.data.structure.timestep_info[-1]
@@ -269,9 +319,14 @@ def run(self, structural_step=None, dt=None):
         else:
             self.settings['dt'] = dt
 
+        if self.data.structure.ini_info.in_global_AFoR:
+            self.data.structure.ini_info.whole_structure_to_local_AFoR(self.data.structure)
+
+        if structural_step.in_global_AFoR:
+            structural_step.whole_structure_to_local_AFoR(self.data.structure)
+
         self.num_LM_eq = lagrangeconstraints.define_num_LM_eq(self.lc_list)
 
-        # TODO: only working for constant forces
         MB_beam, MB_tstep = mb.split_multibody(
             self.data.structure,
             structural_step,
@@ -286,26 +341,19 @@ def run(self, structural_step=None, dt=None):
         dqdt = np.zeros((self.sys_size + num_LM_eq,), dtype=ct.c_double, order='F')
         dqddt = np.zeros((self.sys_size + num_LM_eq,), dtype=ct.c_double, order='F')
 
-        # Predictor step
-        mb.disp_and_accel2state(MB_beam, MB_tstep, q, dqdt, dqddt)
-
         if not num_LM_eq == 0:
             Lambda = self.Lambda.astype(dtype=ct.c_double, copy=True, order='F')
             Lambda_dot = self.Lambda_dot.astype(dtype=ct.c_double, copy=True, order='F')
             Lambda_ddot = self.Lambda_ddot.astype(dtype=ct.c_double, copy=True, order='F')
-
-            q[-num_LM_eq:] = Lambda.astype(dtype=ct.c_double, copy=True, order='F')
-            dqdt[-num_LM_eq:] = Lambda_dot.astype(dtype=ct.c_double, copy=True, order='F')
-            dqddt[-num_LM_eq:] = Lambda_ddot.astype(dtype=ct.c_double, copy=True, order='F')
         else:
             Lambda = 0
             Lambda_dot = 0
 
-        q += dt*dqdt + (0.5 - self.beta)*dt*dt*dqddt
-        dqdt += (1.0 - self.gamma)*dt*dqddt
-        dqddt = np.zeros((self.sys_size + num_LM_eq,), dtype=ct.c_double, order='F')
+        # Predictor step
+        q, dqdt, dqddt = mb.disp_and_accel2state(MB_beam, MB_tstep, Lambda, Lambda_dot, self.sys_size, num_LM_eq)
+        self.time_integrator.predictor(q, dqdt, dqddt)
 
-        # Newmark-beta iterations
+        # Reference residuals
         old_Dq = 1.0
         LM_old_Dq = 1.0
 
@@ -318,25 +366,26 @@ def run(self, structural_step=None, dt=None):
                 raise exc.NotConvergedSolver(error)
 
             # Update positions and velocities
-            mb.state2disp_and_accel(q, dqdt, dqddt, MB_beam, MB_tstep)
-            MB_Asys, MB_Q = self.assembly_MB_eq_system(MB_beam,
-                                                       MB_tstep,
-                                                       self.data.ts,
-                                                       dt,
-                                                       Lambda,
-                                                       Lambda_dot,
-                                                       MBdict)
-
-            # Compute the correction
-            # ADC next line not necessary
-            # Dq = np.zeros((self.sys_size+num_LM_eq,), dtype=ct.c_double, order='F')
-            # MB_Asys_balanced, T = scipy.linalg.matrix_balance(MB_Asys)
-            # invT = np.matrix(T).I
-            # MB_Q_balanced = np.dot(invT, MB_Q).T
-
-            Dq = np.linalg.solve(MB_Asys, -MB_Q)
-            # least squares solver
-            # Dq = np.linalg.lstsq(np.dot(MB_Asys_balanced, invT), -MB_Q_balanced, rcond=None)[0]
+            Lambda, Lambda_dot = mb.state2disp_and_accel(q, dqdt, dqddt, MB_beam, MB_tstep, num_LM_eq)
+            if self.settings['write_lm'] and iteration:
+                self.write_lm_cond_num(iteration, Lambda, Lambda_dot, Lambda_ddot, cond_num, cond_num_lm)
+
+            MB_M, MB_C, MB_K, MB_Q, kBnh, LM_Q = self.assembly_MB_eq_system(MB_beam,
+                                                                MB_tstep,
+                                                                self.data.ts,
+                                                                dt,
+                                                                Lambda,
+                                                                Lambda_dot,
+                                                                MBdict)
+
+            Asys, Q = self.time_integrator.build_matrix(MB_M, MB_C, MB_K, MB_Q,
+                                                        kBnh, LM_Q)
+
+            if self.settings['write_lm']:
+                cond_num = np.linalg.cond(Asys[:self.sys_size, :self.sys_size])
+                cond_num_lm = np.linalg.cond(Asys)
+
+            Dq = np.linalg.solve(Asys, -Q)
 
             # Evaluate convergence
             if iteration:
@@ -345,49 +394,36 @@ def run(self, structural_step=None, dt=None):
                     raise exc.NotConvergedSolver('Multibody res = NaN')
                 if num_LM_eq:
                     LM_res = np.max(np.abs(Dq[self.sys_size:self.sys_size+num_LM_eq]))/LM_old_Dq
-                    # LM_res = np.max(np.abs(Dq[self.sys_size:self.sys_size+num_LM_eq]))
                 else:
                     LM_res = 0.0
                 if (res < self.settings['min_delta']) and (LM_res < self.settings['min_delta']):
                     converged = True
 
-            # Compute variables from previous values and increments
-            # TODO:decide If I want other way of updating lambda
-            # this for least sq
-            # q[:, np.newaxis] += Dq
-            # dqdt[:, np.newaxis] += self.gamma/(self.beta*dt)*Dq
-            # dqddt[:, np.newaxis] += 1.0/(self.beta*dt*dt)*Dq
-
-            # this for direct solver
-            q += Dq
-            dqdt += self.gamma/(self.beta*dt)*Dq
-            dqddt += 1.0/(self.beta*dt*dt)*Dq
-
-            if not num_LM_eq == 0:
-                Lambda = q[-num_LM_eq:].astype(dtype=ct.c_double, copy=True, order='F')
-                Lambda_dot = dqdt[-num_LM_eq:].astype(dtype=ct.c_double, copy=True, order='F')
-                Lambda_ddot = dqddt[-num_LM_eq:].astype(dtype=ct.c_double, copy=True, order='F')
-            else:
-                Lambda = 0
-                Lambda_dot = 0
+            # 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)
 
             if converged:
                 break
 
             if not iteration:
                 old_Dq = np.max(np.abs(Dq[0:self.sys_size]))
-                if old_Dq < 1.0:
-                    old_Dq = 1.0
                 if num_LM_eq:
                     LM_old_Dq = np.max(np.abs(Dq[self.sys_size:self.sys_size+num_LM_eq]))
-                else:
-                    LM_old_Dq = 1.0
 
-        mb.state2disp_and_accel(q, dqdt, dqddt, MB_beam, MB_tstep)
+        Lambda, Lambda_dot = mb.state2disp_and_accel(q, dqdt, dqddt, MB_beam, MB_tstep, num_LM_eq)
+        if self.settings['write_lm']:
+            self.write_lm_cond_num(iteration, Lambda, Lambda_dot, Lambda_ddot, cond_num, cond_num_lm)
         # end: comment time stepping
 
         # End of Newmark-beta iterations
-        self.integrate_position(MB_beam, MB_tstep, dt)
+        # self.integrate_position(MB_beam, MB_tstep, dt)
         lagrangeconstraints.postprocess(self.lc_list, MB_beam, MB_tstep, "dynamic")
         self.compute_forces_constraints(MB_beam, MB_tstep, self.data.ts, dt, Lambda, Lambda_dot)
         if self.settings['gravity_on']:
@@ -395,6 +431,9 @@ def run(self, structural_step=None, dt=None):
                 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, MBdict, dt)
 
+        if not structural_step.in_global_AFoR:
+            structural_step.whole_structure_to_global_AFoR(self.data.structure)
+
         self.Lambda = Lambda.astype(dtype=ct.c_double, copy=True, order='F')
         self.Lambda_dot = Lambda_dot.astype(dtype=ct.c_double, copy=True, order='F')
         self.Lambda_ddot = Lambda_ddot.astype(dtype=ct.c_double, copy=True, order='F')
diff --git a/sharpy/solvers/nonlinearstaticmultibody.py b/sharpy/solvers/nonlinearstaticmultibody.py
deleted file mode 100644
index f9c37a944..000000000
--- a/sharpy/solvers/nonlinearstaticmultibody.py
+++ /dev/null
@@ -1,411 +0,0 @@
-import ctypes as ct
-import numpy as np
-
-from sharpy.utils.solver_interface import solver, BaseSolver, solver_from_string
-import sharpy.utils.settings as settings
-import sharpy.structure.utils.xbeamlib as xbeamlib
-import sharpy.utils.multibody as mb
-import sharpy.structure.utils.lagrangeconstraints as lagrangeconstraints
-import sharpy.utils.cout_utils as cout
-
-
-_BaseStructural = solver_from_string('_BaseStructural')
-
-
-@solver
-class NonLinearStaticMultibody(_BaseStructural):
-    """
-    Nonlinear static multibody
-
-    Nonlinear static solver for multibody structures.
-
-    """
-    solver_id = 'NonLinearStaticMultibody'
-    solver_classification = 'structural'
-
-    settings_types = _BaseStructural.settings_types.copy()
-    settings_default = _BaseStructural.settings_default.copy()
-    settings_description = _BaseStructural.settings_description.copy()
-
-    settings_table = settings.SettingsTable()
-    __doc__ += settings_table.generate(settings_types, settings_default, settings_description)
-
-    def __init__(self):
-        self.data = None
-        self.settings = None
-
-        # Total number of unknowns in the Multybody sistem
-        self.sys_size = None
-
-        # Total number of equations associated to the Lagrange multipliers
-        self.lc_list = None
-        self.num_LM_eq = None
-
-        # self.gamma = None
-        # self.beta = None
-
-    def initialise(self, data, custom_settings=None):
-
-        self.data = data
-        if custom_settings is None:
-            self.settings = data.settings[self.solver_id]
-        else:
-            self.settings = custom_settings
-        settings.to_custom_types(self.settings, self.settings_types, self.settings_default)
-
-        # load info from dyn dictionary
-        # self.data.structure.add_unsteady_information(
-        #     self.data.structure.dyn_dict, self.settings['num_steps'])
-        #
-        # # Define Newmark constants
-        # self.gamma = 0.5 + self.settings['newmark_damp']
-        # self.beta = 0.25*(self.gamma + 0.5)*(self.gamma + 0.5)
-
-        # Define the number of equations
-        self.lc_list = lagrangeconstraints.initialize_constraints(self.data.structure.ini_mb_dict)
-        self.num_LM_eq = lagrangeconstraints.define_num_LM_eq(self.lc_list)
-
-        # Define the number of dofs
-        self.define_sys_size()
-
-    def add_step(self):
-        self.data.structure.next_step()
-
-    def next_step(self):
-        pass
-
-    def define_sys_size(self):
-        """
-        This function defines the number of degrees of freedom in a multibody systems
-
-        Each body contributes with ``num_dof`` degrees of freedom and 10 more if the
-        associated local FoR can move or has Lagrange Constraints associated
-        """
-        MBdict = 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 (MBdict['body_%02d' % ibody]['FoR_movement'] == 'free'):
-                self.sys_size += 10
-
-    def assembly_MB_eq_system(self, MB_beam, MB_tstep, Lambda, MBdict, iLoadStep):
-        """
-        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.
-
-        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 array
-            MBdict (dict): Dictionary including the multibody information
-            iLoadStep (int): load step
-
-        Returns:
-            MB_K (np.ndarray): Matrix of the multibody system
-            MB_Q (np.ndarray): Vector of the systems of equations
-        """
-        self.lc_list = lagrangeconstraints.initialize_constraints(MBdict)
-        self.num_LM_eq = lagrangeconstraints.define_num_LM_eq(self.lc_list)
-
-        # MB_M = np.zeros((self.sys_size+self.num_LM_eq, self.sys_size+self.num_LM_eq), dtype=ct.c_double, order='F')
-        # MB_C = np.zeros((self.sys_size+self.num_LM_eq, self.sys_size+self.num_LM_eq), dtype=ct.c_double, order='F')
-        MB_K = np.zeros((self.sys_size+self.num_LM_eq, self.sys_size+self.num_LM_eq), dtype=ct.c_double, order='F')
-        # MB_Asys = np.zeros((self.sys_size+self.num_LM_eq, self.sys_size+self.num_LM_eq), dtype=ct.c_double, order='F')
-        MB_Q = np.zeros((self.sys_size+self.num_LM_eq,), dtype=ct.c_double, order='F')
-        #ipdb.set_trace()
-        first_dof = 0
-        last_dof = 0
-        # Loop through the different bodies
-        for ibody in range(len(MB_beam)):
-
-            # Initialize matrices
-            # M = None
-            # C = None
-            K = None
-            Q = None
-
-            # Generate the matrices for each body
-            if MB_beam[ibody].FoR_movement == 'prescribed':
-                last_dof = first_dof + MB_beam[ibody].num_dof.value
-            elif MB_beam[ibody].FoR_movement == 'free':
-                last_dof = first_dof + MB_beam[ibody].num_dof.value + 10
-
-            K, Q = xbeamlib.cbeam3_asbly_static(MB_beam[ibody], MB_tstep[ibody], self.settings, iLoadStep)
-
-            ############### Assembly into the global matrices
-            # Flexible and RBM contribution to Asys
-            # MB_M[first_dof:last_dof, first_dof:last_dof] = M.astype(dtype=ct.c_double, copy=True, order='F')
-            # MB_C[first_dof:last_dof, first_dof:last_dof] = C.astype(dtype=ct.c_double, copy=True, order='F')
-            MB_K[first_dof:last_dof, first_dof:last_dof] = K.astype(dtype=ct.c_double, copy=True, order='F')
-
-            #Q
-            MB_Q[first_dof:last_dof] = Q
-
-            first_dof = last_dof
-
-        # Define the number of equations
-        # Generate matrices associated to Lagrange multipliers
-        LM_C, LM_K, LM_Q = lagrangeconstraints.generate_lagrange_matrix(
-            self.lc_list,
-            MB_beam,
-            MB_tstep,
-            0,
-            self.num_LM_eq,
-            self.sys_size,
-            0.,
-            Lambda,
-            np.zeros_like(Lambda),
-            "static")
-
-        # Include the matrices associated to Lagrange Multipliers
-        # MB_C += LM_C
-        MB_K += LM_K
-        MB_Q += LM_Q
-
-        # MB_Asys = MB_K + MB_C*self.gamma/(self.beta*dt) + MB_M/(self.beta*dt*dt)
-
-        return MB_K, MB_Q
-
-    def integrate_position(self, MB_beam, MB_tstep, dt):
-        pass
-        # vel = np.zeros((6,),)
-        # acc = np.zeros((6,),)
-        # for ibody in range(0, len(MB_tstep)):
-        #     # I think this is the right way to do it, but to make it match the rest I change it temporally
-        #     if True:
-        #         # MB_tstep[ibody].mb_quat[ibody,:] =  algebra.quaternion_product(MB_tstep[ibody].quat, MB_tstep[ibody].mb_quat[ibody,:])
-        #         acc[0:3] = (0.5-self.beta)*np.dot(MB_beam[ibody].timestep_info.cga(),MB_beam[ibody].timestep_info.for_acc[0:3])+self.beta*np.dot(MB_tstep[ibody].cga(),MB_tstep[ibody].for_acc[0:3])
-        #         vel[0:3] = np.dot(MB_beam[ibody].timestep_info.cga(),MB_beam[ibody].timestep_info.for_vel[0:3])
-        #         MB_tstep[ibody].for_pos[0:3] += dt*(vel[0:3] + dt*acc[0:3])
-        #     else:
-        #         MB_tstep[ibody].for_pos[0:3] += dt*np.dot(MB_tstep[ibody].cga(),MB_tstep[ibody].for_vel[0:3])
-        #
-        # # Use next line for double pendulum (fix position of the second FoR)
-        # # MB_tstep[ibody].for_pos[0:3] = np.dot(algebra.quat2rotation(MB_tstep[0].quat), MB_tstep[0].pos[-1,:])
-        # # print("tip final pos: ", np.dot(algebra.quat2rotation(MB_tstep[0].quat), MB_tstep[0].pos[-1,:]))
-        # # print("FoR final pos: ", MB_tstep[ibody].for_pos[0:3])
-        # # print("pause")
-
-    def extract_resultants(self, tstep):
-        # TODO: code
-        return np.zeros((3,)), np.zeros((3,))
-
-    def update(self, tstep=None):
-        self.create_q_vector(tstep)
-
-    def create_q_vector(self, tstep=None):
-        import sharpy.structure.utils.xbeamlib as xb
-        if tstep is None:
-            tstep = self.data.structure.timestep_info[-1]
-
-        xb.xbeam_solv_disp2state(self.data.structure, tstep)
-
-    def compute_forces_constraints(self, MB_beam, MB_tstep, Lambda):
-        """
-        This function computes the forces generated at Lagrange Constraints
-
-        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(float): Time step increment
-            Lambda (np.ndarray): Lagrange Multipliers array
-            Lambda_dot (np.ndarray): Time derivarive of ``Lambda``
-
-        Notes:
-            This function is underdevelopment and not fully functional
-        """
-        try:
-            self.lc_list[0]
-        except IndexError:
-            return
-
-        # TODO the output of this routine is wrong. check at some point.
-        LM_C, LM_K, LM_Q = lagrangeconstraints.generate_lagrange_matrix(self.lc_list, MB_beam, MB_tstep, 0, self.num_LM_eq, self.sys_size, 0., Lambda, np.zeros_like(Lambda), "static")
-        F = -np.dot(LM_K[:, -self.num_LM_eq:], Lambda)
-
-        first_dof = 0
-        for ibody in range(len(MB_beam)):
-            # Forces associated to nodes
-            body_numdof = MB_beam[ibody].num_dof.value
-            body_freenodes = np.sum(MB_beam[ibody].vdof > -1)
-            last_dof = first_dof + body_numdof
-            MB_tstep[ibody].forces_constraints_nodes[(MB_beam[ibody].vdof > -1), :] = F[first_dof:last_dof].reshape(body_freenodes, 6, order='C')
-
-            # Forces associated to the frame of reference
-            if MB_beam[ibody].FoR_movement == 'free':
-                # TODO: How are the forces in the quaternion equation interpreted?
-                MB_tstep[ibody].forces_constraints_FoR[ibody, :] = F[last_dof:last_dof+10]
-                last_dof += 10
-
-            first_dof = last_dof
-            # print(MB_tstep[ibody].forces_constraints_nodes)
-        # TODO: right now, these forces are only used as an output, they are not read when the multibody is splitted
-
-    def run(self, structural_step=None, dt=None):
-        if structural_step is None:
-            structural_step = self.data.structure.timestep_info[-1]
-
-        if structural_step.mb_dict is not None:
-            MBdict = structural_step.mb_dict
-        else:
-            MBdict = self.data.structure.ini_mb_dict
-
-        if dt is None:
-            dt = ct.c_float(0.)
-
-        self.lc_list = lagrangeconstraints.initialize_constraints(MBdict)
-        self.num_LM_eq = lagrangeconstraints.define_num_LM_eq(self.lc_list)
-
-        # TODO: only working for constant forces
-        MB_beam, MB_tstep = mb.split_multibody(self.data.structure, structural_step, MBdict, 0)
-        q = np.zeros((self.sys_size + self.num_LM_eq,), dtype=ct.c_double, order='F')
-        dqdt = np.zeros((self.sys_size + self.num_LM_eq,), dtype=ct.c_double, order='F')
-        dqddt = np.zeros((self.sys_size + self.num_LM_eq,), dtype=ct.c_double, order='F')
-        mb.disp_and_accel2state(MB_beam, MB_tstep, q, dqdt, dqddt)
-        # Lagrange multipliers parameters
-        num_LM_eq = self.num_LM_eq
-        Lambda = np.zeros((num_LM_eq,), dtype=ct.c_double, order='F')
-        # Lambda_dot = np.zeros((num_LM_eq,), dtype=ct.c_double, order='F')
-        Dq_old = 0.
-        Dq = np.zeros((self.sys_size,))
-
-        for iLoadStep in range(0, self.settings['num_load_steps'].value + 1):
-            iter = -1
-            # delta = settings.min_delta + 1.
-            converged = False
-            while converged == False:
-                iter += 1
-                if (iter == self.settings['max_iterations'].value - 1):
-                    cout.cout_wrap(("Residual is: %f" % np.amax(np.abs(Dq))), 4)
-                    cout.cout_wrap("Static equations did not converge", 4)
-                    break
-
-                MB_K, MB_Q = self.assembly_MB_eq_system(MB_beam, MB_tstep, Lambda, MBdict, iLoadStep)
-                Dq = np.linalg.solve(MB_K, -MB_Q)
-
-                # Dq *= 0.7
-                q += Dq
-                mb.state2disp_and_accel(q, dqdt, dqddt, MB_beam, MB_tstep)
-
-                if (iter > 0):
-                    if (np.amax(np.abs(Dq)) < Dq_old):
-                        converged = True
-
-                if iter == 0:
-                    Dq_old = np.amax(np.array([1., np.amax(np.abs(Dq))]))*self.settings['min_delta'].value
-
-                lagrangeconstraints.postprocess(self.lc_list, MB_beam, MB_tstep, "static")
-
-        self.compute_forces_constraints(MB_beam, MB_tstep, Lambda)
-        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, MBdict, 0.)
-
-        # # Initialize
-        # q = np.zeros((self.sys_size + num_LM_eq,), dtype=ct.c_double, order='F')
-        # dqdt = np.zeros((self.sys_size + num_LM_eq,), dtype=ct.c_double, order='F')
-        # dqddt = np.zeros((self.sys_size + num_LM_eq,), dtype=ct.c_double, order='F')
-        #
-        # # Predictor step
-        # mb.disp2state(MB_beam, MB_tstep, q, dqdt, dqddt)
-        #
-        # q += dt*dqdt + (0.5 - self.beta)*dt*dt*dqddt
-        # dqdt += (1.0 - self.gamma)*dt*dqddt
-        # dqddt = np.zeros((self.sys_size + num_LM_eq,), dtype=ct.c_double, order='F')
-        # if not num_LM_eq == 0:
-        #     Lambda = q[-num_LM_eq:].astype(dtype=ct.c_double, copy=True, order='F')
-        #     Lambda_dot = dqdt[-num_LM_eq:].astype(dtype=ct.c_double, copy=True, order='F')
-        # else:
-        #     Lambda = 0
-        #     Lambda_dot = 0
-        #
-        # # Newmark-beta iterations
-        # old_Dq = 1.0
-        # LM_old_Dq = 1.0
-        #
-        # converged = False
-        # for iter in range(self.settings['max_iterations'].value):
-        #     # Check if the maximum of iterations has been reached
-        #     if (iter == self.settings['max_iterations'].value - 1):
-        #         print('Solver did not converge in ', iter, ' iterations.')
-        #         print('res = ', res)
-        #         print('LM_res = ', LM_res)
-        #         import pdb; pdb.set_trace()
-        #         break
-        #
-        #     # Update positions and velocities
-        #     mb.state2disp(q, dqdt, dqddt, MB_beam, MB_tstep)
-        #     MB_Asys, MB_Q = self.assembly_MB_eq_system(MB_beam, MB_tstep, self.data.ts, dt, Lambda, Lambda_dot, MBdict)
-        #
-        #     # Compute the correction
-        #     # ADC next line not necessary
-        #     # Dq = np.zeros((self.sys_size+num_LM_eq,), dtype=ct.c_double, order='F')
-        #     # MB_Asys_balanced, T = scipy.linalg.matrix_balance(MB_Asys)
-        #     # invT = np.matrix(T).I
-        #     # MB_Q_balanced = np.dot(invT, MB_Q).T
-        #
-        #     Dq = np.linalg.solve(MB_Asys, -MB_Q)
-        #     # least squares solver
-        #     # Dq = np.linalg.lstsq(np.dot(MB_Asys_balanced, invT), -MB_Q_balanced, rcond=None)[0]
-        #
-        #     # Evaluate convergence
-        #     if (iter > 0):
-        #         res = np.max(np.abs(Dq[0:self.sys_size]))/old_Dq
-        #         if not num_LM_eq == 0:
-        #             LM_res = np.max(np.abs(Dq[self.sys_size:self.sys_size+num_LM_eq]))/LM_old_Dq
-        #         else:
-        #             LM_res = 0.0
-        #         if (res < self.settings['min_delta'].value) and (LM_res < self.settings['min_delta'].value*1e-2):
-        #             converged = True
-        #
-        #     # Compute variables from previous values and increments
-        #     # TODO:decide If I want other way of updating lambda
-        #     # this for least sq
-        #     # q[:, np.newaxis] += Dq
-        #     # dqdt[:, np.newaxis] += self.gamma/(self.beta*dt)*Dq
-        #     # dqddt[:, np.newaxis] += 1.0/(self.beta*dt*dt)*Dq
-        #
-        #     # this for direct solver
-        #     q += Dq
-        #     dqdt += self.gamma/(self.beta*dt)*Dq
-        #     dqddt += 1.0/(self.beta*dt*dt)*Dq
-        #
-        #     if not num_LM_eq == 0:
-        #         Lambda = q[-num_LM_eq:].astype(dtype=ct.c_double, copy=True, order='F')
-        #         Lambda_dot = dqdt[-num_LM_eq:].astype(dtype=ct.c_double, copy=True, order='F')
-        #     else:
-        #         Lambda = 0
-        #         Lambda_dot = 0
-        #
-        #     if converged:
-        #         break
-        #
-        #     if iter == 0:
-        #         old_Dq = np.max(np.abs(Dq[0:self.sys_size]))
-        #         if old_Dq < 1.0:
-        #             old_Dq = 1.0
-        #         if num_LM_eq:
-        #             LM_old_Dq = np.max(np.abs(Dq[self.sys_size:self.sys_size+num_LM_eq]))
-        #         else:
-        #             LM_old_Dq = 1.0
-        #
-        # mb.state2disp(q, dqdt, dqddt, MB_beam, MB_tstep)
-        # # end: comment time stepping
-        #
-        # # End of Newmark-beta iterations
-        # self.integrate_position(MB_beam, MB_tstep, dt)
-        # # lagrangeconstraints.postprocess(self.lc_list, MB_beam, MB_tstep, MBdict, "dynamic")
-        # lagrangeconstraints.postprocess(self.lc_list, MB_beam, MB_tstep, "dynamic")
-        # self.compute_forces_constraints(MB_beam, MB_tstep, self.data.ts, dt, Lambda, Lambda_dot)
-        # 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, MBdict, dt)
-
-        # structural_step.q[:] = q[:self.sys_size].copy()
-        # structural_step.dqdt[:] = dqdt[:self.sys_size].copy()
-        # structural_step.dqddt[:] = dqddt[:self.sys_size].copy()
-
-        return self.data
diff --git a/sharpy/solvers/timeintegrators.py b/sharpy/solvers/timeintegrators.py
new file mode 100644
index 000000000..9b8412dfe
--- /dev/null
+++ b/sharpy/solvers/timeintegrators.py
@@ -0,0 +1,239 @@
+import numpy as np
+import ctypes as ct
+
+import sharpy.utils.settings as settings
+from sharpy.utils.solver_interface import solver
+
+
+@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
+
+
+    def initialise(self, data, custom_settings=None):
+        pass
+
+
+    def predictor(self, q, dqdt, dqddt):
+        pass
+
+
+    def build_matrix(self, M, C, K):
+        pass
+
+
+    def corrector(self, q, dqdt, dqddt, Dq):
+        pass
+
+
+@solver
+class NewmarkBeta(_BaseTimeIntegrator):
+    """
+    Time integration according to the Newmark-beta scheme
+    """
+
+    solver_id = 'NewmarkBeta'
+    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 contraint equations'
+
+    def __init__(self):
+
+        self.dt = None
+        self.beta = None
+        self.gamma = None
+
+    def initialise(self, data, custom_settings=None):
+
+        if custom_settings is None:
+            self.settings = data.settings[self.solver_id]
+        else:
+            self.settings = custom_settings
+        settings.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):
+
+        sys_size = self.sys_size
+        q[:sys_size] += self.dt*dqdt[:sys_size] + (0.5 - self.beta)*self.dt*self.dt*dqddt[:sys_size]
+        dqdt[:sys_size] += (1.0 - self.gamma)*self.dt*dqddt[:sys_size]
+        dqddt *= 0.
+
+        # q[sys_size:] = q[sys_size:]
+        dqdt[sys_size:] = dqdt[sys_size:]
+
+    def build_matrix(self, M, C, K, Q, kBnh, LM_Q):
+
+        sys_size = self.sys_size
+        num_LM_eq = self.num_LM_eq
+
+        Asys = np.zeros((sys_size + num_LM_eq, sys_size + num_LM_eq),
+                         dtype=ct.c_double, order='F')
+        Qout = np.zeros((sys_size + num_LM_eq), dtype=ct.c_double, order='F')
+
+        Asys[:sys_size, :sys_size] = K + C*self.gamma/(self.beta*self.dt) + M/(self.beta*self.dt*self.dt)
+        Qout[:sys_size] = Q.copy()
+
+        Asys[sys_size:, :sys_size] = (self.gamma/self.beta/self.dt)*kBnh
+        Asys[:sys_size, sys_size:] = kBnh.T
+        Qout[sys_size:] = LM_Q.copy()
+
+        return Asys, Qout
+
+    def corrector(self, q, dqdt, dqddt, Dq):
+
+        sys_size = self.sys_size
+
+        q[:sys_size] += Dq[:sys_size]
+        dqdt[:sys_size] += self.gamma/(self.beta*self.dt)*Dq[:sys_size]
+        dqddt[:sys_size] += 1.0/(self.beta*self.dt*self.dt)*Dq[:sys_size]
+
+        dqdt[sys_size:] += Dq[sys_size:]
+
+
+@solver
+class GeneralisedAlpha(_BaseTimeIntegrator):
+    """
+    Time integration according to the Generalised-Alpha scheme
+    """
+
+    solver_id = 'GeneralisedAlpha'
+    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):
+
+        self.dt = None
+        self.am = None
+        self.af = None
+        self.gamma = None
+        self.beta = None
+
+    def initialise(self, data, custom_settings=None):
+
+        if custom_settings is None:
+            self.settings = data.settings[self.solver_id]
+        else:
+            self.settings = custom_settings
+        settings.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, dqdt, dqddt):
+
+        sys_size = self.sys_size
+        q[:sys_size] += self.dt*dqdt[:sys_size] + (0.5 - self.beta)*self.dt*self.dt*dqddt[:sys_size]
+        dqdt[:sys_size] += (1.0 - self.gamma)*self.dt*dqddt[:sys_size]
+        dqddt *= 0.
+
+        dqdt[sys_size:] = dqdt[sys_size:]
+
+    def build_matrix(self, M, C, K, Q, kBnh, LM_Q):
+
+        sys_size = self.sys_size
+        num_LM_eq = self.num_LM_eq
+
+        Asys = np.zeros((sys_size + num_LM_eq, sys_size + num_LM_eq),
+                         dtype=ct.c_double, order='F')
+        Qout = np.zeros((sys_size + num_LM_eq), dtype=ct.c_double, order='F')
+
+        Asys[:sys_size, :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)
+
+        Qout[:sys_size] = Q.copy()
+
+        Asys[sys_size:, :sys_size] = (self.gamma*self.om_af/self.beta/self.dt)*kBnh
+        Asys[:sys_size, sys_size:] = kBnh.T
+        Qout[sys_size:] = LM_Q.copy()
+
+        return Asys, Qout
+
+    def corrector(self, q, dqdt, dqddt, Dq):
+
+        sys_size = self.sys_size
+
+        q[:sys_size] += self.om_af*Dq[:sys_size]
+        dqdt[:sys_size] += self.gamma*self.om_af/self.beta/self.dt*Dq[:sys_size]
+        dqddt[:sys_size] += self.om_am/self.beta/self.dt**2*Dq[:sys_size]
+       
+        dqdt[sys_size:] += Dq[sys_size:]
diff --git a/sharpy/structure/utils/lagrangeconstraints.py b/sharpy/structure/utils/lagrangeconstraints.py
index 85a154585..f1e6e5170 100644
--- a/sharpy/structure/utils/lagrangeconstraints.py
+++ b/sharpy/structure/utils/lagrangeconstraints.py
@@ -39,7 +39,7 @@
 import os
 import ctypes as ct
 import numpy as np
-import sharpy.utils.algebra as algebra
+import sharpy.utils.algebra as ag
 
 ###############################################################################
 # Basic structures
@@ -142,8 +142,7 @@ def initialise(self, MBdict_entry, ieq):
     @abstractmethod
     # def staticmat(self, **kwargs):
     def staticmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq,
-                  sys_size, dt, Lambda, Lambda_dot,
-                  scalingFactor, penaltyFactor):
+                  sys_size, dt, Lambda, Lambda_dot):
         """
         Generates the structural matrices (damping, stiffness) and the independent vector
         associated to the LagrangeConstraint in a static simulation
@@ -153,8 +152,7 @@ def staticmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq,
     @abstractmethod
     # def dynamicmat(self, **kwargs):
     def dynamicmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq,
-                   sys_size, dt, Lambda, Lambda_dot,
-                  scalingFactor, penaltyFactor):
+                   sys_size, dt, Lambda, Lambda_dot):
         """
         Generates the structural matrices (damping, stiffness) and the independent vector
         associated to the LagrangeConstraint in a dynamic simulation
@@ -226,9 +224,81 @@ def define_FoR_dof(MB_beam, FoR_body):
     return FoR_dof
 
 
+def set_value_or_default(dictionary, key, default_val):
+    try:
+        value = dictionary[key]
+    except KeyError:
+        value = default_val
+    return value
+
 ################################################################################
 # Equations
 ################################################################################
+def equal_pos_node_FoR(MB_tstep, MB_beam, FoR_body, node_body, inode_in_body, node_FoR_dof, node_dof, FoR_dof, sys_size, Lambda, scalingFactor, penaltyFactor, ieq, LM_K, LM_C, LM_Q):
+    """
+    This function generates the stiffness and damping matrices and the independent vector associated to a constraint that
+    imposes equal positions between a node and a frame of reference
+
+    See ``LagrangeConstraints`` for the description of variables
+
+    Args:
+        node_FoR_dof (int): position of the first degree of freedom of the FoR to which the "node" belongs
+        node_dof (int): position of the first degree of freedom associated to the "node"
+        FoR_body (int): body number of the "FoR"
+        FoR_dof (int): position of the first degree of freedom associated to the "FoR"
+    """
+    num_LM_eq_specific = 3
+    Bnh = np.zeros((num_LM_eq_specific, sys_size), dtype=ct.c_double, order = 'F')
+    B = np.zeros((num_LM_eq_specific, sys_size), dtype=ct.c_double, order = 'F')
+
+    # Simplify notation
+    node_cga = MB_tstep[node_body].cga()
+    node_pos = MB_tstep[node_body].pos[inode_in_body, :]
+    node_FoR_pos = MB_tstep[node_body].for_pos[0:3]
+    FoR_pos = MB_tstep[FoR_body].for_pos[0:3]
+
+    # if MB_beam[node_body].FoR_movement == 'free':
+    B[:, node_FoR_dof:node_FoR_dof+3] = np.eye(3)
+    B[:, node_dof:node_dof+3] = node_cga
+    B[:, FoR_dof:FoR_dof+3] = -np.eye(3)
+
+    LM_K[sys_size + ieq : sys_size + ieq + num_LM_eq_specific, :sys_size] += scalingFactor*B
+    LM_K[:sys_size, sys_size + ieq : sys_size + ieq + num_LM_eq_specific] += scalingFactor*np.transpose(B)
+
+    LM_Q[:sys_size] += scalingFactor*np.dot(np.transpose(B), Lambda[ieq:ieq+num_LM_eq_specific])
+    LM_Q[sys_size+ieq:sys_size+ieq+num_LM_eq_specific] += scalingFactor*(node_FoR_pos +
+                                                                         np.dot(node_cga, node_pos) -
+                                                                         FoR_pos)
+
+    LM_C[node_dof:node_dof+3, node_FoR_dof+6:node_FoR_dof+10] += scalingFactor*ag.der_CquatT_by_v(MB_tstep[node_body].quat, Lambda[ieq : ieq + num_LM_eq_specific])
+
+    if penaltyFactor:
+        q = np.zeros((sys_size, ))
+        q[node_FoR_dof:node_FoR_dof+3] = node_FoR_pos
+        q[node_dof:node_dof+3] = node_pos
+        q[FoR_dof:FoR_dof+3] = FoR_pos
+
+        LM_Q[:sys_size] += penaltyFactor*np.dot(B.T, np.dot(B, q))
+
+        LM_K[node_FoR_dof:node_FoR_dof+3, node_FoR_dof:node_FoR_dof+3] += penaltyFactor*np.eye(3)
+        LM_K[node_FoR_dof:node_FoR_dof+3, node_dof:node_dof+3] += penaltyFactor*node_cga
+        LM_K[node_FoR_dof:node_FoR_dof+3, FoR_dof:FoR_dof+3] += -penaltyFactor*np.eye(3)
+        LM_C[node_FoR_dof:node_FoR_dof+3, node_FoR_dof+6:node_FoR_dof+10] += penaltyFactor*ag.der_Cquat_by_v(MB_tstep[node_body].quat, node_pos)
+
+        LM_K[node_dof:node_dof+3, node_FoR_dof:node_FoR_dof+3] += penaltyFactor*node_cga.T
+        LM_K[node_dof:node_dof+3, node_dof:node_dof+3] += penaltyFactor*np.eye(3)
+        LM_K[node_dof:node_dof+3, FoR_dof:FoR_dof+3] += -penaltyFactor*node_cga.T
+        LM_C[node_dof:node_dof+3, node_FoR_dof+6:node_FoR_dof+10] += penaltyFactor*(ag.der_CquatT_by_v(MB_tstep[node_body].quat, node_FoR_pos - FoR_pos))
+
+        LM_K[FoR_dof:FoR_dof+3, node_FoR_dof:node_FoR_dof+3] += -penaltyFactor*np.eye(3)
+        LM_K[FoR_dof:FoR_dof+3, node_dof:node_dof+3] += -penaltyFactor*node_cga.T
+        LM_K[FoR_dof:FoR_dof+3, FoR_dof:FoR_dof+3] += penaltyFactor*np.eye(3)
+        LM_C[FoR_dof:FoR_dof+3, node_FoR_dof+6:node_FoR_dof+10] += -penaltyFactor*ag.der_Cquat_by_v(MB_tstep[node_body].quat, node_pos)
+
+    ieq += 3
+    return ieq
+
+
 def equal_lin_vel_node_FoR(MB_tstep, MB_beam, FoR_body, node_body, node_number, node_FoR_dof, node_dof, FoR_dof, sys_size, Lambda_dot, scalingFactor, penaltyFactor, ieq, LM_K, LM_C, LM_Q):
     """
     This function generates the stiffness and damping matrices and the independent vector associated to a constraint that
@@ -244,55 +314,102 @@ def equal_lin_vel_node_FoR(MB_tstep, MB_beam, FoR_body, node_body, node_number,
         FoR_body (int): body number of the "FoR"
         FoR_dof (int): position of the first degree of freedom associated to the "FoR"
     """
-    # Variables names. The naming of the variables can be quite confusing. The reader should think that
-    # the BC relates one "node" and one "FoR" (writen between quotes in these lines).
-    # If a variable is related to one of them starts with "node_" or "FoR_" respectively
-    # node_number: number of the "node" within its own body
-    # node_body: body number of the "node"
-    # node_FoR_dof: position of the first degree of freedom of the FoR to which the "node" belongs
-    # node_dof: position of the first degree of freedom associated to the "node"
-    # FoR_body: body number of the "FoR"
-    # FoR_dof: position of the first degree of freedom associated to the "FoR"
 
     num_LM_eq_specific = 3
     Bnh = np.zeros((num_LM_eq_specific, sys_size), dtype=ct.c_double, order = 'F')
     B = np.zeros((num_LM_eq_specific, sys_size), dtype=ct.c_double, order = 'F')
 
-    Bnh[:, FoR_dof:FoR_dof+3] = algebra.quat2rotation(MB_tstep[FoR_body].quat)
+    # Simplify notation
+    node_cga = MB_tstep[node_body].cga()
+    node_FoR_va = MB_tstep[node_body].for_vel[0:3]
+    node_FoR_wa = MB_tstep[node_body].for_vel[3:6]
+    node_Ra = MB_tstep[node_body].pos[node_number,:]
+    node_dot_Ra = MB_tstep[node_body].pos_dot[node_number,:]
+
+    FoR_cga = MB_tstep[FoR_body].cga()
+    FoR_va = MB_tstep[FoR_body].for_vel[0:3]
 
-    Bnh[:, node_dof:node_dof+3] = -1.0*algebra.quat2rotation(MB_tstep[node_body].quat)
+    Bnh[:, FoR_dof:FoR_dof+3] = FoR_cga
+    Bnh[:, node_dof:node_dof+3] = -1.0*node_cga
     if MB_beam[node_body].FoR_movement == 'free':
-        Bnh[:, node_FoR_dof:node_FoR_dof+3] = -1.0*algebra.quat2rotation(MB_tstep[node_body].quat)
-        Bnh[:, node_FoR_dof+3:node_FoR_dof+6] = 1.0*np.dot(algebra.quat2rotation(MB_tstep[node_body].quat),algebra.skew(MB_tstep[node_body].pos[node_number,:]))
+        Bnh[:, node_FoR_dof:node_FoR_dof+3] = -1.0*node_cga
+        Bnh[:, node_FoR_dof+3:node_FoR_dof+6] = np.dot(node_cga,ag.skew(node_Ra))
 
     LM_C[sys_size+ieq:sys_size+ieq+num_LM_eq_specific,:sys_size] += scalingFactor*Bnh
     LM_C[:sys_size,sys_size+ieq:sys_size+ieq+num_LM_eq_specific] += scalingFactor*np.transpose(Bnh)
 
-    LM_Q[:sys_size] += scalingFactor*np.dot(np.transpose(Bnh),Lambda_dot[ieq:ieq+num_LM_eq_specific])
-    LM_Q[sys_size+ieq:sys_size+ieq+num_LM_eq_specific] += (np.dot(algebra.quat2rotation(MB_tstep[FoR_body].quat),MB_tstep[FoR_body].for_vel[0:3]) +
-                                                          -1.0*np.dot(algebra.quat2rotation(MB_tstep[node_body].quat),
-                                                                      MB_tstep[node_body].pos_dot[node_number,:] +
-                                                                      MB_tstep[node_body].for_vel[0:3] +
-                                                                      -1.0*np.dot(algebra.skew(MB_tstep[node_body].pos[node_number,:]),MB_tstep[node_body].for_vel[3:6])))
+    LM_Q[:sys_size] += scalingFactor*np.dot(np.transpose(Bnh), Lambda_dot[ieq:ieq+num_LM_eq_specific])
+    LM_Q[sys_size+ieq:sys_size+ieq+num_LM_eq_specific] += scalingFactor*(np.dot(FoR_cga, FoR_va) +
+                                                          -1.0*np.dot(node_cga,
+                                                                      node_dot_Ra +
+                                                                      node_FoR_va +
+                                                                      -1.0*np.dot(ag.skew(node_Ra), node_FoR_wa)))
 
-    LM_C[FoR_dof:FoR_dof+3,FoR_dof+6:FoR_dof+10] += algebra.der_CquatT_by_v(MB_tstep[FoR_body].quat,scalingFactor*Lambda_dot[ieq:ieq+num_LM_eq_specific])
+    LM_C[FoR_dof:FoR_dof+3, FoR_dof+6:FoR_dof+10] += scalingFactor*ag.der_CquatT_by_v(MB_tstep[FoR_body].quat, Lambda_dot[ieq:ieq+num_LM_eq_specific])
 
     if MB_beam[node_body].FoR_movement == 'free':
-        LM_C[node_dof:node_dof+3,node_FoR_dof+6:node_FoR_dof+10] -= algebra.der_CquatT_by_v(MB_tstep[node_body].quat,scalingFactor*Lambda_dot[ieq:ieq+num_LM_eq_specific])
+        LM_C[node_dof:node_dof+3,node_FoR_dof+6:node_FoR_dof+10] -= scalingFactor*ag.der_CquatT_by_v(MB_tstep[node_body].quat, Lambda_dot[ieq:ieq+num_LM_eq_specific])
+
+        LM_C[node_FoR_dof:node_FoR_dof+3,node_FoR_dof+6:node_FoR_dof+10] -= scalingFactor*ag.der_CquatT_by_v(MB_tstep[node_body].quat,Lambda_dot[ieq:ieq+num_LM_eq_specific])
+
+        LM_C[node_FoR_dof+3:node_FoR_dof+6,node_FoR_dof+6:node_FoR_dof+10] += scalingFactor*np.dot(ag.skew(node_Ra).T,
+                                                                                     ag.der_CquatT_by_v(MB_tstep[node_body].quat,
+                                                                                                             Lambda_dot[ieq:ieq+num_LM_eq_specific]))
+
+        LM_K[node_FoR_dof+3:node_FoR_dof+6,node_dof:node_dof+3] += scalingFactor*ag.skew(np.dot(node_cga.T,Lambda_dot[ieq:ieq+num_LM_eq_specific]))
 
-        LM_C[node_FoR_dof:node_FoR_dof+3,node_FoR_dof+6:node_FoR_dof+10] -= algebra.der_CquatT_by_v(MB_tstep[node_body].quat,scalingFactor*Lambda_dot[ieq:ieq+num_LM_eq_specific])
+    if penaltyFactor:
+        q = np.zeros((sys_size))
+        q[FoR_dof:FoR_dof+3] = FoR_va
+        q[node_dof:node_dof+3] = node_dot_Ra
+        if MB_beam[node_body].FoR_movement == 'free':
+            q[node_FoR_dof:node_FoR_dof+3] = node_FoR_va
+            q[node_FoR_dof+3:node_FoR_dof+6] = node_FoR_wa
 
-        LM_C[node_FoR_dof+3:node_FoR_dof+6,node_FoR_dof+6:node_FoR_dof+10] -= np.dot(algebra.skew(MB_tstep[node_body].pos[node_number,:]),
-                                                                                     algebra.der_CquatT_by_v(MB_tstep[node_body].quat,
-                                                                                                             scalingFactor*Lambda_dot[ieq:ieq+num_LM_eq_specific]))
+        LM_Q[:sys_size] += penaltyFactor*np.dot(np.dot(Bnh.T, Bnh), q)
 
-        LM_K[node_FoR_dof+3:node_FoR_dof+6,node_dof:node_dof+3] += algebra.skew(np.dot(algebra.quat2rotation(MB_tstep[node_body].quat).T,Lambda_dot[ieq:ieq+num_LM_eq_specific]))
+        LM_C[:sys_size, :sys_size] += penaltyFactor*np.dot(Bnh.T, Bnh)
+
+        # Derivatives wrt the FoR quaterion
+        LM_C[FoR_dof:FoR_dof+3, FoR_dof+6:FoR_dof+10] -= penaltyFactor*ag.der_CquatT_by_v(MB_tstep[FoR_body].quat,
+                                                                                                 np.dot(node_cga, node_dot_Ra + node_FoR_va +
+                                                                                                         np.dot(ag.skew(node_Ra), node_FoR_wa)))
+
+        LM_C[node_dof:node_dof+3, FoR_dof+6:FoR_dof+10] -= penaltyFactor*np.dot(node_cga.T, ag.der_CquatT_by_v(MB_tstep[FoR_body].quat,
+                                                                                                               FoR_va))
+
+        if MB_beam[node_body].FoR_movement == 'free':
+            LM_C[node_FoR_dof:node_FoR_dof+3, FoR_dof+6:FoR_dof+10] -= penaltyFactor*np.dot(node_cga.T, ag.der_CquatT_by_v(MB_tstep[FoR_body].quat,
+                                                                                                               FoR_va))
+
+            mat = ag.multiply_matrices(ag.skew(node_Ra).T, node_cga.T)
+            LM_C[node_FoR_dof+3:node_FoR_dof+6, FoR_dof+6:FoR_dof+10] += penaltyFactor*np.dot(mat, ag.der_CquatT_by_v(MB_tstep[FoR_body].quat,
+                                                                                                               FoR_va))
+
+        # Derivatives wrt the node quaternion
+        if MB_beam[node_body].FoR_movement == 'free':
+            vec = -node_dot_Ra - node_FoR_va + np.dot(ag.skew(node_Ra), node_FoR_wa)
+            LM_C[FoR_dof:FoR_dof+3, node_FoR_dof+6:node_FoR_dof+10] += penaltyFactor*np.dot(FoR_cga.T, ag.der_Cquat_by_v(MB_tstep[node_body].quat, vec))
+
+            derivative = -ag.der_CquatT_by_v(MB_tstep[node_body].quat, np.dot(FoR_cga, FoR_va))
+            LM_C[node_dof:node_dof+3, node_FoR_dof+6:node_FoR_dof+10] += penaltyFactor*derivative
+            LM_C[node_FoR_dof:node_FoR_dof+3, node_FoR_dof+6:node_FoR_dof+10] += penaltyFactor*derivative
+            LM_C[node_FoR_dof+3:node_FoR_dof+6, node_FoR_dof+6:node_FoR_dof+10] -= penaltyFactor*np.dot(ag.skew(node_Ra), derivative)
+
+        # Derivatives wrt the node Ra
+        LM_K[FoR_dof:FoR_dof+3, node_dof:node_dof+3] -= penaltyFactor*ag.multiply_matrices(FoR_cga.T, node_cga, ag.skew(node_FoR_wa))
+        LM_K[node_dof:node_dof+3, node_dof:node_dof+3] += penaltyFactor*ag.skew(node_FoR_wa)
+        if MB_beam[node_body].FoR_movement == 'free':
+            LM_K[node_FoR_dof:node_FoR_dof+3, node_dof:node_dof+3] += penaltyFactor*ag.skew(node_FoR_wa)
+            vec = ag.multiply_matrices(node_cga.T, FoR_cga, FoR_va) - node_dot_Ra - node_FoR_va
+            LM_K[node_FoR_dof+3:node_FoR_dof+6, node_dof:node_dof+3] += penaltyFactor*ag.skew(vec)
+            LM_K[node_FoR_dof+3:node_FoR_dof+6, node_dof:node_dof+3] -= penaltyFactor*ag.der_skewp_skewp_v(node_Ra, node_FoR_wa)
 
     ieq += 3
     return ieq
 
 
-def def_rot_axis_FoR_wrt_node(MB_tstep, MB_beam, FoR_body, node_body, node_number, node_FoR_dof, node_dof, FoR_dof, sys_size, Lambda_dot, rot_axisB, scalingFactor, penaltyFactor, ieq, LM_K, LM_C, LM_Q, indep):
+def def_rot_axis_FoR_wrt_node_general(MB_tstep, MB_beam, FoR_body, node_body, node_number, node_FoR_dof, node_dof, FoR_dof, sys_size, Lambda_dot, rot_axisB, scalingFactor, penaltyFactor, ieq, LM_K, LM_C, LM_Q, indep):
     """
     This function generates the stiffness and damping matrices and the independent vector associated to a joint that
     forces the rotation axis of a FoR to be parallel to a certain direction. This direction is defined in the
@@ -311,23 +428,19 @@ def def_rot_axis_FoR_wrt_node(MB_tstep, MB_beam, FoR_body, node_body, node_numbe
         FoR_dof (int): position of the first degree of freedom associated to the "FoR"
     """
 
-    # Variables names. The naming of the variables can be quite confusing. The reader should think that
-    # the BC relates one "node" and one "FoR" (writen between quotes in these lines).
-    # If a variable is related to one of them starts with "node_" or "FoR_" respectively
-    # node_number: number of the "node" within its own body
-    # node_body: body number of the "node"
-    # node_FoR_dof: position of the first degree of freedom of the FoR to which the "node" belongs
-    # node_dof: position of the first degree of freedom associated to the "node"
-    # FoR_body: body number of the "FoR"
-    # FoR_dof: position of the first degree of freedom associated to the "FoR"
-
     ielem, inode_in_elem = MB_beam[node_body].node_master_elem[node_number]
 
+    # Simplify notation
+    cab = ag.crv2rotation(MB_tstep[node_body].psi[ielem,inode_in_elem,:])
+    node_cga = MB_tstep[node_body].cga()
+    FoR_cga = MB_tstep[FoR_body].cga()
+    FoR_wa = MB_tstep[FoR_body].for_vel[3:6]
+
     if not indep:
-        aux_Bnh = algebra.multiply_matrices(algebra.skew(rot_axisB),
-                                  algebra.crv2rotation(MB_tstep[node_body].psi[ielem,inode_in_elem,:]).T,
-                                  algebra.quat2rotation(MB_tstep[node_body].quat).T,
-                                  algebra.quat2rotation(MB_tstep[FoR_body].quat))
+        aux_Bnh = ag.multiply_matrices(ag.skew(rot_axisB),
+                                  cab.T,
+                                  node_cga.T,
+                                  FoR_cga)
 
         # indep = None
         n0 = np.linalg.norm(aux_Bnh[0,:])
@@ -357,41 +470,236 @@ def def_rot_axis_FoR_wrt_node(MB_tstep, MB_beam, FoR_body, node_body, node_numbe
     # Lambda_dot[ieq:ieq+num_LM_eq_specific]
     # np.concatenate((Lambda_dot[ieq:ieq+num_LM_eq_specific], np.array([0.])))
 
-    # print(indep)
-    Bnh[:, FoR_dof+3:FoR_dof+6] = algebra.multiply_matrices(algebra.skew(rot_axisB),
-                                                  algebra.crv2rotation(MB_tstep[node_body].psi[ielem,inode_in_elem,:]).T,
-                                                  algebra.quat2rotation(MB_tstep[node_body].quat).T,
-                                                  algebra.quat2rotation(MB_tstep[FoR_body].quat))[indep,:]
+    Bnh[:, FoR_dof+3:FoR_dof+6] = ag.multiply_matrices(ag.skew(rot_axisB),
+                                                       cab.T,
+                                                       node_cga.T,
+                                                       FoR_cga)[indep,:]
 
     # Constrain angular velocities
-    LM_Q[:sys_size] += scalingFactor*np.dot(np.transpose(Bnh),Lambda_dot[ieq:ieq+num_LM_eq_specific])
-    LM_Q[sys_size+ieq:sys_size+ieq+num_LM_eq_specific] += algebra.multiply_matrices(algebra.skew(rot_axisB),
-                                                  algebra.crv2rotation(MB_tstep[node_body].psi[ielem,inode_in_elem,:]).T,
-                                                  algebra.quat2rotation(MB_tstep[node_body].quat).T,
-                                                  algebra.quat2rotation(MB_tstep[FoR_body].quat),
-                                                  MB_tstep[FoR_body].for_vel[3:6])[indep]
+    LM_Q[:sys_size] += scalingFactor*np.dot(np.transpose(Bnh), Lambda_dot[ieq:ieq+num_LM_eq_specific])
+    LM_Q[sys_size+ieq:sys_size+ieq+num_LM_eq_specific] += scalingFactor*ag.multiply_matrices(ag.skew(rot_axisB),
+                                                  cab.T,
+                                                  node_cga.T,
+                                                  FoR_cga,
+                                                  FoR_wa)[indep]
 
     LM_C[sys_size+ieq:sys_size+ieq+num_LM_eq_specific,:sys_size] += scalingFactor*Bnh
     LM_C[:sys_size,sys_size+ieq:sys_size+ieq+num_LM_eq_specific] += scalingFactor*np.transpose(Bnh)
 
     if MB_beam[node_body].FoR_movement == 'free':
-        LM_C[FoR_dof+3:FoR_dof+6,node_FoR_dof+6:node_FoR_dof+10] += np.dot(algebra.quat2rotation(MB_tstep[FoR_body].quat).T,
-                                                                           algebra.der_Cquat_by_v(MB_tstep[node_body].quat,
-                                                                                                  algebra.multiply_matrices(algebra.crv2rotation(MB_tstep[node_body].psi[ielem,inode_in_elem,:]),
-                                                                                                                    algebra.skew(rot_axisB).T,
+        LM_C[FoR_dof+3:FoR_dof+6,node_FoR_dof+6:node_FoR_dof+10] += scalingFactor*np.dot(FoR_cga.T,
+                                                                           ag.der_Cquat_by_v(MB_tstep[node_body].quat,
+                                                                                                  ag.multiply_matrices(cab,
+                                                                                                                    ag.skew(rot_axisB).T,
                                                                                                                     new_Lambda_dot)))
 
-    LM_C[FoR_dof+3:FoR_dof+6,FoR_dof+6:FoR_dof+10] += algebra.der_CquatT_by_v(MB_tstep[FoR_body].quat,
-                                                                              algebra.multiply_matrices(algebra.quat2rotation(MB_tstep[node_body].quat),
-                                                                                                algebra.crv2rotation(MB_tstep[node_body].psi[ielem,inode_in_elem,:]),
-                                                                                                algebra.skew(rot_axisB).T,
+    LM_C[FoR_dof+3:FoR_dof+6,FoR_dof+6:FoR_dof+10] += scalingFactor*ag.der_CquatT_by_v(MB_tstep[FoR_body].quat,
+                                                                              ag.multiply_matrices(node_cga,
+                                                                                                cab,
+                                                                                                ag.skew(rot_axisB).T,
                                                                                                 new_Lambda_dot))
 
-    LM_K[FoR_dof+3:FoR_dof+6,node_dof+3:node_dof+6] += algebra.multiply_matrices(algebra.quat2rotation(MB_tstep[FoR_body].quat).T,
-                                                                         algebra.quat2rotation(MB_tstep[node_body].quat),
-                                                                         algebra.der_CcrvT_by_v(MB_tstep[node_body].psi[ielem,inode_in_elem,:],
-                                                                                                np.dot(algebra.skew(rot_axisB).T,
-                                                                                                       new_Lambda_dot)))
+    LM_K[FoR_dof+3:FoR_dof+6,node_dof+3:node_dof+6] += scalingFactor*ag.multiply_matrices(FoR_cga.T,
+                                                                         node_cga,
+                                                                         cab,
+                                                                         ag.skew(rot_axisB).T,
+                                                                         new_Lambda_dot)
+
+    if penaltyFactor:
+        q = np.zeros((sys_size,))
+        q[FoR_dof+3:FoR_dof+6] = MB_tstep[FoR_body].for_vel[3:6]
+
+        LM_Q[:sys_size] += penaltyFactor*np.dot(Bnh.T, np.dot(Bnh, q))
+
+        LM_C[:sys_size, :sys_size] += penaltyFactor*np.dot(Bnh.T, Bnh)
+
+        sq_rot_axisB = np.dot(ag.skew(rot_axisB).T, ag.skew(rot_axisB))
+
+        # Derivatives with the quaternion of the FoR
+        vec = ag.multiply_matrices(node_cga,
+                                   cab,
+                                   sq_rot_axisB,
+                                   cab.T,
+                                   node_cga.T,
+                                   FoR_cga,
+                                   FoR_wa)
+        LM_C[FoR_dof+3:FoR_dof+6, FoR_dof+6:FoR_dof+10] += penaltyFactor*ag.der_CquatT_by_v(MB_tstep[FoR_body].quat, vec)
+
+        mat = ag.multiply_matrices(FoR_cga.T,
+                                    node_cga,
+                                    cab,
+                                    sq_rot_axisB,
+                                    cab.T,
+                                    node_cga.T)
+        LM_C[FoR_dof+3:FoR_dof+6, FoR_dof+6:FoR_dof+10] += penaltyFactor*np.dot(mat, ag.der_Cquat_by_v(MB_tstep[FoR_body].quat, FoR_wa))
+
+        if MB_beam[node_body].FoR_movement == 'free':
+            # Derivatives with the quaternion of the FoR of the node
+            vec = ag.multiply_matrices(cab,
+                                        sq_rot_axisB,
+                                        cab.T,
+                                        node_cga.T,
+                                        FoR_cga,
+                                        FoR_wa)
+            LM_C[FoR_dof+3:FoR_dof+6, node_FoR_dof+6:node_FoR_dof+10] += penaltyFactor*np.dot(FoR_cga.T,
+                                                                                          ag.der_Cquat_by_v(MB_tstep[node_body].quat, vec))
+            mat = ag.multiply_matrices(FoR_cga.T,
+                                        node_cga,
+                                        cab,
+                                        sq_rot_axisB,
+                                        cab.T)
+            vec = np.dot(FoR_cga, FoR_wa)
+            LM_C[FoR_dof+3:FoR_dof+6, node_FoR_dof+6:node_FoR_dof+10] += penaltyFactor*np.dot(mat, ag.der_CquatT_by_v(MB_tstep[node_body].quat, vec))
+
+        # Derivatives with the CRV
+        mat = np.dot(FoR_cga.T, node_cga)
+        vec = ag.multiply_matrices(sq_rot_axisB,
+                                    cab.T,
+                                    node_cga.T,
+                                    FoR_cga,
+                                    FoR_wa)
+        LM_K[FoR_dof+3:FoR_dof+6, node_dof+3:node_dof+6] += penaltyFactor*np.dot(mat, ag.der_Ccrv_by_v(MB_tstep[node_body].psi[ielem,inode_in_elem,:], vec))
+
+        mat = ag.multiply_matrices(FoR_cga.T,
+                                    node_cga,
+                                    cab,
+                                    sq_rot_axisB)
+        vec = ag.multiply_matrices(node_cga.T,
+                                    FoR_cga,
+                                    FoR_wa)
+        LM_K[FoR_dof+3:FoR_dof+6, node_dof+3:node_dof+6] += penaltyFactor*np.dot(mat, ag.der_CcrvT_by_v(MB_tstep[node_body].psi[ielem,inode_in_elem,:], vec))
+
+    ieq += 2
+    return ieq
+
+
+def def_rot_axis_FoR_wrt_node_xyz(MB_tstep, MB_beam, FoR_body, node_body, node_number, node_FoR_dof, node_dof, FoR_dof, sys_size, Lambda_dot, rot_axisB, scalingFactor, penaltyFactor, ieq, LM_K, LM_C, LM_Q, zero_comp):
+    """
+    This function generates the stiffness and damping matrices and the independent vector associated to a joint that
+    forces the rotation axis of a FoR to be parallel to a certain direction. This direction is defined in the
+    B FoR of a node and parallel to x, y or z
+
+    See ``LagrangeConstraints`` for the description of variables
+
+    Args:
+        rot_axisB (np.ndarray): Rotation axis with respect to the node B FoR
+        indep (np.ndarray): Number of the equations that are used as independent
+        node_number (int): number of the "node" within its own body
+        node_body (int): body number of the "node"
+        node_FoR_dof (int): position of the first degree of freedom of the FoR to which the "node" belongs
+        node_dof (int): position of the first degree of freedom associated to the "node"
+        FoR_body (int): body number of the "FoR"
+        FoR_dof (int): position of the first degree of freedom associated to the "FoR"
+    """
+
+    ielem, inode_in_elem = MB_beam[node_body].node_master_elem[node_number]
+
+    num_LM_eq_specific = 2
+    Bnh = np.zeros((num_LM_eq_specific, sys_size), dtype=ct.c_double, order = 'F')
+    B = np.zeros((num_LM_eq_specific, sys_size), dtype=ct.c_double, order = 'F')
+
+    # Simplify notation
+    cab = ag.crv2rotation(MB_tstep[node_body].psi[ielem,inode_in_elem,:])
+    node_cga = MB_tstep[node_body].cga()
+    FoR_cga = MB_tstep[FoR_body].cga()
+    FoR_wa = MB_tstep[FoR_body].for_vel[3:6]
+    psi = MB_tstep[node_body].psi[ielem,inode_in_elem,:]
+    psi_dot = MB_tstep[node_body].psi_dot[ielem,inode_in_elem,:]
+
+    # Components to be zero
+    Z = np.zeros((2,3))
+    Z[:, zero_comp] = np.eye(2)
+    
+    Bnh[:, FoR_dof+3:FoR_dof+6] += ag.multiply_matrices(Z, cab.T, node_cga.T, FoR_cga)
+    Bnh[:, node_dof+3:node_dof+6] -= ag.multiply_matrices(Z, ag.crv2tan(psi))
+    Bnh[:, node_FoR_dof+3:node_FoR_dof+6] -= ag.multiply_matrices(Z, cab.T)
+
+    # Constrain angular velocities
+    LM_Q[:sys_size] += scalingFactor*np.dot(np.transpose(Bnh), Lambda_dot[ieq:ieq+num_LM_eq_specific])
+    LM_Q[sys_size+ieq:sys_size+ieq+num_LM_eq_specific] += scalingFactor*ag.multiply_matrices(Z, cab.T, node_cga.T, FoR_cga, FoR_wa)
+    LM_Q[sys_size+ieq:sys_size+ieq+num_LM_eq_specific] -= scalingFactor*ag.multiply_matrices(Z, ag.crv2tan(psi), psi_dot)
+    LM_Q[sys_size+ieq:sys_size+ieq+num_LM_eq_specific] -= scalingFactor*ag.multiply_matrices(Z, cab.T, MB_tstep[node_body].for_vel[3:6])
+
+    LM_C[sys_size+ieq:sys_size+ieq+num_LM_eq_specific,:sys_size] += scalingFactor*Bnh
+    LM_C[:sys_size,sys_size+ieq:sys_size+ieq+num_LM_eq_specific] += scalingFactor*np.transpose(Bnh)
+
+    vec = ag.multiply_matrices(node_cga, cab, Z.T, Lambda_dot[ieq:ieq+num_LM_eq_specific])
+    LM_C[FoR_dof+3:FoR_dof+6, FoR_dof+6:FoR_dof+10] += scalingFactor*ag.der_CquatT_by_v(MB_tstep[FoR_body].quat, vec)
+
+    if MB_beam[node_body].FoR_movement == 'free':
+        vec = ag.multiply_matrices(cab, Z.T, Lambda_dot[ieq:ieq+num_LM_eq_specific])
+        LM_C[FoR_dof+3:FoR_dof+6, node_FoR_dof+6:node_FoR_dof+10] += scalingFactor*ag.multiply_matrices(FoR_cga.T, ag.der_Cquat_by_v(MB_tstep[node_body].quat, vec))
+
+    LM_K[FoR_dof+3:FoR_dof+6, node_dof+3:node_dof+6] += scalingFactor*ag.multiply_matrices(FoR_cga.T, node_cga, ag.der_Ccrv_by_v(MB_tstep[node_body].psi[ielem,inode_in_elem,:],
+                                                                                                                                  np.dot(Z.T, Lambda_dot[ieq:ieq+num_LM_eq_specific])))
+
+    LM_K[node_dof+3:node_dof+6, node_dof+3:node_dof+6] -= scalingFactor*ag.der_TanT_by_xv(psi, ag.multiply_matrices(Z.T, Lambda_dot[ieq:ieq+num_LM_eq_specific]))
+    LM_K[node_FoR_dof+3:node_FoR_dof+6, node_dof+3:node_dof+6] -= scalingFactor*ag.der_Ccrv_by_v(psi, ag.multiply_matrices(Z.T, Lambda_dot[ieq:ieq+num_LM_eq_specific]))
+
+    if penaltyFactor:
+        q = np.zeros((sys_size,))
+        q[FoR_dof+3:FoR_dof+6] = FoR_wa
+
+        LM_Q[:sys_size] += penaltyFactor*np.dot(Bnh.T, np.dot(Bnh, q))
+
+        LM_C[:sys_size, :sys_size] += penaltyFactor*np.dot(Bnh.T, Bnh)
+
+        ZTZ = np.dot(Z.T, Z)
+
+        # Derivatives with the quaternion of the FoR
+        vec = ag.multiply_matrices(node_cga,
+                                   cab,
+                                   ZTZ,
+                                   cab.T,
+                                   node_cga.T,
+                                   FoR_cga,
+                                   FoR_wa)
+        LM_C[FoR_dof+3:FoR_dof+6, FoR_dof+6:FoR_dof+10] += penaltyFactor*ag.der_CquatT_by_v(MB_tstep[FoR_body].quat, vec)
+
+        mat = ag.multiply_matrices(FoR_cga.T,
+                                   node_cga,
+                                   cab,
+                                   ZTZ,
+                                   cab.T,
+                                   node_cga.T)
+        LM_C[FoR_dof+3:FoR_dof+6, FoR_dof+6:FoR_dof+10] += penaltyFactor*np.dot(mat, ag.der_Cquat_by_v(MB_tstep[FoR_body].quat, FoR_wa))
+
+        if MB_beam[node_body].FoR_movement == 'free':
+            # Derivatives with the quaternion of the FoR of the node
+            vec = ag.multiply_matrices(cab,
+                                       ZTZ,
+                                       cab.T,
+                                       node_cga.T,
+                                       FoR_cga,
+                                       FoR_wa)
+            LM_C[FoR_dof+3:FoR_dof+6, node_FoR_dof+6:node_FoR_dof+10] += penaltyFactor*np.dot(FoR_cga.T,
+                                                                                          ag.der_Cquat_by_v(MB_tstep[node_body].quat, vec))
+
+            mat = ag.multiply_matrices(FoR_cga.T,
+                                       node_cga,
+                                       cab,
+                                       ZTZ,
+                                       cab.T)
+            vec = np.dot(FoR_cga, FoR_wa)
+            LM_C[FoR_dof+3:FoR_dof+6, node_FoR_dof+6:node_FoR_dof+10] += penaltyFactor*np.dot(mat, ag.der_CquatT_by_v(MB_tstep[node_body].quat, vec))
+
+        # Derivatives with the CRV
+        mat = np.dot(FoR_cga.T, node_cga)
+        vec = ag.multiply_matrices(ZTZ,
+                                   cab.T,
+                                   node_cga.T,
+                                   FoR_cga,
+                                   FoR_wa)
+        LM_K[FoR_dof+3:FoR_dof+6, node_dof+3:node_dof+6] += penaltyFactor*np.dot(mat, ag.der_Ccrv_by_v(MB_tstep[node_body].psi[ielem,inode_in_elem,:], vec))
+
+        mat = ag.multiply_matrices(FoR_cga.T,
+                                   node_cga,
+                                   cab,
+                                   ZTZ)
+        vec = ag.multiply_matrices(node_cga.T,
+                                   FoR_cga,
+                                   FoR_wa)
+        LM_K[FoR_dof+3:FoR_dof+6, node_dof+3:node_dof+6] += penaltyFactor*np.dot(mat, ag.der_CcrvT_by_v(MB_tstep[node_body].psi[ielem,inode_in_elem,:], vec))
 
     ieq += 2
     return ieq
@@ -414,60 +722,100 @@ def def_rot_vel_FoR_wrt_node(MB_tstep, MB_beam, FoR_body, node_body, node_number
         FoR_body (int): body number of the "FoR"
         FoR_dof (int): position of the first degree of freedom associated to the "FoR"
     """
-    # Variables names. The naming of the variables can be quite confusing. The reader should think that
-    # the BC relates one "node" and one "FoR" (writen between quotes in these lines).
-    # If a variable is related to one of them starts with "node_" or "FoR_" respectively
-    # node_number: number of the "node" within its own body
-    # node_body: body number of the "node"
-    # node_FoR_dof: position of the first degree of freedom of the FoR to which the "node" belongs
-    # node_dof: position of the first degree of freedom associated to the "node"
-    # FoR_body: body number of the "FoR"
-    # FoR_dof: position of the first degree of freedom associated to the "FoR"
 
     num_LM_eq_specific = 1
     Bnh = np.zeros((num_LM_eq_specific, sys_size), dtype=ct.c_double, order = 'F')
     B = np.zeros((num_LM_eq_specific, sys_size), dtype=ct.c_double, order = 'F')
 
-    # Lambda_dot[ieq:ieq+num_LM_eq_specific]
-    # np.concatenate((Lambda_dot[ieq:ieq+num_LM_eq_specific], np.array([0.])))
-
     ielem, inode_in_elem = MB_beam[node_body].node_master_elem[node_number]
-    Bnh[:, FoR_dof+3:FoR_dof+6] = algebra.multiply_matrices(rot_axisB,
-                                                  algebra.crv2rotation(MB_tstep[node_body].psi[ielem,inode_in_elem,:]).T,
-                                                  algebra.quat2rotation(MB_tstep[node_body].quat).T,
-                                                  algebra.quat2rotation(MB_tstep[FoR_body].quat))
+    Bnh[:, FoR_dof+3:FoR_dof+6] = ag.multiply_matrices(rot_axisB,
+                                                  ag.crv2rotation(MB_tstep[node_body].psi[ielem,inode_in_elem,:]).T,
+                                                  MB_tstep[node_body].cga().T,
+                                                  MB_tstep[FoR_body].cga())
 
     # Constrain angular velocities
     LM_Q[:sys_size] += scalingFactor*np.dot(np.transpose(Bnh),Lambda_dot[ieq:ieq+num_LM_eq_specific])
-    LM_Q[sys_size+ieq:sys_size+ieq+num_LM_eq_specific] += algebra.multiply_matrices(rot_axisB,
-                                                  algebra.crv2rotation(MB_tstep[node_body].psi[ielem,inode_in_elem,:]).T,
-                                                  algebra.quat2rotation(MB_tstep[node_body].quat).T,
-                                                  algebra.quat2rotation(MB_tstep[FoR_body].quat),
-                                                  MB_tstep[FoR_body].for_vel[3:6]) - rot_vel
+    LM_Q[sys_size+ieq:sys_size+ieq+num_LM_eq_specific] += scalingFactor*ag.multiply_matrices(rot_axisB,
+                                                  ag.crv2rotation(MB_tstep[node_body].psi[ielem,inode_in_elem,:]).T,
+                                                  MB_tstep[node_body].cga().T,
+                                                  MB_tstep[FoR_body].cga(),
+                                                  MB_tstep[FoR_body].for_vel[3:6]) - scalingFactor*rot_vel
 
     LM_C[sys_size+ieq:sys_size+ieq+num_LM_eq_specific,:sys_size] += scalingFactor*Bnh
     LM_C[:sys_size,sys_size+ieq:sys_size+ieq+num_LM_eq_specific] += scalingFactor*np.transpose(Bnh)
 
     if MB_beam[node_body].FoR_movement == 'free':
-        LM_C[FoR_dof+3:FoR_dof+6,node_FoR_dof+6:node_FoR_dof+10] += np.dot(algebra.quat2rotation(MB_tstep[FoR_body].quat).T,
-                                                                           algebra.der_Cquat_by_v(MB_tstep[node_body].quat,
-                                                                                                  algebra.multiply_matrices(algebra.crv2rotation(MB_tstep[node_body].psi[ielem,inode_in_elem,:]),
+        LM_C[FoR_dof+3:FoR_dof+6,node_FoR_dof+6:node_FoR_dof+10] += scalingFactor*np.dot(MB_tstep[FoR_body].cga().T,
+                                                                           ag.der_Cquat_by_v(MB_tstep[node_body].quat,
+                                                                                                  ag.multiply_matrices(ag.crv2rotation(MB_tstep[node_body].psi[ielem,inode_in_elem,:]),
                                                                                                                     # rot_axisB.T,
                                                                                                                     rot_axisB.T*Lambda_dot[ieq:ieq+num_LM_eq_specific])))
 
-    LM_C[FoR_dof+3:FoR_dof+6,FoR_dof+6:FoR_dof+10] += algebra.der_CquatT_by_v(MB_tstep[FoR_body].quat,
-                                                                              algebra.multiply_matrices(algebra.quat2rotation(MB_tstep[node_body].quat),
-                                                                                                algebra.crv2rotation(MB_tstep[node_body].psi[ielem,inode_in_elem,:]).T,
+    LM_C[FoR_dof+3:FoR_dof+6,FoR_dof+6:FoR_dof+10] += scalingFactor*ag.der_CquatT_by_v(MB_tstep[FoR_body].quat,
+                                                                              ag.multiply_matrices(MB_tstep[node_body].cga(),
+                                                                                                ag.crv2rotation(MB_tstep[node_body].psi[ielem,inode_in_elem,:]).T,
                                                                                                 rot_axisB.T*Lambda_dot[ieq:ieq+num_LM_eq_specific]))
 
-    LM_K[FoR_dof+3:FoR_dof+6,node_dof+3:node_dof+6] += algebra.multiply_matrices(algebra.quat2rotation(MB_tstep[FoR_body].quat).T,
-                                                                         algebra.quat2rotation(MB_tstep[node_body].quat),
-                                                                         algebra.der_Ccrv_by_v(MB_tstep[node_body].psi[ielem,inode_in_elem,:],
+    LM_K[FoR_dof+3:FoR_dof+6,node_dof+3:node_dof+6] += scalingFactor*ag.multiply_matrices(MB_tstep[FoR_body].cga().T,
+                                                                         MB_tstep[node_body].cga(),
+                                                                         ag.der_Ccrv_by_v(MB_tstep[node_body].psi[ielem,inode_in_elem,:],
                                                                                                 rot_axisB.T*Lambda_dot[ieq:ieq+num_LM_eq_specific]))
 
     ieq += 1
     return ieq
 
+def def_rot_vect_FoR_wrt_node(MB_tstep, MB_beam, FoR_body, node_body, node_number, node_FoR_dof, node_dof, FoR_dof, sys_size, Lambda_dot, rot_vect, scalingFactor, penaltyFactor, ieq, LM_K, LM_C, LM_Q):
+    """
+        This function fixes the rotation velocity VECTOR of a FOR equal to a velocity vector defined in the B FoR of a node
+        This function is a new implementation that combines and simplifies the use of 'def_rot_vel_FoR_wrt_node' and 'def_rot_axis_FoR_wrt_node' together
+    """
+
+    num_LM_eq_specific = 3
+    Bnh = np.zeros((num_LM_eq_specific, sys_size), dtype=ct.c_double, order = 'F')
+
+    # Simplify notation
+    ielem, inode_in_elem = MB_beam[node_body].node_master_elem[node_number]
+    node_cga = MB_tstep[node_body].cga()
+    cab = ag.crv2rotation(MB_tstep[node_body].psi[ielem,inode_in_elem,:])
+    FoR_cga = MB_tstep[FoR_body].cga()
+    FoR_wa = MB_tstep[FoR_body].for_vel[3:6]
+
+    Bnh[:, FoR_dof+3:FoR_dof+6] = ag.multiply_matrices(cab.T,
+                                                       node_cga.T,
+                                                       FoR_cga)
+
+    # Constrain angular velocities
+    LM_Q[:sys_size] += scalingFactor*np.dot(np.transpose(Bnh), Lambda_dot[ieq:ieq+num_LM_eq_specific])
+    LM_Q[sys_size+ieq:sys_size+ieq+num_LM_eq_specific] += scalingFactor*(np.dot(Bnh[:, FoR_dof+3:FoR_dof+6], FoR_wa) -
+                                                                         rot_vect)
+
+    LM_C[sys_size+ieq:sys_size+ieq+num_LM_eq_specific,:sys_size] += scalingFactor*Bnh
+    LM_C[:sys_size,sys_size+ieq:sys_size+ieq+num_LM_eq_specific] += scalingFactor*np.transpose(Bnh)
+
+    if MB_beam[node_body].FoR_movement == 'free':
+        LM_C[FoR_dof+3:FoR_dof+6,node_FoR_dof+6:node_FoR_dof+10] += scalingFactor*np.dot(FoR_cga.T,
+                                                                           ag.der_Cquat_by_v(MB_tstep[node_body].quat,
+                                                                                                  np.dot(cab, Lambda_dot[ieq:ieq+num_LM_eq_specific])))
+
+    LM_C[FoR_dof+3:FoR_dof+6,FoR_dof+6:FoR_dof+10] += scalingFactor*ag.der_CquatT_by_v(MB_tstep[FoR_body].quat,
+                                                                              ag.multiply_matrices(node_cga, cab, Lambda_dot[ieq:ieq+num_LM_eq_specific]))
+
+    LM_K[FoR_dof+3:FoR_dof+6,node_dof+3:node_dof+6] += scalingFactor*ag.multiply_matrices(FoR_cga.T,
+                                                                         node_cga,
+                                                                         ag.der_Ccrv_by_v(MB_tstep[node_body].psi[ielem,inode_in_elem,:],
+                                                                                                Lambda_dot[ieq:ieq+num_LM_eq_specific]))
+
+    if penaltyFactor:
+        LM_C[FoR_dof+3:FoR_dof+6, FoR_dof+3:FoR_dof+6] += penaltyFactor*np.eye(3)
+
+        q = np.zeros((sys_size))
+        q[FoR_dof+3:FoR_dof+6] = FoR_wa
+        LM_Q[:sys_size] += penaltyFactor*np.dot(np.dot(Bnh.T, Bnh), q)
+
+    ieq += 3
+    return ieq
+
+
 ################################################################################
 # Lagrange constraints
 ################################################################################
@@ -496,29 +844,35 @@ def get_n_eq(self):
         return self._n_eq
 
     def initialise(self, MBdict_entry, ieq, print_info=True):
-        # if print_info:
-            # cout.cout_wrap('Type of LC: ', self._lc_id)
-            # cout.cout_wrap('Arguments and values:')
-            # for k, v in MBdict_entry.items():
-                # cout.cout_wrap(k, v)
-
         self.node_number = MBdict_entry['node_in_body']
         self.node_body = MBdict_entry['body']
         self.FoR_body = MBdict_entry['body_FoR']
         self.rot_axisB = MBdict_entry['rot_axisB']
         self._ieq = ieq
-        self.indep = []
-
+        self.scalingFactor = set_value_or_default(MBdict_entry, "scalingFactor", 1.)
+        self.penaltyFactor = set_value_or_default(MBdict_entry, "penaltyFactor", 0.)
+
+        if (self.rot_axisB[[1, 2]]  == 0).all():
+            self.rot_dir = 'x'
+            self.zero_comp = np.array([1, 2], dtype=int)
+        elif (self.rot_axisB[[0, 2]]  == 0).all():
+            self.rot_dir = 'y'
+            self.zero_comp = np.array([0, 2], dtype=int)
+        elif (self.rot_axisB[[0, 1]]  == 0).all():
+            self.rot_dir = 'z'
+            self.zero_comp = np.array([0, 1], dtype=int)
+        else:
+            self.rot_dir = 'general'
+            self.indep = []
+    
         return self._ieq + self._n_eq
 
     def staticmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq,
-                sys_size, dt, Lambda, Lambda_dot,
-                scalingFactor, penaltyFactor):
+                sys_size, dt, Lambda, Lambda_dot):
         return
 
     def dynamicmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq,
-                sys_size, dt, Lambda, Lambda_dot,
-                scalingFactor, penaltyFactor):
+                sys_size, dt, Lambda, Lambda_dot):
 
         # Define the position of the first degree of freedom associated to the node
         node_dof = define_node_dof(MB_beam, self.node_body, self.node_number)
@@ -527,8 +881,12 @@ def dynamicmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq,
         ieq = self._ieq
 
         # Define the equations
-        ieq = equal_lin_vel_node_FoR(MB_tstep, MB_beam, self.FoR_body, self.node_body, self.node_number, node_FoR_dof, node_dof, FoR_dof, sys_size, Lambda_dot, scalingFactor, penaltyFactor, ieq, LM_K, LM_C, LM_Q)
-        ieq = def_rot_axis_FoR_wrt_node(MB_tstep, MB_beam, self.FoR_body, self.node_body, self.node_number, node_FoR_dof, node_dof, FoR_dof, sys_size, Lambda_dot, self.rot_axisB, scalingFactor, penaltyFactor, ieq, LM_K, LM_C, LM_Q, self.indep)
+        # ieq =  equal_pos_node_FoR(MB_tstep, MB_beam, self.FoR_body, self.node_body, self.node_number, node_FoR_dof, node_dof, FoR_dof, sys_size, Lambda, self.scalingFactor, self.penaltyFactor, ieq, LM_K, LM_C, LM_Q)
+        ieq = equal_lin_vel_node_FoR(MB_tstep, MB_beam, self.FoR_body, self.node_body, self.node_number, node_FoR_dof, node_dof, FoR_dof, sys_size, Lambda_dot, self.scalingFactor, self.penaltyFactor, ieq, LM_K, LM_C, LM_Q)
+        if self.rot_dir == 'general':
+            ieq = def_rot_axis_FoR_wrt_node_general(MB_tstep, MB_beam, self.FoR_body, self.node_body, self.node_number, node_FoR_dof, node_dof, FoR_dof, sys_size, Lambda_dot, self.rot_axisB, self.scalingFactor, self.penaltyFactor, ieq, LM_K, LM_C, LM_Q, self.indep)
+        else:
+            ieq = def_rot_axis_FoR_wrt_node_xyz(MB_tstep, MB_beam, self.FoR_body, self.node_body, self.node_number, node_FoR_dof, node_dof, FoR_dof, sys_size, Lambda_dot, self.rot_axisB, self.scalingFactor, self.penaltyFactor, ieq, LM_K, LM_C, LM_Q, self.zero_comp)
 
         return
 
@@ -536,7 +894,7 @@ def staticpost(self, lc_list, MB_beam, MB_tstep):
         return
 
     def dynamicpost(self, lc_list, MB_beam, MB_tstep):
-        MB_tstep[self.FoR_body].for_pos[0:3] = np.dot(algebra.quat2rotation(MB_tstep[self.node_body].quat), MB_tstep[self.node_body].pos[self.node_number,:]) + MB_tstep[self.node_body].for_pos[0:3]
+        MB_tstep[self.FoR_body].for_pos[0:3] = np.dot(MB_tstep[self.node_body].cga(), MB_tstep[self.node_body].pos[self.node_number,:]) + MB_tstep[self.node_body].for_pos[0:3]
         return
 
 
@@ -554,43 +912,39 @@ class hinge_node_FoR_constant_vel(BaseLagrangeConstraint):
         node_number (int): number of the "node" within its own body
         node_body (int): body number of the "node"
         FoR_body (int): body number of the "FoR"
-        rot_axisB (np.ndarray): Rotation axis with respect to the node B FoR
-        rot_vel (float): Rotation velocity
+        rot_vect (np.ndarray): Rotation velocity vector in the node B FoR
     """
     _lc_id = 'hinge_node_FoR_constant_vel'
 
     def __init__(self):
-        self.required_parameters = ['node_in_body', 'body', 'body_FoR', 'rot_axisB', 'rot_vel']
+        self.required_parameters = ['node_in_body', 'body', 'body_FoR', 'rot_vect']
         self._n_eq = 6
 
     def get_n_eq(self):
         return self._n_eq
 
     def initialise(self, MBdict_entry, ieq, print_info=True):
-        # if print_info:
-            # cout.cout_wrap('Type of LC: ', self._lc_id)
-            # cout.cout_wrap('Arguments and values:')
-            # for k, v in MBdict_entry.items():
-                # cout.cout_wrap(k, v)
 
         self.node_number = MBdict_entry['node_in_body']
         self.node_body = MBdict_entry['body']
         self.FoR_body = MBdict_entry['body_FoR']
-        self.rot_axisB = MBdict_entry['rot_axisB']
-        self.rot_vel = MBdict_entry['rot_vel']
+        self.rot_vect = MBdict_entry['rot_vect']
         self._ieq = ieq
         self.indep = []
+        self.scalingFactor = set_value_or_default(MBdict_entry, "scalingFactor", 1.)
+        self.penaltyFactor = set_value_or_default(MBdict_entry, "penaltyFactor", 0.)
+
+        # self.static_constraint = fully_constrained_node_FoR()
+        # self.static_constraint.initialise(MBdict_entry, ieq)
 
         return self._ieq + self._n_eq
 
     def staticmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq,
-                sys_size, dt, Lambda, Lambda_dot,
-                scalingFactor, penaltyFactor):
+                sys_size, dt, Lambda, Lambda_dot):
         return
 
     def dynamicmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq,
-                sys_size, dt, Lambda, Lambda_dot,
-                scalingFactor, penaltyFactor):
+                sys_size, dt, Lambda, Lambda_dot):
 
         # Define the position of the first degree of freedom associated to the node
         node_dof = define_node_dof(MB_beam, self.node_body, self.node_number)
@@ -599,16 +953,30 @@ def dynamicmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq,
         ieq = self._ieq
 
         # Define the equations
-        ieq = equal_lin_vel_node_FoR(MB_tstep, MB_beam, self.FoR_body, self.node_body, self.node_number, node_FoR_dof, node_dof, FoR_dof, sys_size, Lambda_dot, scalingFactor, penaltyFactor, ieq, LM_K, LM_C, LM_Q)
-        ieq = def_rot_axis_FoR_wrt_node(MB_tstep, MB_beam, self.FoR_body, self.node_body, self.node_number, node_FoR_dof, node_dof, FoR_dof, sys_size, Lambda_dot, self.rot_axisB, scalingFactor, penaltyFactor, ieq, LM_K, LM_C, LM_Q, self.indep)
-        ieq = def_rot_vel_FoR_wrt_node(MB_tstep, MB_beam, self.FoR_body, self.node_body, self.node_number, node_FoR_dof, node_dof, FoR_dof, sys_size, Lambda_dot, self.rot_axisB, self.rot_vel, scalingFactor, penaltyFactor, ieq, LM_K, LM_C, LM_Q)
+        # ieq =  equal_pos_node_FoR(MB_tstep, MB_beam, self.FoR_body, self.node_body, self.node_number, node_FoR_dof, node_dof, FoR_dof, sys_size, Lambda, self.scalingFactor, self.penaltyFactor, ieq, LM_K, LM_C, LM_Q)
+        ieq = equal_lin_vel_node_FoR(MB_tstep, MB_beam, self.FoR_body, self.node_body, self.node_number, node_FoR_dof, node_dof, FoR_dof, sys_size, Lambda_dot, self.scalingFactor, self.penaltyFactor, ieq, LM_K, LM_C, LM_Q)
+        ieq = def_rot_vect_FoR_wrt_node(MB_tstep, MB_beam, self.FoR_body, self.node_body, self.node_number, node_FoR_dof, node_dof, FoR_dof, sys_size, Lambda_dot, self.rot_vect, self.scalingFactor, self.penaltyFactor, ieq, LM_K, LM_C, LM_Q)
+        # ieq = def_rot_axis_FoR_wrt_node(MB_tstep, MB_beam, self.FoR_body, self.node_body, self.node_number, node_FoR_dof, node_dof, FoR_dof, sys_size, Lambda_dot, self.rot_axisB, self.scalingFactor, self.penaltyFactor, ieq, LM_K, LM_C, LM_Q, self.indep)
+        # ieq = def_rot_vel_FoR_wrt_node(MB_tstep, MB_beam, self.FoR_body, self.node_body, self.node_number, node_FoR_dof, node_dof, FoR_dof, sys_size, Lambda_dot, self.rot_axisB, self.rot_vel, self.scalingFactor, self.penaltyFactor, ieq, LM_K, LM_C, LM_Q)
         return
 
     def staticpost(self, lc_list, MB_beam, MB_tstep):
         return
 
     def dynamicpost(self, lc_list, MB_beam, MB_tstep):
-        MB_tstep[self.FoR_body].for_pos[0:3] = np.dot(algebra.quat2rotation(MB_tstep[self.node_body].quat), MB_tstep[self.node_body].pos[self.node_number,:]) + MB_tstep[self.node_body].for_pos[0:3]
+        MB_tstep[self.FoR_body].for_pos[0:3] = np.dot(MB_tstep[self.node_body].cga(), MB_tstep[self.node_body].pos[self.node_number,:]) + MB_tstep[self.node_body].for_pos[0:3]
+
+        ielem, inode_in_elem = MB_beam[self.node_body].node_master_elem[self.node_number]
+        node_cga = MB_tstep[self.node_body].cga()
+        cab = ag.crv2rotation(MB_tstep[self.node_body].psi[ielem,inode_in_elem,:])
+        FoR_cga = MB_tstep[self.FoR_body].cga()
+
+        rot_vect_A = ag.multiply_matrices(FoR_cga.T,
+                                         node_cga,
+                                         cab,
+                                         self.rot_vect)
+
+        MB_tstep[self.FoR_body].for_vel[3:6] = rot_vect_A.copy()
         return
 
 
@@ -636,27 +1004,22 @@ def get_n_eq(self):
         return self._n_eq
 
     def initialise(self, MBdict_entry, ieq, print_info=True):
-        # if print_info:
-            # cout.cout_wrap('Type of LC: ', self._lc_id)
-            # cout.cout_wrap('Arguments and values:')
-            # for k, v in MBdict_entry.items():
-                # cout.cout_wrap(k, v)
 
         self.node_number = MBdict_entry['node_in_body']
         self.node_body = MBdict_entry['body']
         self.FoR_body = MBdict_entry['body_FoR']
         self._ieq = ieq
+        self.scalingFactor = set_value_or_default(MBdict_entry, "scalingFactor", 1.)
+        self.penaltyFactor = set_value_or_default(MBdict_entry, "penaltyFactor", 0.)
 
         return self._ieq + self._n_eq
 
     def staticmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq,
-                sys_size, dt, Lambda, Lambda_dot,
-                scalingFactor, penaltyFactor):
+                sys_size, dt, Lambda, Lambda_dot):
         return
 
     def dynamicmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq,
-                sys_size, dt, Lambda, Lambda_dot,
-                scalingFactor, penaltyFactor):
+                sys_size, dt, Lambda, Lambda_dot):
 
         # Define the position of the first degree of freedom associated to the node
         node_dof = define_node_dof(MB_beam, self.node_body, self.node_number)
@@ -665,7 +1028,7 @@ def dynamicmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq,
         ieq = self._ieq
 
         # Define the equations
-        ieq = equal_lin_vel_node_FoR(MB_tstep, MB_beam, self.FoR_body, self.node_body, self.node_number, node_FoR_dof, node_dof, FoR_dof, sys_size, Lambda_dot, scalingFactor, penaltyFactor, ieq, LM_K, LM_C, LM_Q)
+        ieq = equal_lin_vel_node_FoR(MB_tstep, MB_beam, self.FoR_body, self.node_body, self.node_number, node_FoR_dof, node_dof, FoR_dof, sys_size, Lambda_dot, self.scalingFactor, self.penaltyFactor, ieq, LM_K, LM_C, LM_Q)
 
         return
 
@@ -673,7 +1036,7 @@ def staticpost(self, lc_list, MB_beam, MB_tstep):
         return
 
     def dynamicpost(self, lc_list, MB_beam, MB_tstep):
-        MB_tstep[self.FoR_body].for_pos[0:3] = np.dot(algebra.quat2rotation(MB_tstep[self.node_body].quat), MB_tstep[self.node_body].pos[self.node_number,:]) + MB_tstep[self.node_body].for_pos[0:3]
+        MB_tstep[self.FoR_body].for_pos[0:3] = np.dot(MB_tstep[self.node_body].cga(), MB_tstep[self.node_body].pos[self.node_number,:]) + MB_tstep[self.node_body].for_pos[0:3]
         return
 
 
@@ -690,23 +1053,16 @@ def get_n_eq(self):
         return self._n_eq
 
     def initialise(self, MBdict_entry, ieq, print_info=True):
-        # if print_info:
-            # cout.cout_wrap('Type of LC: ', self._lc_id)
-            # cout.cout_wrap('Arguments and values:')
-            # for k, v in MBdict_entry.items():
-                # cout.cout_wrap(k, v)
 
         self._ieq = ieq
         return self._ieq + self._n_eq
 
     def staticmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq,
-                sys_size, dt, Lambda, Lambda_dot,
-                scalingFactor, penaltyFactor):
+                sys_size, dt, Lambda, Lambda_dot):
         return
 
     def dynamicmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq,
-                sys_size, dt, Lambda, Lambda_dot,
-                scalingFactor, penaltyFactor):
+                sys_size, dt, Lambda, Lambda_dot):
         return
 
     def staticpost(self, lc_list, MB_beam, MB_tstep):
@@ -738,25 +1094,20 @@ def get_n_eq(self):
         return self._n_eq
 
     def initialise(self, MBdict_entry, ieq, print_info=True):
-        # if print_info:
-            # cout.cout_wrap('Type of LC: ', self._lc_id)
-            # cout.cout_wrap('Arguments and values:')
-            # for k, v in MBdict_entry.items():
-                # cout.cout_wrap(k, v)
 
         self.body_FoR = MBdict_entry['body_FoR']
         self._ieq = ieq
+        self.scalingFactor = set_value_or_default(MBdict_entry, "scalingFactor", 1.)
+        self.penaltyFactor = set_value_or_default(MBdict_entry, "penaltyFactor", 0.)
 
         return self._ieq + self._n_eq
 
     def staticmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq,
-                sys_size, dt, Lambda, Lambda_dot,
-                scalingFactor, penaltyFactor):
+                sys_size, dt, Lambda, Lambda_dot):
         return
 
     def dynamicmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq,
-                sys_size, dt, Lambda, Lambda_dot,
-                scalingFactor, penaltyFactor):
+                sys_size, dt, Lambda, Lambda_dot):
         num_LM_eq_specific = self._n_eq
         Bnh = np.zeros((num_LM_eq_specific, sys_size), dtype=ct.c_double, order = 'F')
         B = np.zeros((num_LM_eq_specific, sys_size), dtype=ct.c_double, order = 'F')
@@ -767,12 +1118,12 @@ def dynamicmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq,
 
         Bnh[:3, FoR_dof:FoR_dof+3] = 1.0*np.eye(3)
 
-        LM_C[sys_size+ieq:sys_size+ieq+num_LM_eq_specific,:sys_size] += scalingFactor*Bnh
-        LM_C[:sys_size,sys_size+ieq:sys_size+ieq+num_LM_eq_specific] += scalingFactor*np.transpose(Bnh)
+        LM_C[sys_size+ieq:sys_size+ieq+num_LM_eq_specific,:sys_size] += self.scalingFactor*Bnh
+        LM_C[:sys_size,sys_size+ieq:sys_size+ieq+num_LM_eq_specific] += self.scalingFactor*np.transpose(Bnh)
 
-        LM_Q[:sys_size] += scalingFactor*np.dot(np.transpose(Bnh),Lambda_dot[ieq:ieq+num_LM_eq_specific])
+        LM_Q[:sys_size] += self.scalingFactor*np.dot(np.transpose(Bnh),Lambda_dot[ieq:ieq+num_LM_eq_specific])
 
-        LM_Q[sys_size+ieq:sys_size+ieq+3] += MB_tstep[self.body_FoR].for_vel[0:3].astype(dtype=ct.c_double, copy=True, order='F')
+        LM_Q[sys_size+ieq:sys_size+ieq+3] += self.scalingFactor*MB_tstep[self.body_FoR].for_vel[0:3].astype(dtype=ct.c_double, copy=True, order='F')
 
         ieq += 3
         return
@@ -807,26 +1158,33 @@ def get_n_eq(self):
         return self._n_eq
 
     def initialise(self, MBdict_entry, ieq, print_info=True):
-        # if print_info:
-            # cout.cout_wrap('Type of LC: ', self._lc_id)
-            # cout.cout_wrap('Arguments and values:')
-            # for k, v in MBdict_entry.items():
-                # cout.cout_wrap(k, v)
 
         self.body_FoR = MBdict_entry['body_FoR']
         self.rot_axis = MBdict_entry['rot_axis_AFoR']
         self._ieq = ieq
+        self.scalingFactor = set_value_or_default(MBdict_entry, "scalingFactor", 1.)
+        self.penaltyFactor = set_value_or_default(MBdict_entry, "penaltyFactor", 0.)
+
+        if (self.rot_axis[[1, 2]]  == 0).all():
+            self.rot_dir = 'x'
+            self.zero_comp = np.array([1, 2], dtype=int)
+        elif (self.rot_axis[[0, 2]]  == 0).all():
+            self.rot_dir = 'y'
+            self.zero_comp = np.array([0, 2], dtype=int)
+        elif (self.rot_axis[[0, 1]]  == 0).all():
+            self.rot_dir = 'z'
+            self.zero_comp = np.array([0, 1], dtype=int)
+        else:
+            self.rot_dir = 'general'
 
         return self._ieq + self._n_eq
 
     def staticmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq,
-                sys_size, dt, Lambda, Lambda_dot,
-                scalingFactor, penaltyFactor):
+                sys_size, dt, Lambda, Lambda_dot):
         return
 
     def dynamicmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq,
-                sys_size, dt, Lambda, Lambda_dot,
-                scalingFactor, penaltyFactor):
+                sys_size, dt, Lambda, Lambda_dot):
         num_LM_eq_specific = self._n_eq
         Bnh = np.zeros((num_LM_eq_specific, sys_size), dtype=ct.c_double, order = 'F')
         B = np.zeros((num_LM_eq_specific, sys_size), dtype=ct.c_double, order = 'F')
@@ -837,30 +1195,47 @@ def dynamicmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq,
 
         Bnh[:3, FoR_dof:FoR_dof+3] = 1.0*np.eye(3)
 
-        # Only two of these equations are linearly independent
-        skew_rot_axis = algebra.skew(self.rot_axis)
-        n0 = np.linalg.norm(skew_rot_axis[0,:])
-        n1 = np.linalg.norm(skew_rot_axis[1,:])
-        n2 = np.linalg.norm(skew_rot_axis[2,:])
-        if ((n0 < n1) and (n0 < n2)):
-            row0 = 1
-            row1 = 2
-        elif ((n1 < n0) and (n1 < n2)):
-            row0 = 0
-            row1 = 2
-        elif ((n2 < n0) and (n2 < n1)):
-            row0 = 0
-            row1 = 1
+        if self.rot_dir == 'general':
+            # Only two of these equations are linearly independent
+            skew_rot_axis = ag.skew(self.rot_axis)
+            n0 = np.linalg.norm(skew_rot_axis[0,:])
+            n1 = np.linalg.norm(skew_rot_axis[1,:])
+            n2 = np.linalg.norm(skew_rot_axis[2,:])
+            if ((n0 < n1) and (n0 < n2)):
+                row0 = 1
+                row1 = 2
+            elif ((n1 < n0) and (n1 < n2)):
+                row0 = 0
+                row1 = 2
+            elif ((n2 < n0) and (n2 < n1)):
+                row0 = 0
+                row1 = 1
+            Bnh[3:5, FoR_dof+3:FoR_dof+6] = skew_rot_axis[[row0,row1],:]
+        else:
+            Bnh[3:5, FoR_dof+3+self.zero_comp] = np.eye(2)
 
-        Bnh[3:5, FoR_dof+3:FoR_dof+6] = skew_rot_axis[[row0,row1],:]
+        LM_C[sys_size+ieq:sys_size+ieq+num_LM_eq_specific,:sys_size] += self.scalingFactor*Bnh
+        LM_C[:sys_size,sys_size+ieq:sys_size+ieq+num_LM_eq_specific] += self.scalingFactor*np.transpose(Bnh)
+
+        LM_Q[:sys_size] += self.scalingFactor*np.dot(np.transpose(Bnh),Lambda_dot[ieq:ieq+num_LM_eq_specific])
 
-        LM_C[sys_size+ieq:sys_size+ieq+num_LM_eq_specific,:sys_size] += scalingFactor*Bnh
-        LM_C[:sys_size,sys_size+ieq:sys_size+ieq+num_LM_eq_specific] += scalingFactor*np.transpose(Bnh)
+        LM_Q[sys_size+ieq:sys_size+ieq+3] += self.scalingFactor*MB_tstep[self.body_FoR].for_vel[0:3].astype(dtype=ct.c_double, copy=True, order='F')
+        if self.rot_dir == 'general':
+            LM_Q[sys_size+ieq+3:sys_size+ieq+5] += self.scalingFactor*np.dot(skew_rot_axis[[row0,row1],:], MB_tstep[self.body_FoR].for_vel[3:6])
+        else:
+            LM_Q[sys_size+ieq+3:sys_size+ieq+5] += self.scalingFactor*MB_tstep[self.body_FoR].for_vel[3 + self.zero_comp]
 
-        LM_Q[:sys_size] += scalingFactor*np.dot(np.transpose(Bnh),Lambda_dot[ieq:ieq+num_LM_eq_specific])
+        if self.penaltyFactor:
+            LM_Q[FoR_dof:FoR_dof+3] += self.penaltyFactor*MB_tstep[self.body_FoR].for_vel[0:3]
+            LM_C[FoR_dof:FoR_dof+3, FoR_dof:FoR_dof+3] += self.penaltyFactor*np.eye(3)
 
-        LM_Q[sys_size+ieq:sys_size+ieq+3] += MB_tstep[self.body_FoR].for_vel[0:3].astype(dtype=ct.c_double, copy=True, order='F')
-        LM_Q[sys_size+ieq+3:sys_size+ieq+5] += np.dot(skew_rot_axis[[row0,row1],:], MB_tstep[self.body_FoR].for_vel[3:6])
+            if self.rot_dir == 'general':
+                sq_rot_axis = np.dot(ag.skew(self.rot_axis).T, ag.skew(self.rot_axis))
+                LM_Q[FoR_dof+3:FoR_dof+6] += self.penaltyFactor*np.dot(sq_rot_axis, MB_tstep[self.body_FoR].for_vel[3:6])
+                LM_C[FoR_dof+3:FoR_dof+6, FoR_dof+3:FoR_dof+6] += self.penaltyFactor*sq_rot_axis
+            else:
+                LM_Q[FoR_dof+3:FoR_dof+6] += self.penaltyFactor*MB_tstep[self.body_FoR].for_vel[3:6]
+                LM_C[FoR_dof+3:FoR_dof+6, FoR_dof+3:FoR_dof+6] += self.penaltyFactor*np.eye(3)
 
         ieq += 5
         return
@@ -895,26 +1270,21 @@ def get_n_eq(self):
         return self._n_eq
 
     def initialise(self, MBdict_entry, ieq, print_info=True):
-        # if print_info:
-            # cout.cout_wrap('Type of LC: ', self._lc_id)
-            # cout.cout_wrap('Arguments and values:')
-            # for k, v in MBdict_entry.items():
-                # cout.cout_wrap(k, v)
 
         self.body_FoR = MBdict_entry['body_FoR']
         self.rot_axis = MBdict_entry['rot_axis_AFoR']
         self._ieq = ieq
+        self.scalingFactor = set_value_or_default(MBdict_entry, "scalingFactor", 1.)
+        self.penaltyFactor = set_value_or_default(MBdict_entry, "penaltyFactor", 0.)
 
         return self._ieq + self._n_eq
 
     def staticmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq,
-                sys_size, dt, Lambda, Lambda_dot,
-                scalingFactor, penaltyFactor):
+                sys_size, dt, Lambda, Lambda_dot):
         return
 
     def dynamicmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq,
-                sys_size, dt, Lambda, Lambda_dot,
-                scalingFactor, penaltyFactor):
+                sys_size, dt, Lambda, Lambda_dot):
         num_LM_eq_specific = self._n_eq
         Bnh = np.zeros((num_LM_eq_specific, sys_size), dtype=ct.c_double, order = 'F')
         B = np.zeros((num_LM_eq_specific, sys_size), dtype=ct.c_double, order = 'F')
@@ -923,10 +1293,10 @@ def dynamicmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq,
         FoR_dof = define_FoR_dof(MB_beam, self.body_FoR)
         ieq = self._ieq
 
-        Bnh[:3, FoR_dof:FoR_dof+3] = algebra.quat2rotation(MB_tstep[self.body_FoR].quat)
+        Bnh[:3, FoR_dof:FoR_dof+3] = MB_tstep[self.body_FoR].cga()
 
         # Only two of these equations are linearly independent
-        skew_rot_axis = algebra.skew(self.rot_axis)
+        skew_rot_axis = ag.skew(self.rot_axis)
         n0 = np.linalg.norm(skew_rot_axis[0,:])
         n1 = np.linalg.norm(skew_rot_axis[1,:])
         n2 = np.linalg.norm(skew_rot_axis[2,:])
@@ -942,15 +1312,15 @@ def dynamicmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq,
 
         Bnh[3:5, FoR_dof+3:FoR_dof+6] = skew_rot_axis[[row0,row1],:]
 
-        LM_C[sys_size+ieq:sys_size+ieq+num_LM_eq_specific,:sys_size] += scalingFactor*Bnh
-        LM_C[:sys_size,sys_size+ieq:sys_size+ieq+num_LM_eq_specific] += scalingFactor*np.transpose(Bnh)
+        LM_C[sys_size+ieq:sys_size+ieq+num_LM_eq_specific,:sys_size] += self.scalingFactor*Bnh
+        LM_C[:sys_size,sys_size+ieq:sys_size+ieq+num_LM_eq_specific] += self.scalingFactor*np.transpose(Bnh)
 
-        LM_C[FoR_dof:FoR_dof+3,FoR_dof+6:FoR_dof+10] += algebra.der_CquatT_by_v(MB_tstep[self.body_FoR].quat,Lambda_dot[ieq:ieq+3])
+        LM_C[FoR_dof:FoR_dof+3,FoR_dof+6:FoR_dof+10] += self.scalingFactor*ag.der_CquatT_by_v(MB_tstep[self.body_FoR].quat,Lambda_dot[ieq:ieq+3])
 
-        LM_Q[:sys_size] += scalingFactor*np.dot(np.transpose(Bnh),Lambda_dot[ieq:ieq+num_LM_eq_specific])
+        LM_Q[:sys_size] += self.scalingFactor*np.dot(np.transpose(Bnh),Lambda_dot[ieq:ieq+num_LM_eq_specific])
 
-        LM_Q[sys_size+ieq:sys_size+ieq+3] += np.dot(algebra.quat2rotation(MB_tstep[self.body_FoR].quat),MB_tstep[self.body_FoR].for_vel[0:3])
-        LM_Q[sys_size+ieq+3:sys_size+ieq+5] += np.dot(skew_rot_axis[[row0,row1],:], MB_tstep[self.body_FoR].for_vel[3:6])
+        LM_Q[sys_size+ieq:sys_size+ieq+3] += self.scalingFactor*np.dot(MB_tstep[self.body_FoR].cga(),MB_tstep[self.body_FoR].for_vel[0:3])
+        LM_Q[sys_size+ieq+3:sys_size+ieq+5] += self.scalingFactor*np.dot(skew_rot_axis[[row0,row1],:], MB_tstep[self.body_FoR].for_vel[3:6])
 
         ieq += 5
         return
@@ -987,27 +1357,23 @@ def get_n_eq(self):
         return self._n_eq
 
     def initialise(self, MBdict_entry, ieq, print_info=True):
-        # if print_info:
-            # cout.cout_wrap('Type of LC: ', self._lc_id)
-            # cout.cout_wrap('Arguments and values:')
-            # for k, v in MBdict_entry.items():
-                # cout.cout_wrap(k, v)
 
         cout.cout_wrap("WARNING: do not use fully_constrained_node_FoR. It is outdated. Definetly not working if 'body' has velocity", 3)
         self.node_number = MBdict_entry['node_in_body']
         self.node_body = MBdict_entry['body']
         self.FoR_body = MBdict_entry['body_FoR']
         self._ieq = ieq
+        self.scalingFactor = set_value_or_default(MBdict_entry, "scalingFactor", 1.)
+        self.penaltyFactor = set_value_or_default(MBdict_entry, "penaltyFactor", 0.)
+
         return self._ieq + self._n_eq
 
     def staticmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq,
-                sys_size, dt, Lambda, Lambda_dot,
-                scalingFactor, penaltyFactor):
+                sys_size, dt, Lambda, Lambda_dot):
         return
 
     def dynamicmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq,
-                sys_size, dt, Lambda, Lambda_dot,
-                scalingFactor, penaltyFactor):
+                sys_size, dt, Lambda, Lambda_dot):
         num_LM_eq_specific = self._n_eq
         Bnh = np.zeros((num_LM_eq_specific, sys_size), dtype=ct.c_double, order = 'F')
         B = np.zeros((num_LM_eq_specific, sys_size), dtype=ct.c_double, order = 'F')
@@ -1019,27 +1385,27 @@ def dynamicmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq,
         # Option with non holonomic constraints
         # BC for linear velocities
         Bnh[:3, node_dof:node_dof+3] = -1.0*np.eye(3)
-        quat = algebra.quat_bound(MB_tstep[self.FoR_body].quat)
-        Bnh[:3, FoR_dof:FoR_dof+3] = algebra.quat2rotation(quat)
+        quat = ag.quat_bound(MB_tstep[self.FoR_body].quat)
+        Bnh[:3, FoR_dof:FoR_dof+3] = ag.quat2rotation(quat)
 
         # BC for angular velocities
-        Bnh[3:6,FoR_dof+3:FoR_dof+6] = -1.0*algebra.quat2rotation(quat)
+        Bnh[3:6,FoR_dof+3:FoR_dof+6] = -1.0*ag.quat2rotation(quat)
         ielem, inode_in_elem = MB_beam[0].node_master_elem[self.node_number]
-        Bnh[3:6,node_dof+3:node_dof+6] = algebra.crv2tan(MB_tstep[0].psi[ielem, inode_in_elem, :])
+        Bnh[3:6,node_dof+3:node_dof+6] = ag.crv2tan(MB_tstep[0].psi[ielem, inode_in_elem, :])
 
-        LM_C[sys_size+ieq:sys_size+ieq+num_LM_eq_specific,:sys_size] += scalingFactor*Bnh
-        LM_C[:sys_size,sys_size+ieq:sys_size+ieq+num_LM_eq_specific] += scalingFactor*np.transpose(Bnh)
+        LM_C[sys_size+ieq:sys_size+ieq+num_LM_eq_specific,:sys_size] += self.scalingFactor*Bnh
+        LM_C[:sys_size,sys_size+ieq:sys_size+ieq+num_LM_eq_specific] += self.scalingFactor*np.transpose(Bnh)
 
-        LM_Q[:sys_size] += scalingFactor*np.dot(np.transpose(Bnh),Lambda_dot[ieq:ieq+num_LM_eq_specific])
-        LM_Q[sys_size+ieq:sys_size+ieq+3] += -MB_tstep[0].pos_dot[-1,:] + np.dot(algebra.quat2rotation(quat),MB_tstep[1].for_vel[0:3])
-        LM_Q[sys_size+ieq+3:sys_size+ieq+6] += (np.dot(algebra.crv2tan(MB_tstep[0].psi[ielem, inode_in_elem, :]),MB_tstep[0].psi_dot[ielem, inode_in_elem, :]) -
-                                      np.dot(algebra.quat2rotation(quat), MB_tstep[self.FoR_body].for_vel[3:6]))
+        LM_Q[:sys_size] += self.scalingFactor*np.dot(np.transpose(Bnh),Lambda_dot[ieq:ieq+num_LM_eq_specific])
+        LM_Q[sys_size+ieq:sys_size+ieq+3] += -self.scalingFactor*MB_tstep[0].pos_dot[-1,:] + np.dot(ag.quat2rotation(quat),MB_tstep[1].for_vel[0:3])
+        LM_Q[sys_size+ieq+3:sys_size+ieq+6] += self.scalingFactor*(np.dot(ag.crv2tan(MB_tstep[0].psi[ielem, inode_in_elem, :]),MB_tstep[0].psi_dot[ielem, inode_in_elem, :]) -
+                                      np.dot(ag.quat2rotation(quat), MB_tstep[self.FoR_body].for_vel[3:6]))
 
-        #LM_K[FoR_dof:FoR_dof+3,FoR_dof+6:FoR_dof+10] = algebra.der_CquatT_by_v(MB_tstep[body_FoR].quat,Lambda_dot)
-        LM_C[FoR_dof:FoR_dof+3,FoR_dof+6:FoR_dof+10] += algebra.der_CquatT_by_v(quat,scalingFactor*Lambda_dot[ieq:ieq+3])
-        LM_C[FoR_dof+3:FoR_dof+6,FoR_dof+6:FoR_dof+10] -= algebra.der_CquatT_by_v(quat,scalingFactor*Lambda_dot[ieq+3:ieq+6])
+        #LM_K[FoR_dof:FoR_dof+3,FoR_dof+6:FoR_dof+10] = ag.der_CquatT_by_v(MB_tstep[body_FoR].quat,Lambda_dot)
+        LM_C[FoR_dof:FoR_dof+3,FoR_dof+6:FoR_dof+10] += self.scalingFactor*ag.der_CquatT_by_v(quat,Lambda_dot[ieq:ieq+3])
+        LM_C[FoR_dof+3:FoR_dof+6,FoR_dof+6:FoR_dof+10] -= self.scalingFactor*ag.der_CquatT_by_v(quat,Lambda_dot[ieq+3:ieq+6])
 
-        LM_K[node_dof+3:node_dof+6,node_dof+3:node_dof+6] += algebra.der_TanT_by_xv(MB_tstep[0].psi[ielem, inode_in_elem, :],scalingFactor*Lambda_dot[ieq+3:ieq+6])
+        LM_K[node_dof+3:node_dof+6,node_dof+3:node_dof+6] += self.scalingFactor*ag.der_TanT_by_xv(MB_tstep[0].psi[ielem, inode_in_elem, :],Lambda_dot[ieq+3:ieq+6])
 
         ieq += 6
         return
@@ -1048,45 +1414,10 @@ def staticpost(self, lc_list, MB_beam, MB_tstep):
         return
 
     def dynamicpost(self, lc_list, MB_beam, MB_tstep):
-        # MB_tstep[self.FoR_body].for_pos[0:3] = np.dot(algebra.quat2rotation(MB_tstep[self.node_body].quat), MB_tstep[self.node_body].pos[self.node_number,:]) + MB_tstep[self.node_body].for_pos[0:3]
+        # MB_tstep[self.FoR_body].for_pos[0:3] = np.dot(ag.quat2rotation(MB_tstep[self.node_body].quat), MB_tstep[self.node_body].pos[self.node_number,:]) + MB_tstep[self.node_body].for_pos[0:3]
         return
 
 
-# @lagrangeconstraint
-# class hinge_node_FoR_constant_rotation(BaseLagrangeConstraint):
-#     _lc_id = 'hinge_node_FoR_constant_rotation'
-#
-#     def __init__(self):
-#         self._n_eq = 4
-#
-#     def get_n_eq(self):
-#         return self._n_eq
-#
-#     def initialise(self, MBdict_entry, ieq):
-#         print('Type of LC: ', self._lc_id)
-#         print('Arguments and values:')
-#         for k, v in MBdict_entry.items():
-#             print(k, v)
-#
-#         self._ieq = ieq
-#         return self._ieq + self._n_eq
-#
-#     def staticmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq,
-#                 sys_size, dt, Lambda, Lambda_dot,
-#                 scalingFactor, penaltyFactor):
-#         return
-#
-#     def dynamicmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq,
-#                 sys_size, dt, Lambda, Lambda_dot,
-#                 scalingFactor, penaltyFactor):
-#         return
-#
-#     def staticpost(self, lc_list, MB_beam, MB_tstep):
-#         return
-#
-#     def dynamicpost(self, lc_list, MB_beam, MB_tstep):
-#         return
-
 @lagrangeconstraint
 class constant_rot_vel_FoR(BaseLagrangeConstraint):
     __doc__ = """
@@ -1109,26 +1440,21 @@ def get_n_eq(self):
         return self._n_eq
 
     def initialise(self, MBdict_entry, ieq, print_info=True):
-        # if print_info:
-            # cout.cout_wrap('Type of LC: ', self._lc_id)
-            # cout.cout_wrap('Arguments and values:')
-            # for k, v in MBdict_entry.items():
-                # cout.cout_wrap(k, v)
 
         self.rot_vel = MBdict_entry['rot_vel']
         self.FoR_body = MBdict_entry['FoR_body']
         self._ieq = ieq
+        self.scalingFactor = set_value_or_default(MBdict_entry, "scalingFactor", 1.)
+        self.penaltyFactor = set_value_or_default(MBdict_entry, "penaltyFactor", 0.)
 
         return self._ieq + self._n_eq
 
     def staticmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq,
-                sys_size, dt, Lambda, Lambda_dot,
-                scalingFactor, penaltyFactor):
+                sys_size, dt, Lambda, Lambda_dot):
         return
 
     def dynamicmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq,
-                sys_size, dt, Lambda, Lambda_dot,
-                scalingFactor, penaltyFactor):
+                sys_size, dt, Lambda, Lambda_dot):
         num_LM_eq_specific = self._n_eq
         Bnh = np.zeros((num_LM_eq_specific, sys_size), dtype=ct.c_double, order = 'F')
         B = np.zeros((num_LM_eq_specific, sys_size), dtype=ct.c_double, order = 'F')
@@ -1139,11 +1465,11 @@ def dynamicmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq,
 
         Bnh[:3,FoR_dof+3:FoR_dof+6] = np.eye(3)
 
-        LM_C[sys_size+ieq:sys_size+ieq+num_LM_eq_specific,:sys_size] += scalingFactor*Bnh
-        LM_C[:sys_size,sys_size+ieq:sys_size+ieq+num_LM_eq_specific] += scalingFactor*np.transpose(Bnh)
+        LM_C[sys_size+ieq:sys_size+ieq+num_LM_eq_specific,:sys_size] += self.scalingFactor*Bnh
+        LM_C[:sys_size,sys_size+ieq:sys_size+ieq+num_LM_eq_specific] += self.scalingFactor*np.transpose(Bnh)
 
-        LM_Q[:sys_size] += scalingFactor*np.dot(np.transpose(Bnh),Lambda_dot[ieq:ieq+num_LM_eq_specific])
-        LM_Q[sys_size+ieq:sys_size+ieq+num_LM_eq_specific] += MB_tstep[self.FoR_body].for_vel[3:6] - self.rot_vel
+        LM_Q[:sys_size] += self.scalingFactor*np.dot(np.transpose(Bnh),Lambda_dot[ieq:ieq+num_LM_eq_specific])
+        LM_Q[sys_size+ieq:sys_size+ieq+num_LM_eq_specific] += self.scalingFactor*(MB_tstep[self.FoR_body].for_vel[3:6] - self.rot_vel)
 
         ieq += 3
         return
@@ -1177,26 +1503,21 @@ def get_n_eq(self):
         return self._n_eq
 
     def initialise(self, MBdict_entry, ieq, print_info=True):
-        # if print_info:
-            # cout.cout_wrap('Type of LC: ', self._lc_id)
-            # cout.cout_wrap('Arguments and values:')
-            # for k, v in MBdict_entry.items():
-                # cout.cout_wrap(k, v)
 
         self.vel = MBdict_entry['vel']
         self.FoR_body = MBdict_entry['FoR_body']
         self._ieq = ieq
+        self.scalingFactor = set_value_or_default(MBdict_entry, "scalingFactor", 1.)
+        self.penaltyFactor = set_value_or_default(MBdict_entry, "penaltyFactor", 0.)
 
         return self._ieq + self._n_eq
 
     def staticmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq,
-                sys_size, dt, Lambda, Lambda_dot,
-                scalingFactor, penaltyFactor):
+                sys_size, dt, Lambda, Lambda_dot):
         return
 
     def dynamicmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq,
-                sys_size, dt, Lambda, Lambda_dot,
-                scalingFactor, penaltyFactor):
+                sys_size, dt, Lambda, Lambda_dot):
         num_LM_eq_specific = self._n_eq
         Bnh = np.zeros((num_LM_eq_specific, sys_size), dtype=ct.c_double, order='F')
         B = np.zeros((num_LM_eq_specific, sys_size), dtype=ct.c_double, order='F')
@@ -1207,11 +1528,11 @@ def dynamicmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq,
 
         Bnh[:num_LM_eq_specific, FoR_dof:FoR_dof+6] = np.eye(6)
 
-        LM_C[sys_size + ieq:sys_size + ieq + num_LM_eq_specific, :sys_size] += scalingFactor * Bnh
-        LM_C[:sys_size, sys_size + ieq:sys_size + ieq + num_LM_eq_specific] += scalingFactor * np.transpose(Bnh)
+        LM_C[sys_size + ieq:sys_size + ieq + num_LM_eq_specific, :sys_size] += self.scalingFactor * Bnh
+        LM_C[:sys_size, sys_size + ieq:sys_size + ieq + num_LM_eq_specific] += self.scalingFactor * np.transpose(Bnh)
 
-        LM_Q[:sys_size] += scalingFactor * np.dot(np.transpose(Bnh), Lambda_dot[ieq:ieq + num_LM_eq_specific])
-        LM_Q[sys_size + ieq:sys_size + ieq + num_LM_eq_specific] += MB_tstep[self.FoR_body].for_vel - self.vel
+        LM_Q[:sys_size] += self.scalingFactor * np.dot(np.transpose(Bnh), Lambda_dot[ieq:ieq + num_LM_eq_specific])
+        LM_Q[sys_size + ieq:sys_size + ieq + num_LM_eq_specific] += self.scalingFactor*(MB_tstep[self.FoR_body].for_vel - self.vel)
 
         ieq += 6
         return
@@ -1247,46 +1568,41 @@ def get_n_eq(self):
         return self._n_eq
 
     def initialise(self, MBdict_entry, ieq, print_info=True):
-        # if print_info:
-            # cout.cout_wrap('Type of LC: ', self._lc_id)
-            # cout.cout_wrap('Arguments and values:')
-            # for k, v in MBdict_entry.items():
-                # cout.cout_wrap(k, v)
 
         self.vel = MBdict_entry['velocity']
         self.body_number = MBdict_entry['body_number']
         self.node_number = MBdict_entry['node_number']
         self._ieq = ieq
+        self.scalingFactor = set_value_or_default(MBdict_entry, "scalingFactor", 1.)
+        self.penaltyFactor = set_value_or_default(MBdict_entry, "penaltyFactor", 0.)
 
         return self._ieq + self._n_eq
 
     def staticmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq,
-                sys_size, dt, Lambda, Lambda_dot,
-                scalingFactor, penaltyFactor):
+                sys_size, dt, Lambda, Lambda_dot):
 
         num_LM_eq_specific = self._n_eq
         B = np.zeros((num_LM_eq_specific, sys_size), dtype=ct.c_double, order='F')
 
         # Define the position of the first degree of freedom associated to the FoR
-        # FoR_dof = define_FoR_dof(MB_beam, self.body_number)
         node_dof = define_node_dof(MB_beam, self.body_number, self.node_number)
         ieq = self._ieq
 
         B[:num_LM_eq_specific, node_dof:node_dof+3] = np.eye(3)
 
-        LM_K[sys_size + ieq:sys_size + ieq + num_LM_eq_specific, :sys_size] += scalingFactor * B
-        LM_K[:sys_size, sys_size + ieq:sys_size + ieq + num_LM_eq_specific] += scalingFactor * np.transpose(B)
+        LM_K[sys_size + ieq:sys_size + ieq + num_LM_eq_specific, :sys_size] += self.scalingFactor * B
+        LM_K[:sys_size, sys_size + ieq:sys_size + ieq + num_LM_eq_specific] += self.scalingFactor * np.transpose(B)
 
-        LM_Q[:sys_size] += scalingFactor * np.dot(np.transpose(B), Lambda[ieq:ieq + num_LM_eq_specific])
-        LM_Q[sys_size + ieq:sys_size + ieq + num_LM_eq_specific] += MB_tstep[self.body_number].pos[self.node_number,:] - MB_beam[self.body_number].ini_info.pos[self.node_number,:]
+        LM_Q[:sys_size] += self.scalingFactor * np.dot(np.transpose(B), Lambda[ieq:ieq + num_LM_eq_specific])
+        LM_Q[sys_size + ieq:sys_size + ieq + num_LM_eq_specific] += self.scalingFactor*(MB_tstep[self.body_number].pos[self.node_number,:] -
+                                                                                        MB_beam[self.body_number].ini_info.pos[self.node_number,:])
 
         ieq += 3
 
         return
 
     def dynamicmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq,
-                sys_size, dt, Lambda, Lambda_dot,
-                scalingFactor, penaltyFactor):
+                sys_size, dt, Lambda, Lambda_dot):
 
         if len(self.vel.shape) > 1:
             current_vel = self.vel[ts-1, :]
@@ -1298,17 +1614,16 @@ def dynamicmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq,
         B = np.zeros((num_LM_eq_specific, sys_size), dtype=ct.c_double, order='F')
 
         # Define the position of the first degree of freedom associated to the FoR
-        # FoR_dof = define_FoR_dof(MB_beam, self.body_number)
         node_dof = define_node_dof(MB_beam, self.body_number, self.node_number)
         ieq = self._ieq
 
         Bnh[:num_LM_eq_specific, node_dof:node_dof+3] = np.eye(3)
 
-        LM_C[sys_size + ieq:sys_size + ieq + num_LM_eq_specific, :sys_size] += scalingFactor * Bnh
-        LM_C[:sys_size, sys_size + ieq:sys_size + ieq + num_LM_eq_specific] += scalingFactor * np.transpose(Bnh)
+        LM_C[sys_size + ieq:sys_size + ieq + num_LM_eq_specific, :sys_size] += self.scalingFactor * Bnh
+        LM_C[:sys_size, sys_size + ieq:sys_size + ieq + num_LM_eq_specific] += self.scalingFactor * np.transpose(Bnh)
 
-        LM_Q[:sys_size] += scalingFactor * np.dot(np.transpose(Bnh), Lambda_dot[ieq:ieq + num_LM_eq_specific])
-        LM_Q[sys_size + ieq:sys_size + ieq + num_LM_eq_specific] += MB_tstep[self.body_number].pos_dot[self.node_number,:] - current_vel
+        LM_Q[:sys_size] += self.scalingFactor * np.dot(np.transpose(Bnh), Lambda_dot[ieq:ieq + num_LM_eq_specific])
+        LM_Q[sys_size + ieq:sys_size + ieq + num_LM_eq_specific] += self.scalingFactor*(MB_tstep[self.body_number].pos_dot[self.node_number,:] - current_vel)
 
         ieq += 3
         return
@@ -1344,39 +1659,35 @@ def get_n_eq(self):
         return self._n_eq
 
     def initialise(self, MBdict_entry, ieq, print_info=True):
-        # if print_info:
-            # cout.cout_wrap('Type of LC: ' + str(self._lc_id))
-            # cout.cout_wrap('Arguments and values:')
-            # for k, v in MBdict_entry.items():
-                # cout.cout_wrap(str(k) + str(v))
 
         self.vel = MBdict_entry['velocity']
         self.body_number = MBdict_entry['body_number']
         self.node_number = MBdict_entry['node_number']
         self._ieq = ieq
+        self.scalingFactor = set_value_or_default(MBdict_entry, "scalingFactor", 1.)
+        self.penaltyFactor = set_value_or_default(MBdict_entry, "penaltyFactor", 0.)
+
         return self._ieq + self._n_eq
 
     def staticmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq,
-                sys_size, dt, Lambda, Lambda_dot,
-                scalingFactor, penaltyFactor):
+                sys_size, dt, Lambda, Lambda_dot):
 
         num_LM_eq_specific = self._n_eq
         B = np.zeros((num_LM_eq_specific, sys_size), dtype=ct.c_double, order='F')
 
         # Define the position of the first degree of freedom associated to the FoR
-        # FoR_dof = define_FoR_dof(MB_beam, self.body_number)
         node_dof = define_node_dof(MB_beam, self.body_number, self.node_number)
         ieq = self._ieq
 
-        B[:num_LM_eq_specific, node_dof:node_dof+3] = algebra.quat2rotation(MB_tstep[self.body_number].quat)
+        B[:num_LM_eq_specific, node_dof:node_dof+3] = MB_tstep[self.body_number].cga()
 
-        LM_K[sys_size + ieq:sys_size + ieq + num_LM_eq_specific, :sys_size] += scalingFactor * B
-        LM_K[:sys_size, sys_size + ieq:sys_size + ieq + num_LM_eq_specific] += scalingFactor * np.transpose(B)
+        LM_K[sys_size + ieq:sys_size + ieq + num_LM_eq_specific, :sys_size] += self.scalingFactor * B
+        LM_K[:sys_size, sys_size + ieq:sys_size + ieq + num_LM_eq_specific] += self.scalingFactor * np.transpose(B)
 
-        LM_Q[:sys_size] += scalingFactor * np.dot(np.transpose(B), Lambda[ieq:ieq + num_LM_eq_specific])
-        LM_Q[sys_size + ieq:sys_size + ieq + num_LM_eq_specific] += (np.dot(algebra.quat2rotation(MB_tstep[self.body_number].quat), MB_tstep[self.body_number].pos[self.node_number,:]) +
+        LM_Q[:sys_size] += self.scalingFactor * np.dot(np.transpose(B), Lambda[ieq:ieq + num_LM_eq_specific])
+        LM_Q[sys_size + ieq:sys_size + ieq + num_LM_eq_specific] += self.scalingFactor*(np.dot(MB_tstep[self.body_number].cga(), MB_tstep[self.body_number].pos[self.node_number,:]) +
                                                                      MB_tstep[self.body_number].for_pos)
-        LM_Q[sys_size + ieq:sys_size + ieq + num_LM_eq_specific] -= (np.dot(algebra.quat2rotation(MB_beam[self.body_number].ini_info.quat), MB_beam[self.body_number].ini_info.pos[self.node_number,:]) +
+        LM_Q[sys_size + ieq:sys_size + ieq + num_LM_eq_specific] -= self.scalingFactor*(np.dot(MB_beam[self.body_number].ini_info.cga(), MB_beam[self.body_number].ini_info.pos[self.node_number,:]) +
                                                                      MB_beam[self.body_number].ini_info.for_pos)
 
         ieq += 3
@@ -1384,8 +1695,7 @@ def staticmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq,
         return
 
     def dynamicmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq,
-                sys_size, dt, Lambda, Lambda_dot,
-                scalingFactor, penaltyFactor):
+                sys_size, dt, Lambda, Lambda_dot):
         if len(self.vel.shape) > 1:
             current_vel = self.vel[ts-1, :]
         else:
@@ -1401,24 +1711,24 @@ def dynamicmat(self, LM_C, LM_K, LM_Q, MB_beam, MB_tstep, ts, num_LM_eq,
         ieq = self._ieq
 
         if MB_beam[self.body_number].FoR_movement == 'free':
-            Bnh[:num_LM_eq_specific, FoR_dof:FoR_dof+3] = algebra.quat2rotation(MB_tstep[self.body_number].quat)
-            Bnh[:num_LM_eq_specific, FoR_dof+3:FoR_dof+6] = -np.dot(algebra.quat2rotation(MB_tstep[self.body_number].quat), algebra.skew(MB_tstep[self.body_number].pos[self.node_number,:]))
-        Bnh[:num_LM_eq_specific, node_dof:node_dof+3] = algebra.quat2rotation(MB_tstep[self.body_number].quat)
+            Bnh[:num_LM_eq_specific, FoR_dof:FoR_dof+3] = MB_tstep[self.body_number].cga()
+            Bnh[:num_LM_eq_specific, FoR_dof+3:FoR_dof+6] = -np.dot(MB_tstep[self.body_number].cga(), ag.skew(MB_tstep[self.body_number].pos[self.node_number,:]))
+        Bnh[:num_LM_eq_specific, node_dof:node_dof+3] = MB_tstep[self.body_number].cga()
 
-        LM_C[sys_size + ieq:sys_size + ieq + num_LM_eq_specific, :sys_size] += scalingFactor * Bnh
-        LM_C[:sys_size, sys_size + ieq:sys_size + ieq + num_LM_eq_specific] += scalingFactor * np.transpose(Bnh)
+        LM_C[sys_size + ieq:sys_size + ieq + num_LM_eq_specific, :sys_size] += self.scalingFactor * Bnh
+        LM_C[:sys_size, sys_size + ieq:sys_size + ieq + num_LM_eq_specific] += self.scalingFactor * np.transpose(Bnh)
 
         if MB_beam[self.body_number].FoR_movement == 'free':
-            LM_C[FoR_dof:FoR_dof+3, FoR_dof+6:FoR_dof+10] += algebra.der_CquatT_by_v(MB_tstep[self.body_number].quat,Lambda_dot[ieq:ieq + num_LM_eq_specific])
-            LM_C[node_dof:node_dof+3, FoR_dof+6:FoR_dof+10] += algebra.der_CquatT_by_v(MB_tstep[self.body_number].quat,Lambda_dot[ieq:ieq + num_LM_eq_specific])
-            LM_C[FoR_dof+3:FoR_dof+6, FoR_dof+6:FoR_dof+10] += np.dot(algebra.skew(MB_tstep[self.body_number].pos[self.node_number,:]), algebra.der_CquatT_by_v(MB_tstep[self.body_number].quat,Lambda_dot[ieq:ieq + num_LM_eq_specific]))
+            LM_C[FoR_dof:FoR_dof+3, FoR_dof+6:FoR_dof+10] += self.scalingFactor*ag.der_CquatT_by_v(MB_tstep[self.body_number].quat,Lambda_dot[ieq:ieq + num_LM_eq_specific])
+            LM_C[node_dof:node_dof+3, FoR_dof+6:FoR_dof+10] += self.scalingFactor*ag.der_CquatT_by_v(MB_tstep[self.body_number].quat,Lambda_dot[ieq:ieq + num_LM_eq_specific])
+            LM_C[FoR_dof+3:FoR_dof+6, FoR_dof+6:FoR_dof+10] += self.scalingFactor*np.dot(ag.skew(MB_tstep[self.body_number].pos[self.node_number,:]), ag.der_CquatT_by_v(MB_tstep[self.body_number].quat,Lambda_dot[ieq:ieq + num_LM_eq_specific]))
 
-            LM_K[FoR_dof+3:FoR_dof+6, node_dof:node_dof+3] -= algebra.skew(np.dot(algebra.quat2rotation(MB_tstep[self.body_number].quat).T, Lambda_dot[ieq:ieq + num_LM_eq_specific]))
+            LM_K[FoR_dof+3:FoR_dof+6, node_dof:node_dof+3] -= self.scalingFactor*ag.skew(np.dot(MB_tstep[self.body_number].cga().T, Lambda_dot[ieq:ieq + num_LM_eq_specific]))
 
-        LM_Q[:sys_size] += scalingFactor * np.dot(np.transpose(Bnh), Lambda_dot[ieq:ieq + num_LM_eq_specific])
-        LM_Q[sys_size + ieq:sys_size + ieq + num_LM_eq_specific] += (np.dot( algebra.quat2rotation(MB_tstep[self.body_number].quat), (
+        LM_Q[:sys_size] += self.scalingFactor * np.dot(np.transpose(Bnh), Lambda_dot[ieq:ieq + num_LM_eq_specific])
+        LM_Q[sys_size + ieq:sys_size + ieq + num_LM_eq_specific] += self.scalingFactor*(np.dot( MB_tstep[self.body_number].cga(), (
                 MB_tstep[self.body_number].for_vel[0:3] +
-                np.dot(algebra.skew(MB_tstep[self.body_number].for_vel[3:6]), MB_tstep[self.body_number].pos[self.node_number,:]) +
+                np.dot(ag.skew(MB_tstep[self.body_number].for_vel[3:6]), MB_tstep[self.body_number].pos[self.node_number,:]) +
                 MB_tstep[self.body_number].pos_dot[self.node_number,:])) -
                 current_vel)
 
@@ -1498,11 +1808,6 @@ def generate_lagrange_matrix(lc_list, MB_beam, MB_tstep, ts, num_LM_eq, sys_size
         LM_K (np.ndarray): Stiffness matrix associated to the Lagrange Multipliers equations
         LM_Q (np.ndarray): Vector of independent terms associated to the Lagrange Multipliers equations
     """
-    # Lagrange multipliers parameters
-    # TODO: set them as an input variable (at this point they should not be changed)
-    penaltyFactor = 0.0
-    scalingFactor = 1.0
-
     # Initialize matrices
     LM_C = np.zeros((sys_size + num_LM_eq,sys_size + num_LM_eq), dtype=ct.c_double, order = 'F')
     LM_K = np.zeros((sys_size + num_LM_eq,sys_size + num_LM_eq), dtype=ct.c_double, order = 'F')
@@ -1516,7 +1821,6 @@ def generate_lagrange_matrix(lc_list, MB_beam, MB_tstep, ts, num_LM_eq, sys_size
             lc.staticmat(LM_C=LM_C,
                         LM_K=LM_K,
                         LM_Q=LM_Q,
-                        # MBdict=MBdict,
                         MB_beam=MB_beam,
                         MB_tstep=MB_tstep,
                         ts=ts,
@@ -1524,16 +1828,12 @@ def generate_lagrange_matrix(lc_list, MB_beam, MB_tstep, ts, num_LM_eq, sys_size
                         sys_size=sys_size,
                         dt=dt,
                         Lambda=Lambda,
-                        Lambda_dot=Lambda_dot,
-                        # ieq=ieq,
-                        scalingFactor=scalingFactor,
-                        penaltyFactor=penaltyFactor)
+                        Lambda_dot=Lambda_dot)
 
         elif dynamic_or_static.lower() == "dynamic":
             lc.dynamicmat(LM_C=LM_C,
                         LM_K=LM_K,
                         LM_Q=LM_Q,
-                        # MBdict=MBdict,
                         MB_beam=MB_beam,
                         MB_tstep=MB_tstep,
                         ts=ts,
@@ -1541,10 +1841,7 @@ def generate_lagrange_matrix(lc_list, MB_beam, MB_tstep, ts, num_LM_eq, sys_size
                         sys_size=sys_size,
                         dt=dt,
                         Lambda=Lambda,
-                        Lambda_dot=Lambda_dot,
-                        # ieq=ieq,
-                        scalingFactor=scalingFactor,
-                        penaltyFactor=penaltyFactor)
+                        Lambda_dot=Lambda_dot)
 
     return LM_C, LM_K, LM_Q
 
@@ -1558,13 +1855,11 @@ def postprocess(lc_list, MB_beam, MB_tstep, dynamic_or_static):
             lc.staticpost(lc_list = lc_list,
                            MB_beam = MB_beam,
                            MB_tstep = MB_tstep)
-                           # MBdict = MBdict)
 
         elif dynamic_or_static.lower() == "dynamic":
             lc.dynamicpost(lc_list = lc_list,
                            MB_beam = MB_beam,
                            MB_tstep = MB_tstep)
-                           # MBdict = MBdict)
 
     return
 
@@ -1587,16 +1882,4 @@ def remove_constraint(MBdict, constraint):
 ################################################################################
 ################################################################################
 ################################################################################
-# this at the end of the file
 print_available_lc()
-
-# test
-# if __name__ == '__main__':
-    # lc_list = list()
-    # lc_list.append(lc_from_string('SampleLagrange')())
-    # lc_list.append(lc_from_string('SampleLagrange')())
-
-    # counter = -1
-    # for lc in lc_list:
-        # counter += 1
-        # lc.initialise(counter=counter)
diff --git a/sharpy/structure/utils/xbeamlib.py b/sharpy/structure/utils/xbeamlib.py
index 6a6d044d7..39a690f28 100644
--- a/sharpy/structure/utils/xbeamlib.py
+++ b/sharpy/structure/utils/xbeamlib.py
@@ -606,6 +606,7 @@ def xbeam_solv_state2disp(beam, tstep, cbeam3=False):
     numdof = beam.num_dof.value
     cbeam3_solv_state2disp(beam, tstep)
     if not cbeam3:
+        tstep.for_pos[0:3] = tstep.q[numdof:numdof+3].astype(dtype=ct.c_double, order='F', copy=True)
         tstep.for_vel = tstep.dqdt[numdof:numdof+6].astype(dtype=ct.c_double, order='F', copy=True)
         # tstep.for_acc = tstep.dqddt[numdof:numdof+6].astype(dtype=ct.c_double, order='F', copy=True)
         tstep.quat = algebra.unit_vector(tstep.dqdt[numdof+6:]).astype(dtype=ct.c_double, order='F', copy=True)
@@ -679,6 +680,7 @@ def cbeam3_solv_state2accel(beam, tstep):
 def xbeam_solv_disp2state(beam, tstep):
     numdof = beam.num_dof.value
     cbeam3_solv_disp2state(beam, tstep)
+    tstep.q[numdof:numdof+3] = tstep.for_pos[0:3]
     tstep.dqdt[numdof:numdof+6] = tstep.for_vel
     # tstep.dqddt[numdof:numdof+6] = tstep.for_acc
     tstep.dqdt[numdof+6:] = algebra.unit_vector(tstep.quat)
diff --git a/sharpy/utils/algebra.py b/sharpy/utils/algebra.py
index f39b52fca..80a371090 100644
--- a/sharpy/utils/algebra.py
+++ b/sharpy/utils/algebra.py
@@ -1950,3 +1950,26 @@ def cgb(psi, quat):
         return cgb
     else:
         raise NameError('Unknown transformation.')
+
+
+def der_skewp_skewp_v(p, v):
+    """
+    This function computes:
+
+        .. math:: \frac{d}{d\boldsymbol{p}} (\tilde{\boldsymbol{p}} \tilde{\boldsymbol{p}} v)
+    """
+    der = np.zeros((3,3))
+
+    der[0, 0] = v[1]*p[1] + v[2]*p[2]
+    der[0, 1] = -2*v[0]*p[1] + v[1]*p[0]
+    der[0, 2] = -2*v[0]*p[2] + v[2]*p[0]
+
+    der[1, 0] = v[0]*p[1] - 2*v[1]*p[0]
+    der[1, 1] = v[0]*p[0] + v[2]*p[2]
+    der[1, 2] = -2*v[1]*p[2] + v[2]*p[1]
+
+    der[2, 0] = v[0]*p[2] - 2*v[2]*p[0]
+    der[2, 1] = v[1]*p[2] - 2*v[2]*p[1]
+    der[2, 2] = v[0]*p[0] + v[1]*p[1]
+
+    return der
diff --git a/sharpy/utils/datastructures.py b/sharpy/utils/datastructures.py
index 5b9434789..346313a3a 100644
--- a/sharpy/utils/datastructures.py
+++ b/sharpy/utils/datastructures.py
@@ -858,6 +858,10 @@ def whole_structure_to_local_AFoR(self, beam):
             self.pos[ibody_nodes,:] = MB_tstep[ibody].pos.astype(dtype=ct.c_double, order='F', copy=True)
             self.psi[ibody_elems,:,:] = MB_tstep[ibody].psi.astype(dtype=ct.c_double, order='F', copy=True)
             self.gravity_forces[ibody_nodes,:] = MB_tstep[ibody].gravity_forces.astype(dtype=ct.c_double, order='F', copy=True)
+
+            self.pos_dot[ibody_nodes,:] = MB_tstep[ibody].pos_dot.astype(dtype=ct.c_double, order='F', copy=True)
+            self.psi_dot[ibody_elems,:,:] = MB_tstep[ibody].psi_dot.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)
@@ -898,6 +902,9 @@ def whole_structure_to_global_AFoR(self, beam):
             self.psi[ibody_elems,:,:] = MB_tstep[ibody].psi.astype(dtype=ct.c_double, order='F', copy=True)
             self.gravity_forces[ibody_nodes,:] = MB_tstep[ibody].gravity_forces.astype(dtype=ct.c_double, order='F',
                                                                                        copy=True)
+            
+            self.pos_dot[ibody_nodes,:] = MB_tstep[ibody].pos_dot.astype(dtype=ct.c_double, order='F', copy=True)
+            self.psi_dot[ibody_elems,:,:] = MB_tstep[ibody].psi_dot.astype(dtype=ct.c_double, order='F', copy=True)
 
 
 class LinearTimeStepInfo(object):
diff --git a/sharpy/utils/generate_cases.py b/sharpy/utils/generate_cases.py
index fb5e40d62..042a91361 100644
--- a/sharpy/utils/generate_cases.py
+++ b/sharpy/utils/generate_cases.py
@@ -1912,10 +1912,10 @@ def check(self):
                 raise RuntimeError(("'%s' parameter required in '%s' lagrange constraint" % (param, self.behaviour)))
         has_behaviour = False
         for param, value in self.__dict__.items():
-            if not param == 'behaviour':
+            if not param in ['behaviour', 'scalingFactor', 'penaltyFactor']:
                 if param not in required_parameters:
                     raise RuntimeError(("'%s' parameter is not required in '%s' lagrange constraint" % (param, self.behaviour)))
-            else:
+            if param == 'behaviour':
                 has_behaviour = True
 
         if not has_behaviour:
@@ -1949,6 +1949,16 @@ def generate_multibody_file(list_LagrangeConstraints, list_Bodies, route, case_n
             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:
+                pass
+            try:
+                constraint_id.create_dataset("penaltyFactor",
+                                             data=getattr(constraint, "penaltyFactor"))
+            except:
+                pass
 
             iconstraint += 1
 
diff --git a/sharpy/utils/multibody.py b/sharpy/utils/multibody.py
index ff634c200..d6eaa6113 100644
--- a/sharpy/utils/multibody.py
+++ b/sharpy/utils/multibody.py
@@ -30,8 +30,6 @@ def split_multibody(beam, tstep, mb_data_dict, ts):
         MB_tstep (list(:class:`~sharpy.utils.datastructures.StructTimeStepInfo`)): each entry represents a body
     """
 
-    update_mb_db_before_split(tstep, beam, mb_data_dict, ts)
-
     MB_beam = []
     MB_tstep = []
 
@@ -86,7 +84,6 @@ def merge_multibody(MB_tstep, MB_beam, beam, tstep, mb_data_dict, dt):
         ibody_nodes = MB_beam[ibody].global_nodes_num
 
         # Merge tstep
-        # MB_tstep[ibody].change_to_global_AFoR(ibody)
         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)
@@ -94,7 +91,8 @@ def merge_multibody(MB_tstep, MB_beam, beam, tstep, mb_data_dict, dt):
         tstep.psi_dot[ibody_elems,:,:] = MB_tstep[ibody].psi_dot.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)
-        # TODO: Do I need a change in FoR for the following variables? Maybe for the FoR ones.
+        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.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)
 
@@ -119,55 +117,6 @@ def merge_multibody(MB_tstep, MB_beam, beam, tstep, mb_data_dict, dt):
     tstep.quat = MB_tstep[0].quat.astype(dtype=ct.c_double, order='F', copy=True)
 
 
-def update_mb_db_before_split(tstep, beam, mb_data_dict, ts):
-    """
-    update_mb_db_before_split
-
-    Updates the FoR information database before splitting system
-
-    Args:
-    	tstep (:class:`~sharpy.utils.datastructures.StructTimeStepInfo`): timestep information of the multibody system
-    	beam (:class:`~sharpy.structure.models.beam.Beam`): structural information of the multibody system
-        mb_data_dict (dict): Dictionary including the multibody information
-        ts (int): time step number
-
-    Notes:
-        At this point, this function does nothing, but we might need it at some point
-
-    """
-
-    return
-
-    raise NotImplementedError("This function is useless right now")
-
-    # TODO: Right now, the Amaster FoR is not expected to move
-    # when it does, the rest of FoR positions should be updated accordingly
-    # right now, this function should be useless (I check it below)
-
-    # if mb_data_dict['body_%02d' % 0]['FoR_movement']:
-    #     CGAmaster = algebra.quat2rotation(tstep.quat)
-    #     tstep.mb_FoR_vel[0, 0:3] = np.dot(CGAmaster, tstep.for_vel[0:3])
-    #     tstep.mb_FoR_vel[0, 3:6] = np.dot(CGAmaster, tstep.for_vel[3:6])
-    #     tstep.mb_FoR_acc[0, 0:3] = np.dot(CGAmaster, tstep.for_acc[0:3])
-    #     tstep.mb_FoR_acc[0, 3:6] = np.dot(CGAmaster, tstep.for_acc[3:6])
-
-    if ((mb_data_dict['body_00']['FoR_movement'] == 'prescribed') and (ts > 0)):
-        tstep.for_vel[:] = beam.dynamic_input[ts - 1]['for_vel'].astype(dtype=ct.c_double, order='F', copy=True)
-        tstep.for_acc[:] = beam.dynamic_input[ts - 1]['for_acc'].astype(dtype=ct.c_double, order='F', copy=True)
-
-    if True:
-        CGAmaster = algebra.quat2rotation(tstep.quat)
-
-        tstep.mb_FoR_pos[0,:] = tstep.for_pos.astype(dtype=ct.c_double, order='F', copy=True)
-        tstep.mb_FoR_vel[0,0:3] = np.dot(CGAmaster,tstep.for_vel[0:3])
-        tstep.mb_FoR_vel[0,3:6] = np.dot(CGAmaster,tstep.for_vel[3:6])
-        tstep.mb_FoR_acc[0,0:3] = np.dot(CGAmaster,tstep.for_acc[0:3])
-        tstep.mb_FoR_acc[0,3:6] = np.dot(CGAmaster,tstep.for_acc[3:6])
-        tstep.mb_quat[0,:] = tstep.quat.astype(dtype=ct.c_double, order='F', copy=True)
-    else:
-        pass
-
-
 def update_mb_dB_before_merge(tstep, MB_tstep):
     """
     update_mb_db_before_merge
@@ -181,8 +130,6 @@ def update_mb_dB_before_merge(tstep, MB_tstep):
 
     for ibody in range(len(MB_tstep)):
 
-        # CAslaveG = algebra.quat2rotation(MB_tstep[ibody].quat).T
-
         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)
@@ -191,7 +138,7 @@ def update_mb_dB_before_merge(tstep, MB_tstep):
         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, q, dqdt, dqddt):
+def disp_and_accel2state(MB_beam, MB_tstep, Lambda, Lambda_dot, sys_size, num_LM_eq):
     """
     disp2state
 
@@ -200,10 +147,19 @@ def disp_and_accel2state(MB_beam, MB_tstep, q, dqdt, dqddt):
     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)):
@@ -224,12 +180,15 @@ def disp_and_accel2state(MB_beam, MB_tstep, q, dqdt, dqddt):
             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]=MB_tstep[ibody].mb_dqddt_quat[ibody,:].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
 
+    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)
 
-def state2disp_and_accel(q, dqdt, dqddt, MB_beam, MB_tstep):
+    return q, dqdt, dqddt
+
+def state2disp_and_accel(q, dqdt, dqddt, MB_beam, MB_tstep, num_LM_eq):
     """
     state2disp
 
@@ -243,9 +202,17 @@ def state2disp_and_accel(q, dqdt, dqddt, MB_beam, MB_tstep):
         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
 
     """
 
+    Lambda = np.zeros((num_LM_eq, ), dtype=ct.c_double, order='F')
+    Lambda_dot = np.zeros((num_LM_eq, ), dtype=ct.c_double, order='F')
+
     first_dof = 0
     for ibody in range(len(MB_beam)):
 
@@ -265,12 +232,12 @@ def state2disp_and_accel(q, dqdt, dqddt, MB_beam, MB_tstep):
             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])
-            # if onlyFlex:
-            #     xbeamlib.cbeam3_solv_state2disp(MB_beam[ibody], MB_tstep[ibody])
-            # else:
-            #     xbeamlib.xbeam_solv_state2disp(MB_beam[ibody], MB_tstep[ibody])
             first_dof += ibody_num_dof + 10
 
+    Lambda = q[first_dof:].astype(dtype=ct.c_double, order='F', copy=True)
+    Lambda_dot = dqdt[first_dof:].astype(dtype=ct.c_double, order='F', copy=True)
+
+    return Lambda, Lambda_dot
 
 def get_elems_nodes_list(beam, ibody):
     """


Test patch:
diff --git a/tests/coupled/multibody/double_pendulum/test_double_pendulum_geradin.py b/tests/coupled/multibody/double_pendulum/test_double_pendulum_geradin.py
index f06d252de..2783b8b71 100644
--- a/tests/coupled/multibody/double_pendulum/test_double_pendulum_geradin.py
+++ b/tests/coupled/multibody/double_pendulum/test_double_pendulum_geradin.py
@@ -212,11 +212,10 @@ def setUp(self):
 
         SimInfo.solvers['SHARPy']['flow'] = ['BeamLoader',
                                 'AerogridLoader',
-                                # 'InitializeMultibody',
                                 'DynamicCoupled']
-        global name
-        name = 'double_pendulum_geradin'
-        SimInfo.solvers['SHARPy']['case'] = 'double_pendulum_geradin'
+        global name_hinge
+        name_hinge = 'dpg_hinge'
+        SimInfo.solvers['SHARPy']['case'] = name_hinge
         SimInfo.solvers['SHARPy']['write_screen'] = 'off'
         SimInfo.solvers['SHARPy']['route'] = os.path.abspath(os.path.dirname(os.path.realpath(__file__))) + '/'
         SimInfo.solvers['SHARPy']['log_folder'] = os.path.abspath(os.path.dirname(os.path.realpath(__file__))) + '/output/'
@@ -242,7 +241,11 @@ def setUp(self):
         SimInfo.solvers['WriteVariablesTime']['structure_variables'] = ['pos']
 
         SimInfo.solvers['NonLinearDynamicMultibody']['gravity_on'] = True
-        SimInfo.solvers['NonLinearDynamicMultibody']['newmark_damp'] = 0.15
+        SimInfo.solvers['NonLinearDynamicMultibody']['print_cond_number'] = False
+        SimInfo.solvers['NonLinearDynamicMultibody']['time_integrator'] = 'NewmarkBeta'
+        SimInfo.solvers['NonLinearDynamicMultibody']['time_integrator_settings'] = {'newmark_damp': 0.15,
+                                                                                    'dt': dt}
+        SimInfo.solvers['NonLinearDynamicMultibody']['write_lm'] = True
 
         SimInfo.solvers['BeamPlot']['include_FoR'] = True
 
@@ -263,6 +266,8 @@ def setUp(self):
         LC1.behaviour = 'hinge_FoR'
         LC1.body_FoR = 0
         LC1.rot_axis_AFoR = np.array([0.0,1.0,0.0])
+        LC1.scalingFactor = 1e6
+        LC1.penaltyFactor = 1e-12
 
         LC2 = gc.LagrangeConstraint()
         LC2.behaviour = 'hinge_node_FoR'
@@ -270,6 +275,8 @@ def setUp(self):
         LC2.body = 0
         LC2.body_FoR = 1
         LC2.rot_axisB = np.array([0.0,1.0,0.0])
+        LC2.scalingFactor = 1e6
+        LC2.penaltyFactor = 1e-12
 
         LC = []
         LC.append(LC1)
@@ -302,28 +309,112 @@ def setUp(self):
         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'])
 
-    def test_doublependulum(self):
+        # Same case without dissipation
+        global name_nb_zero_dis
+        name_nb_zero_dis = 'dpg_nb_zero_dis'
+        SimInfo.solvers['SHARPy']['case'] = name_nb_zero_dis
+
+        SimInfo.solvers['NonLinearDynamicMultibody']['time_integrator_settings'] = {'newmark_damp': 0.,
+                                                                                    'dt': dt}
+
+        gc.clean_test_files(SimInfo.solvers['SHARPy']['route'], SimInfo.solvers['SHARPy']['case'])
+        SimInfo.generate_solver_file()
+        SimInfo.generate_dyn_file(numtimesteps)
+        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 generalised alpha
+        global name_ga
+        name_ga = 'dpg_ga'
+        SimInfo.solvers['SHARPy']['case'] = name_ga
+
+        SimInfo.solvers['NonLinearDynamicMultibody']['time_integrator'] = 'GeneralisedAlpha'
+        SimInfo.solvers['NonLinearDynamicMultibody']['time_integrator_settings'] = {'am': 0.5,
+                                                                                    'af': 0.5,
+                                                                                    'dt': dt}
+
+        gc.clean_test_files(SimInfo.solvers['SHARPy']['route'], SimInfo.solvers['SHARPy']['case'])
+        SimInfo.generate_solver_file()
+        SimInfo.generate_dyn_file(numtimesteps)
+        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)
+        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'])
+
+    def run_and_assert(self, name):
         import sharpy.sharpy_main
 
-        solver_path = os.path.abspath(os.path.dirname(os.path.realpath(__file__)) + '/double_pendulum_geradin.sharpy')
+        solver_path = os.path.abspath(os.path.dirname(os.path.realpath(__file__)) + '/' + name + '.sharpy')
         sharpy.sharpy_main.main(['', solver_path])
 
         # read output and compare
-        output_path = os.path.abspath(os.path.dirname(os.path.realpath(__file__))) + '/output/double_pendulum_geradin/WriteVariablesTime/'
+        output_path = os.path.abspath(os.path.dirname(os.path.realpath(__file__))) + '/output/' + name + '/WriteVariablesTime/'
         pos_tip_data = np.loadtxt(("%sstruct_pos_node%d.dat" % (output_path, nnodes1*2-1)), )
         self.assertAlmostEqual(pos_tip_data[-1, 1], 1.051004, 4)
         self.assertAlmostEqual(pos_tip_data[-1, 2], 0.000000, 4)
         self.assertAlmostEqual(pos_tip_data[-1, 3], -0.9986984, 4)
 
+    def test_doublependulum_hinge(self):
+        self.run_and_assert(name_hinge)
+
+    def test_doublependulum_spherical(self):
+        self.run_and_assert(name_spherical)
+
+    def test_doublependulum_ga(self):
+        import sharpy.sharpy_main
+
+        nb_solver_path = os.path.abspath(os.path.dirname(os.path.realpath(__file__)) + '/' + name_nb_zero_dis + '.sharpy')
+        sharpy.sharpy_main.main(['', nb_solver_path])
+
+        ga_solver_path = os.path.abspath(os.path.dirname(os.path.realpath(__file__)) + '/' + name_ga + '.sharpy')
+        sharpy.sharpy_main.main(['', ga_solver_path])
+
+        # read output and compare
+        nb_output_path = os.path.abspath(os.path.dirname(os.path.realpath(__file__))) + '/output/' + name_nb_zero_dis + '/WriteVariablesTime/'
+        nb_pos_tip_data = np.loadtxt(("%sstruct_pos_node%d.dat" % (nb_output_path, nnodes1*2-1)), )
+
+        ga_output_path = os.path.abspath(os.path.dirname(os.path.realpath(__file__))) + '/output/' + name_ga + '/WriteVariablesTime/'
+        ga_pos_tip_data = np.loadtxt(("%sstruct_pos_node%d.dat" % (ga_output_path, nnodes1*2-1)), )
+
+        self.assertAlmostEqual(nb_pos_tip_data[-1, 1], ga_pos_tip_data[-1, 1], 4)
+        self.assertAlmostEqual(nb_pos_tip_data[-1, 2], ga_pos_tip_data[-1, 2], 4)
+        self.assertAlmostEqual(nb_pos_tip_data[-1, 3], ga_pos_tip_data[-1, 3], 4)
+
     def tearDown(self):
         solver_path = os.path.abspath(os.path.dirname(os.path.realpath(__file__)))
         solver_path += '/'
-        files_to_delete = [name + '.aero.h5',
-                           name + '.dyn.h5',
-                           name + '.fem.h5',
-                           name + '.mb.h5',
-                           name + '.sharpy']
-        for f in files_to_delete:
-            os.remove(solver_path + f)
+        for name in [name_hinge, name_spherical, name_ga, name_nb_zero_dis]:
+            files_to_delete = [name + '.aero.h5',
+                               name + '.dyn.h5',
+                               name + '.fem.h5',
+                               name + '.mb.h5',
+                               name + '.sharpy']
+            for f in files_to_delete:
+                os.remove(solver_path + f)
 
         shutil.rmtree(solver_path + 'output/')
diff --git a/tests/coupled/multibody/fix_node_velocity_wrtA/test_fix_node_velocity_wrtA.py b/tests/coupled/multibody/fix_node_velocity_wrtA/test_fix_node_velocity_wrtA.py
index 0262637bc..e2eea8cc2 100644
--- a/tests/coupled/multibody/fix_node_velocity_wrtA/test_fix_node_velocity_wrtA.py
+++ b/tests/coupled/multibody/fix_node_velocity_wrtA/test_fix_node_velocity_wrtA.py
@@ -104,7 +104,10 @@ def setUp(self):
         SimInfo.solvers['NonLinearDynamicMultibody']['relaxation_factor'] = 0.2
         SimInfo.solvers['NonLinearDynamicMultibody']['min_delta'] = 1e-6
         SimInfo.solvers['NonLinearDynamicMultibody']['max_iterations'] = 200
-        SimInfo.solvers['NonLinearDynamicMultibody']['newmark_damp'] = 1e-3
+        SimInfo.solvers['NonLinearDynamicMultibody']['write_lm'] = False
+        SimInfo.solvers['NonLinearDynamicMultibody']['time_integrator'] = 'NewmarkBeta'
+        SimInfo.solvers['NonLinearDynamicMultibody']['time_integrator_settings'] = {'newmark_damp': 1e-3,
+                                                                                    'dt': 0.05}
 
         SimInfo.solvers['WriteVariablesTime']['cleanup_old_solution'] = 'on'
 
@@ -117,7 +120,7 @@ def setUp(self):
                                                                         'BeamPlot': SimInfo.solvers['BeamPlot'],
                                                                         'AerogridPlot': SimInfo.solvers['AerogridPlot']}
 
-        ntimesteps = 100
+        ntimesteps = 10
 
         SimInfo.define_num_steps(ntimesteps)
 
@@ -168,13 +171,13 @@ def test_testfixnodevelocitywrta(self):
         output_path = folder + '/output/fix_node_velocity_wrtA/WriteVariablesTime/'
         # quat_data = np.matrix(np.genfromtxt(output_path + 'FoR_00_mb_quat.dat', delimiter=' '))
         pos_tip_data = np.loadtxt(("%sstruct_pos_node-1.dat" % output_path), )
-        self.assertAlmostEqual(pos_tip_data[0, 1], 9.993, 3)
-        self.assertAlmostEqual(pos_tip_data[0, 2], 0., 2)
-        self.assertAlmostEqual(pos_tip_data[0, 3], -3.40215e-1, 3)
+        self.assertAlmostEqual(pos_tip_data[0, 1], 9.993007e+00, 3)
+        self.assertAlmostEqual(pos_tip_data[0, 2], 0., 3)
+        self.assertAlmostEqual(pos_tip_data[0, 3], -3.402154e-01, 3)
 
-        self.assertAlmostEqual(pos_tip_data[-1, 1], 9.9858, 3)
-        self.assertAlmostEqual(pos_tip_data[-1, 2], 0., 2)
-        self.assertAlmostEqual(pos_tip_data[-1, 3], -4.58856e-1, 3)
+        self.assertAlmostEqual(pos_tip_data[-1, 1], 9.962520e+00, 3)
+        self.assertAlmostEqual(pos_tip_data[-1, 2], 0., 3)
+        self.assertAlmostEqual(pos_tip_data[-1, 3], -6.943423e-01, 3)
 
         pos_root_data = np.loadtxt(("%sstruct_pos_node0.dat" % output_path), )
         self.assertAlmostEqual(pos_root_data[0, 1], 0.0, 2)
diff --git a/tests/coupled/multibody/fix_node_velocity_wrtG/test_fix_node_velocity_wrtG.py b/tests/coupled/multibody/fix_node_velocity_wrtG/test_fix_node_velocity_wrtG.py
index 7d38d78c1..ad395a5c2 100644
--- a/tests/coupled/multibody/fix_node_velocity_wrtG/test_fix_node_velocity_wrtG.py
+++ b/tests/coupled/multibody/fix_node_velocity_wrtG/test_fix_node_velocity_wrtG.py
@@ -103,8 +103,10 @@ def setUp(self):
         SimInfo.solvers['NonLinearDynamicMultibody']['relaxation_factor'] = 0.0
         SimInfo.solvers['NonLinearDynamicMultibody']['min_delta'] = 1e-5
         SimInfo.solvers['NonLinearDynamicMultibody']['max_iterations'] = 200
-        SimInfo.solvers['NonLinearDynamicMultibody']['newmark_damp'] = 1e-3
         # SimInfo.solvers['NonLinearDynamicMultibody']['gravity_on'] = 'off'
+        SimInfo.solvers['NonLinearDynamicMultibody']['time_integrator'] = 'NewmarkBeta'
+        SimInfo.solvers['NonLinearDynamicMultibody']['time_integrator_settings'] = {'newmark_damp': 1e-3,
+                                                                                    'dt': 0.1}
 
         SimInfo.solvers['NonLinearDynamicMultibody']['relaxation_factor'] = 0.0