case fixed FoR
@@ -371,6 +371,7 @@ def equal_lin_vel_node_FoR(MB_tstep, MB_beam, FoR_body, node_body, node_number,
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
@@ -386,6 +387,7 @@ def equal_lin_vel_node_FoR(MB_tstep, MB_beam, FoR_body, node_body, node_number,
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))
@@ -406,6 +408,7 @@ def equal_lin_vel_node_FoR(MB_tstep, MB_beam, FoR_body, node_body, node_number,
# 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)
