def p8(): print('P8') Tsa = np.array([[0, -1, 0, 0], [0, 0, -1, 0], [1, 0, 0, 1], [0, 0, 0, 1]]) Rsa = Tsa[:3, :3] so3mat = mr.MatrixLog3(Rsa) expc3 = mr.so3ToVec(so3mat) omghat, theta = mr.AxisAng3(expc3) print(theta)
def transformToPose(T): """Converts a homogeneous transformation matrix to a 3D pose (Wx, Wy, Wz, x, y, z) 6-vector :param: Homogeneous Transformation matrix corresponding to given pose :return pose: 2D pose [Wx, Wy, Wz, x, y, z] Wx, Wy, Wz : Rotation angles x, y, z : Position """ R, position = mr.TransToRp(T) R_so3 = mr.MatrixLog3(R) rotation = mr.so3ToVec(R_so3) return np.concatenate((rotation, position))
def baseConfiguration(Tbase): """Computes the configuration of youBot base given a base transformation in space frame :param configuration: Transform of robot base in space frame :return: A 3-vector representing the current configuration of the robot base. 3 variables for the chassis configuration (theta, x, y), """ R, position = mr.TransToRp(Tbase) R_so3 = mr.MatrixLog3(R) rotation = mr.so3ToVec(R_so3) theta = rotation[2] x, y = position[0:2] return theta, x, y
def NextState(curConfig,controls,del_t,limits): nextState = [] curJointConfig = np.array(curConfig[3:8]) q_cur= np.array(curConfig[0:3]) curWheelConfig = np.array(curConfig[8:]) jointSpeeds = np.array(controls[0:5]) u = np.array(controls[5:]) ## checking limits for i in range(len(controls)): if controls[i] > limits: controls[i] = limits elif controls[i] < -limits: controls[i] = -limits ## Euler step for joints and wheels nextJointConfig = curJointConfig + jointSpeeds*del_t nextWheelConfig = curWheelConfig + u*del_t ## YouBot Properties: l = 0.47/2 w = 0.3/2 r = 0.0475 phi_k = q_cur[0] ## Odometry calculations for chassis Euler step F = (r/4)*np.array([[-1/(l+w), 1/(l+w), 1/(l+w),-1/(l+w)],[1, 1, 1, 1],[-1, 1, -1, 1]]) del_theta = u*del_t Vb = np.dot(F,del_theta) Vb6 = np.array([0,0,Vb[0],Vb[1],Vb[2],0]) [R,p] = mr.TransToRp(mr.MatrixExp6(mr.VecTose3(Vb6))) omg = mr.so3ToVec(mr.MatrixLog3(R)) del_x = p[0] del_y = p[1] del_w = omg[2] if del_w == 0: del_qb = np.array([del_w,del_x,del_y]) else: del_qb = np.array([del_w, \ (del_x*m.sin(del_w) + del_y*(m.cos(del_w)-1))/del_w, \ (del_y*m.sin(del_w) + del_y*(1-m.cos(del_w)))/del_w]) del_q = np.dot(np.array([[1,0,0],[0,m.cos(phi_k),-m.sin(phi_k)],[0,m.sin(phi_k),m.cos(phi_k)]]), \ del_qb) q_next = q_cur + del_q nextConfig = [list(q_next),list(nextJointConfig),list(nextWheelConfig)] nextState = list(itertools.chain(*nextConfig)) return nextState
pb = np.append(pb, 1) Tsb = mr.TransInv(Tbs) pb = np.dot(Tsb, pb) pb = np.delete(pb, 3) print(pb) # [1,5,-2] print("Ex7, va:") Tsa = mr.Adjoint(Tas) va = np.dot(Tsa, vs) print(va) # [1,-3,-2,-3,-1,5] print("Ex8, theta:") MatLogTsa = mr.MatrixLog6(Tsa) vec = mr.so3ToVec(MatLogTsa) theta = mr.AxisAng6(vec)[-1] print(theta) # 2.094395102393196 print("Ex9, Matrix exponential:") se3 = mr.VecTose3(stheta) MatExp = mr.MatrixExp6(se3) print(MatExp) # [[-0.61727288,-0.70368982,0.35184491,1.05553472],[0.70368982,-0.2938183,0.64690915,1.94072745],[-0.35184491,0.64690915,0.67654542,-0.97036373],[0,0,0,1]] print("Ex10, fb:") Tbs = mr.Adjoint(Tbs) Tsb = np.transpose(Tbs) fb = np.dot(Tsb, fb) print(fb)
H0 = np.array([[-5, 1, -1], [5, 1, 1], [5, 1, -1], [-5, 1, 1]]) u = np.array([-1.18, 0.68, 0.02, -0.52]) Hinverse = np.linalg.pinv(H0) V = np.dot(Hinverse, u) # print V #Q5 V6 = np.array([0, 0, V[0], V[1], V[2], 0]) se3mat = mr.VecTose3(V6) T = mr.MatrixExp6(se3mat) R, p = mr.TransToRp(T) so3mat = mr.MatrixLog3(R) omega = mr.so3ToVec(so3mat) # print p # print omega #Q06 F = np.array([[0, 0], [0, 0], [-0.25, 0.25], [0.25, 0.25], [0, 0], [0, 0]]) # Finding Tbe omega = np.array([0, 0, math.pi / 2]) p = np.array([2, 3, 0]) so3mat = mr.VecToso3(omega) R = mr.MatrixExp3(so3mat) Tbe = mr.RpToTrans(R, p) Teb = np.linalg.inv(Tbe) Ad_Teb = mr.Adjoint(Teb) Jbase = np.dot(Ad_Teb, F) print Tbe
# Getting angles of rotation """ * mr.MatrixLog3 takes R from SO3 to so3 (skew symmetric) in exponential coordinates * mr.so3ToVec converts 3x3 skew symmetric matrix to 3-vector * mr.AxisAng3 returns axis and rotation angle from 3-vector in exponential coordinates """ so3mat_s1 = mr.MatrixLog3(rs1) so3mat_12 = mr.MatrixLog3(r12) so3mat_23 = mr.MatrixLog3(r23) so3mat_34 = mr.MatrixLog3(r34) so3mat_45 = mr.MatrixLog3(r45) so3mat_56 = mr.MatrixLog3(r56) #so3mat_6b = mr.MatrixLog3(r6b) so3ToVec_s1 = mr.so3ToVec(so3mat_s1) so3ToVec_12 = mr.so3ToVec(so3mat_12) so3ToVec_23 = mr.so3ToVec(so3mat_23) so3ToVec_34 = mr.so3ToVec(so3mat_34) so3ToVec_45 = mr.so3ToVec(so3mat_45) so3ToVec_56 = mr.so3ToVec(so3mat_56) #so3ToVec_6b = mr.so3ToVec(so3mat_6b) theta_s1 = mr.AxisAng3(so3ToVec_s1)[1] theta_12 = mr.AxisAng3(so3ToVec_12)[1] theta_23 = mr.AxisAng3(so3ToVec_23)[1] theta_34 = mr.AxisAng3(so3ToVec_34)[1] theta_45 = mr.AxisAng3(so3ToVec_45)[1] theta_56 = mr.AxisAng3(so3ToVec_56)[1] #theta_6b = mr.AxisAng3(so3ToVec_6b)[1]
def registerLocalCloud(self, target, source): ''' function: get local transformation matrix input: two clouds output: transformation from target cloud to source cloud ''' # print("target: ",id(target)) source_temp = copy.deepcopy(source) target_temp = copy.deepcopy(target) # print("target_temp: ",id(target_temp)) # source_temp = source # target_temp = target source_temp = voxel_down_sample(source_temp, 0.004) target_temp = voxel_down_sample(target_temp, 0.004) estimate_normals(source_temp, search_param=KDTreeSearchParamHybrid(radius=0.1, max_nn=30)) estimate_normals(target_temp, search_param=KDTreeSearchParamHybrid(radius=0.1, max_nn=30)) current_transformation = np.identity(4) # use Point-to-plane ICP registeration to obtain initial pose guess result_icp_p2l = registration_icp( source_temp, target_temp, 0.02, current_transformation, TransformationEstimationPointToPlane()) # 0.1 is searching distance #'''TEST # current_transformation = result_icp.transformation # result_icp = registration_icp(source, target, 0.1, # current_transformation, TransformationEstimationPointToPlane()) #''' print("----------------") print("initial guess from Point-to-plane ICP registeration") print(result_icp_p2l) print(result_icp_p2l.transformation) p2l_init_trans_guess = result_icp_p2l.transformation print("----------------") # print("Colored point cloud registration") ################ #### testing registration ################ # voxel_radius = [ 0.004,0.001 ]; # max_iter = [ 50, 30 ]; # current_transformation = p2l_init_trans_guess # for scale in range(2): # iter = max_iter[scale] # radius = voxel_radius[scale] # source_down = voxel_down_sample(source_temp, radius) # target_down = voxel_down_sample(target_temp, radius) # estimate_normals(source_down, KDTreeSearchParamHybrid( # radius = radius * 2, max_nn = 30)) # estimate_normals(target_down, KDTreeSearchParamHybrid( # radius = radius * 2, max_nn = 30)) # result_icp = registration_colored_icp(source_down, target_down, # radius, current_transformation, # ICPConvergenceCriteria(relative_fitness = 1e-6, # relative_rmse = 1e-6, max_iteration = iter)) # current_transformation = result_icp.transformation ################# #### original one ################# # result_icp = registration_colored_icp(source_temp, target_temp, # 0.001, p2l_init_trans_guess, # ICPConvergenceCriteria(relative_fitness = 1e-6, # relative_rmse = 1e-6, max_iteration = 50)) ################# ################# ################# ################# #### double nomral ICP ################# result_icp = registration_icp(source_temp, target_temp, 0.01, p2l_init_trans_guess, TransformationEstimationPointToPlane()) # result_icp = registration_icp(source_temp, target_temp, 0.002, # p2l_init_trans_guess, TransformationEstimationPointToPlane(),ICPConvergenceCriteria(relative_fitness = 1e-6,relative_rmse = 1e-6, max_iteration = 50)) # ################# ################# ################# # result_icp = registration_colored_icp(source, target, # 0.02, p2l_init_trans_guess) print(result_icp) print(result_icp.transformation) # print(result_icp.fitness) # draw_registration_result_original_color( # source, target, result_icp.transformation) #### write intermediate result for test, #### but found it will make registration result worse... #### ---->>>> cause source.transform will change the source!!!!!! # source.transform(result_icp.transformation) # temp_cloud = target + source # write_point_cloud("/home/donyan/Desktop/kinect/src/scanner/src/temp/pointCloudInRviz/data/result/{}-{}.pcd".format(self.cloud_index,self.cloud_index-1), temp_cloud , write_ascii = False) # print("*****") # print(source) # print(target) # print(result_icp.correspondence_set) ###################################### ##### original kick-out rule ###################################### # self.goodResultFlag = True # if (result_icp.fitness < 0.9 * result_icp_p2l.fitness): # return p2l_init_trans_guess # when result of colored ICP is bad, use p2l ICP # else: # return result_icp.transformation ###################################### ##### kick-out rule ###################################### #### New rule: #### 1/ rotation is out of plane (5 degree, or 0.087266 in radians); #### 2/ too big rotation; #### 3/ too big translation; # first calculate what is the rotation direction and rotation angle tf = result_icp.transformation R = tf[:3, :3] # rotation matrix so3mat = MatrixLog3(R) omg = so3ToVec(so3mat) R_dir, theta = AxisAng3(omg) # rotation direction # rotation angle (in radians) theta_degree = theta / np.pi * 180 # in degree angle_with_pl_norm = self.cal_angle(self.rotation_dir, R_dir) trans_tol = 0.5 # transformation tolerance rotation_tol = 30 # 30 degrees # angle_with_pl_norm_tol = 0.087266 # in radians (= 5 degrees) # angle_with_pl_norm_tol = 0.174533 # in radians (= 10 degrees) angle_with_pl_norm_tol = 0.35 # in radians (= 20 degrees) if ( tf[0,3] > trans_tol or tf[0,3] < -trans_tol or \ tf[1,3] > trans_tol or tf[1,3] < -trans_tol or \ tf[2,3] > trans_tol or tf[2,3] < -trans_tol ): self.goodResultFlag = False print(">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>") # print("here in 1 ") rospy.logwarn( 'Something wrong with 1/ translation : (, turn back a little bit...' ) rospy.logwarn('>> the translation is [{},{},{}]'.format( tf[0, 3], tf[1, 3], tf[2, 3])) return np.identity(4) elif ( theta_degree > rotation_tol or \ theta_degree < - rotation_tol): self.goodResultFlag = False print(">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>") # print("here in 2 ") rospy.logwarn( 'Something wrong with 2/ rotation angle : (, turn back a little bit...' ) rospy.logwarn('>> the rotation angle is {} (in degrees)'.format( theta_degree)) return np.identity(4) elif (angle_with_pl_norm > angle_with_pl_norm_tol): self.goodResultFlag = False print(">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>") print("here in 3 ") print(" angle with pl norm") print(angle_with_pl_norm) rospy.logwarn( 'Something wrong with 3/ rotation axis : (, turn back a little bit...' ) rospy.logwarn( '>> the rotation axis is {} (in radians) with plane normal'. format(angle_with_pl_norm)) return np.identity(4) else: self.goodResultFlag = True print(">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>") # print("here in 4 ") return result_icp.transformation
# [[0,-1,0],[1,0,0],[0,0,1]] print("Ex5, pb:") Rsb = mr.RotInv(Rbs) pb = Rsb * pb print(pb) # [1,3,-2] print("Ex7, wa:") wa = Rsa * ws print(wa) # [1,3,2] print("Ex8, theta:") MatLogRsa = mr.MatrixLog3(Rsa) vec = mr.so3ToVec(MatLogRsa) theta = mr.AxisAng3(vec)[-1] print(theta) # 2.094395102393196 print("Ex9, Matrix exponential:") skew = mr.VecToso3(wtheta) MatExp = mr.MatrixExp3(skew) print(MatExp) # [[-0.2938183,0.64690915,0.70368982],[0.64690915,0.67654542,-0.35184491],[-0.70368982,0.35184491,-0.61727288]] print("Ex10, skew.symmetric matrix:") skewMat = mr.VecToso3(wskew) print(skewMat) # [[0,-0.5,2],[0.5,0,-1],[-2,1,0]]
from modern_robotics import MatrixExp3 from modern_robotics import so3ToVec from modern_robotics import Adjoint import numpy as np #question 5 Vb = np.array([[0, -.33, -.25], [.33, 0, -.12], [.25, .12, 0]]) print(so3ToVec(MatrixExp3(Vb))) #question 6 Teb = [[0, -1, 0, 2], [1, 0, 0, 3], [0, 0, 1, 0], [0, 0, 0, 1]] r = .5 d = 1 F = [[-r / (2 * d), r / (2 * d)], [r / 2, r / 2]] print(Adjoint(Teb))