pose_ekf_slam: motion_transformations.py Source File

Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 # ROS imports
00004 import roslib 
00005 roslib.load_manifest('pose_ekf_slam')
00006 import tf
00007 from tf.transformations import euler_from_quaternion, quaternion_from_euler
00008 
00009 # More imports
00010 from numpy import array, cross, dot
00011 import PyKDL
00012 import math
00013 
00014 def position(measured_pose, vehicle_orientation, trans_sensor_from_vehicle):
00015     p = PyKDL.Vector(measured_pose.x, measured_pose.y, measured_pose.z)
00016     angle = tf.transformations.euler_from_quaternion([vehicle_orientation.x,
00017                                                       vehicle_orientation.y,
00018                                                       vehicle_orientation.z,
00019                                                       vehicle_orientation.w])
00020     O = PyKDL.Rotation.RPY(angle[0], angle[1], angle[2])
00021     t = PyKDL.Vector(trans_sensor_from_vehicle[0], 
00022                      trans_sensor_from_vehicle[1], 
00023                      trans_sensor_from_vehicle[2])
00024     return p - (O * t)
00025 
00026 
00027 def landmarkPosition(landmark_pose, rot_tf, trans_tf):
00028     p = PyKDL.Vector(landmark_pose.x, landmark_pose.y, landmark_pose.z)
00029     angle = tf.transformations.euler_from_quaternion([rot_tf[0],
00030                                                       rot_tf[1],
00031                                                       rot_tf[2],
00032                                                       rot_tf[3]])
00033     O = PyKDL.Rotation.RPY(angle[0], angle[1], angle[2])
00034     t = PyKDL.Vector(trans_tf[0], 
00035                      trans_tf[1], 
00036                      trans_tf[2])
00037     f = PyKDL.Frame(O, t)
00038 
00039     return f*p
00040     
00041 
00042 def orientation(measured_orientation, rot_sensor_from_vehicle):
00043     angle = euler_from_quaternion([measured_orientation.x, 
00044                                    measured_orientation.y, 
00045                                    measured_orientation.z, 
00046                                    measured_orientation.w])
00047     orientation_data =  PyKDL.Rotation.RPY(angle[0], angle[1], angle[2])
00048     
00049     rot_angle = euler_from_quaternion([rot_sensor_from_vehicle[0],
00050                                        rot_sensor_from_vehicle[1],
00051                                        rot_sensor_from_vehicle[2],
00052                                        rot_sensor_from_vehicle[3]])
00053     R = PyKDL.Rotation.RPY(rot_angle[0], rot_angle[1], rot_angle[2])
00054     # R.Inverse() == transpose R
00055     orientation_data = orientation_data * R.Inverse() 
00056     
00057     new_angle = orientation_data.GetRPY()
00058     
00059     return quaternion_from_euler(new_angle[0], new_angle[1], new_angle[2])
00060 
00061 
00062 def linearVelocity(measured_linear_velocity, 
00063                    vehicle_angular_velocity, 
00064                    trans_sensor_from_vehicle, 
00065                    rot_sensor_from_vehicle):
00066 
00067     v = PyKDL.Vector(measured_linear_velocity.x, 
00068                      measured_linear_velocity.y, 
00069                      measured_linear_velocity.z)
00070                      
00071     angle = euler_from_quaternion([rot_sensor_from_vehicle[0],
00072                                    rot_sensor_from_vehicle[1],
00073                                    rot_sensor_from_vehicle[2],
00074                                    rot_sensor_from_vehicle[3]])
00075                                  
00076     R = PyKDL.Rotation.RPY(angle[0], angle[1], angle[2])
00077     v_r = R * v
00078             
00079     #Once the velocity is rotates to the vehicle frame it is necessary to add
00080     #the linear velocity that appears when combining vehicle's angular velocity
00081     #and displacement between sensor and vehicle's frame
00082     w = array([vehicle_angular_velocity.x,
00083                vehicle_angular_velocity.y,
00084                vehicle_angular_velocity.z])
00085 
00086     return array([v_r[0], v_r[1], v_r[2]]) - cross(w, array(trans_sensor_from_vehicle))
00087 
00088 
00089 def angularVelocity(measured_angular_velocity, rot_sensor_from_vehicle):
00090     angle = tf.transformations.euler_from_quaternion([rot_sensor_from_vehicle[0],
00091                                                       rot_sensor_from_vehicle[1],
00092                                                       rot_sensor_from_vehicle[2],
00093                                                       rot_sensor_from_vehicle[3]])
00094     R = PyKDL.Rotation.RPY(angle[0], angle[1], angle[2])
00095     w = PyKDL.Vector(measured_angular_velocity.x, 
00096                      measured_angular_velocity.y, 
00097                      measured_angular_velocity.z)
00098     return R * w 
00099 
00100 
00101 def positionCovariance(m_cov, vh_or_cov, vh_or, trans):
00102     # print 'vh_or_cov: ', vh_or_cov
00103     angle = euler_from_quaternion([vh_or.x, 
00104                                    vh_or.y, 
00105                                    vh_or.z, 
00106                                    vh_or.w])
00107     r = angle[0]
00108     p = angle[1]
00109     y = angle[2]
00110     # print 'r, p, y: ', r, p, y
00111     
00112     dx = trans[0]
00113     dy = trans[1]
00114     dz = trans[2]
00115     # print 'dx, dy, dz: ', dx, dy, dz
00116 
00117     cr = math.cos(r)
00118     sr = math.sin(r)
00119     cp = math.cos(p)
00120     sp = math.sin(p)
00121     cy = math.cos(y)
00122     sy = math.sin(y)
00123 
00124 #    RPY rotation matrix
00125 #    rpy = array([cy*cp,  cy*sp*sr - sy*cr,   cy*sp*cr + sy*sr,
00126 #                 sy*cp,  sy*sp*sr + cy*cr,   sy*sp*cr - cy*sr,
00127 #                 -sp,    cp*sr,              cp*cr]).reshape(3,3)
00128 
00129 #    Z = measured_pose - RPY*trans
00130 #    Zx = Mx - cy*cp*dx + (cy*sp*sr - sy*cr)*dy + (cy*sp*cr + sy*sr)*dz
00131 #    Zy = My - sy*cp*dx + (sy*sp*sr + cy*cr)*dy + (sy*sp*cr - cy*sr)*dz
00132 #    Zz = Mz - (-sp*dx) + cp*sr*dy + cp*cr*dz
00133     
00134     d_r_x = (cy*sp*cr + sy*sr)*dy + (-cy*sp*sr + sy*cr)*dz
00135     d_r_y = (sy*sp*cr - cy*sr)*dy + (-sy*sp*sr - cy*cr)*dz
00136     d_r_z = -cp*cr*dy + cp*sr*dz
00137     d_p_x = -cy*sp*dx + cy*cp*sr*dy + cy*cp*cr*dz
00138     d_p_y = -sy*sp*dx + sy*cp*sr*dy + sy*cp*cr*dz
00139     d_p_z = -cp*dx - sp*sr*dy - sp*cr*dz
00140     d_y_x = -sy*cp*dx + (-sy*sp*sr - cy*cr)*dy + (-sy*sp*cr + cy*sr)*dz
00141     d_y_y = cy*cp*dx + (cy*sp*sr - sy*cr)*dy + (cy*sp*cr + sy*sr)*dz
00142     d_y_z = 0
00143     
00144     rpy_cov = array([d_r_x, d_p_x, d_y_x,
00145                      d_r_y, d_p_y, d_y_y,
00146                      d_r_z, d_p_z, d_y_z]).reshape(3,3)
00147     
00148     return m_cov + dot(rpy_cov, dot(vh_or_cov, rpy_cov.T))
00149 
00150 
00151 def linearVelocityCov(v_cov, vh_w_cov, trans, rot):
00152     angle = euler_from_quaternion([rot[0], rot[1], rot[2], rot[3]])
00153     r = angle[0]
00154     p = angle[1]
00155     y = angle[2]
00156     tx = trans[0]
00157     ty = trans[1]
00158     tz = trans[2]    
00159 
00160 #    Z = RPY*Measured_v - cross(w, trans)
00161 #    Take first RPY*M_v
00162 #    Z'vx = cy*cp*vx + (cy*sp*sr - sy*cr)*vy + (cy*sp*cr + sy*sr)*vz
00163 #    Z'vy = sy*cp*vx + (sy*sp*sr + cy*cr)*vy + (sy*sp*cr - cy*sr)*vz
00164 #    Z'vz = -sp*vx + cp*sr*vy + cp*cr*vz
00165 #    This is a linera operation because the covariance is in M_v then:
00166     rpy = PyKDL.Rotation.RPY(r, p, y)
00167     rpy_v_cov = array([rpy[0,0], rpy[0,1], rpy[0,2],
00168                        rpy[1,0], rpy[1,1], rpy[1,2],
00169                        rpy[2,0], rpy[2,1], rpy[2,2]]).reshape(3,3)
00170     
00171 #    Take second cross(W, T)
00172 #    x = wy*tz - wz*ty
00173 #    y = wx*tz - wz*tx
00174 #    z = wx*ty - wy*tx
00175 #    This is also linear
00176 
00177     d_wx_x = 0
00178     d_wx_y = tz
00179     d_wx_z = ty
00180     d_wy_x = tz
00181     d_wy_y = 0
00182     d_wy_z = -tx
00183     d_wz_x = tz
00184     d_wz_y = -tx
00185     d_wz_z = 0
00186     
00187     w_cov = array([d_wx_x, d_wy_x, d_wz_x,
00188                    d_wx_y, d_wy_y, d_wz_y,
00189                    d_wx_z, d_wy_z, d_wz_z]).reshape(3,3)
00190                      
00191     return (dot(rpy_v_cov, dot(v_cov, rpy_v_cov.T)) + 
00192             dot(w_cov, dot(vh_w_cov, w_cov.T)))
00193 
00194 
00195 def angularVelocityCov(w_cov, rot):
00196     angle = euler_from_quaternion([rot[0], rot[1], rot[2], rot[3]])
00197     r = angle[0]
00198     p = angle[1]
00199     y = angle[2]
00200 
00201 #    Z = RPY*W
00202 #    Zwx = cy*cp*wx + (cy*sp*sr - sy*cr)*wy + (cy*sp*cr + sy*sr)*wz
00203 #    Zwy = sy*cp*wx + (sy*sp*sr + cy*cr)*wy + (sy*sp*cr - cy*sr)*wz
00204 #    Zwz = -sp*wx + cp*sr*wy + cp*cr*wz
00205 #   As the covariance is in the W the function is linear
00206     
00207     rpy = PyKDL.Rotation.RPY(r, p, y)
00208     rpy_w_cov = array([rpy[0,0], rpy[0,1], rpy[0,2],
00209                        rpy[1,0], rpy[1,1], rpy[1,2],
00210                        rpy[2,0], rpy[2,1], rpy[2,2]]).reshape(3,3)
00211 
00212     return dot(rpy_w_cov, dot(w_cov, rpy_w_cov.T)) 
00213            
00214 
00215 # TODO: To be check!
00216 def orientationCov(orientation_cov, 
00217                    measured_orientation, 
00218                    rot_sensor_from_vehicle):
00219                        
00220     angle = euler_from_quaternion([measured_orientation.x, 
00221                                    measured_orientation.y, 
00222                                    measured_orientation.z, 
00223                                    measured_orientation.w])
00224 
00225     r = angle[0]
00226     p = angle[1]
00227     y = angle[2]
00228     cr = math.cos(r)
00229     sr = math.sin(r)
00230     cp = math.cos(p)
00231     sp = math.sin(p)
00232     cy = math.cos(y)
00233     sy = math.sin(y)
00234     
00235     rot_angle = euler_from_quaternion([rot_sensor_from_vehicle[0],
00236                                        rot_sensor_from_vehicle[1],
00237                                        rot_sensor_from_vehicle[2],
00238                                        rot_sensor_from_vehicle[3]])
00239     rot_angle = rot_sensor_from_vehicle
00240     rR = rot_angle[0]
00241     pR = rot_angle[1]
00242     yR = rot_angle[2]
00243     crR = math.cos(rR)
00244     srR = math.sin(rR)
00245     cpR = math.cos(pR)
00246     spR = math.sin(pR)
00247     cyR = math.cos(yR)
00248     syR = math.sin(yR)
00249     
00250 #    |cy*cp,  cy*sp*sr - sy*cr,   cy*sp*cr + sy*sr|
00251 #    |sy*cp,  sy*sp*sr + cy*cr,   sy*sp*cr - cy*sr| *
00252 #    |-sp,    cp*sr,              cp*cr           |
00253 #    
00254 #    |cyR*cpR,               syR*cpR,                -spR   |
00255 #    |cyR*spR*srR - syR*crR, syR*spR*srR + cyR*crR,  cpR*srR| ==>
00256 #    |cyR*spR*crR + syR*srR, cpR*srR,                cpR*crR|
00257 #    
00258     a11 = cyR*cpR
00259     a12 = syR*cpR
00260     a13 = -spR
00261     a21 = cyR*spR*srR - syR*crR
00262     a22 = syR*spR*srR + cyR*crR
00263     a23 = cpR*srR
00264     a31 = cyR*spR*crR + syR*srR
00265     a32 = cpR*srR
00266     a33 = cpR*crR
00267 #    
00268 #    R32 = -sp*(a12) + (cp*sr)*(a22) + (cp*cr)*(a32)
00269 #    R33 = -sp*(a13) + (cp*sr)*(a23) + (cp*cr)*(a33)
00270 #    R21 = (sy*cp)*(a11) + (sy*sp*sr + cy*cr)*(a21) + (sy*sp*cr - cy*sr)*(a31)
00271 #    R11 = (cy*cp)*(a11) + (cy*sp*sr - sy*cr)*(a21) + (cy*sp*cr + sy*sr)*(a31)
00272 #    R31 = -sp*(a11) + (cp*sr)*(a21) + (cp*cr)*(a31)
00273 #    
00274 #    fr = atan2(R32, R33)
00275 #    fr = atan((-sp*(a12) + (cp*sr)*(a22) + (cp*cr)*(a32))/(-sp*(a13) + (cp*sr)*(a23) + (cp*cr)*(a33)))
00276     fr_dr = ((a22*cp*cr - a32*cp*sr)/(a33*cp*cr - a13*sp + a23*cp*sr) - ((a23*cp*cr - a33*cp*sr)*(a32*cp*cr - a12*sp + a22*cp*sr))/(a33*cp*cr - a13*sp + a23*cp*sr)**2)/((a32*cp*cr - a12*sp + a22*cp*sr)**2/(a33*cp*cr - a13*sp + a23*cp*sr)**2 + 1)
00277     fr_dp = -((a12*cp + a32*cr*sp + a22*sp*sr)/(a33*cp*cr - a13*sp + a23*cp*sr) - ((a32*cp*cr - a12*sp + a22*cp*sr)*(a13*cp + a33*cr*sp + a23*sp*sr))/(a33*cp*cr - a13*sp + a23*cp*sr)**2)/((a32*cp*cr - a12*sp + a22*cp*sr)**2/(a33*cp*cr - a13*sp + a23*cp*sr)**2 + 1)
00278     fr_dy = 0
00279     
00280 #    fy = atan2(R21, R11)
00281 #    fy = atan(((sy*cp)*(a11) + (sy*sp*sr + cy*cr)*(a21) + (sy*sp*cr - cy*sr)*(a31))/((cy*cp)*(a11) + (cy*sp*sr - sy*cr)*(a21) + (cy*sp*cr + sy*sr)*(a31)))
00282     YAW = math.atan2((sy*cp)*(a11) + (sy*sp*sr + cy*cr)*(a21) + (sy*sp*cr - cy*sr)*(a31), (cy*cp)*(a11) + (cy*sp*sr - sy*cr)*(a21) + (cy*sp*cr + sy*sr)*(a31))    
00283     cYAW = math.cos(YAW)
00284     sYAW = math.sin(YAW)    
00285     fy_dr = -((a21*(cy*sr - cr*sp*sy) + a31*(cr*cy + sp*sr*sy))/(a31*(sr*sy + cr*cy*sp) - a21*(cr*sy - cy*sp*sr) + a11*cp*cy) + ((a21*(sr*sy + cr*cy*sp) + a31*(cr*sy - cy*sp*sr))*(a21*(cr*cy + sp*sr*sy) - a31*(cy*sr - cr*sp*sy) + a11*cp*sy))/(a31*(sr*sy + cr*cy*sp) - a21*(cr*sy - cy*sp*sr) + a11*cp*cy)**2)/((a21*(cr*cy + sp*sr*sy) - a31*(cy*sr - cr*sp*sy) + a11*cp*sy)**2/(a31*(sr*sy + cr*cy*sp) - a21*(cr*sy - cy*sp*sr) + a11*cp*cy)**2 + 1)
00286     fy_dp = ((a31*cp*cr*sy - a11*sp*sy + a21*cp*sr*sy)/(a31*(sr*sy + cr*cy*sp) - a21*(cr*sy - cy*sp*sr) + a11*cp*cy) - ((a21*(cr*cy + sp*sr*sy) - a31*(cy*sr - cr*sp*sy) + a11*cp*sy)*(a31*cp*cr*cy - a11*cy*sp + a21*cp*cy*sr))/(a31*(sr*sy + cr*cy*sp) - a21*(cr*sy - cy*sp*sr) + a11*cp*cy)**2)/((a21*(cr*cy + sp*sr*sy) - a31*(cy*sr - cr*sp*sy) + a11*cp*sy)**2/(a31*(sr*sy + cr*cy*sp) - a21*(cr*sy - cy*sp*sr) + a11*cp*cy)**2 + 1)
00287     fy_dy = 1
00288 
00289 #    fp = atan2(-R31, (cos(y)*R11 + sin(y)*R21))
00290 #    fp = atan(-(-sp*(a11) + (cp*sr)*(a21) + (cp*cr)*(a31))/(cos(YAW)*((cy*cp)*(a11) + (cy*sp*sr - sy*cr)*(a21) + (cy*sp*cr + sy*sr)*(a31)) + sin(YAW)*((sy*cp)*(a11) + (sy*sp*sr + cy*cr)*(a21) + (sy*sp*cr - cy*sr)*(a31)))
00291 #    fp = atan(-(-sin(p)*(a11) + (cos(p)*sin(r))*(a21) + (cos(p)*cos(r))*(a31))/(cos(YAW)*((cos(y)*cos(p))*(a11) + (cos(y)*sin(p)*sin(r) - sin(y)*cos(r))*(a21) + (cos(y)*sin(p)*cos(r) + sin(y)*sin(r))*(a31)) + sin(YAW)*((sin(y)*cos(p))*(a11) + (sin(y)*sin(p)*sin(r) + cos(y)*cos(r))*(a21) + (sin(y)*sin(p)*cos(r) - cos(y)*sin(r))*(a31))))
00292     fp_dr = -((a21*cp*cr - a31*cp*sr)/(cYAW*(a31*(sr*sy + cr*cy*sp) - a21*(cr*sy - cy*sp*sr) + a11*cp*cy) + sYAW*(a21*(cr*cy + sp*sr*sy) - a31*(cy*sr - cr*sp*sy) + a11*cp*sy)) - ((cYAW*(a21*(sr*sy + cr*cy*sp) + a31*(cr*sy - cy*sp*sr)) - sYAW*(a21*(cy*sr - cr*sp*sy) + a31*(cr*cy + sp*sr*sy)))*(a31*cp*cr - a11*sp + a21*cp*sr))/(cYAW*(a31*(sr*sy + cr*cy*sp) - a21*(cr*sy - cy*sp*sr) + a11*cp*cy) + sYAW*(a21*(cr*cy + sp*sr*sy) - a31*(cy*sr - cr*sp*sy) + a11*cp*sy))**2)/((a31*cp*cr - a11*sp + a21*cp*sr)**2/(cYAW*(a31*(sr*sy + cr*cy*sp) - a21*(cr*sy - cy*sp*sr) + a11*cp*cy) + sYAW*(a21*(cr*cy + sp*sr*sy) - a31*(cy*sr - cr*sp*sy) + a11*cp*sy))**2 + 1)
00293     fp_dp = ((a11*cp + a31*cr*sp + a21*sp*sr)/(cYAW*(a31*(sr*sy + cr*cy*sp) - a21*(cr*sy - cy*sp*sr) + a11*cp*cy) + sYAW*(a21*(cr*cy + sp*sr*sy) - a31*(cy*sr - cr*sp*sy) + a11*cp*sy)) + ((cYAW*(a31*cp*cr*cy - a11*cy*sp + a21*cp*cy*sr) + sYAW*(a31*cp*cr*sy - a11*sp*sy + a21*cp*sr*sy))*(a31*cp*cr - a11*sp + a21*cp*sr))/(cYAW*(a31*(sr*sy + cr*cy*sp) - a21*(cr*sy - cy*sp*sr) + a11*cp*cy) + sYAW*(a21*(cr*cy + sp*sr*sy) - a31*(cy*sr - cr*sp*sy) + a11*cp*sy))**2)/((a31*cp*cr - a11*sp + a21*cp*sr)**2/(cYAW*(a31*(sr*sy + cr*cy*sp) - a21*(cr*sy - cy*sp*sr) + a11*cp*cy) + sYAW*(a21*(cr*cy + sp*sr*sy) - a31*(cy*sr - cr*sp*sy) + a11*cp*sy))**2 + 1)
00294     fp_dy = -((cYAW*(a21*(cr*cy + sp*sr*sy) - a31*(cy*sr - cr*sp*sy) + a11*cp*sy) - sYAW*(a31*(sr*sy + cr*cy*sp) - a21*(cr*sy - cy*sp*sr) + a11*cp*cy))*(a31*cp*cr - a11*sp + a21*cp*sr))/((cYAW*(a31*(sr*sy + cr*cy*sp) - a21*(cr*sy - cy*sp*sr) + a11*cp*cy) + sYAW*(a21*(cr*cy + sp*sr*sy) - a31*(cy*sr - cr*sp*sy) + a11*cp*sy))**2*((a31*cp*cr - a11*sp + a21*cp*sr)**2/(cYAW*(a31*(sr*sy + cr*cy*sp) - a21*(cr*sy - cy*sp*sr) + a11*cp*cy) + sYAW*(a21*(cr*cy + sp*sr*sy) - a31*(cy*sr - cr*sp*sy) + a11*cp*sy))**2 + 1))
00295    
00296     rpy_orientation_cov = array([fr_dr, fr_dp, fr_dy,
00297                        fp_dr, fp_dp, fp_dy,
00298                        fy_dr, fy_dp, fy_dy]).reshape(3,3)
00299 
00300     return dot(rpy_orientation_cov, dot(orientation_cov, rpy_orientation_cov.T))