More JRodSIM2 Progress

I’ve spent some more time on this. So far, I have it so that it grabs the suspension coordinates from the excel sheet, iterates to find the parameter matrix, then increments the bump parameter, finds the new parameter matrix, rinse, repeat, etc. This is all good so far.

Unfortunately, I’ve hit a snag with the roll and steering iterations. Both steering and chassis roll are calculated by moving the chassis coordinates around (either rotating all the chassis points about the roll axis to simulate roll or translating the steering inboard points in the Y direction to simulate a steering rack). The problem with this is that moving the chassis global coordinates will also affect the upright global coordinates… It changes the “base” case from which all other cases are calculated… so how do I get around this?

Right now, all I can think of doing is increasing the scope of the inverse kinematic calculation process to include two more degrees of freedom: Steering link translation and chassis rotation. It sounds simple enough, but how the hell do I actually do that? I have a feeling the numbers will get extremely messy if I try.

Anyway, I’ll think about this for a couple if days and hopefully will come up with an answer (typically when I’m not expecting it). I pasted my code below, just for the hell of it. I never formally learned any programming, so feel free to offer advice (lol, I write this as if I actually have readers).

Function InverseKinematic(ByVal ChassisGlobalCoords, UprightGlobalCoords, staticlinklengthmatrix, LinkLengthDeltaMatrix, AlteredParameterMatrix, epsilon)
‘dim variables
Dim i As Integer
Dim j As Integer

‘load M(S+dS)
Dim RotoTransAllAltered(1 To 4, 1 To 4)
‘load M(S+dSx)
Dim RotoTransOneAltered(1 To 6)
Dim tempfill(1 To 4, 1 To 4)
For i = 1 To 6
RotoTransOneAltered(i) = tempfill
Next i
‘load C(S+dSx)
Dim RototranslatedAlteredUpright(1 To 6)
‘load C(S+dS)
Dim RototranslatedCheckUpright
‘load D(S+dS)
Dim Checklinklengthmatrix(1 To 6)
‘load Dx(S+dSx) – the function calculating link lengths based on the current parameter matrix
Dim AlteredLinkLengthMatrix(1 To 6, 1 To 6)
‘load Jacobian
Dim jacobian(1 To 6, 1 To 6)
‘load InvJacobian
Dim invjacobian ‘(1 To 6, 1 To 6)

‘load the zero parameter matrix
Dim s0(1 To 6)
For i = 1 To 6
s0(i) = 0
Next i

Dim qqk(1 To 6, 1 To 1)
Dim sumerror
sumerror = 1
Dim linklengtherror(1 To 6)

While sumerror > epsilon
For i = 1 To 6
OneAlteredParameterMatrix = s0
OneAlteredParameterMatrix(i) = s0(i) + AlteredParameterMatrix(i, 1)

