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))
