restart; with(LinearAlgebra): with(codegen,optimize): Advanced Robotics: HW 01 Setups skew := (a,b,c) -> Matrix([[0,-c,b],[c,0,-a],[-b,a,0]]); Zio2JUkiYUc2IkkiYkdGJUkiY0dGJUYlNiRJKW9wZXJhdG9yR0YlSSZhcnJvd0dGJUYlLUknTWF0cml4R0YlNiM3JTclIiIhLCQ5JiEiIjklNyVGMkYwLCQ5JEYzNyUsJEY0RjNGN0YwRiVGJUYl Rkt := (i,j,k,tetha) -> Matrix(linalg[exponential](skew(i,j,k)*tetha)); Zio2JkkiaUc2IkkiakdGJUkia0dGJUkmdGV0aGFHRiVGJTYkSSlvcGVyYXRvckdGJUkmYXJyb3dHRiVGJS1JJ01hdHJpeEdGJTYjLSZJJ2xpbmFsZ0dGJTYjSSxleHBvbmVudGlhbEdGJTYjKiYtSSVza2V3R0YlNiU5JDklOSYiIiI5J0Y8RiVGJUYl linalg[exponential](skew(i,j,k)*tetha):
<Text-field style="Text" size="14" underline="true" layout="Normal256"><Font bold="true" underline="true" size="14">rpy2R:</Font><Font underline="false" italic="true" size="14"> Roll, pitch and yaw angles (Euler x-y-z convention) to rotation matrix</Font></Text-field> Rx := Rkt(1,0,0,q1); LUknTWF0cml4RzYkJSpwcm90ZWN0ZWRHSShfc3lzbGliRzYiNiMvSSQlaWRHRiciKltmVmUi Ry := Rkt(0,1,0,q2); LUknTWF0cml4RzYkJSpwcm90ZWN0ZWRHSShfc3lzbGliRzYiNiMvSSQlaWRHRiciKiFRZXk6 Rz := Rkt(0,0,1,q3); LUknTWF0cml4RzYkJSpwcm90ZWN0ZWRHSShfc3lzbGliRzYiNiMvSSQlaWRHRiciKmNlbWQi Rout := Rz . Ry . Rx; LUknTWF0cml4RzYkJSpwcm90ZWN0ZWRHSShfc3lzbGliRzYiNiMvSSQlaWRHRiciKj8lKTR3Ig== IntArg := Rkt(1,0,0,sigma) . Matrix(3,1,[0,0,0]); LUknTWF0cml4RzYkJSpwcm90ZWN0ZWRHSShfc3lzbGliRzYiNiMvSSQlaWRHRiciKmdUbWQi L1 := map( int, IntArg, sigma=0..q1); LUknTWF0cml4RzYkJSpwcm90ZWN0ZWRHSShfc3lzbGliRzYiNiMvSSQlaWRHRiciKlNNTnEi T1 :=Matrix(4,4,[ [Matrix(3,4,[Rout,L1])], [Matrix(1,4,[0,0,0,1])] ]); LUknTWF0cml4RzYkJSpwcm90ZWN0ZWRHSShfc3lzbGliRzYiNiMvSSQlaWRHRiciKic+LDA8
TTdSMApJNlJUQUJMRV9TQVZFLzE1ODQzNTk0OFgsJSlhbnl0aGluZ0c2IjYiW2dsISIlISEhIyoiJCIkIiIiIiIhRihGKC0lJGNvc0c2IyUjcTFHLSUkc2luR0YrRigsJEYtISIiRilGJg==TTdSMApJNlJUQUJMRV9TQVZFLzE1Nzg1ODM4MFgsJSlhbnl0aGluZ0c2IjYiW2dsISIlISEhIyoiJCIkLSUkY29zRzYjJSNxMkciIiEsJC0lJHNpbkdGKSEiIkYrIiIiRitGLUYrRidGJg==TTdSMApJNlJUQUJMRV9TQVZFLzE1NzY2NTg1NlgsJSlhbnl0aGluZ0c2IjYiW2dsISIlISEhIyoiJCIkLSUkY29zRzYjJSNxM0ctJSRzaW5HRikiIiEsJEYrISIiRidGLUYtRi0iIiJGJg==TTdSMApJNlJUQUJMRV9TQVZFLzE3NjA5ODQyMFgsJSlhbnl0aGluZ0c2IjYiW2dsISIlISEhIyoiJCIkKiYtJSRjb3NHNiMlI3EzRyIiIi1GKTYjJSNxMkdGLComLSUkc2luR0YqRixGLUYsLCQtRjJGLiEiIiwmKiZGMUYsLUYpNiMlI3ExR0YsRjUqKEYoRixGNEYsLUYyRjlGLEYsLCYqJkYoRixGOEYsRiwqKEYxRixGNEYsRjxGLEYsKiZGLUYsRjxGLCwmKiZGMUYsRjxGLEYsKihGKEYsRjRGLEY4RixGLCwmKiZGKEYsRjxGLEY1KihGMUYsRjRGLEY4RixGLComRi1GLEY4RixGJg==TTdSMApJNlJUQUJMRV9TQVZFLzE1NzY2NDE2MFgsJSlhbnl0aGluZ0c2IjYiW2dsISIlISEhIyQiJCIiIiIhRidGJ0YmTTdSMApJNlJUQUJMRV9TQVZFLzE3MDM1MzQ0MFgsJSlhbnl0aGluZ0c2IjYiW2dsISIlISEhIyQiJCIiIiIhRidGJ0YmTTdSMApJNlJUQUJMRV9TQVZFLzE3MDUwMTE5NlgsJSlhbnl0aGluZ0c2IjYiW2dsISIlISEhIzEiJSIlKiYtJSRjb3NHNiMlI3EzRyIiIi1GKTYjJSNxMkdGLComLSUkc2luR0YqRixGLUYsLCQtRjJGLiEiIiIiISwmKiZGMUYsLUYpNiMlI3ExR0YsRjUqKEYoRixGNEYsLUYyRjpGLEYsLCYqJkYoRixGOUYsRiwqKEYxRixGNEYsRj1GLEYsKiZGLUYsRj1GLEY2LCYqJkYxRixGPUYsRiwqKEYoRixGNEYsRjlGLEYsLCYqJkYoRixGPUYsRjUqKEYxRixGNEYsRjlGLEYsKiZGLUYsRjlGLEY2RjZGNkY2RixGJg==