‘make M(S+dSx) arrays
RotoTransOneAltered(i)(1, 1) = Cos(OneAlteredParameterMatrix(2)) * Cos(OneAlteredParameterMatrix(1))
RotoTransOneAltered(i)(2, 1) = Cos(OneAlteredParameterMatrix(3)) * Sin(OneAlteredParameterMatrix(2)) * Cos(OneAlteredParameterMatrix(1)) – Sin(OneAlteredParameterMatrix(3)) * Sin(OneAlteredParameterMatrix(1))
RotoTransOneAltered(i)(3, 1) = Sin(OneAlteredParameterMatrix(3)) * Sin(OneAlteredParameterMatrix(2)) * Cos(OneAlteredParameterMatrix(1)) + Cos(OneAlteredParameterMatrix(3)) * Sin(OneAlteredParameterMatrix(1))
RotoTransOneAltered(i)(4, 1) = 0
RotoTransOneAltered(i)(1, 2) = -Sin(OneAlteredParameterMatrix(2))
RotoTransOneAltered(i)(2, 2) = Cos(OneAlteredParameterMatrix(3)) * Cos(OneAlteredParameterMatrix(2))
RotoTransOneAltered(i)(3, 2) = Sin(OneAlteredParameterMatrix(3)) * Cos(OneAlteredParameterMatrix(2))
RotoTransOneAltered(i)(4, 2) = 0
RotoTransOneAltered(i)(1, 3) = -Cos(OneAlteredParameterMatrix(2)) * Sin(OneAlteredParameterMatrix(1))
RotoTransOneAltered(i)(2, 3) = -Cos(OneAlteredParameterMatrix(3)) * Sin(OneAlteredParameterMatrix(2)) * Sin(OneAlteredParameterMatrix(1)) – Cos(OneAlteredParameterMatrix(1)) * Sin(OneAlteredParameterMatrix(3))
RotoTransOneAltered(i)(3, 3) = -Sin(OneAlteredParameterMatrix(3)) * Sin(OneAlteredParameterMatrix(2)) * Sin(OneAlteredParameterMatrix(1)) + Cos(OneAlteredParameterMatrix(1)) * Cos(OneAlteredParameterMatrix(3))
RotoTransOneAltered(i)(4, 3) = 0
RotoTransOneAltered(i)(1, 4) = OneAlteredParameterMatrix(4)
RotoTransOneAltered(i)(2, 4) = OneAlteredParameterMatrix(5)
RotoTransOneAltered(i)(3, 4) = OneAlteredParameterMatrix(6)
RotoTransOneAltered(i)(4, 4) = 1
‘make C(S+dSx) arrays
RototranslatedAlteredUpright(i) = Application.WorksheetFunction.MMult(RotoTransOneAltered(i), UprightGlobalCoords)
Next i
‘make M(S+dS) array
RotoTransAllAltered(1, 1) = Cos(AlteredParameterMatrix(2, 1)) * Cos(AlteredParameterMatrix(1, 1))
RotoTransAllAltered(2, 1) = Cos(AlteredParameterMatrix(3, 1)) * Sin(AlteredParameterMatrix(2, 1)) * Cos(AlteredParameterMatrix(1, 1)) – Sin(AlteredParameterMatrix(3, 1)) * Sin(AlteredParameterMatrix(1, 1))
RotoTransAllAltered(3, 1) = Sin(AlteredParameterMatrix(3, 1)) * Sin(AlteredParameterMatrix(2, 1)) * Cos(AlteredParameterMatrix(1, 1)) + Cos(AlteredParameterMatrix(3, 1)) * Sin(AlteredParameterMatrix(1, 1))
RotoTransAllAltered(4, 1) = 0
RotoTransAllAltered(1, 2) = -Sin(AlteredParameterMatrix(2, 1))
RotoTransAllAltered(2, 2) = Cos(AlteredParameterMatrix(3, 1)) * Cos(AlteredParameterMatrix(2, 1))
RotoTransAllAltered(3, 2) = Sin(AlteredParameterMatrix(3, 1)) * Cos(AlteredParameterMatrix(2, 1))
RotoTransAllAltered(4, 2) = 0
RotoTransAllAltered(1, 3) = -Cos(AlteredParameterMatrix(2, 1)) * Sin(AlteredParameterMatrix(1, 1))
RotoTransAllAltered(2, 3) = -Cos(AlteredParameterMatrix(3, 1)) * Sin(AlteredParameterMatrix(2, 1)) * Sin(AlteredParameterMatrix(1, 1)) – Cos(AlteredParameterMatrix(1, 1)) * Sin(AlteredParameterMatrix(3, 1))
RotoTransAllAltered(3, 3) = -Sin(AlteredParameterMatrix(3, 1)) * Sin(AlteredParameterMatrix(2, 1)) * Sin(AlteredParameterMatrix(1, 1)) + Cos(AlteredParameterMatrix(1, 1)) * Cos(AlteredParameterMatrix(3, 1))
RotoTransAllAltered(4, 3) = 0
RotoTransAllAltered(1, 4) = AlteredParameterMatrix(4, 1)
RotoTransAllAltered(2, 4) = AlteredParameterMatrix(5, 1)
RotoTransAllAltered(3, 4) = AlteredParameterMatrix(6, 1)
RotoTransAllAltered(4, 4) = 1
‘make check C(S+dS) array
RototranslatedCheckUpright = Application.WorksheetFunction.MMult(RotoTransAllAltered, UprightGlobalCoords)

