Hello everyone,
I am trying to add a constraint to a simple rigid body sim that i have been testing.
Here is where i'm at:
- There are 2 rigid bodies
- One slider (prismatic) joint between the 2 bodies
- No contacts or collisions are used
So it's just 2 masses connected by a prismatic joint floating in space.
Here are the example values and initial conditions:
BODY1:
m1 = mass of body 1
I1=[Ixx1, 0, 0
0, Iyy1, 0
0, 0, Izz1] = inertia matrix of body 1
Xinitial1 = [x1,y1,z1] = position vector of CG
Pinitial=[0,0,0] (linear momentum vector)
Linitial=[0,0,0] (angular momentum vector)
R1 = [1,0,0
0,1,0
0,0,1] = rotation matrix at initial conditions
r1 = vector from CG to joint
F1 = [Fx1,Fy1,Fz1] = external force vector on mass 1
T1 = [Tx1,Ty1,Tz1] = external torque vector on mass 1
BODY2:
m2 = mass of body 2
I2=[Ixx2, 0, 0
0, Iyy2, 0
0, 0, Ixx2] = inertia matrix of body 2
Xinitial2 = [x2,y2,z2] = position vector of CG
Pinitial=[0,0,0] (linear momentum vector)
Linitial=[0,0,0] (angular momentum vector)
R2 = [1,0,0
0,1,0
0,0,1] = rotation matrix at initial condition
r2 = vector from CG to joint
F2 = [Fx2,Fy2,Fz2] = external force vector on mass 2
T2 = [Tx2,Ty2,Tz2] = external torque vector on mass 2
Initially say that the prismatic joint's unconstrained axis is in the global Z axis direction.
and: c=r2-r1
t1 = first normal to prismatic free direction ([1,0,0] is one)
t2 = another normal to prismatic free direction ([0,1,0] is another one)
So this gives the following jacobian for the initial condition (i got this from Erleben's Stable, Robust, and Versatile Multibody Dynamics Animation):
J=[0,0,0, 0,0,0, 1,0,0, -1,0,0
0,0,0, 0,0,0, 0,1,0, 0,-1,0
0,0,0, 0,0,0, 0,0,1, 0,0,-1
t1*R1, -t1*R1, 0.5*c x (t1*R1), 0.5*c x (t1*R1)
t2*R2, -t2*R2, 0.5*c x (t2*R2), 0.5*c x (t2*R2)]
And the M matrix:
M=[m1,0,0, 0,0,0, 0,0,0, 0,0,0
0,m1,0, 0,0,0, 0,0,0, 0,0,0
0,0,m1, 0,0,0, 0,0,0, 0,0,0
0,0,0, Ixx1,0,0, 0,0,0, 0,0,0
0,0,0, 0,Iyy1,0, 0,0,0, 0,0,0
0,0,0, 0,0,Izz1, 0,0,0, 0,0,0
0,0,0, 0,0,0, m2,0,0, 0,0,0
0,0,0, 0,0,0, 0,m2,0, 0,0,0
0,0,0, 0,0,0, 0,0,m2, 0,0,0
0,0,0, 0,0,0, 0,0,0, Ixx2,0,0
0,0,0, 0,0,0, 0,0,0, 0,Iyy2,0
0,0,0, 0,0,0, 0,0,0, 0,0,Izz2]
And the Q matrix (external forces/torques):
Q=[Fx1,0,0, 0,0,0, 0,0,0, 0,0,0
0,Fy1,0, 0,0,0, 0,0,0, 0,0,0
0,0,Fz1, 0,0,0, 0,0,0, 0,0,0
0,0,0, Tx1,0,0, 0,0,0, 0,0,0
0,0,0, 0,Ty1,0, 0,0,0, 0,0,0
0,0,0, 0,0,Tz1, 0,0,0, 0,0,0
0,0,0, 0,0,0, Fx2,0,0, 0,0,0
0,0,0, 0,0,0, 0,Fy2,0, 0,0,0
0,0,0, 0,0,0, 0,0,Fz2, 0,0,0
0,0,0, 0,0,0, 0,0,0, Tx2,0,0
0,0,0, 0,0,0, 0,0,0, 0,Ty2,0
0,0,0, 0,0,0, 0,0,0, 0,0,Tz2]
And the q vector:
q=[x1,y1,z1, ?1,?1,?1, x2,y2,z2, ?2,?2,?2] <<< are the ? marks the three angles in the respective quaternions?
I don't know how to complete and solve the equation:
JWJ^T * lambda = -dJdq - JWQ
My questions are with dJ and dq. I assume dJ is found by inspection?
I think i can handle dq, it is just the linear velocities and derivatives of the quaternions?
Then my final question is how do i go about solving for lambda once the other terms are known? Can it be solved for directly or will an iterative approach need to be employed?
thanks and any help is much appreciated!!!
rob