fix hinge_FoR
@@ -1201,8 +1201,8 @@ class hinge_FoR(BaseLagrangeConstraint):
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+self.zero_comp] += self.penaltyFactor*MB_tstep[self.body_FoR].for_vel[3+self.zero_comp]
- LM_C[np.ix_(FoR_dof+3+self.zero_comp, FoR_dof+3+self.zero_comp)] += self.penaltyFactor*np.eye(2)
+ 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