Sheets(“Jrodsim2”).Range(“B14:G17”) = RototranslatedCheckUpright

‘make D(S+dSx) arrays
For i = 1 To 6
For j = 1 To 6
‘steering link error is here
AlteredLinkLengthMatrix(i, j) = Sqr((RototranslatedAlteredUpright(i)(1, j) – ChassisGlobalCoords(1, j)) ^ 2 + (RototranslatedAlteredUpright(i)(2, j) – ChassisGlobalCoords(2, j)) ^ 2 + (RototranslatedAlteredUpright(i)(3, j) – ChassisGlobalCoords(3, j)) ^ 2)
Next j
Next i

‘find Dk
For i = 1 To 6
Checklinklengthmatrix(i) = Sqr((RototranslatedCheckUpright(1, i) – ChassisGlobalCoords(1, i)) ^ 2 + (RototranslatedCheckUpright(2, i) – ChassisGlobalCoords(2, i)) ^ 2 + (RototranslatedCheckUpright(3, i) – ChassisGlobalCoords(3, i)) ^ 2)
Next i
‘fill jacobian
For i = 1 To 6
For j = 1 To 6
If AlteredParameterMatrix(j, 1) = 0 Then
jacobian(i, j) = 1
Else
jacobian(i, j) = (AlteredLinkLengthMatrix(j, i) – staticlinklengthmatrix(i, 1)) / AlteredParameterMatrix(j, 1)
End If
Next j
Next i
‘invert jacobian
invjacobian = Application.WorksheetFunction.MInverse(jacobian)

‘find ([J][dS])
JDS = Application.WorksheetFunction.MMult(jacobian, AlteredParameterMatrix)
‘find Q-Qk= (D0+[J][dS]) – Dk + deltaS
For i = 1 To 6
qqk(i, 1) = staticlinklengthmatrix(i, 1) + JDS(i, 1) – Checklinklengthmatrix(i) + LinkLengthDeltaMatrix(i, 1)
Next i

sumerror = 0
‘find error = D0 – Dk – deltaS, then summ all errors to get a single error number
For i = 1 To 6
linklengtherror(i) = Checklinklengthmatrix(i) – staticlinklengthmatrix(i, 1) – LinkLengthDeltaMatrix(i, 1)
sumerror = sumerror + Abs(linklengtherror(i))
Next i

‘find new altered parameter matrix
temp = Application.WorksheetFunction.MMult(invjacobian, qqk)
For i = 1 To 6
AlteredParameterMatrix(i, 1) = temp(i, 1)
Next i
Wend

Dim combinedoutput(1 To 2)
combinedoutput(1) = AlteredParameterMatrix
combinedoutput(2) = RototranslatedCheckUpright
InverseKinematic = combinedoutput

End Function

Advertisements
This entry was posted in Suspension stuff and tagged , , , , . Bookmark the permalink.

2 Responses to More JRodSIM2 Progress

  1. dontworryyouhavea reader says:

    programming doesn’t look too shabby for someone who’s never formally learned coding! putting things in vb is a good way to get people to read them. draw in the nerdprogrammer crowd. i’m just confused by the lack of “;”s but i’m not familiar with VB anyway

Leave a Reply

Fill in your details below or click an icon to log in:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out / Change )

Twitter picture

You are commenting using your Twitter account. Log Out / Change )

Facebook photo

You are commenting using your Facebook account. Log Out / Change )

Google+ photo

You are commenting using your Google+ account. Log Out / Change )

Connecting to %s