def ctchecker(robot, armname, jnts, obstaclecmlist, objcm, objrelmat):
"""
a ctchecker must follow the same interface!
:param robot:
:param jnts:
:param obstaclecmlist:
:param objcm:
:param objrelmat:
:return:
"""
initjnts = robot.initrgtjnts
if armname == 'lft':
initjnts = robot.initlftjnts
robot.movearmfk(jnts, armname)
eepos, eerot = robot.getee()
robot.movearmfk(initjnts, armname)
axz = eerot[:,2]
if rm.degree_between(axz, np.array([0,0,-1])) < 10:
print("The angle between eez and -worldz is smaller than 10!")
return True
else:
return False
v_s1 = np.array([sppoint1.x, sppoint1.y])
v_g0g1 = v_g1-v_g0
v_g1g0 = v_g0-v_g1
## constraindirect_g0s0
if v_s0[1] >= v_g0[1]:
assert("grppoint0 should be higher than sppoint0")
constraindirect_g0s0 = np.array([1.0,0.0])
if v_g0[0] != v_s0[0]:
v_g0s0 = v_s0-v_g0
v_g0s0_normalized = v_g0s0/np.linalg.norm(v_g0s0)
constraindirect_g0s0 = v_g0+np.dot(-v_g0, v_g0s0_normalized)*v_g0s0_normalized
constraindirect_g0s0 = constraindirect_g0s0/np.linalg.norm(constraindirect_g0s0)
# pg.plotArrow(base.render, np.array([0,0,0]), (np.array([0,0,0])+np.array([constraindirect_g0s0[0], constraindirect_g0s0[1], 0]))*1000)
if rm.degree_between(constraindirect_g0s0, v_g0g1) > 90:
constraindirect_g0s0 = -constraindirect_g0s0
## constraindirect_g0s1
if v_s1[1] >= v_g0[1]:
assert("grppoint0 should be higher than sppoint1")
constraindirect_g0s1 = np.array([1.0,0.0])
if v_g0[0] != v_s1[0]:
v_g0s1 = v_s1-v_g0
v_g0s1_normalized = v_g0s1/np.linalg.norm(v_g0s1)
constraindirect_g0s1 = v_g0+np.dot(-v_g0, v_g0s1_normalized)*v_g0s1_normalized
constraindirect_g0s1 = constraindirect_g0s1/np.linalg.norm(constraindirect_g0s1)
if rm.degree_between(constraindirect_g0s1, v_g0g1) > 90:
constraindirect_g0s1 = -constraindirect_g0s1
## constraindirect_g0g1
constraindirect_g0g1 = np.array([0.0,1.0])
if v_g0[1] != v_g1[1]:
def computePPFwithAlpha(self, points, normals, ddist=5.0, dangle=30.0):
"""
compute the point pair feature f1, f2, f3, f4,
and the alpha_m
dangle in degree
:return: a dictionary
author: weiwei
date: 20170714
"""
gmd = {}
ntemppoint = points.shape[0]
for i in range(ntemppoint):
print i, ntemppoint
for j in range(ntemppoint):
# for i in range(0,1):
# for j in range(3,4):
m_0 = np.asarray(points[i])
m_1 = np.asarray(points[j])
v_m0m1 = m_0 - m_1
v_m1m0 = m_1 - m_0
n_m0 = normals[i]
n_m1 = normals[j]
# f1, namely ||d||2
f1 = np.linalg.norm(m_0 - m_1)
# f2, namely angle between n_m0 and v_m1m0
f2 = rm.degree_between(n_m0, v_m1m0)
# f3, namely angle between n_m1 and v_m0m1
f3 = rm.degree_between(n_m1, v_m0m1)
# f4, namely angle between n_m0 and n_m1
f4 = rm.degree_between(n_m0, n_m1)
# discretize the values
try:
f1d = int(math.floor(f1 / ddist) * ddist + ddist)
f2d = int(math.floor(f2 / dangle) * dangle + dangle)
f3d = int(math.floor(f3 / dangle) * dangle + dangle)
f4d = int(math.floor(f4 / dangle) * dangle + dangle)
except:
continue
key = (f1d, f2d, f3d, f4d)
# angle between n_m0 and x+
xplus = np.asarray([1, 0, 0])
yplus = np.asarray([0, 1, 0])
nm0xangle = math.degrees(rm.radian_between(n_m0, xplus))
rotax = np.cross(xplus, n_m0)
if np.isnan(rotax).any() or not rotax.any():
continue
rotmat = rm.rodrigues(rotax, nm0xangle)
v_m1m0onxplus = np.dot(v_m1m0, rotmat)
v_m1m0onxplusyzproj = np.asarray(
[0, v_m1m0onxplus[1], v_m1m0onxplus[2]])
alpha_m0 = rm.radian_between(v_m1m0onxplusyzproj, yplus)
if v_m1m0onxplus[2] < 0:
alpha_m0 = 2 * math.pi - alpha_m0
# # debug
# # before transform
# pg.plotArrow(base.render, spos = m_0, epos = m_1, rgba=Vec4(0,1,0,1))
# pg.plotArrow(base.render, spos = m_0, epos = m_0+n_m0, rgba = Vec4(1,0,0,1))
# # after transform
# # print v_m1m0onxplus
# # print v_m1m0onxplusyzproj
# pg.plotArrow(base.render, spos = m_0, epos = v_m1m0onxplus+m_0, rgba=Vec4(0,.7,.7,1))
# pg.plotArrow(base.render, spos = m_0, epos = v_m1m0onxplusyzproj+m_0, rgba=Vec4(.70,.7,.7,1))
# pg.plotArrow(base.render, spos = m_0, epos = m_0+xplus, rgba = Vec4(.7,0,.7,1))
# # alpha_m0
# print np.degrees(alpha_m0)
# # plot aixs
# zplus = np.asarray([0,0,1])
# pg.plotArrow(base.render, spos = m_0, epos = m_0+xplus*10, rgba = Vec4(.3,0,0,.3))
# pg.plotArrow(base.render, spos = m_0, epos = m_0+yplus*10, rgba = Vec4(0,.3,0,.3))
# pg.plotArrow(base.render, spos = m_0, epos = m_0+zplus*10, rgba = Vec4(0,0,.3,.3))
if key in gmd.keys():
gmd[key].append([i, j, alpha_m0])
else:
gmd[key] = [[i, j, alpha_m0]]
for key in gmd.keys():
print key
return gmd
def match(self, perceivedpnts, perceivednormals, ddist=5.0, dangle=30.0):
"""
do a match
:return: rotmats of top matches
author: weiwei
date: 20170802
"""
# save txt
pverts = np.array([tuple(x) for x in perceivedpnts],
dtype=[('x', 'f4'), ('y', 'f4'), ('z', 'f4')])
el = PlyElement.describe(pverts, 'vertex')
PlyData([el], text=True).write('perceivedpnts.ply')
# save txt
vandnarray = []
for i in range(len(self.temppnts)):
v = self.temppnts[i]
n = self.tempnormals[i]
vandn = (v[0], v[1], v[2], n[0], n[1], n[2])
vandnarray.append(vandn)
pverts = np.array(vandnarray, dtype=[('x', 'f4'),('y', 'f4'),('z', 'f4'),\
('nx','f4'),('ny','f4'),('nz','f4')])
el = PlyElement.describe(pverts, 'vertex')
PlyData([el], text=True).write('ttube.ply')
accspace = {}
# get the preceived global model descriptor
nperceivedpnts = perceivedpnts.shape[0]
i = np.argmax(perceivedpnts, axis=0)[2]
for j in range(0, nperceivedpnts):
print j, nperceivedpnts
m_0 = np.asarray(perceivedpnts[i])
m_1 = np.asarray(perceivedpnts[j])
v_m0m1 = m_0 - m_1
v_m1m0 = m_1 - m_0
n_m0 = perceivednormals[i]
n_m1 = perceivednormals[j]
# f1, namely ||d||2
f1 = np.linalg.norm(m_0 - m_1)
# f2, namely angle between n_m0 and v_m1m0
f2 = rm.degree_between(n_m0, v_m1m0)
# f3, namely angle between n_m1 and v_m0m1
f3 = rm.degree_between(n_m1, v_m0m1)
# f4, namely angle between n_m0 and n_m1
f4 = rm.degree_between(n_m0, n_m1)
# discretize the values
try:
f1d = int(math.floor(f1 / ddist) * ddist + ddist)
f2d = int(math.floor(f2 / dangle) * dangle + dangle)
f3d = int(math.floor(f3 / dangle) * dangle + dangle)
f4d = int(math.floor(f4 / dangle) * dangle + dangle)
except:
continue
key = (f1d, f2d, f3d, f4d)
# angle between n_m0 and x+
xplus = np.asarray([1, 0, 0])
yplus = np.asarray([0, 1, 0])
nm0xangle = math.degrees(rm.radian_between(n_m0, xplus))
rotax = np.cross(xplus, n_m0)
if np.isnan(rotax).any() or not rotax.any():
continue
rotmat = rm.rodrigues(rotax, nm0xangle)
v_m1m0onxplus = np.dot(v_m1m0, rotmat)
v_m1m0onxplusyzproj = np.asarray(
[0, v_m1m0onxplus[1], v_m1m0onxplus[2]])
alpha_m0 = rm.radian_between(v_m1m0onxplusyzproj, yplus)
if v_m1m0onxplus[2] < 0:
alpha_m0 = 2 * math.pi - alpha_m0
if key in self.gmd.keys():
malist = self.gmd[key]
print len(malist)
for maslot in malist:
alpha = math.degrees(alpha_m0 - maslot[2])
try:
alphadiscrete = int(
math.floor(alpha / dangle) * dangle + dangle)
except:
continue
acckey = (maslot[0], alphadiscrete)
if acckey in accspace.keys():
accspace[acckey] += 1
else:
accspace[acckey] = 1
if len(accspace.keys()) is 0:
return (None, None)
# find top matches and rot matrices
maxn = sorted(accspace.iteritems(),
key=operator.itemgetter(1),
reverse=True)[:5]
rotmat4list = []
silist = []
milist = []
for maxnele in maxn:
mi, alpha = maxnele[0]
# step1 move to temppnts[mi]
displacement0 = -self.temppnts[mi]
rotmat4_0 = Mat4.translateMat(displacement0[0], displacement0[1],
displacement0[2])
# step2 rotate to goal
normalangle = math.degrees(
rm.radian_between(self.tempnormals[mi], perceivednormals[i]))
normalrotax = np.cross(self.tempnormals[mi], perceivednormals[i])
normalrotmat = rm.rodrigues(normalrotax, normalangle)
anglerotmat = rm.rodrigues(perceivednormals[i], -alpha)
rotmat = np.dot(anglerotmat, normalrotmat)
rotmat4_1 = pg.npToMat4(rotmat)
# step3 move to perceivedpnts[i]
displacement1 = perceivedpnts[i]
rotmat4_2 = Mat4.translateMat(displacement1[0], displacement1[1],
displacement1[2])
rotmat4 = rotmat4_0 * rotmat4_1 * rotmat4_2
rotmat4list.append(rotmat4)
silist.append(i)
milist.append(mi)
return rotmat4list, silist, milist
v_g0g1 = v_g1 - v_g0
v_g1g0 = v_g0 - v_g1
## constraindirect_g0s0
if v_s0[1] >= v_g0[1]:
assert ("grppoint0 should be higher than sppoint0")
constraindirect_g0s0 = np.array([1.0, 0.0])
if v_g0[0] != v_s0[0]:
v_g0s0 = v_s0 - v_g0
v_g0s0_normalized = v_g0s0 / np.linalg.norm(v_g0s0)
constraindirect_g0s0 = v_g0 + np.dot(
-v_g0, v_g0s0_normalized) * v_g0s0_normalized
constraindirect_g0s0 = constraindirect_g0s0 / np.linalg.norm(
constraindirect_g0s0)
# pg.plotArrow(base.render, np.array([0,0,0]), (np.array([0,0,0])+np.array([constraindirect_g0s0[0], constraindirect_g0s0[1], 0]))*1000)
if rm.degree_between(constraindirect_g0s0, v_g0g1) > 90:
constraindirect_g0s0 = -constraindirect_g0s0
## constraindirect_g0s1
if v_s1[1] >= v_g0[1]:
assert ("grppoint0 should be higher than sppoint1")
constraindirect_g0s1 = np.array([1.0, 0.0])
if v_g0[0] != v_s1[0]:
v_g0s1 = v_s1 - v_g0
v_g0s1_normalized = v_g0s1 / np.linalg.norm(v_g0s1)
constraindirect_g0s1 = v_g0 + np.dot(
-v_g0, v_g0s1_normalized) * v_g0s1_normalized
constraindirect_g0s1 = constraindirect_g0s1 / np.linalg.norm(
constraindirect_g0s1)
if rm.degree_between(constraindirect_g0s1, v_g0g1) > 90:
constraindirect_g0s1 = -constraindirect_g0s1
## constraindirect_g0g1
def computePPFwithAlpha(self, points, normals, ddist = 5.0, dangle = 30.0):
"""
compute the point pair feature f1, f2, f3, f4,
and the alpha_m
dangle in degree
:return: a dictionary
author: weiwei
date: 20170714
"""
gmd = {}
ntemppoint = points.shape[0]
for i in range(ntemppoint):
print i, ntemppoint
for j in range(ntemppoint):
# for i in range(0,1):
# for j in range(3,4):
m_0 = np.asarray(points[i])
m_1 = np.asarray(points[j])
v_m0m1 = m_0-m_1
v_m1m0 = m_1-m_0
n_m0 = normals[i]
n_m1 = normals[j]
# f1, namely ||d||2
f1 = np.linalg.norm(m_0-m_1)
# f2, namely angle between n_m0 and v_m1m0
f2 = rm.degree_between(n_m0, v_m1m0)
# f3, namely angle between n_m1 and v_m0m1
f3 = rm.degree_between(n_m1, v_m0m1)
# f4, namely angle between n_m0 and n_m1
f4 = rm.degree_between(n_m0, n_m1)
# discretize the values
try:
f1d = int(math.floor(f1/ddist)*ddist+ddist)
f2d = int(math.floor(f2/dangle)*dangle+dangle)
f3d = int(math.floor(f3/dangle)*dangle+dangle)
f4d = int(math.floor(f4/dangle)*dangle+dangle)
except:
continue
key = (f1d, f2d, f3d, f4d)
# angle between n_m0 and x+
xplus = np.asarray([1,0,0])
yplus = np.asarray([0,1,0])
nm0xangle = math.degrees(rm.radian_between(n_m0, xplus))
rotax = np.cross(xplus, n_m0)
if np.isnan(rotax).any() or not rotax.any():
continue
rotmat = rm.rodrigues(rotax, nm0xangle)
v_m1m0onxplus = np.dot(v_m1m0, rotmat)
v_m1m0onxplusyzproj = np.asarray([0, v_m1m0onxplus[1], v_m1m0onxplus[2]])
alpha_m0 = rm.radian_between(v_m1m0onxplusyzproj, yplus)
if v_m1m0onxplus[2] < 0:
alpha_m0 = 2*math.pi - alpha_m0
# # debug
# # before transform
# pg.plotArrow(base.render, spos = m_0, epos = m_1, rgba=Vec4(0,1,0,1))
# pg.plotArrow(base.render, spos = m_0, epos = m_0+n_m0, rgba = Vec4(1,0,0,1))
# # after transform
# # print v_m1m0onxplus
# # print v_m1m0onxplusyzproj
# pg.plotArrow(base.render, spos = m_0, epos = v_m1m0onxplus+m_0, rgba=Vec4(0,.7,.7,1))
# pg.plotArrow(base.render, spos = m_0, epos = v_m1m0onxplusyzproj+m_0, rgba=Vec4(.70,.7,.7,1))
# pg.plotArrow(base.render, spos = m_0, epos = m_0+xplus, rgba = Vec4(.7,0,.7,1))
# # alpha_m0
# print np.degrees(alpha_m0)
# # plot aixs
# zplus = np.asarray([0,0,1])
# pg.plotArrow(base.render, spos = m_0, epos = m_0+xplus*10, rgba = Vec4(.3,0,0,.3))
# pg.plotArrow(base.render, spos = m_0, epos = m_0+yplus*10, rgba = Vec4(0,.3,0,.3))
# pg.plotArrow(base.render, spos = m_0, epos = m_0+zplus*10, rgba = Vec4(0,0,.3,.3))
if key in gmd.keys():
gmd[key].append([i, j, alpha_m0])
else:
gmd[key] = [[i, j, alpha_m0]]
for key in gmd.keys():
print key
return gmd
def match(self, perceivedpnts, perceivednormals, ddist = 5.0, dangle = 30.0):
"""
do a match
:return: rotmats of top matches
author: weiwei
date: 20170802
"""
# save txt
pverts = np.array([tuple(x) for x in perceivedpnts], dtype=[('x', 'f4'),('y', 'f4'),('z', 'f4')])
el = PlyElement.describe(pverts, 'vertex')
PlyData([el], text = True).write('perceivedpnts.ply')
# save txt
vandnarray = []
for i in range(len(self.temppnts)):
v = self.temppnts[i]
n = self.tempnormals[i]
vandn = (v[0],v[1],v[2],n[0],n[1],n[2])
vandnarray.append(vandn)
pverts = np.array(vandnarray, dtype=[('x', 'f4'),('y', 'f4'),('z', 'f4'),\
('nx','f4'),('ny','f4'),('nz','f4')])
el = PlyElement.describe(pverts, 'vertex')
PlyData([el], text = True).write('ttube.ply')
accspace = {}
# get the preceived global model descriptor
nperceivedpnts = perceivedpnts.shape[0]
i = np.argmax(perceivedpnts, axis = 0)[2]
for j in range(0,nperceivedpnts):
print j, nperceivedpnts
m_0 = np.asarray(perceivedpnts[i])
m_1 = np.asarray(perceivedpnts[j])
v_m0m1 = m_0-m_1
v_m1m0 = m_1-m_0
n_m0 = perceivednormals[i]
n_m1 = perceivednormals[j]
# f1, namely ||d||2
f1 = np.linalg.norm(m_0-m_1)
# f2, namely angle between n_m0 and v_m1m0
f2 = rm.degree_between(n_m0, v_m1m0)
# f3, namely angle between n_m1 and v_m0m1
f3 = rm.degree_between(n_m1, v_m0m1)
# f4, namely angle between n_m0 and n_m1
f4 = rm.degree_between(n_m0, n_m1)
# discretize the values
try:
f1d = int(math.floor(f1/ddist)*ddist+ddist)
f2d = int(math.floor(f2/dangle)*dangle+dangle)
f3d = int(math.floor(f3/dangle)*dangle+dangle)
f4d = int(math.floor(f4/dangle)*dangle+dangle)
except:
continue
key = (f1d, f2d, f3d, f4d)
# angle between n_m0 and x+
xplus = np.asarray([1,0,0])
yplus = np.asarray([0,1,0])
nm0xangle = math.degrees(rm.radian_between(n_m0, xplus))
rotax = np.cross(xplus, n_m0)
if np.isnan(rotax).any() or not rotax.any():
continue
rotmat = rm.rodrigues(rotax, nm0xangle)
v_m1m0onxplus = np.dot(v_m1m0, rotmat)
v_m1m0onxplusyzproj = np.asarray([0, v_m1m0onxplus[1], v_m1m0onxplus[2]])
alpha_m0 = rm.radian_between(v_m1m0onxplusyzproj, yplus)
if v_m1m0onxplus[2] < 0:
alpha_m0 = 2*math.pi - alpha_m0
if key in self.gmd.keys():
malist = self.gmd[key]
print len(malist)
for maslot in malist:
alpha = math.degrees(alpha_m0-maslot[2])
try:
alphadiscrete = int(math.floor(alpha/dangle)*dangle+dangle)
except:
continue
acckey = (maslot[0], alphadiscrete)
if acckey in accspace.keys():
accspace[acckey] += 1
else:
accspace[acckey] = 1
if len(accspace.keys()) is 0:
return (None, None)
# find top matches and rot matrices
maxn = sorted(accspace.iteritems(), key=operator.itemgetter(1), reverse=True)[:5]
rotmat4list = []
silist = []
milist = []
for maxnele in maxn:
mi, alpha = maxnele[0]
# step1 move to temppnts[mi]
displacement0 = -self.temppnts[mi]
rotmat4_0 = Mat4.translateMat(displacement0[0], displacement0[1], displacement0[2])
# step2 rotate to goal
normalangle = math.degrees(rm.radian_between(self.tempnormals[mi], perceivednormals[i]))
normalrotax = np.cross(self.tempnormals[mi], perceivednormals[i])
normalrotmat = rm.rodrigues(normalrotax, normalangle)
anglerotmat = rm.rodrigues(perceivednormals[i], -alpha)
rotmat = np.dot(anglerotmat, normalrotmat)
rotmat4_1 = pg.npToMat4(rotmat)
# step3 move to perceivedpnts[i]
displacement1 = perceivedpnts[i]
rotmat4_2 = Mat4.translateMat(displacement1[0], displacement1[1], displacement1[2])
rotmat4 = rotmat4_0*rotmat4_1*rotmat4_2
rotmat4list.append(rotmat4)
silist.append(i)
milist.append(mi)
return rotmat4list, silist, milist